[61] | 1 | /*! |
---|
| 2 | \file |
---|
| 3 | \brief Bayesian Filtering for linear Gaussian models (Kalman Filter) and extensions |
---|
| 4 | \author Vaclav Smidl. |
---|
| 5 | |
---|
| 6 | ----------------------------------- |
---|
[1240] | 7 | BDM++ - C++ library for Bayesian Decision Making under Uncertaint16y |
---|
[61] | 8 | |
---|
| 9 | Using IT++ for numerical operations |
---|
| 10 | ----------------------------------- |
---|
| 11 | */ |
---|
| 12 | |
---|
| 13 | #ifndef EKFfix_H |
---|
| 14 | #define EKFfix_H |
---|
| 15 | |
---|
[262] | 16 | |
---|
[384] | 17 | #include <estim/kalman.h> |
---|
[61] | 18 | #include "fixed.h" |
---|
| 19 | #include "matrix.h" |
---|
[1174] | 20 | #include "matrix_vs.h" |
---|
[1226] | 21 | #include "reference_Q15.h" |
---|
[61] | 22 | #include "parametry_motoru.h" |
---|
[1380] | 23 | #include "mpf_double.h" |
---|
[61] | 24 | |
---|
[254] | 25 | using namespace bdm; |
---|
[61] | 26 | |
---|
| 27 | double minQ(double Q); |
---|
| 28 | |
---|
[1240] | 29 | void mat_to_int16(const imat &M, int16 *I); |
---|
| 30 | void vec_to_int16(const ivec &v, int16 *I); |
---|
[1201] | 31 | void UDtof(const mat &U, const vec &D, imat &Uf, ivec &Df, const vec &xref); |
---|
[1240] | 32 | |
---|
| 33 | #ifdef XXX |
---|
[61] | 34 | /*! |
---|
[1240] | 35 | \brief Extended Kalman Filter with full matrices in fixed point16 arithmetic |
---|
[61] | 36 | |
---|
| 37 | An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 38 | */ |
---|
[283] | 39 | class EKFfixed : public BM { |
---|
[61] | 40 | public: |
---|
| 41 | void init_ekf(double Tv); |
---|
| 42 | void ekf(double ux, double uy, double isxd, double isyd); |
---|
| 43 | |
---|
| 44 | /* Declaration of local functions */ |
---|
[1240] | 45 | void prediction(int16 *ux); |
---|
[61] | 46 | void correction(void); |
---|
| 47 | void update_psi(void); |
---|
| 48 | |
---|
| 49 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
---|
[1240] | 50 | int16 Q[16]; /* matrix [4,4] */ |
---|
| 51 | int16 R[4]; /* matrix [2,2] */ |
---|
[61] | 52 | |
---|
[1240] | 53 | int16 x_est[4]; |
---|
| 54 | int16 x_pred[4]; |
---|
| 55 | int16 P_pred[16]; /* matrix [4,4] */ |
---|
| 56 | int16 P_est[16]; /* matrix [4,4] */ |
---|
| 57 | int16 Y_mes[2]; |
---|
| 58 | int16 ukalm[2]; |
---|
| 59 | int16 Kalm[8]; /* matrix [5,2] */ |
---|
[61] | 60 | |
---|
[1240] | 61 | int16 PSI[16]; /* matrix [4,4] */ |
---|
[61] | 62 | |
---|
[1240] | 63 | int16 temp15a[16]; |
---|
[61] | 64 | |
---|
[1240] | 65 | int16 cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
---|
[61] | 66 | |
---|
[1240] | 67 | int32 temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */ |
---|
[61] | 68 | enorm<fsqmat> E; |
---|
| 69 | mat Ry; |
---|
| 70 | |
---|
| 71 | public: |
---|
| 72 | //! Default constructor |
---|
[283] | 73 | EKFfixed ():BM(),E(),Ry(2,2){ |
---|
[1240] | 74 | int16 i; |
---|
[1168] | 75 | for(i=0;i<16;i++){Q[i]=0;} |
---|
| 76 | for(i=0;i<4;i++){R[i]=0;} |
---|
[61] | 77 | |
---|
[1168] | 78 | for(i=0;i<4;i++){x_est[i]=0;} |
---|
| 79 | for(i=0;i<4;i++){x_pred[i]=0;} |
---|
| 80 | for(i=0;i<16;i++){P_pred[i]=0;} |
---|
| 81 | for(i=0;i<16;i++){P_est[i]=0;} |
---|
| 82 | P_est[0]=0x7FFF; |
---|
| 83 | P_est[5]=0x7FFF; |
---|
| 84 | P_est[10]=0x7FFF; |
---|
| 85 | P_est[15]=0x7FFF; |
---|
| 86 | for(i=0;i<2;i++){Y_mes[i]=0;} |
---|
| 87 | for(i=0;i<2;i++){ukalm[i]=0;} |
---|
| 88 | for(i=0;i<8;i++){Kalm[i]=0;} |
---|
[61] | 89 | |
---|
[1168] | 90 | for(i=0;i<16;i++){PSI[i]=0;} |
---|
| 91 | |
---|
| 92 | set_dim(4); |
---|
| 93 | E._mu()=zeros(4); |
---|
| 94 | E._R()=zeros(4,4); |
---|
| 95 | init_ekf(0.000125); |
---|
| 96 | }; |
---|
[61] | 97 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
[1168] | 98 | void bayes ( const vec &yt, const vec &ut ); |
---|
[61] | 99 | //!dummy! |
---|
[1168] | 100 | const epdf& posterior() const {return E;}; |
---|
| 101 | |
---|
[61] | 102 | }; |
---|
| 103 | |
---|
[1168] | 104 | UIREGISTER(EKFfixed); |
---|
[61] | 105 | |
---|
[1240] | 106 | #endif |
---|
| 107 | |
---|
[1264] | 108 | //! EKF for testing q44 |
---|
[1294] | 109 | class EKFtest: public EKF_UD{ |
---|
[1264] | 110 | void bayes ( const vec &yt, const vec &cond ) { |
---|
[1294] | 111 | EKF_UD::bayes(yt,cond); |
---|
| 112 | vec D = prior()._R()._D(); |
---|
| 113 | |
---|
| 114 | if (D(3)>10) D(3) = 10; |
---|
| 115 | |
---|
| 116 | prior()._R().__D()=D; |
---|
[1264] | 117 | } |
---|
| 118 | }; |
---|
| 119 | UIREGISTER(EKFtest); |
---|
| 120 | |
---|
[1179] | 121 | /*! |
---|
[1240] | 122 | \brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic |
---|
[1179] | 123 | |
---|
| 124 | An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 125 | */ |
---|
| 126 | class EKFfixedUD : public BM { |
---|
| 127 | public: |
---|
[1226] | 128 | LOG_LEVEL(EKFfixedUD,logU, logG, logD, logA, logP); |
---|
[1201] | 129 | |
---|
[1179] | 130 | void init_ekf(double Tv); |
---|
| 131 | void ekf(double ux, double uy, double isxd, double isyd); |
---|
| 132 | |
---|
| 133 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
---|
[1240] | 134 | int16 Q[16]; /* matrix [4,4] */ |
---|
| 135 | int16 R[4]; /* matrix [2,2] */ |
---|
[1179] | 136 | |
---|
[1240] | 137 | int16 x_est[4]; /* estimate and prediction */ |
---|
[1179] | 138 | |
---|
[1240] | 139 | int16 PSI[16]; /* matrix [4,4] */ |
---|
| 140 | int16 PSIU[16]; /* matrix PIS*U, [4,4] */ |
---|
[1179] | 141 | |
---|
[1240] | 142 | int16 Uf[16]; // upper triangular of covariance (inplace) |
---|
| 143 | int16 Df[4]; // diagonal covariance |
---|
| 144 | int16 Dfold[4]; // temp of D |
---|
| 145 | int16 G[16]; // temp for bierman |
---|
[1179] | 146 | |
---|
[1240] | 147 | int16 cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
---|
[1179] | 148 | |
---|
| 149 | enorm<fsqmat> E; |
---|
| 150 | mat Ry; |
---|
| 151 | |
---|
| 152 | public: |
---|
| 153 | //! Default constructor |
---|
| 154 | EKFfixedUD ():BM(),E(),Ry(2,2){ |
---|
[1240] | 155 | int16 i; |
---|
[1179] | 156 | for(i=0;i<16;i++){Q[i]=0;} |
---|
| 157 | for(i=0;i<4;i++){R[i]=0;} |
---|
| 158 | |
---|
| 159 | for(i=0;i<4;i++){x_est[i]=0;} |
---|
| 160 | for(i=0;i<16;i++){Uf[i]=0;} |
---|
| 161 | for(i=0;i<4;i++){Df[i]=0;} |
---|
| 162 | for(i=0;i<16;i++){G[i]=0;} |
---|
| 163 | for(i=0;i<4;i++){Dfold[i]=0;} |
---|
| 164 | |
---|
| 165 | for(i=0;i<16;i++){PSI[i]=0;} |
---|
| 166 | |
---|
| 167 | set_dim(4); |
---|
[1304] | 168 | dimy = 2; |
---|
| 169 | dimc = 2; |
---|
[1179] | 170 | E._mu()=zeros(4); |
---|
| 171 | E._R()=zeros(4,4); |
---|
| 172 | init_ekf(0.000125); |
---|
| 173 | }; |
---|
| 174 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 175 | void bayes ( const vec &yt, const vec &ut ); |
---|
| 176 | //!dummy! |
---|
| 177 | const epdf& posterior() const {return E;}; |
---|
[1201] | 178 | void log_register(logger &L, const string &prefix){ |
---|
| 179 | BM::log_register ( L, prefix ); |
---|
| 180 | |
---|
| 181 | L.add_vector ( log_level, logG, RV("G",16), prefix ); |
---|
| 182 | L.add_vector ( log_level, logU, RV ("U", 16 ), prefix ); |
---|
| 183 | L.add_vector ( log_level, logD, RV ("D", 4 ), prefix ); |
---|
| 184 | L.add_vector ( log_level, logA, RV ("A", 16 ), prefix ); |
---|
[1226] | 185 | L.add_vector ( log_level, logP, RV ("P", 16 ), prefix ); |
---|
[1201] | 186 | |
---|
| 187 | }; |
---|
| 188 | //void from_setting(); |
---|
[1179] | 189 | }; |
---|
| 190 | |
---|
| 191 | UIREGISTER(EKFfixedUD); |
---|
| 192 | |
---|
[1226] | 193 | /*! |
---|
[1305] | 194 | * \brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic |
---|
| 195 | * |
---|
| 196 | * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 197 | */ |
---|
| 198 | class EKFfixedUD2 : public BM { |
---|
| 199 | public: |
---|
[1311] | 200 | LOG_LEVEL(EKFfixedUD2,logU, logG, logD, logA, logC, logP); |
---|
[1305] | 201 | |
---|
| 202 | void init_ekf2(double Tv); |
---|
| 203 | void ekf2(double ux, double uy, double isxd, double isyd); |
---|
| 204 | |
---|
| 205 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
---|
| 206 | int16 Q[4]; /* matrix [4,4] */ |
---|
| 207 | int16 R[4]; /* matrix [2,2] */ |
---|
| 208 | |
---|
| 209 | int16 x_est[2]; /* estimate and prediction */ |
---|
[1311] | 210 | int16 y_est[2]; /* estimate and prediction */ |
---|
| 211 | int16 y_old[2]; /* estimate and prediction */ |
---|
[1305] | 212 | |
---|
| 213 | int16 PSI[4]; /* matrix [4,4] */ |
---|
| 214 | int16 PSIU[4]; /* matrix PIS*U, [4,4] */ |
---|
[1311] | 215 | int16 C[4]; /* matrix [4,4] */ |
---|
[1305] | 216 | |
---|
| 217 | int16 Uf[4]; // upper triangular of covariance (inplace) |
---|
| 218 | int16 Df[2]; // diagonal covariance |
---|
| 219 | int16 Dfold[2]; // temp of D |
---|
| 220 | int16 G[4]; // temp for bierman |
---|
| 221 | |
---|
| 222 | int16 cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
---|
| 223 | |
---|
| 224 | enorm<fsqmat> E; |
---|
| 225 | mat Ry; |
---|
| 226 | |
---|
| 227 | public: |
---|
| 228 | //! Default constructor |
---|
| 229 | EKFfixedUD2 ():BM(),E(),Ry(2,2){ |
---|
| 230 | int16 i; |
---|
| 231 | for(i=0;i<4;i++){Q[i]=0;} |
---|
| 232 | for(i=0;i<4;i++){R[i]=0;} |
---|
| 233 | |
---|
| 234 | for(i=0;i<2;i++){x_est[i]=0;} |
---|
[1311] | 235 | for(i=0;i<2;i++){y_est[i]=0;} |
---|
| 236 | for(i=0;i<2;i++){y_old[i]=0;} |
---|
[1305] | 237 | for(i=0;i<4;i++){Uf[i]=0;} |
---|
| 238 | for(i=0;i<2;i++){Df[i]=0;} |
---|
| 239 | for(i=0;i<4;i++){G[i]=0;} |
---|
| 240 | for(i=0;i<2;i++){Dfold[i]=0;} |
---|
| 241 | |
---|
| 242 | for(i=0;i<4;i++){PSI[i]=0;} |
---|
[1311] | 243 | for(i=0;i<4;i++){C[i]=0;} |
---|
[1305] | 244 | |
---|
| 245 | set_dim(2); |
---|
[1311] | 246 | dimc = 2; |
---|
| 247 | dimy = 2; |
---|
[1305] | 248 | E._mu()=zeros(2); |
---|
| 249 | E._R()=zeros(2,2); |
---|
| 250 | init_ekf2(0.000125); |
---|
| 251 | }; |
---|
| 252 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 253 | void bayes ( const vec &yt, const vec &ut ); |
---|
| 254 | //!dummy! |
---|
| 255 | const epdf& posterior() const {return E;}; |
---|
| 256 | void log_register(logger &L, const string &prefix){ |
---|
| 257 | BM::log_register ( L, prefix ); |
---|
| 258 | |
---|
[1311] | 259 | L.add_vector ( log_level, logG, RV("G2",4), prefix ); |
---|
| 260 | L.add_vector ( log_level, logU, RV ("U2", 4 ), prefix ); |
---|
| 261 | L.add_vector ( log_level, logD, RV ("D2", 2 ), prefix ); |
---|
| 262 | L.add_vector ( log_level, logA, RV ("A2", 4 ), prefix ); |
---|
| 263 | L.add_vector ( log_level, logC, RV ("C2", 4 ), prefix ); |
---|
| 264 | L.add_vector ( log_level, logP, RV ("P2", 4 ), prefix ); |
---|
[1305] | 265 | |
---|
| 266 | }; |
---|
| 267 | //void from_setting(); |
---|
| 268 | }; |
---|
| 269 | |
---|
| 270 | UIREGISTER(EKFfixedUD2); |
---|
| 271 | |
---|
| 272 | /*! |
---|
[1341] | 273 | * \brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic |
---|
| 274 | * |
---|
| 275 | * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 276 | */ |
---|
| 277 | class EKFfixedUD3 : public BM { |
---|
| 278 | public: |
---|
| 279 | LOG_LEVEL(EKFfixedUD3,logU, logG, logD, logA, logC, logP); |
---|
| 280 | |
---|
| 281 | void init_ekf3(double Tv); |
---|
| 282 | void ekf3(double ux, double uy, double isxd, double isyd); |
---|
| 283 | |
---|
| 284 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
---|
| 285 | int16 Q[9]; /* matrix [4,4] */ |
---|
| 286 | int16 R[4]; /* matrix [2,2] */ |
---|
| 287 | |
---|
| 288 | int16 x_est[3]; /* estimate and prediction */ |
---|
| 289 | int16 y_est[2]; /* estimate and prediction */ |
---|
| 290 | int16 y_old[2]; /* estimate and prediction */ |
---|
| 291 | |
---|
| 292 | int16 PSI[9]; /* matrix [4,4] */ |
---|
| 293 | int16 PSIU[9]; /* matrix PIS*U, [4,4] */ |
---|
| 294 | int16 C[6]; /* matrix [4,4] */ |
---|
| 295 | |
---|
| 296 | int16 Uf[9]; // upper triangular of covariance (inplace) |
---|
| 297 | int16 Df[3]; // diagonal covariance |
---|
| 298 | int16 Dfold[3]; // temp of D |
---|
| 299 | int16 G[9]; // temp for bierman |
---|
| 300 | |
---|
| 301 | int16 cA, cB, cC, cG, cF, cH; // cD, cE, cF, cI ... nepouzivane |
---|
| 302 | |
---|
| 303 | enorm<fsqmat> E; |
---|
| 304 | mat Ry; |
---|
| 305 | |
---|
| 306 | public: |
---|
| 307 | //! Default constructor |
---|
| 308 | EKFfixedUD3 ():BM(),E(),Ry(2,2){ |
---|
| 309 | int16 i; |
---|
| 310 | for(i=0;i<9;i++){Q[i]=0;} |
---|
| 311 | for(i=0;i<4;i++){R[i]=0;} |
---|
| 312 | |
---|
| 313 | for(i=0;i<3;i++){x_est[i]=0;} |
---|
| 314 | for(i=0;i<2;i++){y_est[i]=0;} |
---|
| 315 | for(i=0;i<2;i++){y_old[i]=0;} |
---|
| 316 | for(i=0;i<9;i++){Uf[i]=0;} |
---|
| 317 | for(i=0;i<3;i++){Df[i]=0;} |
---|
| 318 | for(i=0;i<4;i++){G[i]=0;} |
---|
| 319 | for(i=0;i<3;i++){Dfold[i]=0;} |
---|
| 320 | |
---|
| 321 | for(i=0;i<9;i++){PSI[i]=0;} |
---|
| 322 | for(i=0;i<6;i++){C[i]=0;} |
---|
| 323 | |
---|
| 324 | set_dim(3); |
---|
| 325 | dimc = 2; |
---|
| 326 | dimy = 2; |
---|
| 327 | E._mu()=zeros(3); |
---|
| 328 | E._R()=zeros(3,3); |
---|
| 329 | init_ekf3(0.000125); |
---|
| 330 | }; |
---|
| 331 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 332 | void bayes ( const vec &yt, const vec &ut ); |
---|
| 333 | //!dummy! |
---|
| 334 | const epdf& posterior() const {return E;}; |
---|
| 335 | void log_register(logger &L, const string &prefix){ |
---|
| 336 | BM::log_register ( L, prefix ); |
---|
| 337 | }; |
---|
| 338 | //void from_setting(); |
---|
| 339 | }; |
---|
| 340 | |
---|
| 341 | UIREGISTER(EKFfixedUD3); |
---|
| 342 | |
---|
| 343 | /*! |
---|
[1240] | 344 | * \brief Extended Kalman Filter with Chol matrices in fixed point16 arithmetic |
---|
[1226] | 345 | * |
---|
| 346 | * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 347 | */ |
---|
| 348 | class EKFfixedCh : public BM { |
---|
| 349 | public: |
---|
| 350 | LOG_LEVEL(EKFfixedCh,logCh, logA, logP); |
---|
| 351 | |
---|
| 352 | void init_ekf(double Tv); |
---|
| 353 | void ekf(double ux, double uy, double isxd, double isyd); |
---|
| 354 | |
---|
| 355 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
---|
[1240] | 356 | int16 Q[16]; /* matrix [4,4] */ |
---|
| 357 | int16 R[4]; /* matrix [2,2] */ |
---|
[1226] | 358 | |
---|
[1240] | 359 | int16 x_est[4]; /* estimate and prediction */ |
---|
[1226] | 360 | |
---|
[1240] | 361 | int16 PSI[16]; /* matrix [4,4] */ |
---|
| 362 | int16 PSICh[16]; /* matrix PIS*U, [4,4] */ |
---|
[1226] | 363 | |
---|
[1240] | 364 | int16 Chf[16]; // upper triangular of covariance (inplace) |
---|
[1226] | 365 | |
---|
[1240] | 366 | int16 cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
---|
[1226] | 367 | |
---|
| 368 | enorm<chmat> E; |
---|
| 369 | mat Ry; |
---|
| 370 | |
---|
| 371 | public: |
---|
| 372 | //! Default constructor |
---|
| 373 | EKFfixedCh ():BM(),E(),Ry(2,2){ |
---|
[1240] | 374 | int16 i; |
---|
[1226] | 375 | for(i=0;i<16;i++){Q[i]=0;} |
---|
| 376 | for(i=0;i<4;i++){R[i]=0;} |
---|
| 377 | |
---|
| 378 | for(i=0;i<4;i++){x_est[i]=0;} |
---|
| 379 | for(i=0;i<16;i++){Chf[i]=0;} |
---|
| 380 | |
---|
| 381 | for(i=0;i<16;i++){PSI[i]=0;} |
---|
| 382 | |
---|
| 383 | set_dim(4); |
---|
[1321] | 384 | dimc = 2; |
---|
| 385 | dimy =2; |
---|
[1226] | 386 | E._mu()=zeros(4); |
---|
| 387 | E._R()=zeros(4,4); |
---|
| 388 | init_ekf(0.000125); |
---|
| 389 | }; |
---|
| 390 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 391 | void bayes ( const vec &yt, const vec &ut ); |
---|
| 392 | //!dummy! |
---|
| 393 | const epdf& posterior() const {return E;}; |
---|
| 394 | void log_register(logger &L, const string &prefix){ |
---|
| 395 | BM::log_register ( L, prefix ); |
---|
| 396 | |
---|
| 397 | L.add_vector ( log_level, logCh, RV ("Ch", 16 ), prefix ); |
---|
| 398 | L.add_vector ( log_level, logA, RV ("A", 16 ), prefix ); |
---|
| 399 | L.add_vector ( log_level, logP, RV ("P", 16 ), prefix ); |
---|
| 400 | |
---|
| 401 | }; |
---|
| 402 | //void from_setting(); |
---|
| 403 | }; |
---|
| 404 | |
---|
| 405 | UIREGISTER(EKFfixedCh); |
---|
| 406 | |
---|
[1326] | 407 | /*! |
---|
| 408 | * \brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic |
---|
| 409 | * |
---|
| 410 | * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 411 | */ |
---|
| 412 | class EKFfixedCh2 : public BM { |
---|
| 413 | public: |
---|
| 414 | LOG_LEVEL(EKFfixedCh2,logCh, logA, logC, logP); |
---|
| 415 | |
---|
| 416 | void init_ekf2(double Tv); |
---|
| 417 | void ekf2(double ux, double uy, double isxd, double isyd); |
---|
| 418 | |
---|
| 419 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
---|
| 420 | int16 Q[4]; /* matrix [4,4] */ |
---|
| 421 | int16 R[4]; /* matrix [2,2] */ |
---|
| 422 | |
---|
| 423 | int16 x_est[2]; /* estimate and prediction */ |
---|
| 424 | int16 y_est[2]; /* estimate and prediction */ |
---|
| 425 | int16 y_old[2]; /* estimate and prediction */ |
---|
| 426 | |
---|
| 427 | int16 PSI[4]; /* matrix [4,4] */ |
---|
| 428 | int16 PSICh[4]; /* matrix PIS*U, [4,4] */ |
---|
| 429 | int16 C[4]; /* matrix [4,4] */ |
---|
| 430 | |
---|
| 431 | int16 Chf[4]; // upper triangular of covariance (inplace) |
---|
| 432 | |
---|
| 433 | int16 cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
---|
| 434 | |
---|
| 435 | enorm<fsqmat> E; |
---|
| 436 | mat Ry; |
---|
| 437 | |
---|
| 438 | public: |
---|
| 439 | //! Default constructor |
---|
| 440 | EKFfixedCh2 ():BM(),E(),Ry(2,2){ |
---|
| 441 | int16 i; |
---|
| 442 | for(i=0;i<4;i++){Q[i]=0;} |
---|
| 443 | for(i=0;i<4;i++){R[i]=0;} |
---|
| 444 | |
---|
| 445 | for(i=0;i<2;i++){x_est[i]=0;} |
---|
| 446 | for(i=0;i<2;i++){y_est[i]=0;} |
---|
| 447 | for(i=0;i<2;i++){y_old[i]=0;} |
---|
| 448 | for(i=0;i<4;i++){Chf[i]=0;} |
---|
| 449 | |
---|
| 450 | for(i=0;i<4;i++){PSI[i]=0;} |
---|
| 451 | for(i=0;i<4;i++){C[i]=0;} |
---|
| 452 | |
---|
| 453 | set_dim(2); |
---|
| 454 | dimc = 2; |
---|
| 455 | dimy = 2; |
---|
| 456 | E._mu()=zeros(2); |
---|
| 457 | E._R()=zeros(2,2); |
---|
| 458 | init_ekf2(0.000125); |
---|
| 459 | }; |
---|
| 460 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 461 | void bayes ( const vec &yt, const vec &ut ); |
---|
| 462 | //!dummy! |
---|
| 463 | const epdf& posterior() const {return E;}; |
---|
| 464 | void log_register(logger &L, const string &prefix){ |
---|
| 465 | BM::log_register ( L, prefix ); |
---|
| 466 | |
---|
| 467 | L.add_vector ( log_level, logCh, RV ("Ch2", 4 ), prefix ); |
---|
| 468 | L.add_vector ( log_level, logA, RV ("A2", 4 ), prefix ); |
---|
| 469 | L.add_vector ( log_level, logC, RV ("C2", 4 ), prefix ); |
---|
| 470 | L.add_vector ( log_level, logP, RV ("P2", 4 ), prefix ); |
---|
| 471 | |
---|
| 472 | }; |
---|
| 473 | //void from_setting(); |
---|
| 474 | }; |
---|
[1226] | 475 | |
---|
[1326] | 476 | UIREGISTER(EKFfixedCh2); |
---|
| 477 | |
---|
| 478 | |
---|
[1240] | 479 | //! EKF for comparison of EKF_UD with its fixed-point16 implementation |
---|
[1174] | 480 | class EKF_UDfix : public BM { |
---|
| 481 | protected: |
---|
| 482 | //! logger |
---|
| 483 | LOG_LEVEL(EKF_UDfix,logU, logG); |
---|
| 484 | //! Internal Model f(x,u) |
---|
| 485 | shared_ptr<diffbifn> pfxu; |
---|
| 486 | |
---|
| 487 | //! Observation Model h(x,u) |
---|
| 488 | shared_ptr<diffbifn> phxu; |
---|
| 489 | |
---|
| 490 | //! U part |
---|
| 491 | mat U; |
---|
| 492 | //! D part |
---|
| 493 | vec D; |
---|
[1201] | 494 | |
---|
[1174] | 495 | mat A; |
---|
| 496 | mat C; |
---|
| 497 | mat Q; |
---|
| 498 | vec R; |
---|
| 499 | |
---|
| 500 | enorm<ldmat> est; |
---|
| 501 | |
---|
| 502 | |
---|
| 503 | public: |
---|
| 504 | |
---|
| 505 | //! copy constructor duplicated |
---|
| 506 | EKF_UDfix* _copy() const { |
---|
| 507 | return new EKF_UDfix(*this); |
---|
| 508 | } |
---|
| 509 | |
---|
| 510 | const enorm<ldmat>& posterior()const{return est;}; |
---|
| 511 | |
---|
| 512 | enorm<ldmat>& prior() { |
---|
| 513 | return const_cast<enorm<ldmat>&>(posterior()); |
---|
| 514 | } |
---|
| 515 | |
---|
| 516 | EKF_UDfix(){} |
---|
| 517 | |
---|
| 518 | |
---|
| 519 | EKF_UDfix(const EKF_UDfix &E0): pfxu(E0.pfxu),phxu(E0.phxu), U(E0.U), D(E0.D){} |
---|
| 520 | |
---|
| 521 | //! Set nonlinear functions for mean values and covariance matrices. |
---|
| 522 | void set_parameters ( const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const mat Q0, const vec R0 ); |
---|
| 523 | |
---|
| 524 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 525 | void bayes ( const vec &yt, const vec &cond = empty_vec ); |
---|
| 526 | |
---|
| 527 | void log_register ( bdm::logger& L, const string& prefix ){ |
---|
| 528 | BM::log_register ( L, prefix ); |
---|
| 529 | |
---|
| 530 | if ( log_level[logU] ) |
---|
| 531 | L.add_vector ( log_level, logU, RV ( dimension()*dimension() ), prefix ); |
---|
| 532 | if ( log_level[logG] ) |
---|
| 533 | L.add_vector ( log_level, logG, RV ( dimension()*dimension() ), prefix ); |
---|
| 534 | |
---|
| 535 | } |
---|
| 536 | /*! Create object from the following structure |
---|
| 537 | |
---|
| 538 | \code |
---|
| 539 | class = 'EKF_UD'; |
---|
| 540 | OM = configuration of bdm::diffbifn; % any offspring of diffbifn, bdm::diffbifn::from_setting |
---|
| 541 | IM = configuration of bdm::diffbifn; % any offspring of diffbifn, bdm::diffbifn::from_setting |
---|
| 542 | dQ = [...]; % vector containing diagonal of Q |
---|
| 543 | dR = [...]; % vector containing diagonal of R |
---|
| 544 | --- optional fields --- |
---|
| 545 | mu0 = [...]; % vector of statistics mu0 |
---|
| 546 | dP0 = [...]; % vector containing diagonal of P0 |
---|
| 547 | -- or -- |
---|
| 548 | P0 = [...]; % full matrix P0 |
---|
| 549 | --- inherited fields --- |
---|
| 550 | bdm::BM::from_setting |
---|
| 551 | \endcode |
---|
| 552 | If the optional fields are not given, they will be filled as follows: |
---|
| 553 | \code |
---|
| 554 | mu0 = [0,0,0,....]; % empty statistics |
---|
| 555 | P0 = eye( dim ); |
---|
| 556 | \endcode |
---|
| 557 | */ |
---|
| 558 | void from_setting ( const Setting &set ); |
---|
| 559 | |
---|
| 560 | void validate() {}; |
---|
| 561 | // TODO dodelat void to_setting( Setting &set ) const; |
---|
| 562 | |
---|
| 563 | }; |
---|
| 564 | UIREGISTER(EKF_UDfix); |
---|
| 565 | |
---|
| 566 | |
---|
[1380] | 567 | class MPF_pmsm_red:public BM{ |
---|
[1179] | 568 | |
---|
| 569 | |
---|
[1380] | 570 | public: |
---|
| 571 | MPF_pmsm_red(){ |
---|
| 572 | dimy=2; |
---|
| 573 | dimc=2; |
---|
| 574 | mpf_init(1e-1, 1e-6, 1e-1); |
---|
| 575 | }; |
---|
| 576 | void bayes ( const vec &val, const vec &cond ) { |
---|
| 577 | const double &isa = val(0); |
---|
| 578 | const double &isb = val(1); |
---|
| 579 | const double &usa = cond(0); |
---|
| 580 | const double &usb = cond(1); |
---|
| 581 | mpf_bayes(isa,isb,usa,usb); |
---|
| 582 | } |
---|
| 583 | |
---|
| 584 | class mp:public epdf{ |
---|
| 585 | public: |
---|
| 586 | mp():epdf(){set_dim(N);} |
---|
| 587 | vec sample() const {return zeros(N);} |
---|
| 588 | double evallog(const vec &v) const {return 0.0;} |
---|
| 589 | vec mean() const {vec tmp(N); mpf_th(tmp._data()); return tmp;} |
---|
| 590 | vec variance() const {return zeros(N);} |
---|
| 591 | }; |
---|
| 592 | |
---|
| 593 | mp mypdf; |
---|
| 594 | const mp& posterior() const {return mypdf;} |
---|
| 595 | }; |
---|
| 596 | UIREGISTER(MPF_pmsm_red); |
---|
| 597 | |
---|
| 598 | |
---|
[61] | 599 | #endif // KF_H |
---|
| 600 | |
---|