Changeset 1245 for applications/pmsm/simulator_zdenek/ekf_example

Timestamp:

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

Author:

smidl

Message:

fixes in UD + Chol

Location:

applications/pmsm/simulator_zdenek/ekf_example

Files:

• ekf_obj.cpp (modified) (1 diff)
• matrix_vs.cpp (modified) (3 diffs)
• matrix_vs.h (modified) (1 diff)

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; */