Changeset 1340 for applications

Timestamp:

04/28/11 22:18:33 (13 years ago)

Author:

smidl

Message:

format of D is now explicitely qD. Good choice for 2d model is 13

Location:

applications/pmsm/simulator_zdenek/ekf_example

Files:

• matrix_vs.cpp (modified) (10 diffs)
• matrix_vs.h (modified) (1 diff)

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

@@ -85 +85 @@
 void bierman_fast(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *R, unsigned int16 dimy, unsigned int16 dimx )
 {
-    int16 alpha;
+    int16 alpha; // in qD!!
     int16 beta,lambda;
-    int16 b[5]; // ok even for 4-dim state
+    int16 b[5]; // ok even for 4-dim state  // in qD!!!
     int16 *a; // in [0,1] -> q15
     unsigned int16 iy,j,i;
@@ -96 +96 @@
     int16 *U_ij;
     int16 *x_i;
-
-    int32 z_pom;
-    int16 z_pom_int16;
         
         int16 Ucopy[16];
@@ -113 +110 @@
             *b_j=((int32)(*D_j)*(*a_j))>>15;
 
-        alpha = R[iy]; //\alpha = R+vDv = R+a*b
-        // R in q15, a in q15, b=q15
-//              gamma = (1<<15)/alpha; //(in q15)
-        //min alpha = R[iy] = 164
-        //max gamma = 0.0061 => gamma_ref = q7
+        alpha = (R[iy])>>(15-qD); //\alpha = R+vDv = R+a*b
         for (j=0,a_j=a,b_j=b,D_j=D; j<dimx; j++,a_j++,b_j++,D_j++) {
-/*            beta=alpha;
-            lambda = -((int32)(*a_j)<<15)/beta;
-            alpha  += ((int32)(*a_j)*(*b_j))>>15;
-            D[j] = ((int32)beta**D_j)/alpha;*/
-/*xx*/
-            lambda=alpha;
+
+                        lambda=alpha; // in qD
             alpha  += ((int32)(*a_j)*(*b_j))>>15;
             D[j] = ((int32)lambda**D_j)/alpha;
-            z_pom_int16 = -((int32)(*a_j)<<15)/lambda;
-/*xx*/
 
             if (*D_j==0) *D_j=1;
@@ -134 +121 @@
             for (i=0,b_i=b,U_ij=U+j; i<j; i++, b_i++,U_ij+=dimx) {
                 beta   = *U_ij;
-//                *U_ij +=  ((int32)lambda*(*b_i))>>15;         // puvodni reseni
                 *U_ij -=  ((int32)(*a_j)*(*b_i))/lambda;         // pozadovane optimalni reseni
-//                *U_ij -=  ((int32)((int16)((int32)(*a_j)<<15)/lambda)**b_i)>>15;      // tohle funguje - problem je s tim pretypovanim na (int16)
-//                              *U_ij -= (int16)((int32)(*a_j)*(*b_i))/lambda;
-//                              z_pom = (((int32)(*a_j)*(*b_i))/lambda);
-/*                          z_pom = (int32)(*U_ij)-(int16)((int32)(*a_j)*(*b_i))/lambda;
-                                if (z_pom > 32767) z_pom = 32767;
-                                if (z_pom < - 32768) z_pom = -32768;
-                                *U_ij = z_pom;                      /**/
-//                *U_ij +=  ((int32)z_pom_int16*(*b_i))>>15;            // puvodni reseni - jen jina konstanta
                 *b_i  +=  ((int32)beta*(*b_j))>>15;
             }
@@ -156 +134 @@
 void bierman_fastC(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *C, int16 *R, unsigned int16 dimy, unsigned int16 dimx )
 {
-        int16 alpha;
+        int16 alpha; // in qD
         int16 beta,lambda;
-        int16 b[5]; // ok even for 4-dim state
+        int16 b[5]; // ok even for 4-dim state  // in qD
         int16 *a; // in [0,1] -> qCU
         unsigned int16 iy,j,i;
@@ -180 +158 @@
                 // a is a row
                 for (j=0,a_j=a,b_j=b,D_j=D; j<dimx; j++,b_j++,D_j++,a_j++)
-                        *b_j=((int32)(*D_j)*(*a_j))>>15;
-                
-                alpha = R[iy]; //\alpha = R+vDv = R+a*b
+                        *b_j=((int32)(*D_j)*(*a_j))>>15; 
+                
+                alpha = (R[iy])>>(15-qD); //\alpha = R+vDv = R+a*b
                 // R in q15, a in q15, b=q15
                 //              gamma = (1<<15)/alpha; //(in q15)
@@ -232 +210 @@
     int16 *G_ik,*G_jk;
     int16 irows,jrows;
-    int32 sigma; // in qAU+15!!
+    int32 sigma; // in qAU+qD!!
     int32 z;
 
@@ -259 +237 @@
                         sigma += (((int32)(*PSIU_ik)**PSIU_ik)>>(qAU))*(*Dold_k);
         }
-        sigma += (int32)(*(Q+i+irows))<<qAU;
+        sigma += (int32)(*(Q+i+irows))<<(qAU+qD-15);
         for (j=i+1, G_ik=G+irows+i+1; j<rows; j++,G_ik++) {
-            sigma += (((int32)(*G_ik)**G_ik)>>(30-qAU))**(Q+j+j*rows);
+            sigma += ((((int32)(*G_ik)**G_ik)>>15)**(Q+j+j*rows))>>(30-qAU-qD);
         }
 
@@ -284 +262 @@
             for (k=i,G_ik=G+irows+i,G_jk=G+jrows+i,Q_kk=Q+k*rows+k;
                     k<rows;k++,G_ik++,G_jk++,Q_kk+=rows+1) {
-                sigma += ((((int32)*G_ik)**G_jk)>>(30-qAU))**Q_kk;
+                sigma += (((((int32)*G_ik)**G_jk)>>15)**Q_kk)>>(30-qD-qAU);
             }
 
@@ -308 +286 @@
                 if (i==0) return;
     }
-}
-
-/* square root of 0<a<1 using taylor at 0.5 in q15*/
-int16 int_sqrt(int16 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);
-#define k1 23170 //0.5*sqrt(2)*32768
-#define k2 11585 //0.25*sqrt(2)*32768
-
-    int16 tmp;
-    if (x>6554) {
-        int16 xm05=x-16384;
-        tmp = ((int32)k1*xm05)>>15;
-        tmp-=(((int32(k2)*xm05)>>15)*xm05)>>15;
-        tmp +=k1;
-    } else {
-        tmp = 4*x;
-        tmp-=int32(8*x)*x>>15;
-    }
-    return tmp;
 }
 

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h

@@ -13 +13 @@
 #define qAU 14
 #define qCU 15
-#define qD 14
+#define qD 13
 #define qCh 14