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

Timestamp:

07/23/12 11:03:24 (12 years ago)

Author:

smidl

Message:

upravy choleskiho + nove qmath

Files:

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

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

@@ -663 +663 @@
 }
 
-void carlson_fastC(int16 *difz, int16 *xp, int16 *Ch, int16 *C, int16 *R, unsigned int16 dimy, unsigned int16 dimx ) {
+void carlson_fastC(int16 *difz, int16 *xp, int16 *Ch, int16 *C, int16 *R, unsigned int16 dimy, unsigned int16 dimx, int16 *detS, int16 *rem ) {
         int16 alpha,beta,gamma;
         int16 delta, eta,epsilon,zeta,sigma,tau;
@@ -672 +672 @@
         int16 *Ch_ij, *w_i, *x_i, *C_yi;
         
-        
+        *detS = 32767;
         for (iy=0; iy<dimy; iy++)
         {
@@ -695 +695 @@
                         alpha=(tmp_long+(1<<14))>>15;                           // vyssi presnost
                                         
-                        //            gamma= qsqrt(((long)alpha*beta));                          // verze pro DSP
+                        //gamma= qsqrt(((long)alpha*beta));                          // verze pro DSP
                         gamma= (int16)(sqrt((double)((int32)alpha*beta)));              // verze pro PC
                                // in q15
@@ -734 +734 @@
                         w_i++;
                 }
-        }
-}
-
+                
+                *detS = ((int32)*detS*alpha+(1<<14))>>8;
+                *rem += ((int32)delta*delta)/alpha;
+        }
+}
+