Changeset 1230

Timestamp:

10/25/10 09:41:23 (14 years ago)

Author:

smidl

Message:

givens + new Q for UD

Location:

applications/pmsm/simulator_zdenek

Files:

• ekf_example/ekf_obj.cpp (modified) (6 diffs)
• ekf_example/matrix_vs.cpp (modified) (10 diffs)
• ekf_example/matrix_vs.h (modified) (1 diff)
• test_Ch.cpp (modified) (4 diffs)

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

@@ -445 +445 @@
         mmultAU(PSI,Uf,PSIU,4,4);
         //thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx);
-        thorton(Uf,Df,PSIU,Q,G,Dfold,4);
+        thorton_fast(Uf,Df,PSIU,Q,G,Dfold,4);
         
          {
@@ -469 +469 @@
         int dR[2];dR[0]=R[0];dR[1]=R[3];
         //int 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(difz,x_est,Uf,Df,dR,2,4);
+        bierman_fast(difz,x_est,Uf,Df,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];
         
@@ -487 +487 @@
 {
         // Tuning of matrix Q
-        Q[0]=prevod(.0005,15);       // 0.05
+        Q[0]=prevod(.001,15);       // 0.05
         Q[5]=Q[0];
-        Q[10]=prevod(0.0001,15);      // 1e-3
+        Q[10]=prevod(0.001,15);      // 1e-3
         Q[15]=prevod(0.0001,15);      // 1e-3
 
@@ -585 +585 @@
         //thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx);
         householder(PSICh,Q,4);
+        // COPY 
+        for (int ii=0; ii<16; ii++){Chf[ii]=PSICh[ii];}
         
         {
@@ -600 +602 @@
         int dR[2];dR[0]=R[0];dR[1]=R[3];
         //int 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);
         //x_est[0] = xb[0]>>2; x_est[1]=xb[1]>>2; x_est[2]=xb[2]>>2; x_est[3]=xb[3];
@@ -618 +621 @@
 {
         // 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.0001,15);      // 1e-3
+        Q[10]=prevod(0.001,15);      // 1e-3
         Q[15]=prevod(0.0001,15);      // 1e-3
         
-        Chf[0]=0x7FFF;// !       // 0.05
+        Chf[0]=0x3FFF;// !       // 0.05
         Chf[1]=Chf[2]=Chf[3]=Chf[4]=0;
-        Chf[5]=0x7FFF;//!
+        Chf[5]=0x3FFF;//!
         Chf[6]=Chf[6]=Chf[8]=Chf[9]=0;
-        Chf[10]=0x7FFF;//!      // 1e-3
+        Chf[10]=0x3FFF;//!      // 1e-3
         Chf[11]=Chf[12]=Chf[13]=Chf[4]=0;
-        Chf[15]=0x7FFF;      // 1e-3
+        Chf[15]=0x3FFF;      // 1e-3
                 
         // Tuning of matrix R

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

@@ -136 +136 @@
         for (j=0;j<rows; j++) {
             //long s1=(((long)PSIU[i+j*rows]*PSIU[i+j*rows])>>15)*(Dold[i]);
-            long s2=((((long)PSIU[i*rows+j]*PSIU[i*rows+j]))>>(2*qAU-15))*Dold[j];
+            long s2=((((long)PSIU[i*rows+j]*Dold[j]))>>(2*qAU-15))*PSIU[i*rows+j];
 //                      printf("%d - %d\n",s1,s2);
             sigma += s2;
         }
-        sigma += Q[i*rows+i]<<15;
+        sigma += Q[i*rows+i]<<15; // Q is in q15
         for (j=i+1;j<rows; j++) {
             sigma += (((long)G[i*rows+j]*G[i*rows+j])>>13)*Q[j*rows+j];
@@ -146 +146 @@
         }
 
-//        if (sigma>16384<<15) sigma = 16384<<15;
+        if (sigma>16384<<15) sigma = 16384<<15;
         *(D+i)=sigma>>15;
         if (D[i]==0) D[i]=1;
@@ -155 +155 @@
             sigma =0;
             for (k=0;k<rows;k++) {
-                                sigma += (((long(PSIU[i*rows+k])*PSIU[j*rows+k]))>>(2*qAU-15))*Dold[k];
+                                sigma += (((long(PSIU[i*rows+k])*Dold[k]))>>(2*qAU-15))*PSIU[j*rows+k];
             }
             for (k=0;k<rows;k++) {
@@ -266 +266 @@
         for (k=0, PSIU_ik=PSIU+irows,Dold_k=Dold;
                 k<rows; k++, PSIU_ik++,Dold_k++) {//Dold_i=
-            sigma += (((long)(*PSIU_ik)**PSIU_ik)>>15)*(*Dold_k);
+                sigma += (((long)(*PSIU_ik)**Dold_k)>>(2*qAU-15))*(*PSIU_ik);
         }
         sigma += *(Q+i+irows)<<15;
@@ -273 +273 @@
 
         }
-
+        
+        if (sigma>16384<<15) sigma = 16384<<15;
         *D_i=sigma>>15;
         if (*D_i==0) *D_i=1;
@@ -285 +286 @@
                     k<rows; k++, PSIU_ik++, PSIU_jk++, Dold_k++) {
 
-                sigma += ((((long)*PSIU_ik)**PSIU_jk)>>15)**Dold_k;
+                                sigma += ((((long)*PSIU_ik)**Dold_k)>>(2*qAU-15))**PSIU_jk;
             }
 
@@ -364 +365 @@
 /* square root of 0<a<1 using taylor at 0.5 in q15*/
 int int_sqrt(int x) {
+        double xd(double(x)/32768.);
+        return round(sqrt(xd)*32768);
+        
     //sqrt(x) == 1/2*2^(1/2)+1/2*2^(1/2)*(x-1/2)-1/4*2^(1/2)*(x-1/2)^2
     //         = k1 + k1*(x-0.5) - k2*(x-0.5)(x-0.5);
@@ -404 +408 @@
         /*               double sigf=double(sigma)/(1<<15);
                          double alpf = sqrt(sigf);*/
+                if (sigma>16384) sigma=16384;
         alpha=int_sqrt(sigma);
         // alpha = alpf*(1<<15);
@@ -421 +426 @@
         }
         alpha=sigma>>1;
+                if (alpha==0) alpha =1;
         for (i=0;i<=k;i++) {
             sigma=0;
@@ -474 +480 @@
     }
 }
+
+/* perform Householder update of Ch matrix using PSI*Ch , Q, */
+extern void givens(int *Ch /*= int *PSICh*/, int *Q, unsigned int dimx){
+        int i,j,k;
+        int rho,s,c,tau;
+        
+        int A[25];//beware
+        // copy Q to A
+        for (i=0;i<dimx*dimx;i++) {
+                A[i]=Q[i];
+        }
+        
+        
+        for (i=dimx-1; i>=0; i--){
+                for (j=0; j<dimx; j++) {
+                        rho=int_sqrt(((long)Ch[i*dimx+i]*Ch[i*dimx+i]+long(A[i*dimx+j])*A[i*dimx+j])>>15);
+                        if (rho==0) break;
+                        s=(long(A[i*dimx+j])<<15)/rho;
+                        c=(long(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;
+                                A[k*dimx +j]=tau;
+                        }
+                }
+        }
+        for (j=0; j<i; j++){
+                rho=int_sqrt((long(Ch[i*dimx+i])*Ch[i*dimx+i]+long(Ch[i*dimx+j])*Ch[i*dimx+j])>>15);
+                if (rho==0) break; 
+                s=(long(Ch[i*dimx+j])<<15)/rho;
+                c=(long(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;
+                        Ch[k*dimx+j]=tau;
+                }
+        }
+}

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h

@@ -34 +34 @@
 extern void householder(int *Ch /*= int *PSICh*/, int *Q, unsigned int dimx);
 
+/* perform Givens update of Ch matrix using PSI*Ch , Q, */
+extern void givens(int *Ch /*= int *PSICh*/, int *Q, unsigned int dimx);
+
 /* perform Carlson update of Ch matrix using difz, R and xp, for size dimx*/
 extern void carlson(int *difz, int *xp, int *Ch, int *R, unsigned int dimy, unsigned int dimx );

applications/pmsm/simulator_zdenek/test_Ch.cpp

@@ -89 +89 @@
         
         householder(PSICh,Qf,5);
+//      givens(PSICh,Qf,5);
 
         /////// disp
@@ -103 +104 @@
         mat Ch2 = 0.9*diag(1./sqrt(d))*Ch;
         mat_to_int(round_i(Ch2*multip),Chf);            
-        
+
+        D = pow(diag(Ch2),2);
+        U = sqrt(diag(1./D));
+        U = Ch2*U;
+                
         vec     ydif = 2*randu(2)-1;
         vec     xp = 2*randu(5)-1;
@@ -159 +164 @@
         
         carlson(difz,xf, Chf, Rf, 2, 5);
+        
         cout << endl<<"after Carlson" <<endl;
         cout << "x: "<< round_i(xp*multip) <<endl;
@@ -164 +170 @@
 
         {
-        imat Chcmp(Chf,5,5);
-        cout << "Delat Ch: " << round_i(Ch*multip-Chcmp) << endl;
+                imat Chcmp(Chf,5,5);
+                cout << "Delat Ch: " << round_i(Ch*multip-Chcmp) << endl;
+                cout << "Delat Ch: " << Chcmp << endl;
         }