[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" |
---|
| 23 | |
---|
[254] | 24 | using namespace bdm; |
---|
[61] | 25 | |
---|
| 26 | double minQ(double Q); |
---|
| 27 | |
---|
[1240] | 28 | void mat_to_int16(const imat &M, int16 *I); |
---|
| 29 | void vec_to_int16(const ivec &v, int16 *I); |
---|
[1201] | 30 | void UDtof(const mat &U, const vec &D, imat &Uf, ivec &Df, const vec &xref); |
---|
[1240] | 31 | |
---|
| 32 | #ifdef XXX |
---|
[61] | 33 | /*! |
---|
[1240] | 34 | \brief Extended Kalman Filter with full matrices in fixed point16 arithmetic |
---|
[61] | 35 | |
---|
| 36 | An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 37 | */ |
---|
[283] | 38 | class EKFfixed : public BM { |
---|
[61] | 39 | public: |
---|
| 40 | void init_ekf(double Tv); |
---|
| 41 | void ekf(double ux, double uy, double isxd, double isyd); |
---|
| 42 | |
---|
| 43 | /* Declaration of local functions */ |
---|
[1240] | 44 | void prediction(int16 *ux); |
---|
[61] | 45 | void correction(void); |
---|
| 46 | void update_psi(void); |
---|
| 47 | |
---|
| 48 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
---|
[1240] | 49 | int16 Q[16]; /* matrix [4,4] */ |
---|
| 50 | int16 R[4]; /* matrix [2,2] */ |
---|
[61] | 51 | |
---|
[1240] | 52 | int16 x_est[4]; |
---|
| 53 | int16 x_pred[4]; |
---|
| 54 | int16 P_pred[16]; /* matrix [4,4] */ |
---|
| 55 | int16 P_est[16]; /* matrix [4,4] */ |
---|
| 56 | int16 Y_mes[2]; |
---|
| 57 | int16 ukalm[2]; |
---|
| 58 | int16 Kalm[8]; /* matrix [5,2] */ |
---|
[61] | 59 | |
---|
[1240] | 60 | int16 PSI[16]; /* matrix [4,4] */ |
---|
[61] | 61 | |
---|
[1240] | 62 | int16 temp15a[16]; |
---|
[61] | 63 | |
---|
[1240] | 64 | int16 cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
---|
[61] | 65 | |
---|
[1240] | 66 | int32 temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */ |
---|
[61] | 67 | enorm<fsqmat> E; |
---|
| 68 | mat Ry; |
---|
| 69 | |
---|
| 70 | public: |
---|
| 71 | //! Default constructor |
---|
[283] | 72 | EKFfixed ():BM(),E(),Ry(2,2){ |
---|
[1240] | 73 | int16 i; |
---|
[1168] | 74 | for(i=0;i<16;i++){Q[i]=0;} |
---|
| 75 | for(i=0;i<4;i++){R[i]=0;} |
---|
[61] | 76 | |
---|
[1168] | 77 | for(i=0;i<4;i++){x_est[i]=0;} |
---|
| 78 | for(i=0;i<4;i++){x_pred[i]=0;} |
---|
| 79 | for(i=0;i<16;i++){P_pred[i]=0;} |
---|
| 80 | for(i=0;i<16;i++){P_est[i]=0;} |
---|
| 81 | P_est[0]=0x7FFF; |
---|
| 82 | P_est[5]=0x7FFF; |
---|
| 83 | P_est[10]=0x7FFF; |
---|
| 84 | P_est[15]=0x7FFF; |
---|
| 85 | for(i=0;i<2;i++){Y_mes[i]=0;} |
---|
| 86 | for(i=0;i<2;i++){ukalm[i]=0;} |
---|
| 87 | for(i=0;i<8;i++){Kalm[i]=0;} |
---|
[61] | 88 | |
---|
[1168] | 89 | for(i=0;i<16;i++){PSI[i]=0;} |
---|
| 90 | |
---|
| 91 | set_dim(4); |
---|
| 92 | E._mu()=zeros(4); |
---|
| 93 | E._R()=zeros(4,4); |
---|
| 94 | init_ekf(0.000125); |
---|
| 95 | }; |
---|
[61] | 96 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
[1168] | 97 | void bayes ( const vec &yt, const vec &ut ); |
---|
[61] | 98 | //!dummy! |
---|
[1168] | 99 | const epdf& posterior() const {return E;}; |
---|
| 100 | |
---|
[61] | 101 | }; |
---|
| 102 | |
---|
[1168] | 103 | UIREGISTER(EKFfixed); |
---|
[61] | 104 | |
---|
[1240] | 105 | #endif |
---|
| 106 | |
---|
[1264] | 107 | //! EKF for testing q44 |
---|
| 108 | class EKFtest: public EKFfull{ |
---|
| 109 | void bayes ( const vec &yt, const vec &cond ) { |
---|
| 110 | EKFfull::bayes(yt,cond); |
---|
| 111 | mat &P = est._R(); |
---|
| 112 | if (P(3,3)>3.14) |
---|
| 113 | P(3,3)=3.14; |
---|
| 114 | } |
---|
| 115 | }; |
---|
| 116 | UIREGISTER(EKFtest); |
---|
| 117 | |
---|
[1179] | 118 | /*! |
---|
[1240] | 119 | \brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic |
---|
[1179] | 120 | |
---|
| 121 | An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 122 | */ |
---|
| 123 | class EKFfixedUD : public BM { |
---|
| 124 | public: |
---|
[1226] | 125 | LOG_LEVEL(EKFfixedUD,logU, logG, logD, logA, logP); |
---|
[1201] | 126 | |
---|
[1179] | 127 | void init_ekf(double Tv); |
---|
| 128 | void ekf(double ux, double uy, double isxd, double isyd); |
---|
| 129 | |
---|
| 130 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
---|
[1240] | 131 | int16 Q[16]; /* matrix [4,4] */ |
---|
| 132 | int16 R[4]; /* matrix [2,2] */ |
---|
[1179] | 133 | |
---|
[1240] | 134 | int16 x_est[4]; /* estimate and prediction */ |
---|
[1179] | 135 | |
---|
[1240] | 136 | int16 PSI[16]; /* matrix [4,4] */ |
---|
| 137 | int16 PSIU[16]; /* matrix PIS*U, [4,4] */ |
---|
[1179] | 138 | |
---|
[1240] | 139 | int16 Uf[16]; // upper triangular of covariance (inplace) |
---|
| 140 | int16 Df[4]; // diagonal covariance |
---|
| 141 | int16 Dfold[4]; // temp of D |
---|
| 142 | int16 G[16]; // temp for bierman |
---|
[1179] | 143 | |
---|
[1240] | 144 | int16 cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
---|
[1179] | 145 | |
---|
| 146 | enorm<fsqmat> E; |
---|
| 147 | mat Ry; |
---|
| 148 | |
---|
| 149 | public: |
---|
| 150 | //! Default constructor |
---|
| 151 | EKFfixedUD ():BM(),E(),Ry(2,2){ |
---|
[1240] | 152 | int16 i; |
---|
[1179] | 153 | for(i=0;i<16;i++){Q[i]=0;} |
---|
| 154 | for(i=0;i<4;i++){R[i]=0;} |
---|
| 155 | |
---|
| 156 | for(i=0;i<4;i++){x_est[i]=0;} |
---|
| 157 | for(i=0;i<16;i++){Uf[i]=0;} |
---|
| 158 | for(i=0;i<4;i++){Df[i]=0;} |
---|
| 159 | for(i=0;i<16;i++){G[i]=0;} |
---|
| 160 | for(i=0;i<4;i++){Dfold[i]=0;} |
---|
| 161 | |
---|
| 162 | for(i=0;i<16;i++){PSI[i]=0;} |
---|
| 163 | |
---|
| 164 | set_dim(4); |
---|
| 165 | E._mu()=zeros(4); |
---|
| 166 | E._R()=zeros(4,4); |
---|
| 167 | init_ekf(0.000125); |
---|
| 168 | }; |
---|
| 169 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 170 | void bayes ( const vec &yt, const vec &ut ); |
---|
| 171 | //!dummy! |
---|
| 172 | const epdf& posterior() const {return E;}; |
---|
[1201] | 173 | void log_register(logger &L, const string &prefix){ |
---|
| 174 | BM::log_register ( L, prefix ); |
---|
| 175 | |
---|
| 176 | L.add_vector ( log_level, logG, RV("G",16), prefix ); |
---|
| 177 | L.add_vector ( log_level, logU, RV ("U", 16 ), prefix ); |
---|
| 178 | L.add_vector ( log_level, logD, RV ("D", 4 ), prefix ); |
---|
| 179 | L.add_vector ( log_level, logA, RV ("A", 16 ), prefix ); |
---|
[1226] | 180 | L.add_vector ( log_level, logP, RV ("P", 16 ), prefix ); |
---|
[1201] | 181 | |
---|
| 182 | }; |
---|
| 183 | //void from_setting(); |
---|
[1179] | 184 | }; |
---|
| 185 | |
---|
| 186 | UIREGISTER(EKFfixedUD); |
---|
| 187 | |
---|
[1226] | 188 | /*! |
---|
[1240] | 189 | * \brief Extended Kalman Filter with Chol matrices in fixed point16 arithmetic |
---|
[1226] | 190 | * |
---|
| 191 | * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 192 | */ |
---|
| 193 | class EKFfixedCh : public BM { |
---|
| 194 | public: |
---|
| 195 | LOG_LEVEL(EKFfixedCh,logCh, logA, logP); |
---|
| 196 | |
---|
| 197 | void init_ekf(double Tv); |
---|
| 198 | void ekf(double ux, double uy, double isxd, double isyd); |
---|
| 199 | |
---|
| 200 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
---|
[1240] | 201 | int16 Q[16]; /* matrix [4,4] */ |
---|
| 202 | int16 R[4]; /* matrix [2,2] */ |
---|
[1226] | 203 | |
---|
[1240] | 204 | int16 x_est[4]; /* estimate and prediction */ |
---|
[1226] | 205 | |
---|
[1240] | 206 | int16 PSI[16]; /* matrix [4,4] */ |
---|
| 207 | int16 PSICh[16]; /* matrix PIS*U, [4,4] */ |
---|
[1226] | 208 | |
---|
[1240] | 209 | int16 Chf[16]; // upper triangular of covariance (inplace) |
---|
[1226] | 210 | |
---|
[1240] | 211 | int16 cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
---|
[1226] | 212 | |
---|
| 213 | enorm<chmat> E; |
---|
| 214 | mat Ry; |
---|
| 215 | |
---|
| 216 | public: |
---|
| 217 | //! Default constructor |
---|
| 218 | EKFfixedCh ():BM(),E(),Ry(2,2){ |
---|
[1240] | 219 | int16 i; |
---|
[1226] | 220 | for(i=0;i<16;i++){Q[i]=0;} |
---|
| 221 | for(i=0;i<4;i++){R[i]=0;} |
---|
| 222 | |
---|
| 223 | for(i=0;i<4;i++){x_est[i]=0;} |
---|
| 224 | for(i=0;i<16;i++){Chf[i]=0;} |
---|
| 225 | |
---|
| 226 | for(i=0;i<16;i++){PSI[i]=0;} |
---|
| 227 | |
---|
| 228 | set_dim(4); |
---|
| 229 | E._mu()=zeros(4); |
---|
| 230 | E._R()=zeros(4,4); |
---|
| 231 | init_ekf(0.000125); |
---|
| 232 | }; |
---|
| 233 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 234 | void bayes ( const vec &yt, const vec &ut ); |
---|
| 235 | //!dummy! |
---|
| 236 | const epdf& posterior() const {return E;}; |
---|
| 237 | void log_register(logger &L, const string &prefix){ |
---|
| 238 | BM::log_register ( L, prefix ); |
---|
| 239 | |
---|
| 240 | L.add_vector ( log_level, logCh, RV ("Ch", 16 ), prefix ); |
---|
| 241 | L.add_vector ( log_level, logA, RV ("A", 16 ), prefix ); |
---|
| 242 | L.add_vector ( log_level, logP, RV ("P", 16 ), prefix ); |
---|
| 243 | |
---|
| 244 | }; |
---|
| 245 | //void from_setting(); |
---|
| 246 | }; |
---|
| 247 | |
---|
| 248 | UIREGISTER(EKFfixedCh); |
---|
| 249 | |
---|
| 250 | |
---|
[1240] | 251 | //! EKF for comparison of EKF_UD with its fixed-point16 implementation |
---|
[1174] | 252 | class EKF_UDfix : public BM { |
---|
| 253 | protected: |
---|
| 254 | //! logger |
---|
| 255 | LOG_LEVEL(EKF_UDfix,logU, logG); |
---|
| 256 | //! Internal Model f(x,u) |
---|
| 257 | shared_ptr<diffbifn> pfxu; |
---|
| 258 | |
---|
| 259 | //! Observation Model h(x,u) |
---|
| 260 | shared_ptr<diffbifn> phxu; |
---|
| 261 | |
---|
| 262 | //! U part |
---|
| 263 | mat U; |
---|
| 264 | //! D part |
---|
| 265 | vec D; |
---|
[1201] | 266 | |
---|
[1174] | 267 | mat A; |
---|
| 268 | mat C; |
---|
| 269 | mat Q; |
---|
| 270 | vec R; |
---|
| 271 | |
---|
| 272 | enorm<ldmat> est; |
---|
| 273 | |
---|
| 274 | |
---|
| 275 | public: |
---|
| 276 | |
---|
| 277 | //! copy constructor duplicated |
---|
| 278 | EKF_UDfix* _copy() const { |
---|
| 279 | return new EKF_UDfix(*this); |
---|
| 280 | } |
---|
| 281 | |
---|
| 282 | const enorm<ldmat>& posterior()const{return est;}; |
---|
| 283 | |
---|
| 284 | enorm<ldmat>& prior() { |
---|
| 285 | return const_cast<enorm<ldmat>&>(posterior()); |
---|
| 286 | } |
---|
| 287 | |
---|
| 288 | EKF_UDfix(){} |
---|
| 289 | |
---|
| 290 | |
---|
| 291 | EKF_UDfix(const EKF_UDfix &E0): pfxu(E0.pfxu),phxu(E0.phxu), U(E0.U), D(E0.D){} |
---|
| 292 | |
---|
| 293 | //! Set nonlinear functions for mean values and covariance matrices. |
---|
| 294 | void set_parameters ( const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const mat Q0, const vec R0 ); |
---|
| 295 | |
---|
| 296 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 297 | void bayes ( const vec &yt, const vec &cond = empty_vec ); |
---|
| 298 | |
---|
| 299 | void log_register ( bdm::logger& L, const string& prefix ){ |
---|
| 300 | BM::log_register ( L, prefix ); |
---|
| 301 | |
---|
| 302 | if ( log_level[logU] ) |
---|
| 303 | L.add_vector ( log_level, logU, RV ( dimension()*dimension() ), prefix ); |
---|
| 304 | if ( log_level[logG] ) |
---|
| 305 | L.add_vector ( log_level, logG, RV ( dimension()*dimension() ), prefix ); |
---|
| 306 | |
---|
| 307 | } |
---|
| 308 | /*! Create object from the following structure |
---|
| 309 | |
---|
| 310 | \code |
---|
| 311 | class = 'EKF_UD'; |
---|
| 312 | OM = configuration of bdm::diffbifn; % any offspring of diffbifn, bdm::diffbifn::from_setting |
---|
| 313 | IM = configuration of bdm::diffbifn; % any offspring of diffbifn, bdm::diffbifn::from_setting |
---|
| 314 | dQ = [...]; % vector containing diagonal of Q |
---|
| 315 | dR = [...]; % vector containing diagonal of R |
---|
| 316 | --- optional fields --- |
---|
| 317 | mu0 = [...]; % vector of statistics mu0 |
---|
| 318 | dP0 = [...]; % vector containing diagonal of P0 |
---|
| 319 | -- or -- |
---|
| 320 | P0 = [...]; % full matrix P0 |
---|
| 321 | --- inherited fields --- |
---|
| 322 | bdm::BM::from_setting |
---|
| 323 | \endcode |
---|
| 324 | If the optional fields are not given, they will be filled as follows: |
---|
| 325 | \code |
---|
| 326 | mu0 = [0,0,0,....]; % empty statistics |
---|
| 327 | P0 = eye( dim ); |
---|
| 328 | \endcode |
---|
| 329 | */ |
---|
| 330 | void from_setting ( const Setting &set ); |
---|
| 331 | |
---|
| 332 | void validate() {}; |
---|
| 333 | // TODO dodelat void to_setting( Setting &set ) const; |
---|
| 334 | |
---|
| 335 | }; |
---|
| 336 | UIREGISTER(EKF_UDfix); |
---|
| 337 | |
---|
| 338 | |
---|
[1179] | 339 | |
---|
| 340 | |
---|
[61] | 341 | #endif // KF_H |
---|
| 342 | |
---|