Changeset 1245 for applications/pmsm/simulator_zdenek/test_UD.cpp

Timestamp:

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

Author:

smidl

Message:

fixes in UD + Chol

Files:

• applications/pmsm/simulator_zdenek/test_UD.cpp (modified) (8 diffs)

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.