Index: applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp (revision 1304)
+++ applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp (revision 1313)
@@ -17,4 +17,5 @@
 #include "matrix_vs.h"
 #include <math.h>
+#include "matrix.h"
 
 /* Matrix multiply Full matrix by upper diagonal matrix; */
@@ -47,4 +48,35 @@
         m1pom+=(columns);
     }
+};
+
+//same as mmultAU but different precision
+void mmultCU(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns) {
+	unsigned int16 i, j, k;
+	int32 tmp_sum=0L; //in 15+qAU
+	int16 *m2pom;
+	int16 *m1pom=m1;
+	int16 *respom=result;
+	
+	for (i=0; i<rows; i++) //rows of result
+    {
+		for (j=0; j<columns; j++) //columns of result
+	{
+		m2pom=up+j;//??
+		
+		for (k=0; k<j; k++) //inner loop up to "j" - U(j,j)==1;
+            {
+				tmp_sum+=((int32)(*(m1pom++))**m2pom)>>(15-qCU);
+				m2pom+=columns;
+			}
+			// add the missing A(i,j)
+			tmp_sum +=(int32)(*m1pom)<<qCU; // no need to shift
+			m1pom-=(j); // shift back to first element
+			
+			*respom++=tmp_sum>>15;
+		
+		tmp_sum=0;
+	}
+	m1pom+=(columns);
+	}
 };
 
@@ -121,4 +153,74 @@
 }
 
+void bierman_fastC(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *C, int16 *R, unsigned int16 dimy, unsigned int16 dimx )
+{
+	int16 alpha;
+	int16 beta,lambda;
+	int16 b[5]; // ok even for 4-dim state
+	int16 *a; // in [0,1] -> qCU
+	unsigned int16 iy,j,i;
+	
+	int16 *b_j,*b_i;
+	int16 *a_j;
+	int16 *D_j;
+	int16 *U_ij;
+	int16 *x_i;
+	
+//	int32 z_pom;
+//	int16 z_pom_int16;
+	
+	int16 UC[16]; // in q15
+	
+	/* copy U for vector a */
+	mmultCU(C,U,UC,dimy,dimx);
+	
+	a = UC; // iyth row of U
+	for (iy=0; iy<dimy; iy++, a+=dimx) {
+		// a is a row
+		for (j=0,a_j=a,b_j=b,D_j=D; j<dimx; j++,b_j++,D_j++,a_j++)
+			*b_j=((int32)(*D_j)*(*a_j))>>15;
+		
+		alpha = R[iy]; //\alpha = R+vDv = R+a*b
+		// R in q15, a in q15, b=q15
+		//		gamma = (1<<15)/alpha; //(in q15)
+		//min alpha = R[iy] = 164
+		//max gamma = 0.0061 => gamma_ref = q7
+		for (j=0,a_j=a,b_j=b,D_j=D; j<dimx; j++,a_j++,b_j++,D_j++) {
+			/*            beta=alpha;
+			 *            lambda = -((int32)(*a_j)<<15)/beta;
+			 *            alpha  += ((int32)(*a_j)*(*b_j))>>15;
+			 *            D[j] = ((int32)beta**D_j)/alpha;*/
+			/*xx*/
+			lambda=alpha;
+			alpha  += ((int32)(*a_j)*(*b_j))>>15;
+			D[j] = ((int32)lambda**D_j)/alpha;
+//			z_pom_int16 = -((int32)(*a_j)<<15)/lambda;
+			/*xx*/
+			
+			if (*D_j==0) *D_j=1;
+			
+			for (i=0,b_i=b,U_ij=U+j; i<j; i++, b_i++,U_ij+=dimx) {
+				beta   = *U_ij;
+				//                *U_ij +=  ((int32)lambda*(*b_i))>>15;		// puvodni reseni
+				*U_ij -=  ((int32)(*a_j)*(*b_i))/lambda;         // pozadovane optimalni reseni
+				//                *U_ij -=  ((int32)((int16)((int32)(*a_j)<<15)/lambda)**b_i)>>15;	// tohle funguje - problem je s tim pretypovanim na (int16)
+				//				*U_ij -= (int16)((int32)(*a_j)*(*b_i))/lambda;
+				//				z_pom = (((int32)(*a_j)*(*b_i))/lambda);
+				/*  			    z_pom = (int32)(*U_ij)-(int16)((int32)(*a_j)*(*b_i))/lambda;
+				 *				if (z_pom > 32767) z_pom = 32767;
+				 *				if (z_pom < - 32768) z_pom = -32768;
+				 *U_ij = z_pom;                      /**/
+				 //                *U_ij +=  ((int32)z_pom_int16*(*b_i))>>15;		// puvodni reseni - jen jina konstanta
+				 *b_i  +=  ((int32)beta*(*b_j))>>15;
+			}
+		}
+		// no shift due to gamma
+		for (i=0,x_i=xp,b_i=b; i<dimx; i++,x_i++,b_i++) {
+			*x_i  += ((int32)difz[iy]*(*b_i))/alpha; // multiply by unscaled Kalman gain
+			
+		}
+	}
+}
+
 // Thorton procedure - Kalman predictive variance in UD
 void thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 rows) {
@@ -166,5 +268,5 @@
         sigma += (int32)(*(Q+i+irows))<<qAU;
         for (j=i+1, G_ik=G+irows+i+1; j<rows; j++,G_ik++) {
-            sigma += (((int32)(*G_ik)**G_ik)>>16)**(Q+j+j*rows);
+            sigma += (((int32)(*G_ik)**G_ik)>>(30-qAU))**(Q+j+j*rows);
         }
 
@@ -189,5 +291,5 @@
             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 += ((((int32)*G_ik)**G_jk)>>16)**Q_kk;
+                sigma += ((((int32)*G_ik)**G_jk)>>(30-qAU))**Q_kk;
             }
 
