Changeset 1201 for applications/pmsm/simulator_zdenek

Timestamp:

09/29/10 11:22:31 (14 years ago)

Author:

smidl

Message:

verbose ekf

Location:

applications/pmsm/simulator_zdenek/ekf_example

Files:

• ekf_obj.cpp (modified) (17 diffs)
• ekf_obj.h (modified) (5 diffs)

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

@@ -34 +34 @@
         
         if ( BM::evalll ) {
-                //from enorm
+/*              //from enorm
                 vec xdif(x,4);//first 4 of x
                 //UGLY HACK!!! reliance on a predictor!!
@@ -42 +42 @@
                 xdif(3)=x[3]-zprevod(x_pred[3],15);*/
                 
-                xdif -=xhat; //(xdif=x-xhat)
+//              xdif -=xhat; //(xdif=x-xhat)
                 
                 mat Pfull(4,4);
@@ -51 +51 @@
                 
                 
-                BM::ll = -0.5* ( 4 * 1.83787706640935 +log ( det ( Pfull ) ) +xdif* ( inv(Pfull)*xdif ) );
+        //      BM::ll = -0.5* ( 4 * 1.83787706640935 +log ( det ( Pfull ) ) +xdif* ( inv(Pfull)*xdif ) );*/
         }
 };
@@ -77 +77 @@
 
   // implementace v PC
-  t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
-  t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);
-
+  //t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
+  //t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);
+
+  t_sin=prevod(sin(Thetaref*x_est[3]/Qm),15);
+  t_cos=prevod(cos(Thetaref*x_est[3]/Qm),15);
+  
   tmp=((long)cB*x_est[2])>>15;
   x_pred[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*ux[0])>>15;
@@ -153 +156 @@
 {
   // Tuning of matrix Q
-  Q[0]=prevod(.01,15);       // 0.05
+  Q[0]=prevod(.05,15);       // 0.05
   Q[5]=Q[0];
-  Q[10]=prevod(0.0001,15);      // 1e-3
-  Q[15]=prevod(0.0001,15);      // 1e-3
+  Q[10]=prevod(0.0002,15);      // 1e-3
+  Q[15]=prevod(0.001,15);      // 1e-3
 
   // Tuning of matrix R
-  R[0]=prevod(0.05,15);         // 0.05
+  R[0]=prevod(0.1,15);         // 0.05
   R[3]=R[0];
 
@@ -170 +173 @@
 //  cF=prevod(p*Tv*Mref/J/Wref,15);
   cG=prevod(Tv*Wref*4/Thetaref,15);
-  cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
-//  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
+//  cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
+  cH=prevod(Tv*Wref*Fmag/Iref/Ls,15);
+  //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
 
   /* Init matrix PSI with permanently constant terms */
@@ -179 +183 @@
   PSI[14]=cG;
   PSI[15]=0x7FFF;
+  
+  P_est[0]=0x7FFF;
+  P_est[5]=0x7FFF;
+  P_est[10]=0x7FFF;
+  P_est[15]=0x7FFF;
 }
 
@@ -232 +241 @@
         D =  est._R()._D();
         
-        bool DBG=true;
-        if (DBG){
-                mat L;
-                ivec tmp;
-                mat X=randn(5,5);
-                mat XX=X*X.T();
-                mat T=diag(sqrt(1.0/diag(XX)));
-                XX= T*XX*T;
-                
-                ldmat ldd(XX);
-                U = ldd._L().T();
-                D = ldd._D()*0.9;
-        }
-                
         ////////////
         
@@ -254 +249 @@
 
         //////
-        vec xref(5);
+/*      vec xref(4);
         xref(0)= 30.0*1.4142;
         xref(1)= 30.0*1.4142;
         xref(2)= 6.283185*200.;
-        xref(3) = 3.141593;
-        xref(4) = 34.0;
-        
-        if (DBG){
-                xref = ones(5);
-        }
-        
-        imat Utf;
-        ivec Dtf;
-        UDtof(U,D,Utf,Dtf,xref);
-        mat invxref=diag(1.0/xref);
-        imat Af=round_i(invxref*A*diag(xref)*(1<<15));
-        mat_to_int(Af, PSI);
-        mat_to_int(Utf,Uf);             
-        vec_to_int(Dtf, Df); 
-
-//      cout << Dtf << endl;
-        
-        // A*U == PSI*U
-        mmultAU(PSI,Uf,PSIU,5,5);       
-/*      cout << ivec(PSIU,25) << endl;
-        cout << (Af*Utf)/(1<<15) <<endl;*/
+        xref(3) = 3.141593;*/
+        //xref(4) = 34.0;
+        
         
         vec Din = D;
@@ -336 +312 @@
                 
                 }
-                UDtof(U,D,Utf,Dtf,xref);
-                //      cout << G << endl << Din << endl;
-                imat Urw=Utf.T();
-                cout << ivec(Urw._data(),25) << endl << Dtf << endl;
-        }
-
-        int Qf[25];
-        imat Qrnd=round_i(diag(elem_div(diag(Q),pow(xref,2)))*(1<<15));
-//      cout << "Qr" << Qrnd<<endl;
-        mat_to_int(Qrnd,Qf);
-
-//      cout << Df <<endl;
-        thorton(Uf,Df,PSIU,Qf,Gf,Dfold,5);
-        
-        if (DBG){
-                _mu = vec("0.500 0.200 0.300 0.400 0.1");
-        }
-        
-        cout << "bierman double I" <<endl;
-        UDtof(U,D,Utf,Dtf,xref);
-        cout << "Uf: " << Utf << endl;
-        cout << "Df: " << Dtf << endl;
-        cout << "xf:" << round_i(elem_div(_mu,xref)*(1<<15)) <<endl;
-        mat_to_int(Utf,Uf);             
-        vec_to_int(Dtf, Df); 
-        
+        }
+
         // bierman
         
@@ -370 +322 @@
         vec xp=_mu; // used in bierman
         
-        if (DBG){
-                yp = vec("0.40 0.300");
-        }
         
         for (int iy=0; iy<dimy; iy++){
@@ -402 +351 @@
         }
 
-UDtof(U,D,Utf,Dtf,xref);
-cout << "Uf: " << Utf << endl;
-cout << "Df: " << Dtf << endl;
-cout << "xf:" << round_i(elem_div(_mu,xref)*(1<<15)) <<endl;
-
-        int difz[2];
-        vec_to_int(round_i((yt-yp)/xref(0)*(1<<15)), difz);
-        int xf[5];
-        vec_to_int(round_i(elem_div(xp,xref)*(1<<15)), xf);
-        int Rf[2];//={1,1};
-        vec_to_int(round_i(R/pow(xref(0),2)*(1<<15)), Rf);
-        //beirman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx 
-        
-        cout <<endl<< "bierman fixed" <<endl;
-        cout << "Uf: "; for (i=0; i<25;i++) cout << Uf[i] << ","; cout <<endl;
-        cout << "Df: "; for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl;
-        cout << "xf: "; for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;
-
-        cout << "dz: "; for (i=0; i<2;i++) cout << difz[i] << ","; cout << endl;
-
-        int xf_old[5];
-        vec_to_int(ivec(xf,5),xf_old);
-        bierman(difz,xf, Uf, Df, Rf, 2, 5);
-        
-        UDtof(U,D,Utf,Dtf,xref);
-        cout << "Uf: "; for (i=0; i<25;i++) cout << Uf[i] << ","; cout <<endl;
-        cout << "Df: "; for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl;
-        cout << "xf: "; for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;
-        cout << endl;
-        
-        
         
         /////
@@ -495 +413 @@
         
         // implementace v PC
+/*      t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
+        t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);*/
         t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
         t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);
         
         tmp=((long)cB*x_est[2])>>15;
-        x_est[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*uf[0])>>15;
-        x_est[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*uf[1])>>15;
+        x_est[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*(uf[0]<<2))>>15;
+        x_est[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*(uf[1]<<2))>>15;
         x_est[2]=x_est[2];
         x_est[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15;
+        
+        if(x_est[3]>(1<<15)) x_est[3]-=2*(1<<15);
+        if(x_est[3]<-(1<<15)) x_est[3]+=2*(1<<15);
         
         //void EKFfixed::update_psi(void)
         {               
                 PSI[2]=((long)cB*t_sin)>>15;
-                tmp=((long)cH*x_est[2])>>15;
+                tmp=((long)cB*x_est[2])>>15;
                 PSI[3]=((long)tmp*t_cos)>>15;
                 PSI[6]=-((long)cB*t_cos)>>15;
                 PSI[7]=((long)tmp*t_sin)>>15;
+        }
+        {
+                ivec Ad(PSI,16);
+                log_level.store(logA,get_from_ivec(Ad));
         }
         
@@ -518 +445 @@
         thorton(Uf,Df,PSIU,Q,G,Dfold,4);
         
+         {
+                ivec Ud(Uf,16);
+                log_level.store(logU,get_from_ivec(Ud));
+        }
+         {
+                ivec Gd(G,16);
+                log_level.store(logG,get_from_ivec(Gd));
+        }
+        
+        
         int difz[2];
-        difz[0]=Y_mes[0]-x_est[0];
-        difz[1]=Y_mes[1]-x_est[1];
+        difz[0]=(Y_mes[0]<<2)-x_est[0]; // Y_mes in q13!!
+        difz[1]=(Y_mes[1]<<2)-x_est[1];
+
+        {
+                vec dd(4);dd(0)=Y_mes[0];dd(1)=Y_mes[1]; dd(2)=difz[0]; dd(3)=difz[1];
+                log_level.store(logD,dd);
+        }
+        
         //bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx );
         int dR[2];dR[0]=R[0];dR[1]=R[3];
@@ -527 +470 @@
         // navrat estimovanych hodnot regulatoru
         vec& mu = E._mu();
-        (mu)(0)=zprevod(x_est[0],Qm)*Iref;
-        (mu)(1)=zprevod(x_est[1],Qm)*Iref;
-        (mu)(2)=zprevod(x_est[2],Qm)*Wref;
+        (mu)(0)=zprevod(x_est[0],15)*Iref;
+        (mu)(1)=zprevod(x_est[1],15)*Iref;
+        (mu)(2)=zprevod(x_est[2],15)*Wref;
         (mu)(3)=zprevod(x_est[3],15)*Thetaref;
         
@@ -544 +487 @@
 
         Uf[0]=0x7FFF;       // 0.05
+        Uf[1]=Uf[2]=Uf[3]=Uf[4]=0;
         Uf[5]=0x7FFF;
+        Uf[6]=Uf[6]=Uf[8]=Uf[9]=0;
         Uf[10]=0x7FFF;      // 1e-3
+        Uf[11]=Uf[12]=Uf[13]=Uf[4]=0;
         Uf[15]=0x7FFF;      // 1e-3
         
@@ -564 +510 @@
         //  cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15);
         //  cF=prevod(p*Tv*Mref/J/Wref,15);
-        cG=prevod(Tv*Wref*4/Thetaref,15);
-        cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
+        cG=prevod(Tv*Wref/Thetaref,15); //no *4!!
+        //cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
+        // cH=prevod(Tv*Wref*Fmag/Iref/Ls,15); <<< use cB instead
         //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
         

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

@@ -28 +28 @@
 void mat_to_int(const imat &M, int *I);
 void vec_to_int(const ivec &v, int *I);
-
+void UDtof(const mat &U, const vec &D, imat &Uf, ivec &Df, const vec &xref);
+        
 /*!
 \brief Extended Kalman Filter with full matrices in fixed point arithmetic
@@ -108 +109 @@
 class EKFfixedUD : public BM {
         public:
+                LOG_LEVEL(EKFfixedUD,logU, logG, logD, logA);
+                
                 void init_ekf(double Tv);
                 void ekf(double ux, double uy, double isxd, double isyd);
@@ -154 +157 @@
                 //!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, logG, RV("G",16), prefix );
+                                L.add_vector ( log_level, logU, RV ("U", 16 ), prefix );
+                                L.add_vector ( log_level, logD, RV ("D", 4 ), prefix );
+                                L.add_vector ( log_level, logA, RV ("A", 16 ), prefix );
+                                
+                };
+                //void from_setting();
 };
 
@@ -174 +186 @@
                 //! D part
                 vec D;
-                
-                int Uf[25];
-                int Df[5];
-                int Dfold[5];
-                
+                                
                 mat A;
                 mat C;
@@ -184 +192 @@
                 vec R;
                 
-                int PSI[25];
-                int PSIU[25];
-                int Gf[25];
-
                 enorm<ldmat> est;