Changeset 1466 for applications

Timestamp:

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

Author:

smidl

Message:

opraven test Ch

Location:

applications/pmsm

Files:

• cfg/zcu.cfg (modified) (1 diff)
• simulator_zdenek/CMakeLists.txt (modified) (1 diff)
• simulator_zdenek/ekf_example/ekf_mm.cpp (modified) (4 diffs)
• simulator_zdenek/ekf_example/ekf_mm.h (modified) (2 diffs)
• simulator_zdenek/ekf_example/ekf_obj.cpp (modified) (3 diffs)
• simulator_zdenek/ekf_example/ekf_obj.h (modified) (10 diffs)
• simulator_zdenek/test_Ch.cpp (modified) (9 diffs)

applications/pmsm/cfg/zcu.cfg

@@ -10 +10 @@
         Bf = 0.0;  // Bearing Friction?
         Uc = 200.0; // ???
-        DT = 3e-6; //Dead Time
+        DT = 1e-6; //Dead Time
 };

applications/pmsm/simulator_zdenek/CMakeLists.txt

@@ -3 +3 @@
 
 #EXEC (test_UD  pmsmsim ekf_obj)
-#EXEC (test_Ch  pmsmsim ekf_obj)
+EXEC (test_Ch  pmsmsim ekf_obj)
 
 add_executable(round_test round_test.cpp)

applications/pmsm/simulator_zdenek/ekf_example/ekf_mm.cpp

@@ -1 +1 @@
 #include "ekf_mm.h"
 #include "qmath.h"
+#include "stdio.h"
 
 
@@ -12 +13 @@
         //               q15              + q15*q13
         (E->x_est)[1]=(((int32)(E->x_est[1])<<15)+(int32)(E->cG)*(E->x_est[0]))>>15;
+        E->x_pred[1]=E->x_est[1];
+        E->x_pred[0]=E->x_est[0];
         
         mmultACh(E->PSI,E->Chf,E->PSICh,2,2);
@@ -36 +39 @@
         E->y_est[1]=((int32)(E->cA)*(E->y_old[1])-(int32)tmp*t_cos+(int32)(E->cC)*(uy))>>15; // q13
         
-        
-        int16 difz[2];
-        difz[0]=(isx-(E->y_est[0])); // shift to q15!!
-        difz[1]=(isy-(E->y_est[1]));//<<2;
+        E->difz[0]=(isx-(E->y_est[0])); // shift to q15!!
+        E->difz[1]=(isy-(E->y_est[1]));//<<2;
         
         E->y_old[0] = isx;
@@ -45 +46 @@
         
         *detS = 32767; *rem=0;
-        carlson_fastC(difz,E->x_est,E->Chf,E->C,E->dR,2,2, detS, rem);
+        carlson_fastC(E->difz,E->x_est,E->Chf,E->C,E->dR,2,2, detS, rem);
 }
 

applications/pmsm/simulator_zdenek/ekf_example/ekf_mm.h

@@ -25 +25 @@
         
         int16 x_est[2]; /* estimate and prediction */
+        int16 x_pred[2]; /* estimate and prediction */
         int16 y_est[2]; /* estimate and prediction */
         int16 y_old[2]; /* estimate and prediction */
@@ -34 +35 @@
         int16 Chf[4]; // upper triangular of covariance (inplace)
         
+        int16 difz[2];
         int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
 };

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

@@ -1 @@
-
 #include <estim/kalman.h>
-
 #include "ekf_obj.h"
 //#include "../simulator.h"
@@ -12 +10 @@
                         *I++ = M(i,j);
                 }
+        }
+}
+void int16_to_mat(int16 *I, imat &M, int rows, int cols){
+        M.set_size(rows,cols);
+        for (int16 i=0;i<M.rows(); i++){
+                for (int16 j=0;j<M.cols(); j++){
+                         M(i,j) = *I++;
+                }
+        }
+}
+void int16_to_vec(int16 *I, ivec &v, int len){
+        v.set_size(len);
+        for (int16 i=0;i<v.length(); i++){
+                        v(i) = *I++;
         }
 }
@@ -1043 +1055 @@
         }
         
-        
         {
                 int Chi[4];     for (int i=0;i<4; i++) Chi[i]=(int)Chf[i];

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
 

applications/pmsm/simulator_zdenek/test_Ch.cpp

@@ -24 +24 @@
                 mat Ch = U*diag(sqrt(D));
                 
-                int PSI[25];
-                int PSICh[25]={0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0};
-                int Chf[25]={0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0};
-                int Cf[10]={0,0,0,0,0, 0,0,0,0,0};
-                int multip=1<<15;
+                int16 PSI[25];
+                int16 PSICh[25]={0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0};
+                int16 Chf[25]={0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0};
+                int16 Cf[10]={0,0,0,0,0, 0,0,0,0,0};
+                int16 multip=(1<<15)-1;
                 
         /////////// COPY
@@ -34 +34 @@
         mat_to_int16(Af, PSI);
         mat_to_int16(round_i(Ch*multip),Chf);           
-        int Qf[25];
+        int16 Qf[25];
         mat_to_int16(round_i(sqrt(Q)*multip), Qf);
-        int Rf[2];
+        int16 Rf[2];
         vec_to_int16(round_i(R*multip), Rf);
         
@@ -43 +43 @@
         mmultACh(PSI,Chf,PSICh,5,5);    
         
-        mat PhiCh =round(A*32768)*round(Ch*32768)/32768/32768;
-/*      cout << "A*U" << round_i(PhiU*multip) <<endl;
-        cout << "PSIU: "; for (i=0; i<25;i++) cout << PSIU[i] << ","; cout <<endl;*/
-        
-        imat PChcmp(PSICh,5,5);
+        mat PhiCh =round(A*32768)*round(Ch*32768)/32768/32768;  
+        imat PChcmp;
+        int16_to_mat(PSICh,PChcmp,5,5);
+        cout << "Psi: " << round_i(PhiCh*multip) <<endl;
+        cout << "PSI fix: " << PChcmp <<endl;
         cout << "Delta PSI: " << round_i(PhiCh*multip-PChcmp) <<endl;
         
@@ -83 +83 @@
         }
 
-cout << "PSICh: " << imat(PSICh,5,5) << endl;
-cout << "Qf: " << imat(Qf,5,5) << endl;
+imat M55(5,5);
+int16_to_mat(PSICh,M55,5,5);
+cout << "PSICh: " << M55 << endl;
+int16_to_mat(Qf,M55,5,5);
+cout << "Qf: " << M55 << endl;
 
 Ch = U*diag(sqrt(D));
@@ -96 +99 @@
         cout << endl<<"after householder " <<endl;
 
-        imat Chcmp(PSICh,5,5);
+        imat Chcmp;
+        int16_to_mat(PSICh,Chcmp,5,5);
         cout << "Ch(UD): " << round_i(Ch*multip) << endl;
         cout << "Ch: " << (Chcmp) << endl;
@@ -113 +117 @@
         vec     xp = 2*randu(5)-1;
         
-        int difz[2];
+        int16 difz[2];
         vec_to_int16(round_i(ydif*multip), difz);
-        int xf[5];
+        int16 xf[5];
         vec_to_int16(round_i(xp*multip), xf);
 
@@ -122 +126 @@
         
         
-        int xf_old[5];
-        vec_to_int16(ivec(xf,5),xf_old);
+        int16 xf_old[5];
+        for (int i=0; i<5;i++) xf_old[i]=xf[i];
         
         /////// Test bierman
@@ -164 +168 @@
         mat_to_int16(round_i(C*multip),Cf);             
 
-        cout << "bef Carlson: " << imat(Chf,5,5) << endl;
+        int16_to_mat(Chf,M55,5,5);
+        cout << "bef Carlson: " << M55 << endl;
 
-        carlson_fastC(difz,xf, Chf, Cf,Rf, 2, 5);
+        int16 detS, rem;
+        carlson_fastC(difz,xf, Chf, Cf,Rf, 2, 5, &detS, &rem);
         
         cout << endl<<"after Carlson" <<endl;
@@ -173 +179 @@
 
         {
-                imat Chcmp(Chf,5,5);
+                imat Chcmp;
+                int16_to_mat(Chf,Chcmp,5,5);
+                cout << "Ch: " << round_i(Ch*multip) << endl;
+                cout << "Ch fix: " << Chcmp << endl;
                 cout << "Delta Ch: " << round_i(Ch*multip-Chcmp) << endl;
-                cout << "Delta Ch: " << Chcmp << endl;
         }