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

Timestamp:

08/02/12 22:43:00 (12 years ago)

Author:

smidl

Message:

opraven test Ch

Files:

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

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

@@ -28 +28 @@
 using namespace bdm;
 
-double minQ(double Q);
+double minQ(double Q); 
 
 void mat_to_int16(const imat &M, int16 *I);
 void vec_to_int16(const ivec &v, int16 *I);
+void int16_to_mat(int16 *I, imat &M, int rows, int cols);
+void int16_to_vec(int16 *I, ivec &v, int len);
 void UDtof(const mat &U, const vec &D, imat &Uf, ivec &Df, const vec &xref);
 
-#ifdef XXX
-/*!
-\brief Extended Kalman Filter with full matrices in fixed point16 arithmetic
-
-An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
-*/
-class EKFfixed : public BM {
-public:
-void init_ekf(double Tv);
-void ekf(double ux, double uy, double isxd, double isyd);
-
-/* Declaration of local functions */
-void prediction(int16 *ux);
-void correction(void);
-void update_psi(void);
-
-/* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
- int16 Q[16]; /* matrix [4,4] */
- int16 R[4]; /* matrix [2,2] */
-
- int16 x_est[4];
- int16 x_pred[4];
- int16 P_pred[16]; /* matrix [4,4] */
- int16 P_est[16]; /* matrix [4,4] */
- int16 Y_mes[2];
- int16 ukalm[2];
- int16 Kalm[8]; /* matrix [5,2] */
-
- int16 PSI[16]; /* matrix [4,4] */
-
- int16 temp15a[16];
-
- int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
-
- int32 temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */
- enorm<fsqmat> E;
- mat Ry;
-  
-public:
-        //! Default constructor
-        EKFfixed ():BM(),E(),Ry(2,2){
-                int16 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<4;i++){x_pred[i]=0;}
-                for(i=0;i<16;i++){P_pred[i]=0;}
-                for(i=0;i<16;i++){P_est[i]=0;}
-                P_est[0]=0x7FFF;
-                P_est[5]=0x7FFF;
-                P_est[10]=0x7FFF;
-                P_est[15]=0x7FFF;
-                for(i=0;i<2;i++){Y_mes[i]=0;}
-                for(i=0;i<2;i++){ukalm[i]=0;}
-                for(i=0;i<8;i++){Kalm[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;};
-        
-};
-
-UIREGISTER(EKFfixed);
-
-#endif
 
 //! EKF for testing q44
@@ -121 +49 @@
 };
 UIREGISTER(EKFtest);
+
+
+class EKFscale: public EKFCh{
+        LOG_LEVEL(EKFscale, logCh, logA, logC);
+public:
+        mat Tx;
+        mat Ty;
+        
+        void from_setting ( const Setting &set ){
+                EKFCh::from_setting(set);
+                vec v;
+                UI::get(v, set, "xmax", UI::compulsory);
+                Tx = diag(1./v);
+                UI::get(v, set, "ymax", UI::compulsory);
+                Ty = diag(1./v);
+                
+                UI::get(log_level, set, "log_level", UI::optional);
+        };
+        
+        void log_register(logger &L, const string &prefix){
+                BM::log_register ( L, prefix );
+                
+                L.add_vector ( log_level, logCh, RV ("Ch", dimension()*dimension() ), prefix );
+                L.add_vector ( log_level, logA, RV ("A", dimension()*dimension() ), prefix );
+                L.add_vector ( log_level, logC, RV ("C", dimensiony()*dimension() ), prefix );
+        };
+        
+        void log_write() const{
+                BM::log_write();
+            if ( log_level[logCh] ) {
+                        mat Chsc=Tx*(est._R()._Ch());
+                        vec v(Chsc._data(), dimension()*dimension());
+                        if (v(0)<0)
+                                v= -v;
+                        log_level.store( logCh, round(v*32767));
+                }
+                if (log_level[logA]){
+                        mat Asc = Tx*A*inv(Tx);
+                        vec v(Asc._data(), dimension()*dimension());
+                        log_level.store( logA, round(v*32767));
+                }
+                if (log_level[logC]){
+                        mat Csc = Ty*C*inv(Tx);
+                        vec v(Csc._data(), dimensiony()*dimension());
+                        log_level.store( logC, round(v*32767));
+                }
+        }
+        
+};
+UIREGISTER(EKFscale);
 
 /*!
@@ -400 +378 @@
                 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 );
-                
+                L.add_vector ( log_level, logP, RV ("P", 16 ), prefix );                
         };
         //void from_setting();
@@ -656 +633 @@
 //! EKF with covariance R estimated by the VB method
 class EKFvbR: public EKFfull{   
-    LOG_LEVEL(EKFvbR,logR,logQ);
+    LOG_LEVEL(EKFvbR,logR,logQ,logP);
 
         //! Statistics of the R estimator
@@ -691 +668 @@
                 L.add_vector(log_level, logR, RV("{R }",vec_1<int>(4)), prefix);
                 L.add_vector(log_level, logQ, RV("{Q }",vec_1<int>(4)), prefix);
+                L.add_vector(log_level, logP, RV("{P }",vec_1<int>(4)), prefix);
         }
         void bayes ( const vec &val, const vec &cond ) {
@@ -709 +687 @@
                         EKFfull::bayes(val,cond);
 
-                        diffy = val - fy._mu();
-                        Psi_vbR = phi*PsiR + (1-phi)*PsiR0+ outer_product(diffy,diffy)/*+C*mat(est._R())*C.T()*/;
-                        R = Psi_vbR/(nu-2);
+//                      diffy = val - fy._mu();
+//                      Psi_vbR = phi*PsiR + (1-phi)*PsiR0+ outer_product(diffy,diffy)/*+C*mat(est._R())*C.T()*/;
+//                      R = Psi_vbR/(nu-2);
         
                         diffx = est._mu() - xpred;
-                        Psi_vbQ = phi*PsiQ + (1-phi)*PsiQ0+ outer_product(diffx,diffx) /*+mat(est._R()) /*+ A*mat(P0)*A.T()*/;
+                        Psi_vbQ = phi*PsiQ + (1-phi)*PsiQ0+ outer_product(diffx,diffx);/*mat(est._R());  A*mat(P0)*A.T();*/
+                        //
+                        Psi_vbQ(0,1) = 0.0;
+                        Psi_vbQ(1,0) = 0.0;
                         Q = Psi_vbQ/(nu-2);
                 }
@@ -722 +703 @@
                 log_level.store(logQ, vec(Q._M()._data(),4));
                 log_level.store(logR, vec(R._M()._data(),4));
-                
+                {
+                        mat Ch(2,2);
+                        Ch=chol(est._R()._M());
+                        log_level.store(logP, vec(Ch._data(),4));
+                }
         }
 };
@@ -729 +714 @@
 
 class ekfChfix: public BM{
-        
         ekf_data E;
 public:
@@ -758 +742 @@
                 vec dQ,dR;
                 UI::get ( dQ, set, "dQ", UI::optional );
-                UI::get ( dQ, set, "dR", UI::optional );
+                UI::get ( dR, set, "dR", UI::optional );
                 if (dQ.length()==2){
                         E.Q[0]=prevod(dQ[0],15);      // 1e-3
@@ -774 +758 @@
 UIREGISTER(ekfChfix);
 
+
+
+class ekfChfixQ: public BM{
+        LOG_LEVEL(ekfChfixQ,logQ,logCh,logC,logRes)
+        ekf_data E;
+        int64 PSI_Q0, PSI_Q1, PSI_Q0_reg, PSI_Q1_reg;
+        int Q_ni;
+        int phi_Q;
+public:
+        ekfChfixQ(){
+                init_ekfCh2(&E,0.000125);set_dim(2);    dimc = 2;
+                dimy = 2;
+                Q_ni = 7;
+                phi_Q = ((1<<Q_ni) -1)<<(15-Q_ni);
+
+                PSI_Q0_reg = ((int64)((1<<15)-phi_Q)*E.Q[0])<<Q_ni;
+                PSI_Q1_reg = ((int64)((1<<15)-phi_Q)*E.Q[3])<<Q_ni;
+                PSI_Q0 = ((int64)E.Q[0])<<Q_ni;
+                PSI_Q1 = ((int64)E.Q[3])<<Q_ni;
+        } 
+        void bayes ( const vec &val, const vec &cond ) {
+                int16 ux,uy;
+                ux=prevod(cond[0]/Uref,15);
+                uy=prevod(cond[1]/Uref,15);
+                
+                int16 yx,yy;
+                // zadani mereni
+                yx=prevod(val[0]/Iref,15);
+                yy=prevod(val[1]/Iref,15);
+                
+                int16 detS, rem;
+                ekfCh2(&E, ux,uy,yx,yy, &detS, &rem);
+                
+                Est._mu()=vec_2(E.x_est[0]*Uref/32768., E.x_est[1]*Uref/32768.);
+                
+                int16 xerr0 = E.x_est[0]-E.x_pred[0];
+                PSI_Q0 = ((int64)(phi_Q*PSI_Q0) + (((int64)xerr0*xerr0)<<15) + PSI_Q0_reg)>>15;         
+                E.Q[0] = PSI_Q0>>Q_ni;
+
+                xerr0 = E.x_est[1]-E.x_pred[1];
+                PSI_Q1 = ((int64)(phi_Q*PSI_Q1) + (((int64)xerr0*xerr0)<<15) + PSI_Q1_reg)>>15;         
+                E.Q[3] = PSI_Q1>>Q_ni;
+                
+                cout << E.Q[0] << "," << E.Q[3] <<endl;
+                ll = -0.5*qlog(detS)-0.5*rem;
+        }
+        const epdf& posterior() const {return Est;};
+        void from_setting ( const Setting &set ){
+                BM::from_setting(set);
+                vec dQ,dR;
+                UI::get ( dQ, set, "dQ", UI::optional );
+                UI::get ( dR, set, "dR", UI::optional );
+                if (dQ.length()==2){
+                        E.Q[0]=prevod(dQ[0],15);      // 1e-3
+                        E.Q[3]=prevod(dQ[1],15);      // 1e-3
+                }
+                if (dR.length()==2){
+                        E.dR[0]=prevod(dR[0],15);      // 1e-3
+                        E.dR[1]=prevod(dR[1],15);      // 1e-3
+                }
+
+                UI::get(Q_ni, set, "Q_ni", UI::optional);
+                { // zmena Q!!
+                        phi_Q = ((1<<Q_ni) -1)<<(15-Q_ni);
+
+                        PSI_Q0_reg = ((int64)((1<<15)-phi_Q)*E.Q[0])<<Q_ni;
+                        PSI_Q1_reg = ((int64)((1<<15)-phi_Q)*E.Q[3])<<Q_ni;
+                        PSI_Q0 = ((int64)E.Q[0])<<Q_ni;
+                        PSI_Q1 = ((int64)E.Q[3])<<Q_ni;
+
+                }
+                UI::get(log_level, set, "log_level", UI::optional);
+
+        }
+        void log_register ( logger &L, const string &prefix ) {
+                BM::log_register(L,prefix);
+                L.add_vector(log_level, logQ, RV("{Q }",vec_1<int>(2)), prefix);
+                L.add_vector(log_level, logCh, RV("{Ch }",vec_1<int>(3)), prefix);
+                L.add_vector(log_level, logC, RV("{C }",vec_1<int>(4)), prefix);
+                L.add_vector(log_level, logRes, RV("{dy }",vec_1<int>(2)), prefix);
+        }
+        void log_write() const {
+                BM::log_write();
+                if ( log_level[logQ] ) {
+                        log_level.store( logQ, vec_2((double)E.Q[0],(double)E.Q[3] ));
+                }
+                if ( log_level[logCh] ) {
+                        log_level.store( logCh, vec_3((double)E.Chf[0],(double)E.Chf[1], (double)E.Chf[3]));
+                }
+                if ( log_level[logRes] ) {
+                        log_level.store( logRes, vec_2((double)E.difz[0],(double)E.difz[1]));
+                }
+                if ( log_level[logC] ) {
+                        vec v(4);
+                        for (int i=0;i<4;i++) v(i)=E.C[i];
+                        log_level.store( logC, v);
+                }
+        }
+        
+        enorm<fsqmat> Est;
+        mat Ry;
+};
+UIREGISTER(ekfChfixQ);
+
 #endif // KF_H