Changeset 1235

Timestamp:

10/27/10 11:10:34 (14 years ago)

Author:

smidl

Message:

thorton_fast: cutoff od D at 1<<15, sigma in qUA+15, G in q15

Files:

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

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

@@ -40 +40 @@
 
             // saturation effect
-            tmp_sum=tmp_sum>>15;
+/*            tmp_sum=tmp_sum>>15;
             if (tmp_sum>32767) {
                                 printf("Au - saturated\n");
@@ -46 +46 @@
             if (tmp_sum<-32768) {
                                 printf("Au - saturated\n");
-            }
+            }*/
 //                      printf("Au - saturated\n");
 
-            *respom++=tmp_sum;
+            *respom++=tmp_sum>>15;
 
             tmp_sum=0;
@@ -244 +244 @@
     int *G_ik,*G_jk;
     int irows,jrows;
-    long sigma; // in q30!!
+    long sigma; // in qAU+15!!
 
     for (i=0,Dold_i=Dold,D_i=D;i<rows;i++,Dold_i++,D_i++) {
@@ -252 +252 @@
     // initialize G = eye()
     G_ik= G;
-    *G_ik++=1<<14;
+    *G_ik++=32767;
     for (i=0;i<rows-1;i++) {
         // clean elem before diag
@@ -258 +258 @@
             *G_ik++=0;
         }
-        *G_ik++=1<<14;
+        *G_ik++=32767;
     }
     // eye created
@@ -268 +268 @@
         for (k=0, PSIU_ik=PSIU+irows,Dold_k=Dold;
                 k<rows; k++, PSIU_ik++,Dold_k++) {
-                sigma += (((long)(*PSIU_ik)**Dold_k)>>(qAU))*(*PSIU_ik)<<(15-qAU);
-        }
-        sigma += *(Q+i+irows)<<15;
+                        sigma += (((long)(*PSIU_ik)**PSIU_ik)>>(qAU))*(*Dold_k);
+        }
+        sigma += *(Q+i+irows)<<qAU;
         for (j=i+1, G_ik=G+irows+i+1; j<rows; j++,G_ik++) {
-            sigma += (((long)(*G_ik)**G_ik)>>13)**(Q+j+j*rows);
-
-        }
-        
-        *D_i=sigma>>15;
+            sigma += (((long)(*G_ik)**G_ik)>>16)**(Q+j+j*rows);
+
+        }        
+
+                if (sigma>(1<<(qAU+15))) {
+                        *D_i = 32767;
+                        *(Dold+i)-=*(Q+i+irows);
+                } else {
+                        *D_i=sigma>>qAU;
+                }
         if (*D_i==0) *D_i=1;
-                if (*D_i>16384) *D_i = 16384;
-                
 
         for (j=0;j<i;j++) {
@@ -288 +291 @@
                     k<rows; k++, PSIU_ik++, PSIU_jk++, Dold_k++) {
 
-                                sigma += (((long(*PSIU_ik)**Dold_k)>>qAU)**PSIU_jk)<<(15-qAU);
-            }
-
-            for (k=i+1,G_ik=G+irows+i,G_jk=G+jrows+i,Q_kk=Q+k*rows+k;
+                                sigma += (((long(*PSIU_ik)**PSIU_jk)>>qAU)**Dold_k);
+            }
+
+            for (k=i,G_ik=G+irows+i,G_jk=G+jrows+i,Q_kk=Q+k*rows+k;
                     k<rows;k++,G_ik++,G_jk++,Q_kk+=rows+1) {
-                sigma += ((((long)*G_ik)**G_jk)>>13)**Q_kk;
-            }
-
-            long z=sigma/(*D_i); // shift by 15
+                sigma += ((((long)*G_ik)**G_jk)>>16)**Q_kk;
+            }
+
+            long z=(sigma/(*D_i))<<(15-qAU); // shift to q15
             if (z>32767) z=32767;
             if (z<-32768) z=-32768;
@@ -315 +318 @@
 
         }
-        if (i==0) return;
+                if (i==0) return;
     }
 }