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

Timestamp:

04/28/11 22:18:55 (13 years ago)

Author:

smidl

Message:

extended reduced model for unknown Torque

Files:

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

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

@@ -270 +270 @@
 
 /*!
+ * \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 EKFfixedUD3 : public BM {
+public:
+        LOG_LEVEL(EKFfixedUD3,logU, logG, logD, logA, logC, logP);
+        
+        void init_ekf3(double Tv);
+        void ekf3(double ux, double uy, double isxd, double isyd);
+        
+        /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
+        int16 Q[9]; /* matrix [4,4] */
+        int16 R[4]; /* matrix [2,2] */
+        
+        int16 x_est[3]; /* estimate and prediction */
+        int16 y_est[2]; /* estimate and prediction */
+        int16 y_old[2]; /* estimate and prediction */
+        
+        int16 PSI[9]; /* matrix [4,4] */
+        int16 PSIU[9]; /* matrix PIS*U, [4,4] */
+        int16 C[6]; /* matrix [4,4] */
+        
+        int16 Uf[9]; // upper triangular of covariance (inplace)
+        int16 Df[3];  // diagonal covariance
+        int16 Dfold[3]; // temp of D
+        int16 G[9];  // temp for bierman
+        
+        int16 cA, cB, cC, cG, cF, cH;  // cD, cE, cF, cI ... nepouzivane
+        
+        enorm<fsqmat> E;
+        mat Ry;
+        
+public:
+        //! Default constructor
+        EKFfixedUD3 ():BM(),E(),Ry(2,2){
+                int16 i;
+                for(i=0;i<9;i++){Q[i]=0;}
+                for(i=0;i<4;i++){R[i]=0;}
+                
+                for(i=0;i<3;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<9;i++){Uf[i]=0;}
+                for(i=0;i<3;i++){Df[i]=0;}
+                for(i=0;i<4;i++){G[i]=0;}
+                for(i=0;i<3;i++){Dfold[i]=0;}
+                
+                for(i=0;i<9;i++){PSI[i]=0;}
+                for(i=0;i<6;i++){C[i]=0;}
+                
+                set_dim(3);
+                dimc = 2;
+                dimy = 2;
+                E._mu()=zeros(3);
+                E._R()=zeros(3,3);
+                init_ekf3(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 );         
+        };
+        //void from_setting();
+};
+
+UIREGISTER(EKFfixedUD3);
+
+/*!
  * \brief Extended Kalman Filter with Chol matrices in fixed point16 arithmetic
  *