Index: applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1305)
+++ applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1311)
@@ -571,72 +571,87 @@
 	int32 tmp;
 	
-	// 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[3]/32768.),15);
-	if (tmp>32767) t_sin =32767; else t_sin=tmp;
-	tmp=prevod(cos(Thetaref*x_est[3]/32768.),15);
-	if (tmp>32767) t_cos =32767; else t_cos=tmp;
-	
-	tmp=((int32)cB*x_est[2])>>15; // q15*q13 -> q13
-	//             q15*q13 +          q13*q15      + q15*q13??
-	x_est[0]=((int32)cA*x_est[0]+(int32)tmp*t_sin+(int32)cC*(uf[0]))>>15; // in q13
-	x_est[1]=((int32)cA*x_est[1]-(int32)tmp*t_cos+(int32)cC*(uf[1]))>>15; // q13
-	x_est[2]=x_est[2];
+	x_est[0]=x_est[0];
 	//               q15              + q15*q13
-	x_est[3]=(((int32)x_est[3]<<15)+(int32)cG*x_est[2])>>15;
-	
-	if(x_est[3]>(1<<15)) x_est[3]-=2*(1<<15);
-	if(x_est[3]<-(1<<15)) x_est[3]+=2*(1<<15);
-	
+	x_est[1]=(((int32)x_est[1]<<15)+(int32)cG*x_est[0])>>15;
+	
+//	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,2,2);
+
 	//void EKFfixed::update_psi(void)
-	{	
-		PSI[2]=((int32)cB*t_sin)>>15;
-		tmp=((int32)cH*x_est[2])>>15; // ! cH =cB with different scale!!
-		PSI[3]=((int32)tmp*t_cos)>>15;
-		PSI[6]=-((int32)cB*t_cos)>>15;
-		PSI[7]=((int32)tmp*t_sin)>>15;
-	}
 	{
-		int Ai[16];
-		for (int i=0;i<16; i++) Ai[i]=(int)(PSI[i]);
-		ivec Ad(Ai,16);
+		int Ai[4];
+		for (int i=0;i<4; i++) Ai[i]=(int)(PSIU[i]);
+		ivec Ad(Ai,4);
 		log_level.store(logA,get_from_ivec(Ad));
 	}
 	
-	///////// end of copy ///////////////
-	mmultAU(PSI,Uf,PSIU,4,4);
 	//thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx);
-	thorton_fast(Uf,Df,PSIU,Q,G,Dfold,4);
-	
+	thorton_fast(Uf,Df,PSIU,Q,G,Dfold,2);
+	
+
+// 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]=-((int32)cB*t_cos)>>15;
+	C[3]=((int32)tmp*t_sin)>>15;
+}
+
+
+{
+	int Ui[4]; 	for (int i=0;i<4; i++) Ui[i]=(int)Uf[i];
+	int Ci[4]; 	for (int i=0;i<4; i++) Ci[i]=(int)C[i];
+	
+	imat Ud(Ui,2,2);
+	imat Cd(Ci,2,2);
+	imat CU = Cd*Ud/(1<<15);
+	log_level.store(logU,get_from_ivec(ivec(CU._data(),CU._datasize())));
+	
+	int di[2]; 	for (int i=0;i<2; i++) di[i]=(int)Df[i];
+	ivec dd(di,2);
+	imat U(Ui,2,2);
+	mat U2=to_mat(U)/32768;
+	mat PP=U2*diag(to_vec(dd))*U2.T();
+	vec pp(PP._data(),4);
+	log_level.store(logP,pp);
+}
+{
+	int Gi[4]; 	for (int i=0;i<4; i++) Gi[i]=(int)G[i];
+	ivec Gd(Gi,4);
+	log_level.store(logG,get_from_ivec(Gd));
+}
+
+
+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
+
+	
+	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];
 	{
-		int Ui[16]; 	for (int i=0;i<16; i++) Ui[i]=(int)Uf[i];
-		
-		ivec Ud(Ui,16);
-		log_level.store(logU,get_from_ivec(Ud));
-		
-		int di[16]; 	for (int i=0;i<16; i++) di[i]=(int)Df[i];
-		ivec dd(di,4);
-		imat U(Ui,4,4);
-		mat U2=to_mat(U)/32768;
-		mat PP=U2*diag(to_vec(dd))*U2.T();
-		vec pp(PP._data(),16);
-		log_level.store(logP,pp);
-	}
-	{
-		int Gi[16]; 	for (int i=0;i<16; i++) Gi[i]=(int)G[i];
-		ivec Gd(Gi,16);
-		log_level.store(logG,get_from_ivec(Gd));
-	}
-	
-	
-	int16 difz[2];
-	difz[0]=(Y_mes[0]-x_est[0]); // shift to q15!!
-	difz[1]=(Y_mes[1]-x_est[1]);//<<2;
-	
-	{
-		int Di[4];		 for (int i=0;i<4; i++) Di[i]=(int)Df[i];
-		ivec dd(Di,4);//;dd(0)=Y_mes[0];dd(1)=Y_mes[1]; dd(2)=difz[0]; dd(3)=difz[1];
+		int Di[2];		 for (int i=0;i<2; i++) Di[i]=(int)Df[i];
+		ivec dd(Di,2);//;dd(0)=Y_mes[0];dd(1)=Y_mes[1]; dd(2)=difz[0]; dd(3)=difz[1];
 		log_level.store(logD,get_from_ivec(dd));
+		int Ci[4];		 for (int i=0;i<4; i++) Ci[i]=(int)C[i];
+		ivec CC(Ci,4);//;dd(0)=Y_mes[0];dd(1)=Y_mes[1]; dd(2)=difz[0]; dd(3)=difz[1];
+		log_level.store(logC,get_from_ivec(CC));
 	}
 	
@@ -644,13 +659,11 @@
 	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_fast(difz,x_est,Uf,Df,dR,2,4);
+	bierman_fastC(difz,x_est,Uf,Df,C,dR,2,2);
 	//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)*Iref;
-	(mu)(1)=zprevod(x_est[1],15)*Iref;
-	(mu)(2)=zprevod(x_est[2],15)*Wref;
-	(mu)(3)=zprevod(x_est[3],15)*Thetaref;
+	(mu)(0)=zprevod(x_est[0],15)*Wref;
+	(mu)(1)=zprevod(x_est[1],15)*Thetaref;
 	
 	//x_est[2]=x[2]*32768/Wref;
@@ -667,5 +680,5 @@
 	Uf[0]=0x7FFF;// !       // 0.05
 	Uf[1]=Uf[2]=0;
-	Uf[5]=0x7FFF;//!
+	Uf[3]=0x7FFF;//!
 	
 	Df[0]=1<<14;
@@ -693,5 +706,4 @@
 	PSI[2]=cG;
 	PSI[3]=0x7FFF;
-	
 }
 
