Changeset 1241

Timestamp:

10/29/10 19:10:12 (14 years ago)

Author:

smidl

Message:

Choleski - covariance 0.01

Location:

applications/pmsm/simulator_zdenek

Files:

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

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

@@ -604 +604 @@
         //thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx);
         householder(PSICh,Q,4);
+        //givens(PSICh,Q,4);
         // COPY 
         for (int16 ii=0; ii<16; ii++){Chf[ii]=PSICh[ii];}
         
         {
-/*              ivec Chd(Chf,16);
-                log_level.store(logCh,get_from_ivec(Chd));*/
+                
+                int Chi[16]; for (int i=0;i<16;i++) Chi[i]=Chf[i];
+                ivec Chd(Chi,16);
+                log_level.store(logCh,get_from_ivec(Chd));
+                imat mCh(Chd._data(), 4,4);
+                imat P = mCh*mCh.T();
+                ivec iP(P._data(),16);
+                log_level.store(logP,get_from_ivec(iP));
         }
         
@@ -640 +647 @@
 {
         // Tuning of matrix Q
-        Q[0]=prevod(.001,15);       // 0.05
+        Q[0]=prevod(.01,15);       // 0.05
         Q[5]=Q[0];
-        Q[10]=prevod(0.001,15);      // 1e-3
-        Q[15]=prevod(0.0001,15);      // 1e-3
+        Q[10]=prevod(0.01,15);      // 1e-3
+        Q[15]=prevod(0.01,15);      // 1e-3
         
         Chf[0]=0x3FFF;// !       // 0.05

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

@@ -16 +16 @@
 
 #include "matrix_vs.h"
+#include <math.h>
 
 /* Matrix multiply Full matrix by upper diagonal matrix; */
@@ -204 +205 @@
 
 /* square root of 0<a<1 using taylor at 0.5 in q15*/
-int int_sqrt(int x) {
+int16 int_sqrt(int16 x) {
         double xd(double(x)/32768.);
         return round(sqrt(xd)*32768);
@@ -213 +214 @@
 #define k2 11585 //0.25*sqrt(2)*32768
 
-    int tmp;
+    int16 tmp;
     if (x>6554) {
-        int xm05=x-16384;
+        int16 xm05=x-16384;
         tmp = ((long)k1*xm05)>>15;
         tmp-=(((long(k2)*xm05)>>15)*xm05)>>15;
@@ -226 +227 @@
 }
 
-void householder(int *Ch /*= int *PSICh*/, int *Q, unsigned int dimx) {
-    int k,j,i;
-    int alpha,beta;
-    long sigma;
-    int B[25];//beware
-    int w[5];
-    int v[5];
+void householder(int16 *Ch /*= int16 *PSICh*/, int16 *Q, unsigned int16 dimx) {
+    int16 k,j,i;
+    int16 alpha,beta;
+    int32  sigma; // 2*qCh
+    int32  tmp_long;
+    int16 B[25];//Q in qCh
+    int16 w[5];
+    int16 v[5];
 
     // copy Q to B
     for (i=0;i<dimx*dimx;i++)
     {
-        B[i]=Q[i];
+        B[i]=Q[i]>>(15-qCh);
     }
 
@@ -245 +247 @@
         for (j=0;j<dimx;j++)
         {
-            sigma+=(long)B[k*dimx+j]*B[k*dimx+j];
+            sigma+=((long)B[k*dimx+j]*B[k*dimx+j]);
         }
         for (j=0;j<=k;j++)
         {
-            sigma+=(long)Ch[k*dimx+j]*Ch[k*dimx+j];
-        }
-        /*               double sigf=double(sigma)/(1<<15);
-                         double alpf = sqrt(sigf);*/
-//              if (sigma>16384) sigma=16384;
-//        alpha=int_sqrt(sigma);
-          alpha = (int)(sqrt((double)sigma)+0.5);   // predelat pro DSP
-        // alpha = alpf*(1<<15);
-        //
-        sigma=0;
+            sigma+=((long)Ch[k*dimx+j]*Ch[k*dimx+j]);
+        }
+        
+        //alpha in qCh
+        alpha = (int)(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+=(long)w[j]*w[j];
         }
         for (j=0; j<=k;j++) {
@@ -272 +271 @@
         }
         
-        alpha=sigma>>16;
+        alpha=sigma>>(qCh+1); // alpha = sigma /2;
         if (alpha==0) alpha =1;
 
@@ -278 +277 @@
             sigma=0;
             for (j=0;j<dimx;j++) {
-                sigma+=(long)B[i*dimx+j]*w[j];
+                sigma+=((long)B[i*dimx+j]*w[j]);
             }
             for (j=0;j<=k;j++) {
@@ -285 +284 @@
 
             sigma = sigma >> 15;               // navrat do Q15
-
+            if (sigma>32767)sigma=32767;
+                        
             for (j=0;j<dimx;j++) 
             {
-                B[i*dimx+j]-=(sigma*w[j])/alpha;
+                                tmp_long=B[i*dimx+j]-(sigma*w[j])/alpha;
+                                if (tmp_long>32767) {
+                                        B[i*dimx+j]=32767;
+                                } else {
+                                        if (tmp_long<-32767){
+                                                B[i*dimx+j]=-32767;
+                                        } else {
+                                                B[i*dimx+j]=tmp_long;
+                                        }
+                                }                       
             };
             
             for (j=0;j<=k;j++)
             {
-                Ch[i*dimx+j]-=(sigma*v[j])/alpha;
-            }
-        }
-    }
-
-}
-
-void carlson(int *difz, int *xp, int *Ch, int *R, unsigned int dimy, unsigned int dimx ) {
-    int alpha,beta,gamma;
-    int delta, eta,epsilon,zeta,sigma,tau;
-    int i,j,iy;
-    int w[5];
+                tmp_long=Ch[i*dimx+j]-(sigma*v[j])/alpha;
+                                if (tmp_long>32767) {
+                                        Ch[i*dimx+j]=32767;
+                                } else {
+                                        if (tmp_long<-32767){
+                                                Ch[i*dimx+j]=-32767;
+                                        } else {
+                                                Ch[i*dimx+j]=tmp_long;
+                                        }
+                                }
+                        }
+        }
+    }
+
+}
+
+void carlson(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;
 
     for (iy=0; iy<dimy; iy++)
@@ -325 +344 @@
             for (i=0;i<=j;i++) {
                 tau=Ch[i*dimx+j];
-                Ch[i*dimx+j]=((long)beta*Ch[i*dimx+j] -(long)sigma*w[i])/gamma;
+                                tmp_long=((long)beta*Ch[i*dimx+j] -(long)sigma*w[i])/gamma;
+                                if (tmp_long>32767) {
+                                        Ch[i*dimx+j]=32767;
+                                } else {
+                                        if (tmp_long<-32767){
+                                                Ch[i*dimx+j]=-32767;
+                                        } else {
+                                                Ch[i*dimx+j]=tmp_long;
+                                        }
+                                }
+                                
                 w[i]+=((long)tau*sigma)>>15;
             }
@@ -338 +367 @@
 
 /* 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
+extern void givens(int16 *Ch /*= int16 *PSICh*/, int16 *Q, unsigned int16 dimx){
+        int16 i,j,k;
+        int16 rho,s,c,tau;
+        int32  tmp_long;
+
+        int16 A[25];//beware
         // copy Q to A
         for (i=0;i<dimx*dimx;i++) {
-                A[i]=Q[i];
+                A[i]=Q[i]>>(15-qCh);
         }
 
@@ -351 +381 @@
         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;
+                        tmp_long=(long)Ch[i*dimx+i]*Ch[i*dimx+i]+long(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;
+                                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;
+                tmp_long=(long(Ch[i*dimx+i])*Ch[i*dimx+i]+long(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;
+                        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;
+                        }
                 }
         }
 }
+
+/* Matrix multiply Full matrix by upper diagonal matrix; */
+void mmultACh(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns) {
+        unsigned int16 i, j, k;
+        int32 tmp_sum=0L;
+        int16 *m2pom;
+        int16 *m1pom=m1;
+        int16 *respom=result;
+        
+        for (i=0; i<rows; i++) //rows of result
+    {
+                for (j=0; j<columns; j++) //columns of result
+        {
+                        m2pom=up+j;//??
+                        
+                        for (k=0; k<=j; k++) //inner loop up to "j" - U(j,j)==1;
+            {
+                                tmp_sum+=(int32)(*(m1pom++))**m2pom;
+                                m2pom+=columns;
+                        }
+                        m1pom-=(j+1); // shift back to first element
+                        
+                        *respom++=tmp_sum>>15;
+                        
+                        tmp_sum=0;
+                }
+                m1pom+=(columns);
+        }
+}

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h

@@ -13 +13 @@
 #define qAU 14
 #define qD 14
+#define qCh 15
 
 #define int16 short int

applications/pmsm/simulator_zdenek/test_Ch.cpp

@@ -31 +31 @@
         /////////// COPY
         imat Af=round_i(A*multip);
-        mat_to_int(Af, PSI);
-        mat_to_int(round_i(Ch*multip),Chf);             
+        mat_to_int16(Af, PSI);
+        mat_to_int16(round_i(Ch*multip),Chf);           
         int Qf[25];
-        mat_to_int(round_i(sqrt(Q)*multip), Qf);
+        mat_to_int16(round_i(sqrt(Q)*multip), Qf);
         int Rf[2];
-        vec_to_int(round_i(R*multip), Rf);
+        vec_to_int16(round_i(R*multip), Rf);
         
         
@@ -49 +49 @@
         cout << "Delta PSI: " << round_i(PhiCh*multip-PChcmp) <<endl;
         
-        mat_to_int(round_i(PhiCh*multip),PSICh); //<< make is same
+        mat_to_int16(round_i(PhiCh*multip),PSICh); //<< make is same
 
         mat PhiU= A*U;
@@ -103 +103 @@
         vec d=diag(Ch*Ch.T());
         mat Ch2 = 0.9*diag(1./sqrt(d))*Ch;
-        mat_to_int(round_i(Ch2*multip),Chf);            
+        mat_to_int16(round_i(Ch2*multip),Chf);          
 
         D = pow(diag(Ch2),2);
@@ -113 +113 @@
         
         int difz[2];
-        vec_to_int(round_i(ydif*multip), difz);
+        vec_to_int16(round_i(ydif*multip), difz);
         int xf[5];
-        vec_to_int(round_i(xp*multip), xf);
+        vec_to_int16(round_i(xp*multip), xf);
 
         cout << "x: "<< round_i(xp*multip) <<endl;
@@ -122 +122 @@
         
         int xf_old[5];
-        vec_to_int(ivec(xf,5),xf_old);
+        vec_to_int16(ivec(xf,5),xf_old);
         
         /////// Test bierman
@@ -171 +171 @@
         {
                 imat Chcmp(Chf,5,5);
-                cout << "Delat Ch: " << round_i(Ch*multip-Chcmp) << endl;
-                cout << "Delat Ch: " << Chcmp << endl;
+                cout << "Delta Ch: " << round_i(Ch*multip-Chcmp) << endl;
+                cout << "Delta Ch: " << Chcmp << endl;
         }