Changeset 1326 for applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

Timestamp:

04/12/11 22:30:24 (15 years ago)

Author:

smidl

Message:

fixed point implemnetation of 2d EKF

Files:

• applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (modified) (1 diff)

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

@@ -332 +332 @@
 
 UIREGISTER(EKFfixedCh);
+
+/*!
+ * \brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic
+ * 
+ * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
+ */
+class EKFfixedCh2 : public BM {
+public:
+        LOG_LEVEL(EKFfixedCh2,logCh, logA, logC, logP);
+        
+        void init_ekf2(double Tv);
+        void ekf2(double ux, double uy, double isxd, double isyd);
+        
+        /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
+        int16 Q[4]; /* matrix [4,4] */
+        int16 R[4]; /* matrix [2,2] */
+        
+        int16 x_est[2]; /* estimate and prediction */
+        int16 y_est[2]; /* estimate and prediction */
+        int16 y_old[2]; /* estimate and prediction */
+        
+        int16 PSI[4]; /* matrix [4,4] */
+        int16 PSICh[4]; /* matrix PIS*U, [4,4] */
+        int16 C[4]; /* matrix [4,4] */
+        
+        int16 Chf[4]; // upper triangular of covariance (inplace)
+        
+        int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
+        
+        enorm<fsqmat> E;
+        mat Ry;
+        
+public:
+        //! Default constructor
+        EKFfixedCh2 ():BM(),E(),Ry(2,2){
+                int16 i;
+                for(i=0;i<4;i++){Q[i]=0;}
+                for(i=0;i<4;i++){R[i]=0;}
+                
+                for(i=0;i<2;i++){x_est[i]=0;}
+                for(i=0;i<2;i++){y_est[i]=0;}
+                for(i=0;i<2;i++){y_old[i]=0;}
+                for(i=0;i<4;i++){Chf[i]=0;}
+                
+                for(i=0;i<4;i++){PSI[i]=0;}
+                for(i=0;i<4;i++){C[i]=0;}
+                
+                set_dim(2);
+                dimc = 2;
+                dimy = 2;
+                E._mu()=zeros(2);
+                E._R()=zeros(2,2);
+                init_ekf2(0.000125);
+        };
+        //! Here dt = [yt;ut] of appropriate dimensions
+        void bayes ( const vec &yt, const vec &ut );
+        //!dummy!
+        const epdf& posterior() const {return E;};
+        void log_register(logger &L, const string &prefix){
+                BM::log_register ( L, prefix );
+                
+                L.add_vector ( log_level, logCh, RV ("Ch2", 4 ), prefix );
+                L.add_vector ( log_level, logA, RV ("A2", 4 ), prefix );
+                L.add_vector ( log_level, logC, RV ("C2", 4 ), prefix );
+                L.add_vector ( log_level, logP, RV ("P2", 4 ), prefix );
+                
+        };
+        //void from_setting();
+};
+
+UIREGISTER(EKFfixedCh2);