Changeset 1311 for applications

Timestamp:

03/25/11 19:52:05 (13 years ago)

Author:

smidl

Message:

ekf2

Location:

applications/pmsm/simulator_zdenek/ekf_example

Files:

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

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

@@ -571 +571 @@
         int32 tmp;
         
-        // implementace v PC
-        /*      t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
-         *      t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);*/
-        tmp=prevod(sin(Thetaref*x_est[3]/32768.),15);
-        if (tmp>32767) t_sin =32767; else t_sin=tmp;
-        tmp=prevod(cos(Thetaref*x_est[3]/32768.),15);
-        if (tmp>32767) t_cos =32767; else t_cos=tmp;
-        
-        tmp=((int32)cB*x_est[2])>>15; // q15*q13 -> q13
-        //             q15*q13 +          q13*q15      + q15*q13??
-        x_est[0]=((int32)cA*x_est[0]+(int32)tmp*t_sin+(int32)cC*(uf[0]))>>15; // in q13
-        x_est[1]=((int32)cA*x_est[1]-(int32)tmp*t_cos+(int32)cC*(uf[1]))>>15; // q13
-        x_est[2]=x_est[2];
+        x_est[0]=x_est[0];
         //               q15              + q15*q13
-        x_est[3]=(((int32)x_est[3]<<15)+(int32)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);
-        
+        x_est[1]=(((int32)x_est[1]<<15)+(int32)cG*x_est[0])>>15;
+        
+//      if(x_est[1]>(1<<15)) x_est[1]-=2*(1<<15);
+//      if(x_est[1]<-(1<<15)) x_est[1]+=2*(1<<15);
+        
+        
+        ///////// end of copy ///////////////
+        mmultAU(PSI,Uf,PSIU,2,2);
+
         //void EKFfixed::update_psi(void)
-        {       
-                PSI[2]=((int32)cB*t_sin)>>15;
-                tmp=((int32)cH*x_est[2])>>15; // ! cH =cB with different scale!!
-                PSI[3]=((int32)tmp*t_cos)>>15;
-                PSI[6]=-((int32)cB*t_cos)>>15;
-                PSI[7]=((int32)tmp*t_sin)>>15;
-        }
         {
-                int Ai[16];
-                for (int i=0;i<16; i++) Ai[i]=(int)(PSI[i]);
-                ivec Ad(Ai,16);
+                int Ai[4];
+                for (int i=0;i<4; i++) Ai[i]=(int)(PSIU[i]);
+                ivec Ad(Ai,4);
                 log_level.store(logA,get_from_ivec(Ad));
         }
         
-        ///////// end of copy ///////////////
-        mmultAU(PSI,Uf,PSIU,4,4);
         //thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx);
-        thorton_fast(Uf,Df,PSIU,Q,G,Dfold,4);
-        
+        thorton_fast(Uf,Df,PSIU,Q,G,Dfold,2);
+        
+
+// implementace v PC
+/*      t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
+ *      t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);*/
+tmp=prevod(sin(Thetaref*x_est[1]/32768.),15);
+if (tmp>32767) t_sin =32767; else t_sin=tmp;
+tmp=prevod(cos(Thetaref*x_est[1]/32768.),15);
+if (tmp>32767) t_cos =32767; else t_cos=tmp;
+
+{       
+        C[0]=((int32)cB*t_sin)>>15;
+        tmp=((int32)cH*x_est[0])>>15; // ! cH =cB with different scale!!
+        C[1]=((int32)tmp*t_cos)>>15;
+        C[2]=-((int32)cB*t_cos)>>15;
+        C[3]=((int32)tmp*t_sin)>>15;
+}
+
+
+{
+        int Ui[4];      for (int i=0;i<4; i++) Ui[i]=(int)Uf[i];
+        int Ci[4];      for (int i=0;i<4; i++) Ci[i]=(int)C[i];
+        
+        imat Ud(Ui,2,2);
+        imat Cd(Ci,2,2);
+        imat CU = Cd*Ud/(1<<15);
+        log_level.store(logU,get_from_ivec(ivec(CU._data(),CU._datasize())));
+        
+        int di[2];      for (int i=0;i<2; i++) di[i]=(int)Df[i];
+        ivec dd(di,2);
+        imat U(Ui,2,2);
+        mat U2=to_mat(U)/32768;
+        mat PP=U2*diag(to_vec(dd))*U2.T();
+        vec pp(PP._data(),4);
+        log_level.store(logP,pp);
+}
+{
+        int Gi[4];      for (int i=0;i<4; i++) Gi[i]=(int)G[i];
+        ivec Gd(Gi,4);
+        log_level.store(logG,get_from_ivec(Gd));
+}
+
+
+tmp=((int32)cB*x_est[0])>>15; // q15*q13 -> q13
+//             q15*q13 +          q13*q15      + q15*q13??
+y_est[0]=((int32)cA*y_old[0]+(int32)tmp*t_sin+(int32)cC*(uf[0]))>>15; // in q13
+y_est[1]=((int32)cA*y_old[1]-(int32)tmp*t_cos+(int32)cC*(uf[1]))>>15; // q13
+
+        
+        int16 difz[2];
+        difz[0]=(Y_mes[0]-y_est[0]); // shift to q15!!
+        difz[1]=(Y_mes[1]-y_est[1]);//<<2;
+        
+        y_old[0] = Y_mes[0];
+        y_old[1] = Y_mes[1];
         {
-                int Ui[16];     for (int i=0;i<16; i++) Ui[i]=(int)Uf[i];
-                
-                ivec Ud(Ui,16);
-                log_level.store(logU,get_from_ivec(Ud));
-                
-                int di[16];     for (int i=0;i<16; i++) di[i]=(int)Df[i];
-                ivec dd(di,4);
-                imat U(Ui,4,4);
-                mat U2=to_mat(U)/32768;
-                mat PP=U2*diag(to_vec(dd))*U2.T();
-                vec pp(PP._data(),16);
-                log_level.store(logP,pp);
-        }
-        {
-                int Gi[16];     for (int i=0;i<16; i++) Gi[i]=(int)G[i];
-                ivec Gd(Gi,16);
-                log_level.store(logG,get_from_ivec(Gd));
-        }
-        
-        
-        int16 difz[2];
-        difz[0]=(Y_mes[0]-x_est[0]); // shift to q15!!
-        difz[1]=(Y_mes[1]-x_est[1]);//<<2;
-        
-        {
-                int Di[4];               for (int i=0;i<4; i++) Di[i]=(int)Df[i];
-                ivec dd(Di,4);//;dd(0)=Y_mes[0];dd(1)=Y_mes[1]; dd(2)=difz[0]; dd(3)=difz[1];
+                int Di[2];               for (int i=0;i<2; i++) Di[i]=(int)Df[i];
+                ivec dd(Di,2);//;dd(0)=Y_mes[0];dd(1)=Y_mes[1]; dd(2)=difz[0]; dd(3)=difz[1];
                 log_level.store(logD,get_from_ivec(dd));
+                int Ci[4];               for (int i=0;i<4; i++) Ci[i]=(int)C[i];
+                ivec CC(Ci,4);//;dd(0)=Y_mes[0];dd(1)=Y_mes[1]; dd(2)=difz[0]; dd(3)=difz[1];
+                log_level.store(logC,get_from_ivec(CC));
         }
         
@@ -644 +659 @@
         int16 dR[2];dR[0]=R[0];dR[1]=R[3];
         //int16 xb[4]; xb[0]=x_est[0]<<2; xb[1]=x_est[1]<<2;  xb[2]=x_est[2]<<2;  xb[3]=x_est[3];  
-        bierman_fast(difz,x_est,Uf,Df,dR,2,4);
+        bierman_fastC(difz,x_est,Uf,Df,C,dR,2,2);
         //x_est[0] = xb[0]>>2; x_est[1]=xb[1]>>2; x_est[2]=xb[2]>>2; x_est[3]=xb[3];
         
         // navrat estimovanych hodnot regulatoru
         vec& mu = E._mu();
-        (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;
+        (mu)(0)=zprevod(x_est[0],15)*Wref;
+        (mu)(1)=zprevod(x_est[1],15)*Thetaref;
         
         //x_est[2]=x[2]*32768/Wref;
@@ -667 +680 @@
         Uf[0]=0x7FFF;// !       // 0.05
         Uf[1]=Uf[2]=0;
-        Uf[5]=0x7FFF;//!
+        Uf[3]=0x7FFF;//!
         
         Df[0]=1<<14;
@@ -693 +706 @@
         PSI[2]=cG;
         PSI[3]=0x7FFF;
-        
 }
 

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

@@ -197 +197 @@
 class EKFfixedUD2 : public BM {
 public:
-        LOG_LEVEL(EKFfixedUD2,logU, logG, logD, logA, logP);
+        LOG_LEVEL(EKFfixedUD2,logU, logG, logD, logA, logC, logP);
         
         void init_ekf2(double Tv);
@@ -207 +207 @@
         
         int16 x_est[2]; /* estimate and prediction */
+        int16 y_est[2]; /* estimate and prediction */
+        int16 y_old[2]; /* estimate and prediction */
         
         int16 PSI[4]; /* matrix [4,4] */
         int16 PSIU[4]; /* matrix PIS*U, [4,4] */
+        int16 C[4]; /* matrix [4,4] */
         
         int16 Uf[4]; // upper triangular of covariance (inplace)
@@ -229 +232 @@
                 
                 for(i=0;i<2;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<4;i++){Uf[i]=0;}
                 for(i=0;i<2;i++){Df[i]=0;}
@@ -235 +240 @@
                 
                 for(i=0;i<4;i++){PSI[i]=0;}
+                for(i=0;i<4;i++){C[i]=0;}
                 
                 set_dim(2);
+                dimc = 2;
+                dimy = 2;
                 E._mu()=zeros(2);
                 E._R()=zeros(2,2);
@@ -248 +256 @@
                 BM::log_register ( L, prefix );
                 
-                L.add_vector ( log_level, logG, RV("G",4), prefix );
-                L.add_vector ( log_level, logU, RV ("U", 4 ), prefix );
-                L.add_vector ( log_level, logD, RV ("D", 2 ), prefix );
-                L.add_vector ( log_level, logA, RV ("A", 4 ), prefix );
-                L.add_vector ( log_level, logP, RV ("P", 4 ), prefix );
+                L.add_vector ( log_level, logG, RV("G2",4), prefix );
+                L.add_vector ( log_level, logU, RV ("U2", 4 ), prefix );
+                L.add_vector ( log_level, logD, RV ("D2", 2 ), prefix );
+                L.add_vector ( log_level, logA, RV ("A2", 4 ), prefix );
+                L.add_vector ( log_level, logC, RV ("C2", 4 ), prefix );
+                L.add_vector ( log_level, logP, RV ("P2", 4 ), prefix );
                 
         };