Index: applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1311)
+++ applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1321)
@@ -686,5 +686,5 @@
 	
 	// Tuning of matrix R
-	R[0]=prevod(0.05,15);         // 0.05
+	R[0]=prevod(0.5,15);         // 0.05
 	R[3]=R[0];
 	
@@ -699,5 +699,5 @@
 	//cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
 	// cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); // < ===========
-	cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); // 
+	cH=prevod(Tv*Wref*Fmag/Iref/Ls,15); // 
 	//  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
 	
@@ -833,6 +833,6 @@
 	cG=prevod(Tv*Wref/Thetaref,15); //in q15!
 	//cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
-	//	cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); //  <=======
-	cH=prevod(Tv*Wref*Fmag/Iref/Ls,15); // 
+	cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); //  <=======
+	//cH=prevod(Tv*Wref*Fmag/Iref/Ls,15); // 
 	//  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
 	
Index: applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (revision 1311)
+++ applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (revision 1321)
@@ -310,4 +310,6 @@
 		
 		set_dim(4);
+		dimc = 2;
+		dimy =2;
 		E._mu()=zeros(4);
 		E._R()=zeros(4,4);
Index: applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp (revision 1313)
+++ applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp (revision 1321)
@@ -19,4 +19,5 @@
 #include "matrix.h"
 
+#include <stdio.h>
 /* Matrix multiply Full matrix by upper diagonal matrix; */
 void mmultAU(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns) {
@@ -204,13 +205,6 @@
 				//                *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;
+				 printf("%d, %d, %d\n", ((int32)(*a_j)*(*b_i))/lambda, *b_i, beta);
 			}
 		}
@@ -218,5 +212,4 @@
 		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
-			
 		}
 	}
@@ -700,2 +693,73 @@
 }
 
+void carlson_fastC(int16 *difz, int16 *xp, int16 *Ch, int16 *C, int16 *R, unsigned int16 dimy, unsigned int16 dimx ) {
+	int16 alpha,beta,gamma;
+	int16 delta, eta,epsilon,zeta,sigma,tau;
+	int16 i,j,iy;
+	int16 w[5];
+	int32 tmp_long;
+	
+	int16 *Ch_ij, *w_i, *x_i, *C_yi;
+	
+	
+	for (iy=0; iy<dimy; iy++)
+	{
+		alpha=R[iy];
+		delta = difz[iy];
+		
+		for (j=0;j<dimx;j++)
+		{
+			C_yi = C+iy*dimx; 
+			sigma = 0;
+			Ch_ij=Ch+j;
+			for (i=0;i<=j;i++){
+				sigma += ((int32)*Ch_ij**C_yi)>>15; //sigma in qCh  
+				Ch_ij+=dimx;
+				C_yi++;
+			}
+			
+			//sigma=Ch[iy*dimx+j];
+			beta=alpha;
+			//            alpha+=((long)sigma*sigma)>>15;
+			alpha=(((int32)alpha<<15)+(int32)sigma*sigma)>>15;				// vyssi presnost
+			//            gamma= qsqrt(((long)alpha*beta));                          // verze pro DSP
+			gamma= (int16)(sqrt((double)((int32)alpha*beta)));              // verze pro PC
+			
+			w[j]=0;
+			
+			Ch_ij=Ch+j;
+			w_i=w;
+			
+			for (i=0;i<=j;i++)
+			{
+				//                tau=Ch[i*dimx+j];
+				tau=*Ch_ij;
+				//				tmp_long=((long)beta*Ch[i*dimx+j] -(long)sigma*w[i])/gamma;
+				tmp_long=((int32)beta**Ch_ij -(int32)sigma**w_i)/gamma;
+				
+				if (tmp_long>32767)
+					tmp_long=32767;
+				if (tmp_long<-32768)
+					tmp_long=-32768;
+				*Ch_ij=tmp_long;
+				
+				//                w_i+=((long)tau*sigma)>>15;
+				*w_i=(((int32)*w_i<<15)+(int32)tau*sigma)>>15;
+				
+				w_i++;
+				Ch_ij+=dimx;
+			}
+		}
+		
+		x_i=xp;
+		w_i=w;
+		for (i=0;i<dimx;i++) {
+			//            xp[i]+=((long)w[i]*delta)/alpha;
+			//            *x_i+=((long)*w_i*delta)/alpha;
+			*x_i=((int32)*x_i*alpha+(int32)*w_i*delta)/alpha;		// vyssi presnost
+			x_i++;
+			w_i++;
+		}
+	}
+}
+
Index: applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h (revision 1313)
+++ applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h (revision 1321)
@@ -16,6 +16,6 @@
 #define qCh 14
 
-#define int16 short
-#define int32 int
+#define int16 int //short
+#define int32 long //int
 
 /* Matrix multiply Full matrix by upper diagonal matrix with unit diagonal; */
@@ -57,2 +57,5 @@
 /* perform Carlson update of Ch matrix using difz, R and xp, for size dimx*/
 extern void carlson_fast(int16 *difz, int16 *xp, int16 *Ch, int16 *R, unsigned int16 dimy, unsigned int16 dimx );
+
+/* perform Carlson update of Ch matrix using difz, R and xp, for size dimx*/
+extern void carlson_fastC(int16 *difz, int16 *xp, int16 *Ch, int16 *C, int16 *R, unsigned int16 dimy, unsigned int16 dimx );
