Changeset 1245 for applications/pmsm/simulator_zdenek

Timestamp:

11/02/10 21:20:26 (14 years ago)

Author:

smidl

Message:

fixes in UD + Chol

Location:

applications/pmsm/simulator_zdenek

Files:

• ekf_example/ekf_obj.cpp (modified) (1 diff)
• ekf_example/matrix_vs.cpp (modified) (3 diffs)
• ekf_example/matrix_vs.h (modified) (1 diff)
• test_UD.cpp (modified) (8 diffs)

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

@@ -661 +661 @@
                 
         // Tuning of matrix R
-        R[0]=prevod(0.05,15);         // 0.05
+        R[0]=prevod(0.1,15);         // 0.05
         R[3]=R[0];
         

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

@@ -114 +114 @@
 }
 
-
+// Thorton procedure - Kalman predictive variance in UD
+void thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 rows) {
+        thorton_fast(U, D, PSIU, Q, G, Dold, rows);
+}
+        
 // Thorton procedure - Kalman predictive variance in UD
 void thorton_fast(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 rows) {
@@ -284 +288 @@
 
             sigma = sigma >> 15;               // navrat do Q15
-            if (sigma>32767)sigma=32767;
+          //  if (sigma>32767)sigma=32767;
                         
             for (j=0;j<dimx;j++) 
@@ -393 +397 @@
                         }
                 }
-        }
-        for (j=0; j<i; j++){
-                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;
+
+                for (j=0; j<i; j++){
+                        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;
+                                }
                         }
                 }
+
         }
 }

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h

@@ -13 +13 @@
 #define qAU 14
 #define qD 14
-#define qCh 15
+#define qCh 14
 
-#define int16 short int
-#define int32 int
+#define int16 int
+#define int32 long
 
 /* Matrix multiply Full matrix by upper diagonal matrix with unit diagonal; */

applications/pmsm/simulator_zdenek/test_UD.cpp

@@ -2 +2 @@
 #include "ekf_example/ekf_obj.h"
 int main(){
-        int i;
+        int16 i;
         mat A = 0.99*eye(5);
         A(0.3) = 0.06;
@@ -13 +13 @@
         
         mat U=eye(5);
-        for (int i=0; i<5;i++) {
-                for (int j=i+1; j<5;j++) U(i,j)=2*randu(1)(0)-1;
+        for (int16 i=0; i<5;i++) {
+                for (int16 j=i+1; j<5;j++) U(i,j)=2*randu(1)(0)-1;
         }
         mat Q = diag(vec(" 0.2000 0.3000 0.4000 0.5000 0.6"));
@@ -22 +22 @@
                 vec xref = ones(5);
 
-                int PSI[25];
-                int PSIU[25]={0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0};
-                int Uf[25]={0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0};
-                int Gf[25]={0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0};
-                int Df[5]={0,0,0,0,0};
-                int Dfold[5]={0,0,0,0,0};
+                int16 PSI[25];
+                int16 PSIU[25]={0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0};
+                int16 Uf[25]={0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0};
+                int16 Gf[25]={0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0};
+                int16 Df[5]={0,0,0,0,0};
+                int16 Dfold[5]={0,0,0,0,0};
                 
-                int multip=1<<15;
+                int16 multip=1<<15;
                 
         /////////// COPY
         imat Af=round_i(A*multip);
-        mat_to_int(Af, PSI);
-        mat_to_int(round_i(U*multip),Uf);               
-        vec_to_int(round_i(D*multip), Df); 
-        int Qf[25];
-        mat_to_int(round_i(Q*multip), Qf);
-        int Rf[2];
-        vec_to_int(round_i(R*multip), Rf);
+        mat_to_int16(Af, PSI);
+        mat_to_int16(round_i(U*multip),Uf);             
+        vec_to_int16(round_i(D*multip), Df); 
+        int16 Qf[25];
+        mat_to_int16(round_i(Q*multip), Qf);
+        int16 Rf[2];
+        vec_to_int16(round_i(R*multip), Rf);
         
         
@@ -52 +52 @@
         cout << "Delta PSI: " << round_i(PhiU*multip-(1<<(15-qAU))*PUcmp) <<endl;
         
-        mat_to_int(round_i(PhiU*multip/(1<<(15-qAU))),PSIU); //<< make is same
+        mat_to_int16(round_i(PhiU*multip/(1<<(15-qAU))),PSIU); //<< make is same
 
 /////////// Test Thorton:
-        int dim=5;double sigma; int j,k; 
+        int16 dim=5;double sigma; int16 j,k; 
         vec Din = D;
         mat G=eye(5);
@@ -102 +102 @@
         
         // synchronize
-        mat_to_int(round_i(U*multip),Uf);               
-        vec_to_int(round_i(D*multip), Df); 
+        mat_to_int16(round_i(U*multip),Uf);             
+        vec_to_int16(round_i(D*multip), Df); 
         
 
@@ -109 +109 @@
         vec     xp = 2*randu(5)-1;
         
-        int difz[2];
-        vec_to_int(round_i(ydif*multip), difz);
-        int xf[5];
-        vec_to_int(round_i(xp*multip), xf);
+        int16 difz[2];
+        vec_to_int16(round_i(ydif*multip), difz);
+        int16 xf[5];
+        vec_to_int16(round_i(xp*multip), xf);
 
         cout << "x: "<< round_i(xp*multip) <<endl;
@@ -118 +118 @@
         
         
-        int xf_old[5];
-        vec_to_int(ivec(xf,5),xf_old);
+        int16 xf_old[5];
+        vec_to_int16(ivec(xf,5),xf_old);
         
         /////// Test bierman
@@ -127 +127 @@
         mat C = zeros(2,5);
         C(0,0)=1.0;C(1,1)=1.0;
-        for (int iy=0; iy<2; iy++){
+        for (int16 iy=0; iy<2; iy++){
                 a     = U.T()*C.get_row(iy);    // a is not modified, but 
                 b     = elem_mult(D,a);                          // b is modified to become unscaled Kalman gain.