| 173 | |
| 174 | /*! |
| 175 | * \brief Extended Kalman Filter with Chol matrices in fixed point arithmetic |
| 176 | * |
| 177 | * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
| 178 | */ |
| 179 | class EKFfixedCh : public BM { |
| 180 | public: |
| 181 | LOG_LEVEL(EKFfixedCh,logCh, logA, logP); |
| 182 | |
| 183 | void init_ekf(double Tv); |
| 184 | void ekf(double ux, double uy, double isxd, double isyd); |
| 185 | |
| 186 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
| 187 | int Q[16]; /* matrix [4,4] */ |
| 188 | int R[4]; /* matrix [2,2] */ |
| 189 | |
| 190 | int x_est[4]; /* estimate and prediction */ |
| 191 | |
| 192 | int PSI[16]; /* matrix [4,4] */ |
| 193 | int PSICh[16]; /* matrix PIS*U, [4,4] */ |
| 194 | |
| 195 | int Chf[16]; // upper triangular of covariance (inplace) |
| 196 | |
| 197 | int cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
| 198 | |
| 199 | enorm<chmat> E; |
| 200 | mat Ry; |
| 201 | |
| 202 | public: |
| 203 | //! Default constructor |
| 204 | EKFfixedCh ():BM(),E(),Ry(2,2){ |
| 205 | int i; |
| 206 | for(i=0;i<16;i++){Q[i]=0;} |
| 207 | for(i=0;i<4;i++){R[i]=0;} |
| 208 | |
| 209 | for(i=0;i<4;i++){x_est[i]=0;} |
| 210 | for(i=0;i<16;i++){Chf[i]=0;} |
| 211 | |
| 212 | for(i=0;i<16;i++){PSI[i]=0;} |
| 213 | |
| 214 | set_dim(4); |
| 215 | E._mu()=zeros(4); |
| 216 | E._R()=zeros(4,4); |
| 217 | init_ekf(0.000125); |
| 218 | }; |
| 219 | //! Here dt = [yt;ut] of appropriate dimensions |
| 220 | void bayes ( const vec &yt, const vec &ut ); |
| 221 | //!dummy! |
| 222 | const epdf& posterior() const {return E;}; |
| 223 | void log_register(logger &L, const string &prefix){ |
| 224 | BM::log_register ( L, prefix ); |
| 225 | |
| 226 | L.add_vector ( log_level, logCh, RV ("Ch", 16 ), prefix ); |
| 227 | L.add_vector ( log_level, logA, RV ("A", 16 ), prefix ); |
| 228 | L.add_vector ( log_level, logP, RV ("P", 16 ), prefix ); |
| 229 | |
| 230 | }; |
| 231 | //void from_setting(); |
| 232 | }; |
| 233 | |
| 234 | UIREGISTER(EKFfixedCh); |
| 235 | |