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

Timestamp:

10/22/10 21:15:11 (15 years ago)

Author:

smidl

Message:

kalman in Ch

Files:

• applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (modified) (4 diffs)

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

@@ -19 +19 @@
 #include "matrix.h"
 #include "matrix_vs.h"
-#include "reference.h"
+#include "reference_Q15.h"
 #include "parametry_motoru.h"
 
@@ -109 +109 @@
 class EKFfixedUD : public BM {
         public:
-                LOG_LEVEL(EKFfixedUD,logU, logG, logD, logA);
+                LOG_LEVEL(EKFfixedUD,logU, logG, logD, logA, logP);
                 
                 void init_ekf(double Tv);
@@ -164 +164 @@
                                 L.add_vector ( log_level, logD, RV ("D", 4 ), prefix );
                                 L.add_vector ( log_level, logA, RV ("A", 16 ), prefix );
+                                L.add_vector ( log_level, logP, RV ("P", 16 ), prefix );
                                 
                 };
@@ -170 +171 @@
 
 UIREGISTER(EKFfixedUD);
+
+/*!
+ * \brief Extended Kalman Filter with Chol matrices in fixed point arithmetic
+ * 
+ * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
+ */
+class EKFfixedCh : public BM {
+public:
+        LOG_LEVEL(EKFfixedCh,logCh, logA, logP);
+        
+        void init_ekf(double Tv);
+        void ekf(double ux, double uy, double isxd, double isyd);
+        
+        /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
+        int Q[16]; /* matrix [4,4] */
+        int R[4]; /* matrix [2,2] */
+        
+        int x_est[4]; /* estimate and prediction */
+        
+        int PSI[16]; /* matrix [4,4] */
+        int PSICh[16]; /* matrix PIS*U, [4,4] */
+        
+        int Chf[16]; // upper triangular of covariance (inplace)
+        
+        int cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
+        
+        enorm<chmat> E;
+        mat Ry;
+        
+public:
+        //! Default constructor
+        EKFfixedCh ():BM(),E(),Ry(2,2){
+                int i;
+                for(i=0;i<16;i++){Q[i]=0;}
+                for(i=0;i<4;i++){R[i]=0;}
+                
+                for(i=0;i<4;i++){x_est[i]=0;}
+                for(i=0;i<16;i++){Chf[i]=0;}
+                
+                for(i=0;i<16;i++){PSI[i]=0;}
+                
+                set_dim(4);
+                E._mu()=zeros(4);
+                E._R()=zeros(4,4);
+                init_ekf(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 ("Ch", 16 ), prefix );
+                L.add_vector ( log_level, logA, RV ("A", 16 ), prefix );
+                L.add_vector ( log_level, logP, RV ("P", 16 ), prefix );
+                
+        };
+        //void from_setting();
+};
+
+UIREGISTER(EKFfixedCh);
+
 
 //! EKF for comparison of EKF_UD with its fixed-point implementation