Changeset 1294

Timestamp:

03/14/11 09:32:33 (14 years ago)

Author:

smidl

Message:

zmeny v experimentech

Files:

• applications/pmsm/experiments/EKFfixed.cfg (modified) (1 diff)
• applications/pmsm/experiments/EKFfixedCh.cfg (modified) (1 diff)
• applications/pmsm/experiments/ctrl.cfg (modified) (2 diffs)
• applications/pmsm/experiments/ctrl_lq_dq.cfg (modified) (1 diff)
• applications/pmsm/pmsm_ctrl.h (modified) (1 diff)
• applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (modified) (5 diffs)
• applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (modified) (1 diff)
• applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp (modified) (11 diffs)
• applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h (modified) (2 diffs)
• applications/pmsm/simulator_zdenek/simulator.cpp (modified) (1 diff)
• library/bdm/estim/kalman.cpp (modified) (1 diff)

applications/pmsm/experiments/EKFfixed.cfg

@@ -9 +9 @@
         params = "pmsm107@../cfg/zcu.cfg";
         tstep = 1; // steps for profile in [s]
-//      profileW = [0, 0, -30, -200, -90, -60, -30, 0, 30, 200];
-//      profileW = [30];//[30, 30, 30, 60, 90, 60, 30, 0, -30, -60, -30];
-        profileW = [0,0,1, 1, 5, 2, 2, 3, 0, 0, -3, -3, -1, -1, -4, 0, 0];
+        profileW = [0, 0, -30, -200, -90, -60, -30, 0, 30, 200];
+//      profileW = [30, 30, 30, 60, 90, 60, 30, 0, -30, -60, -30];
+//      profileW = [0,0,1, 1, 5, 2, 2, 3, 0, 0, -3, -3, -1, -1, -4, 0, 0];
 //      profileM = [0, 30, 0, 0, 30, 0, 0, 0, 0, 0, 10, 0, 0];
         profileM = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];
 };
 
-estimators = (
-         {class="EKF_UD";
-         IM = {class="IMpmsmStat";
+estimators = (  
+         {class="EKFfixedCh";   
+         IM = {class="IMpmsmStat";      
                 params = "pmsm107@../cfg/zcu.cfg";
          };
-         OM = {class="OMpmsmRO";};
-        dQ=[0.01, 0.01, 0.0005, 0.001];
-//      dQ=[18., 18., 157., 0.001];
-        dR=[0.05, 0.05];
-//      dR=[90., 90.];
-        dP0=[18000., 18000., 157000000., 10.];
+         OM = {class="OMpmsm";};
+//      dQ=[0.01, 0.01, 0.0005, 0.001];
+        dQ=[289., 289., 12630., 0.009]; // 0.01*170^2, 0.0005*5026^2, 0.001*3.14^2
+        dR=[1445, 1445];
+//      dR=[0.05., 0.05.];
+        dP0=[14450., 144500., 157000000., 10.];
          //connect
          yrv = {class="RV"; names=("o_ia","o_ib");};  // observed u

applications/pmsm/experiments/EKFfixedCh.cfg

@@ -34 +34 @@
          name = "Est0";
          },
-         {class="EKFfixedUD";
+         {class="EKFfixedCh";
          //connect
          yrv = {class="RV"; names=("o_ia","o_ib");};  // observed u

applications/pmsm/experiments/ctrl.cfg

@@ -2 +2 @@
 
 experiment={
-        Ndat = 20000;
+        Ndat = 50000;
 };
 
 system= {
         class= "pmsmDSctrl";
-        params = "pmsm107@./cfg/zcu.cfg";
+        params = "pmsm107@../cfg/zcu.cfg";
         tstep = 1; // steps for profile in [s]
-        profileW = [0, 30, 0, 2 ];
+        profileW = [0, 50, 0, -50, 0, 50  ];
 //      profileW = [0, -10, 30, 60, 90, 60, 30, 0, -30, -60, -30];
 //      profileW = [1, 10, 50, 200, 200, 30, 0, 0, -30, -30, -100, -100, -40, 0, 0];
@@ -16 +16 @@
 };
 
-UD = {class="EKFCh";
+UD = {class="EKFfixedCh";
          IM = {class="IMpmsmStat";
-                params = "pmsm107@./cfg/zcu.cfg";
+                params = "pmsm107@../cfg/zcu.cfg";
          };
          OM = {class="OMpmsm";};
-        dQ=[0.01, 0.01, 0.0001, 0.000001];
-        dR=[0.05, 0.05];
+         name = "Est0";
+         };
+
+full = {class="EKFCh";
+         IM = {class="IMpmsmStat";
+                params = "pmsm107@../cfg/zcu.cfg";
+                compensate = 1;
+         };
+         OM = {class="OMpmsm";};
+        dQ=[289., 289., 12630., 0.009]; // 0.01*170^2, 0.0005*5026^2, 0.001*3.14^2
+        dR=[1445, 1445];
+        dP0=[14450., 144500., 157000000., 10.];
          //connect
          log_level="logmean,logU,logG,logD";

applications/pmsm/experiments/ctrl_lq_dq.cfg

@@ -30 +30 @@
 controllers=({
                 class = "PMSM_LQCtrl_dq";
-                estim = "UD";
+//              estim = "UD";
                 r = 0.0001;
                 h = 100;

applications/pmsm/pmsm_ctrl.h

@@ -63 +63 @@
         PMSMCtrl():Controller() {
                 rv = RV("{ua ub }");
-                rvc = RV("{o_ia o_ib t_ua t_ub o_om o_th Ww }");
+                rvc = RV("{o_ia o_ib o_ua o_ub o_om o_th Ww }");
         }
         

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

@@ -505 +505 @@
 {
         // Tuning of matrix Q
-        Q[0]=prevod(.01,15);       // 0.05
+        Q[0]=prevod(.001,15);       // 0.05
         Q[5]=Q[0];
         Q[10]=prevod(0.0005,15);      // 1e-3
-        Q[15]=prevod(0.001,15);      // 1e-3
+        Q[15]=prevod(0.0001,15);      // 1e-3
 
         Uf[0]=0x7FFF;// !       // 0.05
@@ -603 +603 @@
         mmultACh(PSI,Chf,PSICh,4,4);
         //thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx);
-        householder(PSICh,Q,4);
-        //givens(PSICh,Q,4);
+        //householder(PSICh,Q,4);
+        givens_fast(PSICh,Q,4);
         // COPY 
         for (int16 ii=0; ii<16; ii++){Chf[ii]=PSICh[ii];}
@@ -629 +629 @@
         //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];  
 
-        carlson(difz,x_est,Chf,dR,2,4);
+        carlson_fast(difz,x_est,Chf,dR,2,4);
         //x_est[0] = xb[0]>>2; x_est[1]=xb[1]>>2; x_est[2]=xb[2]>>2; x_est[3]=xb[3];
         
@@ -649 +649 @@
         Q[0]=prevod(.01,15);       // 0.05
         Q[5]=Q[0];
-        Q[10]=prevod(0.01,15);      // 1e-3
-        Q[15]=prevod(0.01,15);      // 1e-3
+        Q[10]=prevod(0.0005,15);      // 1e-3
+        Q[15]=prevod(0.001,15);      // 1e-3
         
         Chf[0]=0x3FFF;// !       // 0.05
@@ -661 +661 @@
                 
         // Tuning of matrix R
-        R[0]=prevod(0.1,15);         // 0.05
+        R[0]=prevod(0.05,15);         // 0.05
         R[3]=R[0];
         

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

@@ -106 +106 @@
 
 //! EKF for testing q44
-class EKFtest: public EKFfull{
+class EKFtest: public EKF_UD{
         void bayes ( const vec &yt, const vec &cond ) {
-                EKFfull::bayes(yt,cond);
-                mat &P = est._R();
-                if (P(3,3)>3.14) 
-                        P(3,3)=3.14;            
+                EKF_UD::bayes(yt,cond);
+                vec D =  prior()._R()._D();
+                
+                if (D(3)>10) D(3) = 10;
+                
+                prior()._R().__D()=D;
         }
 };

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

@@ -221 +221 @@
     if (x>6554) {
         int16 xm05=x-16384;
-        tmp = ((long)k1*xm05)>>15;
-        tmp-=(((long(k2)*xm05)>>15)*xm05)>>15;
+        tmp = ((int32)k1*xm05)>>15;
+        tmp-=(((int32(k2)*xm05)>>15)*xm05)>>15;
         tmp +=k1;
     } else {
         tmp = 4*x;
-        tmp-=long(8*x)*x>>15;
+        tmp-=int32(8*x)*x>>15;
     }
     return tmp;
@@ -251 +251 @@
         for (j=0;j<dimx;j++)
         {
-            sigma+=((long)B[k*dimx+j]*B[k*dimx+j]);
+            sigma+=((int32)B[k*dimx+j]*B[k*dimx+j]);
         }
         for (j=0;j<=k;j++)
         {
-            sigma+=((long)Ch[k*dimx+j]*Ch[k*dimx+j]);
+            sigma+=((int32)Ch[k*dimx+j]*Ch[k*dimx+j]);
         }
         
         //alpha in qCh
-        alpha = (int)(sqrt((double)sigma)+0.5);   // predelat pro DSP
+        alpha = (int16)(sqrt((double)sigma)+0.5);   // predelat pro DSP
 
                 sigma=0;
         for (j=0;j<dimx;j++) {
             w[j]=B[k*dimx+j];
-            sigma+=(long)w[j]*w[j];
+            sigma+=(int32)w[j]*w[j];
         }
         for (j=0; j<=k;j++) {
@@ -272 +272 @@
                 v[j]=Ch[k*dimx+j];
             }
-            sigma+=(long)v[j]*v[j];
+            sigma+=(int32)v[j]*v[j];
         }
         
@@ -281 +281 @@
             sigma=0;
             for (j=0;j<dimx;j++) {
-                sigma+=((long)B[i*dimx+j]*w[j]);
+                sigma+=((int32)B[i*dimx+j]*w[j]);
             }
             for (j=0;j<=k;j++) {
-                sigma+=(long)Ch[i*dimx+j]*v[j];
+                sigma+=(int32)Ch[i*dimx+j]*v[j];
             }
 
@@ -338 +338 @@
             sigma=Ch[iy*dimx+j];
             beta=alpha;
-            alpha+=((long)sigma*sigma)>>15;
+            alpha+=((int32)sigma*sigma)>>15;
 //                      double ab=(double)alpha*beta/32768./32768.;
 //                      double s_ab=sqrt(ab);
-            gamma=(int)(sqrt((double)((long)alpha*beta))+0.5);            // predelat v DSP
+            gamma=(int16)(sqrt((double)((int32)alpha*beta))+0.5);            // predelat v DSP
             //gamma = round(s_ab*(1<<15));
 //            eta=((long)beta<<15) / gamma;
@@ -348 +348 @@
             for (i=0;i<=j;i++) {
                 tau=Ch[i*dimx+j];
-                                tmp_long=((long)beta*Ch[i*dimx+j] -(long)sigma*w[i])/gamma;
+                                tmp_long=((int32)beta*Ch[i*dimx+j] -(int32)sigma*w[i])/gamma;
                                 if (tmp_long>32767) {
                                         Ch[i*dimx+j]=32767;
@@ -359 +359 @@
                                 }
                                 
-                w[i]+=((long)tau*sigma)>>15;
+                w[i]+=((int32)tau*sigma)>>15;
             }
         }
@@ -365 +365 @@
         //epsilon=(long(difz)<<15) / (alpha); // q15*q13/q13 = q15
         for (i=0;i<dimx;i++) {
-            xp[i]+=((long)w[i]*delta)/alpha;
+            xp[i]+=((int32)w[i]*delta)/alpha;
         }
     }
@@ -385 +385 @@
         for (i=dimx-1; i>=0; i--){
                 for (j=0; j<dimx; j++) {
-                        tmp_long=(long)Ch[i*dimx+i]*Ch[i*dimx+i]+long(A[i*dimx+j])*A[i*dimx+j];
+                        tmp_long=(int32)Ch[i*dimx+i]*Ch[i*dimx+i]+int32(A[i*dimx+j])*A[i*dimx+j];
                         if (tmp_long>0){
                                 rho=sqrt(double(tmp_long));
-                                s=(long(A[i*dimx+j])<<15)/rho;
-                                c=(long(Ch[i*dimx+i])<<15)/rho;
+                                s=(int32(A[i*dimx+j])<<15)/rho;
+                                c=(int32(Ch[i*dimx+i])<<15)/rho;
                                 for (k=0;k<=i; k++){
-                                        tau=(long(c)*A[k*dimx+j]-long(s)*Ch[k*dimx+i])>>15;
-                                        Ch[k*dimx +i]=(long(s)*A[k*dimx+j]+long(c)*Ch[k*dimx+i])>>15;
+                                        tau=(int32(c)*A[k*dimx+j]-int32(s)*Ch[k*dimx+i])>>15;
+                                        Ch[k*dimx +i]=(int32(s)*A[k*dimx+j]+int32(c)*Ch[k*dimx+i])>>15;
                                         A[k*dimx +j]=tau;
                                 }
@@ -399 +399 @@
 
                 for (j=0; j<i; j++){
-                        tmp_long=(long(Ch[i*dimx+i])*Ch[i*dimx+i]+long(Ch[i*dimx+j])*Ch[i*dimx+j]);
+                        tmp_long=(int32(Ch[i*dimx+i])*Ch[i*dimx+i]+int32(Ch[i*dimx+j])*Ch[i*dimx+j]);
                         if (tmp_long>0){
                                 rho=sqrt((double)(tmp_long));
-                                s=(long(Ch[i*dimx+j])<<15)/rho;
-                                c=(long(Ch[i*dimx+i])<<15)/rho;
+                                s=(int32(Ch[i*dimx+j])<<15)/rho;
+                                c=(int32(Ch[i*dimx+i])<<15)/rho;
                                 for (k=0; k<=i; k++){
-                                        tau=(long(c)*Ch[k*dimx+j]-long(s)*Ch[k*dimx+i])>>15;
-                                        Ch[k*dimx+i]=(long(s)*Ch[k*dimx+j]+long(c)*Ch[k*dimx+i])>>15;
+                                        tau=(int32(c)*Ch[k*dimx+j]-int32(s)*Ch[k*dimx+i])>>15;
+                                        Ch[k*dimx+i]=(int32(s)*Ch[k*dimx+j]+int32(c)*Ch[k*dimx+i])>>15;
                                         Ch[k*dimx+j]=tau;
                                 }
@@ -443 +443 @@
         }
 }
+
+
+void givens_fast(int16 *Ch, int16 *Q, unsigned int16 dimx)
+{
+        int16 i,j,k;
+        int16 rho,s,c,tau;
+        int32  tmp_long;
+        
+        int16 A[25];//beware
+        
+        int16 *A_ij, *Q_i, *Ch_ki, *Ch_kj, *Ch_ii, *Ch_ij, *A_kj;
+        
+        A_ij=A;
+        Q_i=Q;
+        // copy Q to A
+        for (i=0;i<dimx*dimx;i++)
+        {
+                //              A[i]=Q[i]>>(15-qCh);
+                *A_ij++=(*Q_i++)>>(15-qCh);
+        }
+        
+        for (i=dimx-1; i>=0; i--)
+        {
+                Ch_ii=Ch+i*dimx+i;
+                A_ij=A+i*dimx;
+                
+                for (j=0; j<dimx; j++) 
+                {
+                        //                      tmp_long=(long)Ch[i*dimx+i]*Ch[i*dimx+i]+(long)A[i*dimx+j]*A[i*dimx+j];
+                        
+                        tmp_long=(int32)*Ch_ii**Ch_ii+(int32)*A_ij**A_ij;
+                        
+                        if (tmp_long>0)
+                        {
+                                //                              rho=qsqrt(tmp_long);                   // verze pro DSP
+                                rho=(int16)(sqrt((double)tmp_long));     // verze pro PC
+                                s=((int32)*A_ij<<15)/rho;
+                                c=((int32)*Ch_ii<<15)/rho;
+                                
+                                Ch_ki=Ch+i;
+                                A_kj=A+j;
+                                
+                                for (k=0;k<=i; k++)
+                                {
+                                        tau=((int32)c**A_kj-(int32)s**Ch_ki)>>15;
+                                        *Ch_ki=((int32)s**A_kj+(int32)c**Ch_ki)>>15;
+                                        *A_kj=tau;
+                                        
+                                        Ch_ki+=dimx;
+                                        A_kj+=dimx;
+                                }
+                        }
+                        A_ij++;
+                }
+                
+                Ch_ij = Ch+i*dimx;
+                
+                for (j=0; j<i; j++)
+                {
+                        tmp_long=(int32)*Ch_ii**Ch_ii+(int32)*Ch_ij**Ch_ij;
+                        
+                        if (tmp_long>0)
+                        {
+                                //                      rho=qsqrt(tmp_long);                     // verze pro DSP
+                                rho=(int16)(sqrt((double)tmp_long));       // verze pro PC
+                                s=((int32)*Ch_ij<<15)/rho;
+                                c=((int32)*Ch_ii<<15)/rho;
+                                
+                                Ch_kj = Ch + j;
+                                Ch_ki = Ch + i;
+                                
+                                for (k=0; k<=i; k++)
+                                {
+                                        tau=((int32)c**Ch_kj-(int32)s**Ch_ki)>>15;
+                                        *Ch_ki =((int32)s**Ch_kj+(int32)c**Ch_ki)>>15;
+                                        *Ch_kj=tau;
+                                        
+                                        Ch_kj += dimx;
+                                        Ch_ki += dimx;
+                                }
+                        }
+                        Ch_ij++;
+                }
+        }
+}
+
+void carlson_fast(int16 *difz, int16 *xp, int16 *Ch, int16 *R, unsigned int16 dimy, unsigned int16 dimx ) {
+        int16 alpha,beta,gamma;
+        int16 delta, eta,epsilon,zeta,sigma,tau;
+        int16 i,j,iy;
+        int16 w[5];
+        int32 tmp_long;
+        
+        int16 *Ch_ij, *w_i, *x_i;
+        
+        
+        for (iy=0; iy<dimy; iy++)
+        {
+                alpha=R[iy];
+                delta = difz[iy];
+                
+                for (j=0;j<dimx;j++)
+                {
+                        sigma=Ch[iy*dimx+j];
+                        beta=alpha;
+                        //            alpha+=((long)sigma*sigma)>>15;
+                        alpha=(((int32)alpha<<15)+(int32)sigma*sigma)>>15;                              // vyssi presnost
+                        //            gamma= qsqrt(((long)alpha*beta));                          // verze pro DSP
+                        gamma= (int16)(sqrt((double)((int32)alpha*beta)));              // verze pro PC
+                        
+                        w[j]=0;
+                        
+                        Ch_ij=Ch+j;
+                        w_i=w;
+                        
+                        for (i=0;i<=j;i++)
+                        {
+                                //                tau=Ch[i*dimx+j];
+                                tau=*Ch_ij;
+                                //                              tmp_long=((long)beta*Ch[i*dimx+j] -(long)sigma*w[i])/gamma;
+                                tmp_long=((int32)beta**Ch_ij -(int32)sigma**w_i)/gamma;
+                                
+                                if (tmp_long>32767)
+                                        tmp_long=32767;
+                                if (tmp_long<-32768)
+                                        tmp_long=-32768;
+                                *Ch_ij=tmp_long;
+                                
+                                //                w_i+=((long)tau*sigma)>>15;
+                                *w_i=(((int32)*w_i<<15)+(int32)tau*sigma)>>15;
+                                
+                                w_i++;
+                                Ch_ij+=dimx;
+                        }
+                }
+                
+                x_i=xp;
+                w_i=w;
+                for (i=0;i<dimx;i++) {
+                        //            xp[i]+=((long)w[i]*delta)/alpha;
+                        //            *x_i+=((long)*w_i*delta)/alpha;
+                        *x_i=((int32)*x_i*alpha+(int32)*w_i*delta)/alpha;               // vyssi presnost
+                        x_i++;
+                        w_i++;
+                }
+        }
+}
+

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h

@@ -15 +15 @@
 #define qCh 14
 
-#define int16 int
-#define int32 long
+#define int16 short
+#define int32 int
 
 /* Matrix multiply Full matrix by upper diagonal matrix with unit diagonal; */
@@ -44 +44 @@
 /* perform Carlson update of Ch matrix using difz, R and xp, for size dimx*/
 extern void carlson(int16 *difz, int16 *xp, int16 *Ch, int16 *R, unsigned int16 dimy, unsigned int16 dimx );
+
+/* perform Givens update of Ch matrix using PSI*Ch , Q, */
+extern void givens_fast(int16 *Ch /*= int16 *PSICh*/, int16 *Q, unsigned int16 dimx);
+
+/* perform Carlson update of Ch matrix using difz, R and xp, for size dimx*/
+extern void carlson_fast(int16 *difz, int16 *xp, int16 *Ch, int16 *R, unsigned int16 dimy, unsigned int16 dimx );

applications/pmsm/simulator_zdenek/simulator.cpp

@@ -475 +475 @@
   *(u+1)+=0.00157;*/
   // uprava velikosti vzhledem k Uc!=Ucn
-  kor_Uc=Ucn/230.;
-  *u*=kor_Uc; /**/
+//  kor_Uc=Ucn/230.;
+//  *u*=kor_Uc; /**/
 
   pwm(modulace);

library/bdm/estim/kalman.cpp

@@ -532 +532 @@
                 mat Pf = T*P*T;
                 
+                mat TQT = T*Q*T;
+                
                 ldmat Pld(Pf);