Show
Ignore:
Timestamp:
10/22/10 21:15:11 (14 years ago)
Author:
smidl
Message:

kalman in Ch

Files:
1 modified

Legend:

Unmodified
Added
Removed
  • applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

    r1201 r1226  
    1919#include "matrix.h" 
    2020#include "matrix_vs.h" 
    21 #include "reference.h" 
     21#include "reference_Q15.h" 
    2222#include "parametry_motoru.h" 
    2323 
     
    109109class EKFfixedUD : public BM { 
    110110        public: 
    111                 LOG_LEVEL(EKFfixedUD,logU, logG, logD, logA); 
     111                LOG_LEVEL(EKFfixedUD,logU, logG, logD, logA, logP); 
    112112                 
    113113                void init_ekf(double Tv); 
     
    164164                                L.add_vector ( log_level, logD, RV ("D", 4 ), prefix ); 
    165165                                L.add_vector ( log_level, logA, RV ("A", 16 ), prefix ); 
     166                                L.add_vector ( log_level, logP, RV ("P", 16 ), prefix ); 
    166167                                 
    167168                }; 
     
    170171 
    171172UIREGISTER(EKFfixedUD); 
     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 */ 
     179class EKFfixedCh : public BM { 
     180public: 
     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         
     202public: 
     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 
     234UIREGISTER(EKFfixedCh); 
     235 
    172236 
    173237//! EKF for comparison of EKF_UD with its fixed-point implementation