Changeset 1230 for applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

Timestamp:

10/25/10 09:41:23 (14 years ago)

Author:

smidl

Message:

givens + new Q for UD

Files:

• applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (modified) (6 diffs)

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

@@ -445 +445 @@
         mmultAU(PSI,Uf,PSIU,4,4);
         //thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx);
-        thorton(Uf,Df,PSIU,Q,G,Dfold,4);
+        thorton_fast(Uf,Df,PSIU,Q,G,Dfold,4);
         
          {
@@ -469 +469 @@
         int dR[2];dR[0]=R[0];dR[1]=R[3];
         //int xb[4]; xb[0]=x_est[0]<<2; xb[1]=x_est[1]<<2;  xb[2]=x_est[2]<<2;  xb[3]=x_est[3];  
-        bierman(difz,x_est,Uf,Df,dR,2,4);
+        bierman_fast(difz,x_est,Uf,Df,dR,2,4);
         //x_est[0] = xb[0]>>2; x_est[1]=xb[1]>>2; x_est[2]=xb[2]>>2; x_est[3]=xb[3];
         
@@ -487 +487 @@
 {
         // Tuning of matrix Q
-        Q[0]=prevod(.0005,15);       // 0.05
+        Q[0]=prevod(.001,15);       // 0.05
         Q[5]=Q[0];
-        Q[10]=prevod(0.0001,15);      // 1e-3
+        Q[10]=prevod(0.001,15);      // 1e-3
         Q[15]=prevod(0.0001,15);      // 1e-3
 
@@ -585 +585 @@
         //thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx);
         householder(PSICh,Q,4);
+        // COPY 
+        for (int ii=0; ii<16; ii++){Chf[ii]=PSICh[ii];}
         
         {
@@ -600 +602 @@
         int dR[2];dR[0]=R[0];dR[1]=R[3];
         //int xb[4]; xb[0]=x_est[0]<<2; xb[1]=x_est[1]<<2;  xb[2]=x_est[2]<<2;  xb[3]=x_est[3];  
+
         carlson(difz,x_est,Chf,dR,2,4);
         //x_est[0] = xb[0]>>2; x_est[1]=xb[1]>>2; x_est[2]=xb[2]>>2; x_est[3]=xb[3];
@@ -618 +621 @@
 {
         // Tuning of matrix Q
-        Q[0]=prevod(.01,15);       // 0.05
+        Q[0]=prevod(.001,15);       // 0.05
         Q[5]=Q[0];
-        Q[10]=prevod(0.0001,15);      // 1e-3
+        Q[10]=prevod(0.001,15);      // 1e-3
         Q[15]=prevod(0.0001,15);      // 1e-3
         
-        Chf[0]=0x7FFF;// !       // 0.05
+        Chf[0]=0x3FFF;// !       // 0.05
         Chf[1]=Chf[2]=Chf[3]=Chf[4]=0;
-        Chf[5]=0x7FFF;//!
+        Chf[5]=0x3FFF;//!
         Chf[6]=Chf[6]=Chf[8]=Chf[9]=0;
-        Chf[10]=0x7FFF;//!      // 1e-3
+        Chf[10]=0x3FFF;//!      // 1e-3
         Chf[11]=Chf[12]=Chf[13]=Chf[4]=0;
-        Chf[15]=0x7FFF;      // 1e-3
+        Chf[15]=0x3FFF;      // 1e-3
                 
         // Tuning of matrix R