Index: applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1330)
+++ applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1341)
@@ -573,5 +573,5 @@
 	x_est[0]=x_est[0];
 	//               q15              + q15*q13
-	x_est[1]=(((int32)x_est[1]<<15)+(int32)cG*x_est[0])>>15;
+	x_est[1]=(((int32)x_est[1]<<15)+(int32)cG*x_est[0]+(1<<14))>>15;
 	
 //	if(x_est[1]>(1<<15)) x_est[1]-=2*(1<<15);
@@ -636,6 +636,6 @@
 tmp=((int32)cB*x_est[0])>>15; // q15*q13 -> q13
 //             q15*q13 +          q13*q15      + q15*q13??
-y_est[0]=((int32)cA*y_old[0]+(int32)tmp*t_sin+(int32)cC*(uf[0]))>>15; // in q13
-y_est[1]=((int32)cA*y_old[1]-(int32)tmp*t_cos+(int32)cC*(uf[1]))>>15; // q13
+y_est[0]=((int32)cA*y_old[0]+(int32)tmp*t_sin+(int32)cC*(uf[0])+(1<<14))>>15; // in q13
+y_est[1]=((int32)cA*y_old[1]-(int32)tmp*t_cos+(int32)cC*(uf[1])+(1<<14))>>15; // q13
 
 	
@@ -674,6 +674,6 @@
 {
 	// Tuning of matrix Q
-	Q[0]=prevod(0.0005,15);      // 1e-3
-	Q[3]=prevod(0.0001,15);      // 1e-3
+	Q[0]=prevod(0.01,15);      // 1e-3
+	Q[3]=prevod(0.01,15);      // 1e-3
 	
 	Uf[0]=0x7FFF;// !       // 0.05
@@ -685,5 +685,5 @@
 	
 	// Tuning of matrix R
-	R[0]=prevod(0.005,15);         // 0.05
+	R[0]=prevod(0.01,15);         // 0.05
 	R[3]=R[0];
 	
@@ -708,4 +708,140 @@
 }
 
+void EKFfixedUD3::bayes(const itpp::vec& yt, const itpp::vec& ut)
+{
+	ekf3(ut[0],ut[1],yt[0],yt[1]);
+}
+
+
+void EKFfixedUD3::ekf3(double ux, double uy, double isxd, double isyd)
+{
+	// vypocet napeti v systemu (x,y)
+	int16 uf[2];
+	uf[0]=prevod(ux/Uref,15);
+	uf[1]=prevod(uy/Uref,15);
+	
+	int16 Y_mes[2];
+	// zadani mereni
+	Y_mes[0]=prevod(isxd/Iref,15);
+	Y_mes[1]=prevod(isyd/Iref,15);
+	
+	////// vlastni rutina EKF -- /////////////////////////
+	int16 t_sin,t_cos;
+	int32 tmp;
+	
+	int16 Mm=x_est[2];
+	x_est[0]=(((int32)x_est[0]<<15) + (int32)cF*x_est[2] + (1<<14))>>15;
+	//               q15              + q15*q13
+	x_est[1]=(((int32)x_est[1]<<15)+(int32)cG*x_est[0] + (1<<14))>>15;
+	x_est[2]=x_est[2];
+	
+	//	if(x_est[1]>(1<<15)) x_est[1]-=2*(1<<15);
+	//	if(x_est[1]<-(1<<15)) x_est[1]+=2*(1<<15);
+	
+	
+	///////// end of copy ///////////////
+	mmultAU(PSI,Uf,PSIU,3,3);
+	
+	
+	//thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx);
+	thorton_fast(Uf,Df,PSIU,Q,G,Dfold,3);
+	
+	// implementace v PC
+	/*	t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
+	 *	t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);*/
+	tmp=prevod(sin(Thetaref*x_est[1]/32768.),15);
+	if (tmp>32767) t_sin =32767; else t_sin=tmp;
+	tmp=prevod(cos(Thetaref*x_est[1]/32768.),15);
+	if (tmp>32767) t_cos =32767; else t_cos=tmp;
+	
+	{	
+		C[0]=((int32)cB*t_sin)>>15;
+		tmp=((int32)cH*x_est[0])>>15; // ! cH =cB with different scale!!
+		C[1]=((int32)tmp*t_cos)>>15;
+		C[2] = 0;
+		
+		C[3]=-((int32)cB*t_cos)>>15;
+		C[4]=((int32)tmp*t_sin)>>15;
+		C[5]=0;
+	}	
+	
+	tmp=((int32)cB*x_est[0])>>15; // q15*q13 -> q13
+	//             q15*q13 +          q13*q15      + q15*q13??
+	y_est[0]=((int32)cA*y_old[0]+(int32)tmp*t_sin+(int32)cC*(uf[0])+(1<<14))>>15; // in q13
+	y_est[1]=((int32)cA*y_old[1]-(int32)tmp*t_cos+(int32)cC*(uf[1])+(1<<14))>>15; // q13
+	
+	
+	int16 difz[2];
+	difz[0]=(Y_mes[0]-y_est[0]); // shift to q15!!
+	difz[1]=(Y_mes[1]-y_est[1]);//<<2;
+	
+	y_old[0] = Y_mes[0];
+	y_old[1] = Y_mes[1];
+	
+	//bierman(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *R, unsigned int16 dimy, unsigned int16 dimx );
+	int16 dR[2];dR[0]=R[0];dR[1]=R[3];
+	//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];  
+	bierman_fastC(difz,x_est,Uf,Df,C,dR,2,3);
+	//x_est[0] = xb[0]>>2; x_est[1]=xb[1]>>2; x_est[2]=xb[2]>>2; x_est[3]=xb[3];
+	
+	// navrat estimovanych hodnot regulatoru
+	vec& mu = E._mu();
+	(mu)(0)=zprevod(x_est[0],15)*Wref;
+	(mu)(1)=zprevod(x_est[1],15)*Thetaref;
+	(mu)(2)=zprevod(x_est[2],15)*Mref;
+	
+	//x_est[2]=x[2]*32768/Wref;
+	//x_est[3]=x[3]*32768/Thetaref;
+	//	mat T=diag(concat(vec_2(Iref,Iref), vec_2(Wref,Thetaref)));
+}
+
+void EKFfixedUD3::init_ekf3(double Tv)
+{
+	// Tuning of matrix Q
+	Q[0]=prevod(0.001,15);      // 1e-3
+	Q[4]=prevod(0.001,15);      // 1e-3
+	Q[8]=prevod(0.001,15);      // 1e-3
+	
+	Uf[0]=0x7FFF;// !       // 0.05
+	Uf[1]=Uf[2]=Uf[3]=0;
+	Uf[4]=0x7FFF;//!
+	Uf[5]=Uf[6]=Uf[7]=0;
+	Uf[8]=0x7FFF;//!
+	
+	Df[0]=1<<14;
+	Df[1]=1<<14;
+	Df[2]=1<<14;
+	
+	// Tuning of matrix R
+	R[0]=prevod(0.01,15);         // 0.05
+	R[3]=R[0];
+	
+	// Motor model parameters 
+	cA=prevod(1-Tv*Rs/Ls,15);
+	cB=prevod(Tv*Wref*Fmag/Iref/Ls,15);
+	cC=prevod(Tv/Ls/Iref*Uref,15);
+	//  cD=prevod(1-Tv*Bf/J,15);
+	//  cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15);
+	cF=prevod(-p*Tv*Mref/J/Wref,15);
+	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*Thetaref/Iref/Ls,15); // 
+	//  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
+	
+	/* Init matrix PSI with permanently constant terms */
+	PSI[0]=0x7FFF;
+	PSI[1]=0;
+	PSI[2]= cF;
+	
+	PSI[3]=cG;
+	PSI[4]=0x7FFF;
+	PSI[5]=0;
+	
+	PSI[6]=0;
+	PSI[7]=0;
+	PSI[8]=0x7FFF;
+}
+
 
 void EKFfixedCh::bayes(const itpp::vec& yt, const itpp::vec& ut)
