Changeset 1245 for applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

Timestamp:

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

Author:

smidl

Message:

fixes in UD + Chol

Files:

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

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;
+                                }
                         }
                 }
+
         }
 }