Changeset 1241 for applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

Timestamp:

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

Author:

smidl

Message:

Choleski - covariance 0.01

Files:

• applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp (modified) (11 diffs)

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);
+        }
+}