Index: applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1229)
+++ applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1230)
@@ -445,5 +445,5 @@
 	mmultAU(PSI,Uf,PSIU,4,4);
 	//thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx);
-	thorton(Uf,Df,PSIU,Q,G,Dfold,4);
+	thorton_fast(Uf,Df,PSIU,Q,G,Dfold,4);
 	
 	 {
@@ -469,5 +469,5 @@
 	int dR[2];dR[0]=R[0];dR[1]=R[3];
 	//int xb[4]; xb[0]=x_est[0]<<2; xb[1]=x_est[1]<<2;  xb[2]=x_est[2]<<2;  xb[3]=x_est[3];  
-	bierman(difz,x_est,Uf,Df,dR,2,4);
+	bierman_fast(difz,x_est,Uf,Df,dR,2,4);
 	//x_est[0] = xb[0]>>2; x_est[1]=xb[1]>>2; x_est[2]=xb[2]>>2; x_est[3]=xb[3];
 	
@@ -487,7 +487,7 @@
 {
 	// Tuning of matrix Q
-	Q[0]=prevod(.0005,15);       // 0.05
+	Q[0]=prevod(.001,15);       // 0.05
 	Q[5]=Q[0];
-	Q[10]=prevod(0.0001,15);      // 1e-3
+	Q[10]=prevod(0.001,15);      // 1e-3
 	Q[15]=prevod(0.0001,15);      // 1e-3
 
@@ -585,4 +585,6 @@
 	//thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx);
 	householder(PSICh,Q,4);
+	// COPY 
+	for (int ii=0; ii<16; ii++){Chf[ii]=PSICh[ii];}
 	
 	{
@@ -600,4 +602,5 @@
 	int dR[2];dR[0]=R[0];dR[1]=R[3];
 	//int xb[4]; xb[0]=x_est[0]<<2; xb[1]=x_est[1]<<2;  xb[2]=x_est[2]<<2;  xb[3]=x_est[3];  
+
 	carlson(difz,x_est,Chf,dR,2,4);
 	//x_est[0] = xb[0]>>2; x_est[1]=xb[1]>>2; x_est[2]=xb[2]>>2; x_est[3]=xb[3];
@@ -618,16 +621,16 @@
 {
 	// Tuning of matrix Q
-	Q[0]=prevod(.01,15);       // 0.05
+	Q[0]=prevod(.001,15);       // 0.05
 	Q[5]=Q[0];
-	Q[10]=prevod(0.0001,15);      // 1e-3
+	Q[10]=prevod(0.001,15);      // 1e-3
 	Q[15]=prevod(0.0001,15);      // 1e-3
 	
-	Chf[0]=0x7FFF;// !       // 0.05
+	Chf[0]=0x3FFF;// !       // 0.05
 	Chf[1]=Chf[2]=Chf[3]=Chf[4]=0;
-	Chf[5]=0x7FFF;//!
+	Chf[5]=0x3FFF;//!
 	Chf[6]=Chf[6]=Chf[8]=Chf[9]=0;
-	Chf[10]=0x7FFF;//!      // 1e-3
+	Chf[10]=0x3FFF;//!      // 1e-3
 	Chf[11]=Chf[12]=Chf[13]=Chf[4]=0;
-	Chf[15]=0x7FFF;      // 1e-3
+	Chf[15]=0x3FFF;      // 1e-3
 		
 	// Tuning of matrix R
Index: applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp (revision 1228)
+++ applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp (revision 1230)
@@ -136,9 +136,9 @@
         for (j=0;j<rows; j++) {
             //long s1=(((long)PSIU[i+j*rows]*PSIU[i+j*rows])>>15)*(Dold[i]);
-            long s2=((((long)PSIU[i*rows+j]*PSIU[i*rows+j]))>>(2*qAU-15))*Dold[j];
+            long s2=((((long)PSIU[i*rows+j]*Dold[j]))>>(2*qAU-15))*PSIU[i*rows+j];
 //			printf("%d - %d\n",s1,s2);
             sigma += s2;
         }
-        sigma += Q[i*rows+i]<<15;
+        sigma += Q[i*rows+i]<<15; // Q is in q15
         for (j=i+1;j<rows; j++) {
             sigma += (((long)G[i*rows+j]*G[i*rows+j])>>13)*Q[j*rows+j];
@@ -146,5 +146,5 @@
         }
 
-//        if (sigma>16384<<15) sigma = 16384<<15;
+        if (sigma>16384<<15) sigma = 16384<<15;
         *(D+i)=sigma>>15;
         if (D[i]==0) D[i]=1;
@@ -155,5 +155,5 @@
             sigma =0;
             for (k=0;k<rows;k++) {
-				sigma += (((long(PSIU[i*rows+k])*PSIU[j*rows+k]))>>(2*qAU-15))*Dold[k];
+				sigma += (((long(PSIU[i*rows+k])*Dold[k]))>>(2*qAU-15))*PSIU[j*rows+k];
             }
             for (k=0;k<rows;k++) {
@@ -266,5 +266,5 @@
         for (k=0, PSIU_ik=PSIU+irows,Dold_k=Dold;
                 k<rows; k++, PSIU_ik++,Dold_k++) {//Dold_i=
-            sigma += (((long)(*PSIU_ik)**PSIU_ik)>>15)*(*Dold_k);
+                sigma += (((long)(*PSIU_ik)**Dold_k)>>(2*qAU-15))*(*PSIU_ik);
         }
         sigma += *(Q+i+irows)<<15;
@@ -273,5 +273,6 @@
 
         }
-
+        
+        if (sigma>16384<<15) sigma = 16384<<15;
         *D_i=sigma>>15;
         if (*D_i==0) *D_i=1;
@@ -285,5 +286,5 @@
                     k<rows; k++, PSIU_ik++, PSIU_jk++, Dold_k++) {
 
-                sigma += ((((long)*PSIU_ik)**PSIU_jk)>>15)**Dold_k;
+				sigma += ((((long)*PSIU_ik)**Dold_k)>>(2*qAU-15))**PSIU_jk;
             }
 
@@ -364,4 +365,7 @@
 /* square root of 0<a<1 using taylor at 0.5 in q15*/
 int int_sqrt(int x) {
+	double xd(double(x)/32768.);
+	return round(sqrt(xd)*32768);
+	
     //sqrt(x) == 1/2*2^(1/2)+1/2*2^(1/2)*(x-1/2)-1/4*2^(1/2)*(x-1/2)^2
     //         = k1 + k1*(x-0.5) - k2*(x-0.5)(x-0.5);
@@ -404,4 +408,5 @@
         /*		 double sigf=double(sigma)/(1<<15);
         		 double alpf = sqrt(sigf);*/
+		if (sigma>16384) sigma=16384;
         alpha=int_sqrt(sigma);
         // alpha = alpf*(1<<15);
@@ -421,4 +426,5 @@
         }
         alpha=sigma>>1;
+		if (alpha==0) alpha =1;
         for (i=0;i<=k;i++) {
             sigma=0;
@@ -474,2 +480,40 @@
     }
 }
+
+/* perform Householder update of Ch matrix using PSI*Ch , Q, */
+extern void givens(int *Ch /*= int *PSICh*/, int *Q, unsigned int dimx){
+	int i,j,k;
+	int rho,s,c,tau;
+	
+	int A[25];//beware
+	// copy Q to A
+	for (i=0;i<dimx*dimx;i++) {
+		A[i]=Q[i];
+	}
+	
+	
+	for (i=dimx-1; i>=0; i--){
+		for (j=0; j<dimx; j++) {
+			rho=int_sqrt(((long)Ch[i*dimx+i]*Ch[i*dimx+i]+long(A[i*dimx+j])*A[i*dimx+j])>>15);
+			if (rho==0) break;
+			s=(long(A[i*dimx+j])<<15)/rho;
+			c=(long(Ch[i*dimx+i])<<15)/rho;
+			for (k=0;k<=i; k++){
+				tau=(long(c)*A[k*dimx+j]-long(s)*Ch[k*dimx+i])>>15;
+				Ch[k*dimx +i]=(long(s)*A[k*dimx+j]+long(c)*Ch[k*dimx+i])>>15;
+				A[k*dimx +j]=tau;
+			}
+		}
+	}
+	for (j=0; j<i; j++){
+		rho=int_sqrt((long(Ch[i*dimx+i])*Ch[i*dimx+i]+long(Ch[i*dimx+j])*Ch[i*dimx+j])>>15);
+		if (rho==0) break; 
+		s=(long(Ch[i*dimx+j])<<15)/rho;
+		c=(long(Ch[i*dimx+i])<<15)/rho;
+		for (k=0; k<=i; k++){
+			tau=(long(c)*Ch[k*dimx+j]-long(s)*Ch[k*dimx+i])>>15;
+			Ch[k*dimx+i]=(long(s)*Ch[k*dimx+j]+long(c)*Ch[k*dimx+i])>>15;
+			Ch[k*dimx+j]=tau;
+		}
+	}
+}
Index: applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h (revision 1228)
+++ applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h (revision 1230)
@@ -34,4 +34,7 @@
 extern void householder(int *Ch /*= int *PSICh*/, int *Q, unsigned int dimx);
 
+/* perform Givens update of Ch matrix using PSI*Ch , Q, */
+extern void givens(int *Ch /*= int *PSICh*/, int *Q, unsigned int dimx);
+
 /* perform Carlson update of Ch matrix using difz, R and xp, for size dimx*/
 extern void carlson(int *difz, int *xp, int *Ch, int *R, unsigned int dimy, unsigned int dimx );
