Index: /applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp
===================================================================
--- /applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1183)
+++ /applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1201)
@@ -34,5 +34,5 @@
 	
 	if ( BM::evalll ) {
-		//from enorm
+/*		//from enorm
 		vec xdif(x,4);//first 4 of x
 		//UGLY HACK!!! reliance on a predictor!!
@@ -42,5 +42,5 @@
 		xdif(3)=x[3]-zprevod(x_pred[3],15);*/
 		
-		xdif -=xhat; //(xdif=x-xhat)
+//		xdif -=xhat; //(xdif=x-xhat)
 		
 		mat Pfull(4,4);
@@ -51,5 +51,5 @@
 		
 		
-		BM::ll = -0.5* ( 4 * 1.83787706640935 +log ( det ( Pfull ) ) +xdif* ( inv(Pfull)*xdif ) );
+	//	BM::ll = -0.5* ( 4 * 1.83787706640935 +log ( det ( Pfull ) ) +xdif* ( inv(Pfull)*xdif ) );*/
 	}
 };
@@ -77,7 +77,10 @@
 
   // implementace v PC
-  t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
-  t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);
-
+  //t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
+  //t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);
+
+  t_sin=prevod(sin(Thetaref*x_est[3]/Qm),15);
+  t_cos=prevod(cos(Thetaref*x_est[3]/Qm),15);
+  
   tmp=((long)cB*x_est[2])>>15;
   x_pred[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*ux[0])>>15;
@@ -153,11 +156,11 @@
 {
   // Tuning of matrix Q
-  Q[0]=prevod(.01,15);       // 0.05
+  Q[0]=prevod(.05,15);       // 0.05
   Q[5]=Q[0];
-  Q[10]=prevod(0.0001,15);      // 1e-3
-  Q[15]=prevod(0.0001,15);      // 1e-3
+  Q[10]=prevod(0.0002,15);      // 1e-3
+  Q[15]=prevod(0.001,15);      // 1e-3
 
   // Tuning of matrix R
-  R[0]=prevod(0.05,15);         // 0.05
+  R[0]=prevod(0.1,15);         // 0.05
   R[3]=R[0];
 
@@ -170,6 +173,7 @@
 //  cF=prevod(p*Tv*Mref/J/Wref,15);
   cG=prevod(Tv*Wref*4/Thetaref,15);
-  cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
-//  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
+//  cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
+  cH=prevod(Tv*Wref*Fmag/Iref/Ls,15);
+  //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
 
   /* Init matrix PSI with permanently constant terms */
@@ -179,4 +183,9 @@
   PSI[14]=cG;
   PSI[15]=0x7FFF;
+  
+  P_est[0]=0x7FFF;
+  P_est[5]=0x7FFF;
+  P_est[10]=0x7FFF;
+  P_est[15]=0x7FFF;
 }
 
@@ -232,18 +241,4 @@
 	D =  est._R()._D();
 	
-	bool DBG=true;
-	if (DBG){
-		mat L;
-		ivec tmp;
-		mat X=randn(5,5);
-		mat XX=X*X.T();
-		mat T=diag(sqrt(1.0/diag(XX)));
-		XX= T*XX*T;
-		
-		ldmat ldd(XX);
-		U = ldd._L().T();
-		D = ldd._D()*0.9;
-	}
-		
 	////////////
 	
@@ -254,30 +249,11 @@
 
 	//////
-	vec xref(5);
+/*	vec xref(4);
 	xref(0)= 30.0*1.4142;
 	xref(1)= 30.0*1.4142;
 	xref(2)= 6.283185*200.;
-	xref(3) = 3.141593;
-	xref(4) = 34.0;
-	
-	if (DBG){
-		xref = ones(5);
-	}
-	
-	imat Utf;
-	ivec Dtf;
-	UDtof(U,D,Utf,Dtf,xref);
-	mat invxref=diag(1.0/xref);
-	imat Af=round_i(invxref*A*diag(xref)*(1<<15));
-	mat_to_int(Af, PSI);
-	mat_to_int(Utf,Uf);		
-	vec_to_int(Dtf, Df); 
-
-// 	cout << Dtf << endl;
-	
-	// A*U == PSI*U
-	mmultAU(PSI,Uf,PSIU,5,5);	
-/*	cout << ivec(PSIU,25) << endl;
-	cout << (Af*Utf)/(1<<15) <<endl;*/
+	xref(3) = 3.141593;*/
+	//xref(4) = 34.0;
+	
 	
 	vec Din = D;
@@ -336,30 +312,6 @@
 		
 		}
-		UDtof(U,D,Utf,Dtf,xref);
-		//	cout << G << endl << Din << endl;
-		imat Urw=Utf.T();
-		cout << ivec(Urw._data(),25) << endl << Dtf << endl;
-	}
-
-	int Qf[25];
-	imat Qrnd=round_i(diag(elem_div(diag(Q),pow(xref,2)))*(1<<15));
-// 	cout << "Qr" << Qrnd<<endl;
-	mat_to_int(Qrnd,Qf);
-
-// 	cout << Df <<endl;
-	thorton(Uf,Df,PSIU,Qf,Gf,Dfold,5);
-	
-	if (DBG){
-		_mu = vec("0.500 0.200 0.300 0.400 0.1");
-	}
-	
-	cout << "bierman double I" <<endl;
-	UDtof(U,D,Utf,Dtf,xref);
-	cout << "Uf: " << Utf << endl;
-	cout << "Df: " << Dtf << endl;
-	cout << "xf:" << round_i(elem_div(_mu,xref)*(1<<15)) <<endl;
-	mat_to_int(Utf,Uf);		
-	vec_to_int(Dtf, Df); 
-	
+	}
+
 	// bierman
 	
@@ -370,7 +322,4 @@
 	vec xp=_mu; // used in bierman
 	
-	if (DBG){
-		yp = vec("0.40 0.300");
-	}
 	
 	for (int iy=0; iy<dimy; iy++){
@@ -402,35 +351,4 @@
 	}
 
-UDtof(U,D,Utf,Dtf,xref);
-cout << "Uf: " << Utf << endl;
-cout << "Df: " << Dtf << endl;
-cout << "xf:" << round_i(elem_div(_mu,xref)*(1<<15)) <<endl;
-
-	int difz[2];
-	vec_to_int(round_i((yt-yp)/xref(0)*(1<<15)), difz);
-	int xf[5];
-	vec_to_int(round_i(elem_div(xp,xref)*(1<<15)), xf);
-	int Rf[2];//={1,1};
-	vec_to_int(round_i(R/pow(xref(0),2)*(1<<15)), Rf);
-	//beirman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx 
-	
-	cout <<endl<< "bierman fixed" <<endl;
-	cout << "Uf: "; for (i=0; i<25;i++) cout << Uf[i] << ","; cout <<endl;
-	cout << "Df: "; for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl;
-	cout << "xf: "; for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;
-
-	cout << "dz: "; for (i=0; i<2;i++) cout << difz[i] << ","; cout << endl;
-
-	int xf_old[5];
-	vec_to_int(ivec(xf,5),xf_old);
-	bierman(difz,xf, Uf, Df, Rf, 2, 5);
-	
-	UDtof(U,D,Utf,Dtf,xref);
-	cout << "Uf: "; for (i=0; i<25;i++) cout << Uf[i] << ","; cout <<endl;
-	cout << "Df: "; for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl;
-	cout << "xf: "; for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;
-	cout << endl;
-	
-	
 	
 	/////
@@ -495,20 +413,29 @@
 	
 	// implementace v PC
+/*	t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
+	t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);*/
 	t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
 	t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);
 	
 	tmp=((long)cB*x_est[2])>>15;
-	x_est[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*uf[0])>>15;
-	x_est[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*uf[1])>>15;
+	x_est[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*(uf[0]<<2))>>15;
+	x_est[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*(uf[1]<<2))>>15;
 	x_est[2]=x_est[2];
 	x_est[3]=(((long)x_est[3]<<15)+(long)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);
 	
 	//void EKFfixed::update_psi(void)
 	{		
 		PSI[2]=((long)cB*t_sin)>>15;
-		tmp=((long)cH*x_est[2])>>15;
+		tmp=((long)cB*x_est[2])>>15;
 		PSI[3]=((long)tmp*t_cos)>>15;
 		PSI[6]=-((long)cB*t_cos)>>15;
 		PSI[7]=((long)tmp*t_sin)>>15;
+	}
+	{
+		ivec Ad(PSI,16);
+		log_level.store(logA,get_from_ivec(Ad));
 	}
 	
@@ -518,7 +445,23 @@
 	thorton(Uf,Df,PSIU,Q,G,Dfold,4);
 	
+	 {
+		ivec Ud(Uf,16);
+		log_level.store(logU,get_from_ivec(Ud));
+	}
+	 {
+		ivec Gd(G,16);
+		log_level.store(logG,get_from_ivec(Gd));
+	}
+	
+	
 	int difz[2];
-	difz[0]=Y_mes[0]-x_est[0];
-	difz[1]=Y_mes[1]-x_est[1];
+	difz[0]=(Y_mes[0]<<2)-x_est[0]; // Y_mes in q13!!
+	difz[1]=(Y_mes[1]<<2)-x_est[1];
+
+	{
+		vec dd(4);dd(0)=Y_mes[0];dd(1)=Y_mes[1]; dd(2)=difz[0]; dd(3)=difz[1];
+		log_level.store(logD,dd);
+	}
+	
 	//bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx );
 	int dR[2];dR[0]=R[0];dR[1]=R[3];
@@ -527,7 +470,7 @@
 	// navrat estimovanych hodnot regulatoru
 	vec& mu = E._mu();
-	(mu)(0)=zprevod(x_est[0],Qm)*Iref;
-	(mu)(1)=zprevod(x_est[1],Qm)*Iref;
-	(mu)(2)=zprevod(x_est[2],Qm)*Wref;
+	(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;
 	
@@ -544,6 +487,9 @@
 
 	Uf[0]=0x7FFF;       // 0.05
+	Uf[1]=Uf[2]=Uf[3]=Uf[4]=0;
 	Uf[5]=0x7FFF;
+	Uf[6]=Uf[6]=Uf[8]=Uf[9]=0;
 	Uf[10]=0x7FFF;      // 1e-3
+	Uf[11]=Uf[12]=Uf[13]=Uf[4]=0;
 	Uf[15]=0x7FFF;      // 1e-3
 	
@@ -564,6 +510,7 @@
 	//  cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15);
 	//  cF=prevod(p*Tv*Mref/J/Wref,15);
-	cG=prevod(Tv*Wref*4/Thetaref,15);
-	cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
+	cG=prevod(Tv*Wref/Thetaref,15); //no *4!!
+	//cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
+	// cH=prevod(Tv*Wref*Fmag/Iref/Ls,15); <<< use cB instead
 	//  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 1179)
+++ /applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (revision 1201)
@@ -28,5 +28,6 @@
 void mat_to_int(const imat &M, int *I);
 void vec_to_int(const ivec &v, int *I);
-
+void UDtof(const mat &U, const vec &D, imat &Uf, ivec &Df, const vec &xref);
+	
 /*!
 \brief Extended Kalman Filter with full matrices in fixed point arithmetic
@@ -108,4 +109,6 @@
 class EKFfixedUD : public BM {
 	public:
+		LOG_LEVEL(EKFfixedUD,logU, logG, logD, logA);
+		
 		void init_ekf(double Tv);
 		void ekf(double ux, double uy, double isxd, double isyd);
@@ -154,5 +157,14 @@
 		//!dummy!
 		const epdf& posterior() const {return E;};
-		
+		void log_register(logger &L, const string &prefix){
+			BM::log_register ( L, prefix );
+			
+				L.add_vector ( log_level, logG, RV("G",16), prefix );
+				L.add_vector ( log_level, logU, RV ("U", 16 ), prefix );
+				L.add_vector ( log_level, logD, RV ("D", 4 ), prefix );
+				L.add_vector ( log_level, logA, RV ("A", 16 ), prefix );
+				
+		};
+		//void from_setting();
 };
 
@@ -174,9 +186,5 @@
 		//! D part
 		vec D;
-		
-		int Uf[25];
-		int Df[5];
-		int Dfold[5];
-		
+				
 		mat A;
 		mat C;
@@ -184,8 +192,4 @@
 		vec R;
 		
-		int PSI[25];
-		int PSIU[25];
-		int Gf[25];
-
 		enorm<ldmat> est;
 		
