Changeset 1329 for applications/pmsm/simulator_zdenek/ekf_example

Timestamp:

04/13/11 12:16:50 (14 years ago)

Author:

smidl

Message:

correct rounding in Choleski

Location:

applications/pmsm/simulator_zdenek/ekf_example

Files:

• matrix_vs.cpp (modified) (5 diffs)
• matrix_vs.h (modified) (1 diff)

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

@@ -542 +542 @@
                         else
                 */      
-                if (tmp_sum>0)
-                        *respom++=(tmp_sum+(1<<14)-1)>>15;
-                else 
-                        *respom++=(tmp_sum-(1<<14)+1)>>15;
+                *respom++=(tmp_sum+(1<<14))>>15;
                 
                         tmp_sum=0;
@@ -597 +594 @@
                                         tau=((int32)c**A_kj-(int32)s**Ch_ki)>>14;
                                         tmp_long=(int32)s**A_kj+(int32)c**Ch_ki;
-                                        if (tmp_long>(1<<28)) //q14 + q15
-                                                *Ch_ki = (1<<14)-1;
+                                        if (tmp_long>(1<<29)) //q14 + q14
+                                                *Ch_ki = (1<<15)-1;
                                         else 
                                                 *Ch_ki=tmp_long>>14;
@@ -741 +738 @@
                         //            alpha+=((long)sigma*sigma)>>15;
                         tmp_long=((int32)alpha<<15)+(((int32)sigma*sigma)<<(30-2*qCh));
-                        if (tmp_long>0)
-                                alpha=(tmp_long+(1<<14)-1)>>15;                         // vyssi presnost
-                        else
-                                alpha=(tmp_long-(1<<14)+1)>>15;                         // vyssi presnost
+                        alpha=(tmp_long+(1<<14))>>15;                           // vyssi presnost
                                         
                         //            gamma= qsqrt(((long)alpha*beta));                          // verze pro DSP
@@ -769 +763 @@
                                 //                w_i+=((long)tau*sigma)>>15;
                                 tmp_long = ((int32)*w_i<<15)+((int32)tau*sigma<<(30-2*qCh));
-                                /*if (tmp_long>0)
-                                        *w_i=(tmp_long+(1<<14)-1)>>15;
-                                else 
-                                        *w_i=(tmp_long-(1<<14)+1)>>15;
-                                */ *w_i=(tmp_long)>>15;
+                                *w_i=(tmp_long+(1<<14))>>15;
                                 
                                 w_i++;
@@ -789 +779 @@
                         w_i++;
                 }
-                printf("Kg: %d %d\n",w[0],w[1]);
-        }
-}
-
+        }
+}
+

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h

@@ -16 +16 @@
 #define qCh 14
 
-#define int16 int //short
-#define int32 long //int
+#define int16 short
+#define int32 int
 
 /* Matrix multiply Full matrix by upper diagonal matrix with unit diagonal; */