Index: applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1245)
+++ applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1294)
@@ -505,8 +505,8 @@
 {
 	// 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.0005,15);      // 1e-3
-	Q[15]=prevod(0.001,15);      // 1e-3
+	Q[15]=prevod(0.0001,15);      // 1e-3
 
 	Uf[0]=0x7FFF;// !       // 0.05
@@ -603,6 +603,6 @@
 	mmultACh(PSI,Chf,PSICh,4,4);
 	//thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx);
-	householder(PSICh,Q,4);
-	//givens(PSICh,Q,4);
+	//householder(PSICh,Q,4);
+	givens_fast(PSICh,Q,4);
 	// COPY 
 	for (int16 ii=0; ii<16; ii++){Chf[ii]=PSICh[ii];}
@@ -629,5 +629,5 @@
 	//int16 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);
+	carlson_fast(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];
 	
@@ -649,6 +649,6 @@
 	Q[0]=prevod(.01,15);       // 0.05
 	Q[5]=Q[0];
-	Q[10]=prevod(0.01,15);      // 1e-3
-	Q[15]=prevod(0.01,15);      // 1e-3
+	Q[10]=prevod(0.0005,15);      // 1e-3
+	Q[15]=prevod(0.001,15);      // 1e-3
 	
 	Chf[0]=0x3FFF;// !       // 0.05
@@ -661,5 +661,5 @@
 		
 	// Tuning of matrix R
-	R[0]=prevod(0.1,15);         // 0.05
+	R[0]=prevod(0.05,15);         // 0.05
 	R[3]=R[0];
 	
Index: applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (revision 1264)
+++ applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (revision 1294)
@@ -106,10 +106,12 @@
 
 //! EKF for testing q44
-class EKFtest: public EKFfull{
+class EKFtest: public EKF_UD{
 	void bayes ( const vec &yt, const vec &cond ) {
-		EKFfull::bayes(yt,cond);
-		mat &P = est._R();
-		if (P(3,3)>3.14) 
-			P(3,3)=3.14;		
+		EKF_UD::bayes(yt,cond);
+		vec D =  prior()._R()._D();
+		
+		if (D(3)>10) D(3) = 10;
+		
+		prior()._R().__D()=D;
 	}
 };
Index: applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp (revision 1245)
+++ applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp (revision 1294)
@@ -221,10 +221,10 @@
     if (x>6554) {
         int16 xm05=x-16384;
-        tmp = ((long)k1*xm05)>>15;
-        tmp-=(((long(k2)*xm05)>>15)*xm05)>>15;
+        tmp = ((int32)k1*xm05)>>15;
+        tmp-=(((int32(k2)*xm05)>>15)*xm05)>>15;
         tmp +=k1;
     } else {
         tmp = 4*x;
-        tmp-=long(8*x)*x>>15;
+        tmp-=int32(8*x)*x>>15;
     }
     return tmp;
@@ -251,18 +251,18 @@
         for (j=0;j<dimx;j++)
         {
-            sigma+=((long)B[k*dimx+j]*B[k*dimx+j]);
+            sigma+=((int32)B[k*dimx+j]*B[k*dimx+j]);
         }
         for (j=0;j<=k;j++)
         {
-            sigma+=((long)Ch[k*dimx+j]*Ch[k*dimx+j]);
+            sigma+=((int32)Ch[k*dimx+j]*Ch[k*dimx+j]);
         }
         
         //alpha in qCh
-        alpha = (int)(sqrt((double)sigma)+0.5);   // predelat pro DSP
+        alpha = (int16)(sqrt((double)sigma)+0.5);   // predelat pro DSP
 
 		sigma=0;
         for (j=0;j<dimx;j++) {
             w[j]=B[k*dimx+j];
-            sigma+=(long)w[j]*w[j];
+            sigma+=(int32)w[j]*w[j];
         }
         for (j=0; j<=k;j++) {
@@ -272,5 +272,5 @@
                 v[j]=Ch[k*dimx+j];
             }
-            sigma+=(long)v[j]*v[j];
+            sigma+=(int32)v[j]*v[j];
         }
         
@@ -281,8 +281,8 @@
             sigma=0;
             for (j=0;j<dimx;j++) {
-                sigma+=((long)B[i*dimx+j]*w[j]);
+                sigma+=((int32)B[i*dimx+j]*w[j]);
             }
             for (j=0;j<=k;j++) {
-                sigma+=(long)Ch[i*dimx+j]*v[j];
+                sigma+=(int32)Ch[i*dimx+j]*v[j];
             }
 
@@ -338,8 +338,8 @@
             sigma=Ch[iy*dimx+j];
             beta=alpha;
-            alpha+=((long)sigma*sigma)>>15;
+            alpha+=((int32)sigma*sigma)>>15;
 //			double ab=(double)alpha*beta/32768./32768.;
 //			double s_ab=sqrt(ab);
-            gamma=(int)(sqrt((double)((long)alpha*beta))+0.5);            // predelat v DSP
+            gamma=(int16)(sqrt((double)((int32)alpha*beta))+0.5);            // predelat v DSP
             //gamma = round(s_ab*(1<<15));
 //            eta=((long)beta<<15) / gamma;
@@ -348,5 +348,5 @@
             for (i=0;i<=j;i++) {
                 tau=Ch[i*dimx+j];
-				tmp_long=((long)beta*Ch[i*dimx+j] -(long)sigma*w[i])/gamma;
+				tmp_long=((int32)beta*Ch[i*dimx+j] -(int32)sigma*w[i])/gamma;
 				if (tmp_long>32767) {
 					Ch[i*dimx+j]=32767;
@@ -359,5 +359,5 @@
 				}
 				
-                w[i]+=((long)tau*sigma)>>15;
+                w[i]+=((int32)tau*sigma)>>15;
             }
         }
@@ -365,5 +365,5 @@
         //epsilon=(long(difz)<<15) / (alpha); // q15*q13/q13 = q15
         for (i=0;i<dimx;i++) {
-            xp[i]+=((long)w[i]*delta)/alpha;
+            xp[i]+=((int32)w[i]*delta)/alpha;
         }
     }
@@ -385,12 +385,12 @@
 	for (i=dimx-1; i>=0; i--){
 		for (j=0; j<dimx; j++) {
-			tmp_long=(long)Ch[i*dimx+i]*Ch[i*dimx+i]+long(A[i*dimx+j])*A[i*dimx+j];
+			tmp_long=(int32)Ch[i*dimx+i]*Ch[i*dimx+i]+int32(A[i*dimx+j])*A[i*dimx+j];
 			if (tmp_long>0){
 				rho=sqrt(double(tmp_long));
-				s=(long(A[i*dimx+j])<<15)/rho;
-				c=(long(Ch[i*dimx+i])<<15)/rho;
+				s=(int32(A[i*dimx+j])<<15)/rho;
+				c=(int32(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;
+					tau=(int32(c)*A[k*dimx+j]-int32(s)*Ch[k*dimx+i])>>15;
+					Ch[k*dimx +i]=(int32(s)*A[k*dimx+j]+int32(c)*Ch[k*dimx+i])>>15;
 					A[k*dimx +j]=tau;
 				}
@@ -399,12 +399,12 @@
 
 		for (j=0; j<i; j++){
-			tmp_long=(long(Ch[i*dimx+i])*Ch[i*dimx+i]+long(Ch[i*dimx+j])*Ch[i*dimx+j]);
+			tmp_long=(int32(Ch[i*dimx+i])*Ch[i*dimx+i]+int32(Ch[i*dimx+j])*Ch[i*dimx+j]);
 			if (tmp_long>0){
 				rho=sqrt((double)(tmp_long));
-				s=(long(Ch[i*dimx+j])<<15)/rho;
-				c=(long(Ch[i*dimx+i])<<15)/rho;
+				s=(int32(Ch[i*dimx+j])<<15)/rho;
+				c=(int32(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;
+					tau=(int32(c)*Ch[k*dimx+j]-int32(s)*Ch[k*dimx+i])>>15;
+					Ch[k*dimx+i]=(int32(s)*Ch[k*dimx+j]+int32(c)*Ch[k*dimx+i])>>15;
 					Ch[k*dimx+j]=tau;
 				}
@@ -443,2 +443,150 @@
 	}
 }
+
+
+void givens_fast(int16 *Ch, int16 *Q, unsigned int16 dimx)
+{
+	int16 i,j,k;
+	int16 rho,s,c,tau;
+	int32  tmp_long;
+	
+	int16 A[25];//beware
+	
+	int16 *A_ij, *Q_i, *Ch_ki, *Ch_kj, *Ch_ii, *Ch_ij, *A_kj;
+	
+	A_ij=A;
+	Q_i=Q;
+	// copy Q to A
+	for (i=0;i<dimx*dimx;i++)
+	{
+		//		A[i]=Q[i]>>(15-qCh);
+		*A_ij++=(*Q_i++)>>(15-qCh);
+	}
+	
+	for (i=dimx-1; i>=0; i--)
+	{
+		Ch_ii=Ch+i*dimx+i;
+		A_ij=A+i*dimx;
+		
+		for (j=0; j<dimx; j++) 
+		{
+			//			tmp_long=(long)Ch[i*dimx+i]*Ch[i*dimx+i]+(long)A[i*dimx+j]*A[i*dimx+j];
+			
+			tmp_long=(int32)*Ch_ii**Ch_ii+(int32)*A_ij**A_ij;
+			
+			if (tmp_long>0)
+			{
+				//				rho=qsqrt(tmp_long);                   // verze pro DSP
+				rho=(int16)(sqrt((double)tmp_long));     // verze pro PC
+				s=((int32)*A_ij<<15)/rho;
+				c=((int32)*Ch_ii<<15)/rho;
+				
+				Ch_ki=Ch+i;
+				A_kj=A+j;
+				
+				for (k=0;k<=i; k++)
+				{
+					tau=((int32)c**A_kj-(int32)s**Ch_ki)>>15;
+					*Ch_ki=((int32)s**A_kj+(int32)c**Ch_ki)>>15;
+					*A_kj=tau;
+					
+					Ch_ki+=dimx;
+					A_kj+=dimx;
+				}
+			}
+			A_ij++;
+		}
+		
+		Ch_ij = Ch+i*dimx;
+		
+		for (j=0; j<i; j++)
+		{
+			tmp_long=(int32)*Ch_ii**Ch_ii+(int32)*Ch_ij**Ch_ij;
+			
+			if (tmp_long>0)
+			{
+				//			rho=qsqrt(tmp_long);                     // verze pro DSP
+				rho=(int16)(sqrt((double)tmp_long));       // verze pro PC
+				s=((int32)*Ch_ij<<15)/rho;
+				c=((int32)*Ch_ii<<15)/rho;
+				
+				Ch_kj = Ch + j;
+				Ch_ki = Ch + i;
+				
+				for (k=0; k<=i; k++)
+				{
+					tau=((int32)c**Ch_kj-(int32)s**Ch_ki)>>15;
+					*Ch_ki =((int32)s**Ch_kj+(int32)c**Ch_ki)>>15;
+					*Ch_kj=tau;
+					
+					Ch_kj += dimx;
+					Ch_ki += dimx;
+				}
+			}
+			Ch_ij++;
+		}
+	}
+}
+
+void carlson_fast(int16 *difz, int16 *xp, int16 *Ch, 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;
+	
+	
+	for (iy=0; iy<dimy; iy++)
+	{
+		alpha=R[iy];
+		delta = difz[iy];
+		
+		for (j=0;j<dimx;j++)
+		{
+			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 1245)
+++ applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h (revision 1294)
@@ -15,6 +15,6 @@
 #define qCh 14
 
-#define int16 int
-#define int32 long
+#define int16 short
+#define int32 int
 
 /* Matrix multiply Full matrix by upper diagonal matrix with unit diagonal; */
@@ -44,2 +44,8 @@
 /* perform Carlson update of Ch matrix using difz, R and xp, for size dimx*/
 extern void carlson(int16 *difz, int16 *xp, int16 *Ch, int16 *R, unsigned int16 dimy, unsigned int16 dimx );
+
+/* perform Givens update of Ch matrix using PSI*Ch , Q, */
+extern void givens_fast(int16 *Ch /*= int16 *PSICh*/, int16 *Q, unsigned int16 dimx);
+
+/* 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 );
