Changeset 1237

Timestamp:

10/29/10 16:17:33 (14 years ago)

Author:

peroutka

Message:

 

Files:

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

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

@@ -196 +196 @@
     int *U_ij;
     int *x_i;
+    int dzs;
+
     a = U; // iyth row of U
     for (iy=0; iy<dimy; iy++, a+=dimx) {
@@ -220 +222 @@
             }
         }
-        int dzs = (((long)difz[iy])<<15)/alpha;  // apply scaling to innovations
+        dzs = (((long)difz[iy])<<15)/alpha;  // apply scaling to innovations
         // no shift due to gamma
         for (i=0,x_i=xp,b_i=b; i<dimx; i++,x_i++,b_i++) {
@@ -245 +247 @@
     int irows,jrows;
     long sigma; // in qAU+15!!
+    long z;
 
     for (i=0,Dold_i=Dold,D_i=D;i<rows;i++,Dold_i++,D_i++) {
@@ -263 +266 @@
 
     for (i=rows-1, Dold_i=Dold+i, D_i=D+i;
-            true; i--, Dold_i--,D_i--) { // stop if i==0 at the END!
+            1; i--, Dold_i--,D_i--) { // stop if i==0 at the END!
         irows=i*rows;
         sigma = 0;
@@ -274 +277 @@
             sigma += (((long)(*G_ik)**G_ik)>>16)**(Q+j+j*rows);
 
-        }        
-
-                if (sigma>(1<<(qAU+15))) {
+        }
+
+                if (sigma>((long)1<<(qAU+15))) {
                         *D_i = 32767;
-                        *(Dold+i)-=*(Q+i+irows);
+//                      *(Dold+i)-=*(Q+i+irows);
                 } else {
                         *D_i=sigma>>qAU;
@@ -291 +294 @@
                     k<rows; k++, PSIU_ik++, PSIU_jk++, Dold_k++) {
 
-                                sigma += (((long(*PSIU_ik)**PSIU_jk)>>qAU)**Dold_k);
+                                sigma += ((((long)(*PSIU_ik)**PSIU_jk)>>qAU)**Dold_k);
             }
 
@@ -299 +302 @@
             }
 
-            long z=(sigma/(*D_i))<<(15-qAU); // shift to q15
+            z=(sigma/(*D_i))<<(15-qAU); // shift to q15
             if (z>32767) z=32767;
             if (z<-32768) z=-32768;
@@ -372 +375 @@
         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);
@@ -393 +396 @@
 void householder(int *Ch /*= int *PSICh*/, int *Q, unsigned int dimx) {
     int k,j,i;
-    int sigma,alpha,beta;
+    int alpha,beta;
+    long sigma;
     int B[25];//beware
     int w[5];
@@ -399 +403 @@
 
     // copy Q to B
-    for (i=0;i<dimx*dimx;i++) {
+    for (i=0;i<dimx*dimx;i++)
+    {
         B[i]=Q[i];
     }
 
-    for (k=dimx-1; k>=0; k--) {
+    for (k=dimx-1; k>=0; k--)
+    {
         sigma=0;
-        for (j=0;j<dimx;j++) {
-            sigma+=(long(B[k*dimx+j])*B[k*dimx+j])>>15;
-        }
-        for (j=0;j<=k;j++) {
-            sigma+=(long(Ch[k*dimx+j])*Ch[k*dimx+j])>>15;
+        for (j=0;j<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);
+//              if (sigma>16384) sigma=16384;
+//        alpha=int_sqrt(sigma);
+          alpha = (int)(sqrt((double)sigma)+0.5);   // predelat pro DSP
         // alpha = alpf*(1<<15);
         //
@@ -420 +429 @@
         for (j=0;j<dimx;j++) {
             w[j]=B[k*dimx+j];
-            sigma+=(long(w[j])*w[j])>>15;
+            sigma+=(long)w[j])*w[j];
         }
         for (j=0; j<=k;j++) {
@@ -428 +437 @@
                 v[j]=Ch[k*dimx+j];
             }
-            sigma+=(long(v[j])*v[j])>>15;
-        }
-        alpha=sigma>>1;
-                if (alpha==0) alpha =1;
+            sigma+=(long)v[j]*v[j];
+        }
+        
+        alpha=sigma>>16;
+        if (alpha==0) alpha =1;
+
         for (i=0;i<=k;i++) {
             sigma=0;
             for (j=0;j<dimx;j++) {
-                sigma+=(long(B[i*dimx+j])*w[j])>>15;
+                sigma+=(long)B[i*dimx+j]*w[j];
             }
             for (j=0;j<=k;j++) {
-                sigma+=(long(Ch[i*dimx+j])*v[j])>>15;
-            }
-            for (j=0;j<dimx;j++) {
-                B[i*dimx+j]-=(long(sigma)*w[j]/alpha);
+                sigma+=(long)Ch[i*dimx+j]*v[j];
+            }
+
+            sigma = sigma >> 15;               // navrat do Q15
+
+            for (j=0;j<dimx;j++) 
+            {
+                B[i*dimx+j]-=(sigma*w[j])/alpha;
             };
-            for (j=0;j<=k;j++) {
-                Ch[i*dimx+j]-=(long(sigma)*v[j]/alpha);
+            
+            for (j=0;j<=k;j++)
+            {
+                Ch[i*dimx+j]-=(sigma*v[j])/alpha;
             }
         }
@@ -457 +474 @@
     int w[5];
 
-    for (iy=0; iy<dimy; iy++) {
+    for (iy=0; iy<dimy; iy++)
+    {
         alpha=R[iy];
         delta = difz[iy];
 
-        for (j=0;j<dimx;j++) {
+        for (j=0;j<dimx;j++)
+        {
             sigma=Ch[iy*dimx+j];
             beta=alpha;
-            alpha+=(long(sigma)*sigma)>>15;
+            alpha+=((long)sigma*sigma)>>15;
 //                      double ab=(double)alpha*beta/32768./32768.;
 //                      double s_ab=sqrt(ab);
-            gamma=int_sqrt(((long)alpha*beta)>>15);
+            gamma=sqrt((double)((long)alpha*beta))+0.5;            // predelat v DSP
             //gamma = round(s_ab*(1<<15));
-            eta=(long (beta)<<15) / gamma;
+//            eta=((long)beta<<15) / gamma;
             //zeta=(long(sigma)<<15)/ gamma;
             w[j]=0;
             for (i=0;i<=j;i++) {
                 tau=Ch[i*dimx+j];
-                Ch[i*dimx+j]=((long(eta)*Ch[i*dimx+j])>>15) -(long(sigma)*w[i])/gamma;
-                w[i]+=(long(tau)*sigma)>>15;
+                Ch[i*dimx+j]=((long)beta*Ch[i*dimx+j] -(long)sigma*w[i])/gamma;
+                w[i]+=((long)tau*sigma)>>15;
             }
         }
@@ -481 +500 @@
         //epsilon=(long(difz)<<15) / (alpha); // q15*q13/q13 = q15
         for (i=0;i<dimx;i++) {
-            xp[i]+=(long(w[i])*delta)/alpha;
+            xp[i]+=((long)w[i]*delta)/alpha;
         }
     }
@@ -490 +509 @@
         int i,j,k;
         int rho,s,c,tau;
-        
+
         int A[25];//beware
         // copy Q to A
@@ -496 +515 @@
                 A[i]=Q[i];
         }
-        
-        
+
+
         for (i=dimx-1; i>=0; i--){
                 for (j=0; j<dimx; j++) {