Index: applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1201)
+++ applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1226)
@@ -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 ) );*/
 	}
 };
@@ -174,5 +174,5 @@
   cG=prevod(Tv*Wref*4/Thetaref,15);
 //  cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
-  cH=prevod(Tv*Wref*Fmag/Iref/Ls,15);
+  cH=prevod(Tv*Wref*Fmag/Iref/Ls,17); //cB in q
   //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
 
@@ -401,12 +401,12 @@
 	// vypocet napeti v systemu (x,y)
 	int uf[2];
-	uf[0]=prevod(ux/Uref,Qm);
-	uf[1]=prevod(uy/Uref,Qm);
+	uf[0]=prevod(ux/Uref,15);
+	uf[1]=prevod(uy/Uref,15);
 	
 	int Y_mes[2];
 	// zadani mereni
-	Y_mes[0]=prevod(isxd/Iref,Qm);
-	Y_mes[1]=prevod(isyd/Iref,Qm);
-	
+	Y_mes[0]=prevod(isxd/Iref,15);
+	Y_mes[1]=prevod(isyd/Iref,15);
+		
 	////// vlastni rutina EKF -- /////////////////////////
 	int t_sin,t_cos, tmp;
@@ -418,8 +418,10 @@
 	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]<<2))>>15;
-	x_est[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*(uf[1]<<2))>>15;
+	tmp=((long)cB*x_est[2])>>15; // q15*q13 -> q13
+	//             q15*q13 +          q13*q15      + q15*q13??
+	x_est[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*(uf[0]))>>15; // in q13
+	x_est[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*(uf[1]))>>15; // q13
 	x_est[2]=x_est[2];
+	//               q15              + q15*q13
 	x_est[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15;
 	
@@ -428,7 +430,7 @@
 	
 	//void EKFfixed::update_psi(void)
-	{		
+	{	
 		PSI[2]=((long)cB*t_sin)>>15;
-		tmp=((long)cB*x_est[2])>>15;
+		tmp=((long)cH*x_est[2])>>15; // ! cH =cB with different scale!!
 		PSI[3]=((long)tmp*t_cos)>>15;
 		PSI[6]=-((long)cB*t_cos)>>15;
@@ -456,15 +458,17 @@
 	
 	int difz[2];
-	difz[0]=(Y_mes[0]<<2)-x_est[0]; // Y_mes in q13!!
-	difz[1]=(Y_mes[1]<<2)-x_est[1];
+	difz[0]=(Y_mes[0]-x_est[0]); // shift to q15!!
+	difz[1]=(Y_mes[1]-x_est[1]);//<<2;
 
 	{
-		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);
+		ivec dd(Df,4);//;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));
 	}
 	
 	//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];
+	//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);
+	//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
@@ -475,4 +479,6 @@
 	(mu)(3)=zprevod(x_est[3],15)*Thetaref;
 	
+	//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)));
 }
@@ -486,16 +492,16 @@
 	Q[15]=prevod(0.0001,15);      // 1e-3
 
-	Uf[0]=0x7FFF;       // 0.05
+	Uf[0]=0x7FFF;// !       // 0.05
 	Uf[1]=Uf[2]=Uf[3]=Uf[4]=0;
-	Uf[5]=0x7FFF;
+	Uf[5]=0x7FFF;//!
 	Uf[6]=Uf[6]=Uf[8]=Uf[9]=0;
-	Uf[10]=0x7FFF;      // 1e-3
+	Uf[10]=0x7FFF;//!      // 1e-3
 	Uf[11]=Uf[12]=Uf[13]=Uf[4]=0;
 	Uf[15]=0x7FFF;      // 1e-3
 	
-	Df[0]=0x7FFF;
-	Df[1]=0x7FFF;
-	Df[2]=0x7FFF;
-	Df[3]=0x7FFF;
+	Df[0]=1<<14;
+	Df[1]=1<<14;
+	Df[2]=1<<14;
+	Df[3]=1<<14;
 	
 	// Tuning of matrix R
@@ -510,7 +516,7 @@
 	//  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); //no *4!!
+	cG=prevod(Tv*Wref/Thetaref,15); //in q15!
 	//cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
-	// cH=prevod(Tv*Wref*Fmag/Iref/Ls,15); <<< use cB instead
+	cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); // 
 	//  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
 	
@@ -522,18 +528,130 @@
 	PSI[15]=0x7FFF;
 	
-	//////////////////////// =================
-	///// TEST thorton vs. thorton_fast
-	
-/*	int Ut[16]={0x7FFF, 100, 200, 300, 0, 0x7FFF, 500, 600, 0,0,0x7FFF, 800, 0,0,0, 0x7FFF};
-	int Dt[4]={100,200,300,400};
-	int PSIu[16] = {100, 200,300, 400 , 500, 600,700,800, 900,1000,1100,1200, 1300,1400,1500,1600};
-	int Dold[4];
-	
-	thorton(Ut,Dt,PSIu,Q,G,Dold,4);
-	int Ut2[16]={0x7FFF, 100, 200, 300, 0, 0x7FFF, 500, 600, 0,0,0x7FFF, 800, 0,0,0, 0x7FFF};
-	int Dt2[4]={100,200,300,400};
-	int PSIu2[16] = {100, 200,300, 400 , 500, 600,700,800, 900,1000,1100,1200, 1300,1400,1500,1600};
-	thorton_fast(Ut2,Dt2,PSIu2,Q,G,Dold,4);
-	cout<< Q<<endl;*/
-}
-
+}
+
+void EKFfixedCh::bayes(const itpp::vec& yt, const itpp::vec& ut)
+{
+	ekf(ut[0],ut[1],yt[0],yt[1]);
+}
+
+
+void EKFfixedCh::ekf(double ux, double uy, double isxd, double isyd)
+{
+	// vypocet napeti v systemu (x,y)
+	int uf[2];
+	uf[0]=prevod(ux/Uref,15);
+	uf[1]=prevod(uy/Uref,15);
+	
+	int Y_mes[2];
+	// zadani mereni
+	Y_mes[0]=prevod(isxd/Iref,15);
+	Y_mes[1]=prevod(isyd/Iref,15);
+	
+	////// vlastni rutina EKF -- /////////////////////////
+	int t_sin,t_cos, tmp;
+	
+	// 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; // q15*q13 -> q13
+	//             q15*q13 +          q13*q15      + q15*q13??
+	x_est[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*(uf[0]))>>15; // in q13
+	x_est[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*(uf[1]))>>15; // q13
+	x_est[2]=x_est[2];
+	//               q15              + q15*q13
+	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; // ! cH =cB with different scale!!
+		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));
+	}
+	
+	///////// end of copy ///////////////
+	mmultACh(PSI,Chf,PSICh,4,4);
+	//thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx);
+	householder(PSICh,Q,4);
+	
+	{
+		ivec Chd(Chf,16);
+		log_level.store(logCh,get_from_ivec(Chd));
+	}
+	
+	
+	int difz[2];
+	difz[0]=(Y_mes[0]-x_est[0]); // shift to q15!!
+	difz[1]=(Y_mes[1]-x_est[1]);//<<2;
+	
+	
+	//
+	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];
+	
+	// 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;
+	
+	//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 EKFfixedCh::init_ekf(double Tv)
+{
+	// Tuning of matrix Q
+	Q[0]=prevod(.01,15);       // 0.05
+	Q[5]=Q[0];
+	Q[10]=prevod(0.0001,15);      // 1e-3
+	Q[15]=prevod(0.0001,15);      // 1e-3
+	
+	Chf[0]=0x7FFF;// !       // 0.05
+	Chf[1]=Chf[2]=Chf[3]=Chf[4]=0;
+	Chf[5]=0x7FFF;//!
+	Chf[6]=Chf[6]=Chf[8]=Chf[9]=0;
+	Chf[10]=0x7FFF;//!      // 1e-3
+	Chf[11]=Chf[12]=Chf[13]=Chf[4]=0;
+	Chf[15]=0x7FFF;      // 1e-3
+		
+	// Tuning of matrix R
+	R[0]=prevod(0.05,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); // 
+	//  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
+	
+	/* Init matrix PSI with permanently constant terms */
+	PSI[0]=cA;
+	PSI[5]=PSI[0];
+	PSI[10]=0x7FFF;
+	PSI[14]=cG;
+	PSI[15]=0x7FFF;
+	
+}
+
Index: applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (revision 1201)
+++ applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (revision 1226)
@@ -19,5 +19,5 @@
 #include "matrix.h"
 #include "matrix_vs.h"
-#include "reference.h"
+#include "reference_Q15.h"
 #include "parametry_motoru.h"
 
@@ -109,5 +109,5 @@
 class EKFfixedUD : public BM {
 	public:
-		LOG_LEVEL(EKFfixedUD,logU, logG, logD, logA);
+		LOG_LEVEL(EKFfixedUD,logU, logG, logD, logA, logP);
 		
 		void init_ekf(double Tv);
@@ -164,4 +164,5 @@
 				L.add_vector ( log_level, logD, RV ("D", 4 ), prefix );
 				L.add_vector ( log_level, logA, RV ("A", 16 ), prefix );
+				L.add_vector ( log_level, logP, RV ("P", 16 ), prefix );
 				
 		};
@@ -170,4 +171,67 @@
 
 UIREGISTER(EKFfixedUD);
+
+/*!
+ * \brief Extended Kalman Filter with Chol matrices in fixed point arithmetic
+ * 
+ * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
+ */
+class EKFfixedCh : public BM {
+public:
+	LOG_LEVEL(EKFfixedCh,logCh, logA, logP);
+	
+	void init_ekf(double Tv);
+	void ekf(double ux, double uy, double isxd, double isyd);
+	
+	/* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
+	int Q[16]; /* matrix [4,4] */
+	int R[4]; /* matrix [2,2] */
+	
+	int x_est[4]; /* estimate and prediction */
+	
+	int PSI[16]; /* matrix [4,4] */
+	int PSICh[16]; /* matrix PIS*U, [4,4] */
+	
+	int Chf[16]; // upper triangular of covariance (inplace)
+	
+	int cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
+	
+	enorm<chmat> E;
+	mat Ry;
+	
+public:
+	//! Default constructor
+	EKFfixedCh ():BM(),E(),Ry(2,2){
+		int i;
+		for(i=0;i<16;i++){Q[i]=0;}
+		for(i=0;i<4;i++){R[i]=0;}
+		
+		for(i=0;i<4;i++){x_est[i]=0;}
+		for(i=0;i<16;i++){Chf[i]=0;}
+		
+		for(i=0;i<16;i++){PSI[i]=0;}
+		
+		set_dim(4);
+		E._mu()=zeros(4);
+		E._R()=zeros(4,4);
+		init_ekf(0.000125);
+	};
+	//! Here dt = [yt;ut] of appropriate dimensions
+	void bayes ( const vec &yt, const vec &ut );
+	//!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, logCh, RV ("Ch", 16 ), prefix );
+		L.add_vector ( log_level, logA, RV ("A", 16 ), prefix );
+		L.add_vector ( log_level, logP, RV ("P", 16 ), prefix );
+		
+	};
+	//void from_setting();
+};
+
+UIREGISTER(EKFfixedCh);
+
 
 //! EKF for comparison of EKF_UD with its fixed-point implementation
