Index: /applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp
===================================================================
--- /applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1236)
+++ /applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1240)
@@ -7,17 +7,18 @@
 double minQ(double Q){if (Q>1.0){ return 1.0;} else {return Q;};};
 
-void mat_to_int(const imat &M, int *I){
-	for (int i=0;i<M.rows(); i++){
-		for (int j=0;j<M.cols(); j++){
+void mat_to_int16(const imat &M, int16 *I){
+	for (int16 i=0;i<M.rows(); i++){
+		for (int16 j=0;j<M.cols(); j++){
 			*I++ = M(i,j);
 		}
 	}
 }
-void vec_to_int(const ivec &v, int *I){
-	for (int i=0;i<v.length(); i++){
+void vec_to_int16(const ivec &v, int16 *I){
+	for (int16 i=0;i<v.length(); i++){
 			*I++ = v(i);
 	}
 }
 
+#ifdef XXX
 ///////////////
 void EKFfixed::bayes(const vec &yt, const vec &ut){
@@ -46,5 +47,5 @@
 		mat Pfull(4,4);
 		double* Pp=Pfull._data();
-		for(int i=0;i<16;i++){*(Pp++) = zprevod(P_est[i],15);}
+		for(int16 i=0;i<16;i++){*(Pp++) = zprevod(P_est[i],15);}
 		
 		E._R()._M()=Pfull;
@@ -58,5 +59,5 @@
 void EKFfixed::update_psi(void)
 {
-  int t_sin,t_cos,tmp;
+  int16 t_sin,t_cos,tmp;
 
   // implementace v PC
@@ -64,15 +65,15 @@
   t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);
 
-  PSI[2]=((long)cB*t_sin)>>15;
-  tmp=((long)cH*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;
-}
-
-
-void EKFfixed::prediction(int *ux)
-{
-  int t_sin,t_cos, tmp;
+  PSI[2]=((int32)cB*t_sin)>>15;
+  tmp=((int32)cH*x_est[2])>>15;
+  PSI[3]=((int32)tmp*t_cos)>>15;
+  PSI[6]=-((int32)cB*t_cos)>>15;
+  PSI[7]=((int32)tmp*t_sin)>>15;
+}
+
+
+void EKFfixed::prediction(int16 *ux)
+{
+  int16 t_sin,t_cos, tmp;
 
   // implementace v PC
@@ -83,9 +84,9 @@
   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;
-  x_pred[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*ux[1])>>15;
+  tmp=((int32)cB*x_est[2])>>15;
+  x_pred[0]=((int32)cA*x_est[0]+(int32)tmp*t_sin+(int32)cC*ux[0])>>15;
+  x_pred[1]=((int32)cA*x_est[1]-(int32)tmp*t_cos+(int32)cC*ux[1])>>15;
   x_pred[2]=x_est[2];
-  x_pred[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15;
+  x_pred[3]=(((int32)x_est[3]<<15)+(int32)cG*x_est[2])>>15;
 
   update_psi();
@@ -99,6 +100,6 @@
 void EKFfixed::correction(void)
 {
-  int Y_error[2];
-  long temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */
+  int16 Y_error[2];
+  int32 temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */
 
   choice_P(P_pred,temp15a,3);
@@ -189,5 +190,5 @@
   P_est[15]=0x7FFF;
 }
-
+#endif
 
 void EKF_UDfix::set_parameters ( const shared_ptr<diffbifn> &pfxu0, const shared_ptr<diffbifn> &phxu0, const mat Q0, const vec R0 ) {
@@ -228,5 +229,5 @@
 	Df=round_i(Pld._D()*(1<<15));
 	ivec zer=find(Df==0);
-	for(int i=0; i<zer.length(); i++) Df(zer(i))=1;
+	for(int16 i=0; i<zer.length(); i++) Df(zer(i))=1;
 }
 
@@ -236,5 +237,5 @@
 	vec &_mu=est._mu();
 	const vec &u=cond;
-	int dim = dimension();
+	int16 dim = dimension();
 	///// !!!!!!!!!!!!!!!!
 	U = est._R()._L().T();
@@ -258,5 +259,5 @@
 	
 	vec Din = D;
-	int i,j,k;
+	int16 i,j,k;
 	double sigma;
 	mat G = eye(dim);
@@ -323,5 +324,5 @@
 	
 	
-	for (int iy=0; iy<dimy; iy++){
+	for (int16 iy=0; iy<dimy; iy++){
 		a     = U.T()*C.get_row(iy);    // a is not modified, but 
 		b     = elem_mult(D,a);    			 // b is modified to become unscaled Kalman gain. 
@@ -366,5 +367,5 @@
 	
 	//statistics
-	int dim = IM->dimension();
+	int16 dim = IM->dimension();
 	vec mu0;
 	if ( !UI::get ( mu0, set, "mu0" ) )
@@ -400,9 +401,9 @@
 {
 	// vypocet napeti v systemu (x,y)
-	int uf[2];
+	int16 uf[2];
 	uf[0]=prevod(ux/Uref,15);
 	uf[1]=prevod(uy/Uref,15);
 	
-	int Y_mes[2];
+	int16 Y_mes[2];
 	// zadani mereni
 	Y_mes[0]=prevod(isxd/Iref,15);
@@ -410,19 +411,22 @@
 		
 	////// vlastni rutina EKF -- /////////////////////////
-	int t_sin,t_cos, tmp;
+	int16 t_sin,t_cos;
+	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);*/
-	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
+	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]=((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[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];
 	//               q15              + q15*q13
-	x_est[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15;
+	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);
@@ -431,12 +435,14 @@
 	//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;
+		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;
 	}
 	{
-		ivec Ad(PSI,16);
+		int Ai[16];
+		for (int i=0;i<16; i++) Ai[i]=(int)(PSI[i]);
+		ivec Ad(Ai,16);
 		log_level.store(logA,get_from_ivec(Ad));
 	}
@@ -444,12 +450,16 @@
 	///////// end of copy ///////////////
 	mmultAU(PSI,Uf,PSIU,4,4);
-	//thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx);
+	//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);
 	
 	 {
-		ivec Ud(Uf,16);
+		 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));
-		ivec dd(Df,4);
-		imat U(Uf,4,4);
+		
+		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();
@@ -458,21 +468,23 @@
 	 }
 	 {
-		ivec Gd(G,16);
+		 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));
 	}
 	
 	
-	int difz[2];
+	int16 difz[2];
 	difz[0]=(Y_mes[0]-x_est[0]); // shift to q15!!
 	difz[1]=(Y_mes[1]-x_est[1]);//<<2;
 
 	{
-		ivec dd(Df,4);//;dd(0)=Y_mes[0];dd(1)=Y_mes[1]; dd(2)=difz[0]; dd(3)=difz[1];
+		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];
 		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(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_fast(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];
@@ -495,6 +507,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
 
 	Uf[0]=0x7FFF;// !       // 0.05
@@ -524,4 +536,5 @@
 	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*Thetaref*Fmag/Iref/Ls,15); // 
 	//  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
@@ -545,9 +558,9 @@
 {
 	// vypocet napeti v systemu (x,y)
-	int uf[2];
+	int16 uf[2];
 	uf[0]=prevod(ux/Uref,15);
 	uf[1]=prevod(uy/Uref,15);
 	
-	int Y_mes[2];
+	int16 Y_mes[2];
 	// zadani mereni
 	Y_mes[0]=prevod(isxd/Iref,15);
@@ -555,5 +568,5 @@
 	
 	////// vlastni rutina EKF -- /////////////////////////
-	int t_sin,t_cos, tmp;
+	int16 t_sin,t_cos, tmp;
 	
 	// implementace v PC
@@ -563,11 +576,11 @@
 	t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);
 	
-	tmp=((long)cB*x_est[2])>>15; // q15*q13 -> q13
+	tmp=((int32)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[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];
 	//               q15              + q15*q13
-	x_est[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15;
+	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);
@@ -576,29 +589,29 @@
 	//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;
+		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;
 	}
 	{
-		ivec Ad(PSI,16);
-		log_level.store(logA,get_from_ivec(Ad));
+/*		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);
+	//thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx);
 	householder(PSICh,Q,4);
 	// COPY 
-	for (int ii=0; ii<16; ii++){Chf[ii]=PSICh[ii];}
+	for (int16 ii=0; ii<16; ii++){Chf[ii]=PSICh[ii];}
 	
 	{
-		ivec Chd(Chf,16);
-		log_level.store(logCh,get_from_ivec(Chd));
-	}
-	
-	
-	int difz[2];
+/*		ivec Chd(Chf,16);
+		log_level.store(logCh,get_from_ivec(Chd));*/
+	}
+	
+	
+	int16 difz[2];
 	difz[0]=(Y_mes[0]-x_est[0]); // shift to q15!!
 	difz[1]=(Y_mes[1]-x_est[1]);//<<2;
@@ -606,6 +619,6 @@
 	
 	//
-	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];  
+	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];  
 
 	carlson(difz,x_est,Chf,dR,2,4);
@@ -653,5 +666,6 @@
 	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*Thetaref*Fmag/Iref/Ls,15); //  <=======
+	cH=prevod(Tv*Wref*Fmag/Iref/Ls,15); // 
 	//  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 1226)
+++ /applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (revision 1240)
@@ -5,5 +5,5 @@
 
   -----------------------------------
-  BDM++ - C++ library for Bayesian Decision Making under Uncertainty
+  BDM++ - C++ library for Bayesian Decision Making under Uncertaint16y
 
   Using IT++ for numerical operations
@@ -26,10 +26,11 @@
 double minQ(double Q);
 
-void mat_to_int(const imat &M, int *I);
-void vec_to_int(const ivec &v, int *I);
+void mat_to_int16(const imat &M, int16 *I);
+void vec_to_int16(const ivec &v, int16 *I);
 void UDtof(const mat &U, const vec &D, imat &Uf, ivec &Df, const vec &xref);
-	
+
+#ifdef XXX
 /*!
-\brief Extended Kalman Filter with full matrices in fixed point arithmetic
+\brief Extended Kalman Filter with full matrices in fixed point16 arithmetic
 
 An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
@@ -41,27 +42,27 @@
 
 /* Declaration of local functions */
-void prediction(int *ux);
+void prediction(int16 *ux);
 void correction(void);
 void update_psi(void);
 
 /* 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];
- int x_pred[4];
- int P_pred[16]; /* matrix [4,4] */
- int P_est[16]; /* matrix [4,4] */
- int Y_mes[2];
- int ukalm[2];
- int Kalm[8]; /* matrix [5,2] */
-
- int PSI[16]; /* matrix [4,4] */
-
- int temp15a[16];
-
- int cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
-
- long temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */
+ int16 Q[16]; /* matrix [4,4] */
+ int16 R[4]; /* matrix [2,2] */
+
+ int16 x_est[4];
+ int16 x_pred[4];
+ int16 P_pred[16]; /* matrix [4,4] */
+ int16 P_est[16]; /* matrix [4,4] */
+ int16 Y_mes[2];
+ int16 ukalm[2];
+ int16 Kalm[8]; /* matrix [5,2] */
+
+ int16 PSI[16]; /* matrix [4,4] */
+
+ int16 temp15a[16];
+
+ int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
+
+ int32 temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */
  enorm<fsqmat> E;
  mat Ry;
@@ -70,5 +71,5 @@
 	//! Default constructor
 	EKFfixed ():BM(),E(),Ry(2,2){
-		int i;
+		int16 i;
 		for(i=0;i<16;i++){Q[i]=0;}
 		for(i=0;i<4;i++){R[i]=0;}
@@ -102,6 +103,8 @@
 UIREGISTER(EKFfixed);
 
+#endif
+
 /*!
-\brief Extended Kalman Filter with UD matrices in fixed point arithmetic
+\brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic
 
 An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
@@ -115,18 +118,18 @@
 				
 		/* 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 PSIU[16]; /* matrix PIS*U, [4,4] */
-		
-		int Uf[16]; // upper triangular of covariance (inplace)
-		int Df[4];  // diagonal covariance
-		int Dfold[4]; // temp of D
-		int G[16];  // temp for bierman
-		
-		int cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
+		int16 Q[16]; /* matrix [4,4] */
+		int16 R[4]; /* matrix [2,2] */
+		
+		int16 x_est[4]; /* estimate and prediction */
+		
+		int16 PSI[16]; /* matrix [4,4] */
+		int16 PSIU[16]; /* matrix PIS*U, [4,4] */
+		
+		int16 Uf[16]; // upper triangular of covariance (inplace)
+		int16 Df[4];  // diagonal covariance
+		int16 Dfold[4]; // temp of D
+		int16 G[16];  // temp for bierman
+		
+		int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
 		
 		enorm<fsqmat> E;
@@ -136,5 +139,5 @@
 		//! Default constructor
 		EKFfixedUD ():BM(),E(),Ry(2,2){
-			int i;
+			int16 i;
 			for(i=0;i<16;i++){Q[i]=0;}
 			for(i=0;i<4;i++){R[i]=0;}
@@ -173,5 +176,5 @@
 
 /*!
- * \brief Extended Kalman Filter with Chol matrices in fixed point arithmetic
+ * \brief Extended Kalman Filter with Chol matrices in fixed point16 arithmetic
  * 
  * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
@@ -185,15 +188,15 @@
 	
 	/* 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
+	int16 Q[16]; /* matrix [4,4] */
+	int16 R[4]; /* matrix [2,2] */
+	
+	int16 x_est[4]; /* estimate and prediction */
+	
+	int16 PSI[16]; /* matrix [4,4] */
+	int16 PSICh[16]; /* matrix PIS*U, [4,4] */
+	
+	int16 Chf[16]; // upper triangular of covariance (inplace)
+	
+	int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
 	
 	enorm<chmat> E;
@@ -203,5 +206,5 @@
 	//! Default constructor
 	EKFfixedCh ():BM(),E(),Ry(2,2){
-		int i;
+		int16 i;
 		for(i=0;i<16;i++){Q[i]=0;}
 		for(i=0;i<4;i++){R[i]=0;}
@@ -235,5 +238,5 @@
 
 
-//! EKF for comparison of EKF_UD with its fixed-point implementation
+//! EKF for comparison of EKF_UD with its fixed-point16 implementation
 class EKF_UDfix : public BM {
 	protected:
Index: /applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp
===================================================================
--- /applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp (revision 1239)
+++ /applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp (revision 1240)
@@ -3,50 +3,41 @@
         Matrix operations
 
-        V. Smidl
-
-Rev. 30.8.2010
-
-30.8.2010      Prvni verze
+        V. Smidl, Z. Peroutka
+
+Rev. 28.10.2010   (ZP)
+
+26.10.2010      Prvni verze (VS)
+
+26.10.2010      Upravena chyba v Thorton_fast - spatne shiftovani o vypoctu SIGMA.
+27.10.2010      Pokus o odstraneni problemu v Thorton_fast - potize dela omezovani (orezavani) varianci.
+28.10.2010      Drobne upravy v kodu.
 
 *************************************/
-#include "fixed.h"
-#include "stdio.h"
-#include <math.h>
 
 #include "matrix_vs.h"
 
 /* Matrix multiply Full matrix by upper diagonal matrix; */
-void mmultAU(int *m1, int *up, int *result, unsigned int rows, unsigned int columns) {
-    unsigned int i, j, k;
-    long tmp_sum=0L; //in 15+qAU
-    int *m2pom;
-    int *m1pom=m1;
-    int *respom=result;
+void mmultAU(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns) {
+    unsigned int16 i, j, k;
+    int32 tmp_sum=0L; //in 15+qAU
+    int16 *m2pom;
+    int16 *m1pom=m1;
+    int16 *respom=result;
 
     for (i=0; i<rows; i++) //rows of result
     {
         for (j=0; j<columns; j++) //columns of result
-        {
+	{
             m2pom=up+j;//??
 
             for (k=0; k<j; k++) //inner loop up to "j" - U(j,j)==1;
             {
-                tmp_sum+=((long)(*(m1pom++))**m2pom)>>(15-qAU);
+                tmp_sum+=((int32)(*(m1pom++))**m2pom)>>(15-qAU);
                 m2pom+=columns;
             }
             // add the missing A(i,j)
-            tmp_sum +=(long)(*m1pom)<<qAU; // no need to shift
+            tmp_sum +=(int32)(*m1pom)<<qAU; // no need to shift
             m1pom-=(j); // shift back to first element
 
-            // saturation effect
-/*            tmp_sum=tmp_sum>>15;
-            if (tmp_sum>32767) {
-				printf("Au - saturated\n");
-            }
-            if (tmp_sum<-32768) {
-				printf("Au - saturated\n");
-            }*/
-// 			printf("Au - saturated\n");
-
             *respom++=tmp_sum>>15;
 
@@ -57,144 +48,21 @@
 };
 
-/* Matrix multiply Full matrix by upper diagonal matrix; */
-void mmultACh(int *m1, int *up, int *result, unsigned int rows, unsigned int columns) {
-    unsigned int i, j, k;
-    long tmp_sum=0L;
-    int *m2pom;
-    int *m1pom=m1;
-    int *respom=result;
-
-    for (i=0; i<rows; i++) //rows of result
-    {
-        for (j=0; j<columns; j++) //columns of result
-        {
-            m2pom=up+j;//??
-
-            for (k=0; k<=j; k++) //inner loop up to "j" - U(j,j)==1;
-            {
-                tmp_sum+=(long)(*(m1pom++))**m2pom;
-                m2pom+=columns;
-            }
-            m1pom-=(j+1); // shift back to first element
-
-            // saturation effect
-            tmp_sum=tmp_sum>>15;
-            if (tmp_sum>32767) {
-                if (i!=3) tmp_sum=32767;
-            }
-            if (tmp_sum<-32768) {
-                tmp_sum=-32768;
-            }
-            // 			printf("Au - saturated\n");
-
-            *respom++=tmp_sum;
-
-            tmp_sum=0;
-        }
-        m1pom+=(columns);
-    }
-};
-
-bool DBG=true;
-
-void show(const char name[10], int *I, int n) {
-    if (!DBG) return;
-
-    printf("%s: ",name);
-    for (int i=0;i<n;i++) {
-        printf("%d ",*(I+i));
-    }
-    printf("\n");
-}
-
-// Thorton procedure - Kalman predictive variance in UD
-void thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int rows) {
-    unsigned int i,j,k;
-    // copy D to Dold
-    int *Dold_pom=Dold;
-    for (i=0;i<rows;i++) {
-        *Dold_pom++=*D++;
-    }
-    D-=rows; // back to first D
-
-    // initialize G = eye()
-    int *G_pom = G;
-    *G_pom++=1<<14;
-    for (i=0;i<rows-1;i++) {
-        // clean elem before diag
-        for (j=0; j<rows; j++) {
-            *G_pom++=0.0;
-        }
-        *G_pom++=1<<14;
-    }
-    // eye created
-
-    long sigma; // in q30!!!!!!
-    for (i=rows-1; true;i--) { // check i==0 at the END!
-        sigma = 0;
-
-        for (j=0;j<rows; j++) {
-            //long s1=(((long)PSIU[i+j*rows]*PSIU[i+j*rows])>>15)*(Dold[i]);
-            long s2=((((long)PSIU[i*rows+j]*Dold[j]))>>(2*qAU-15))*PSIU[i*rows+j];
-//			printf("%d - %d\n",s1,s2);
-            sigma += s2;
-        }
-        sigma += Q[i*rows+i]<<15; // Q is in q15
-        for (j=i+1;j<rows; j++) {
-            sigma += (((long)G[i*rows+j]*G[i*rows+j])>>13)*Q[j*rows+j];
-//			sigma += (((long)G[i+j*rows]*G[i+j*rows])>>13)*Q[j+j*rows];
-        }
-
-        if (sigma>16384<<15) sigma = 16384<<15;
-        *(D+i)=sigma>>15;
-        if (D[i]==0) D[i]=1;
-//show("D",D,5);
-
-        for (j=0;j<i;j++) {
-// 			printf("\n%d,%d\n",i,j);
-            sigma =0;
-            for (k=0;k<rows;k++) {
-				int tmp= (((long(PSIU[i*rows+k])*Dold[k]))>>(2*qAU-15));
-				if (tmp>32767) printf("!");
-				sigma += (((long(PSIU[i*rows+k])*Dold[k]))>>(2*qAU-15))*PSIU[j*rows+k];
-            }
-            for (k=0;k<rows;k++) {
-                sigma += ((((long)G[i*rows+k])*G[j*rows+k])>>13)*Q[k*rows+k];
-            }
-            long z=sigma/D[i]; // shift by 15
-            if (z>32767) z=32767;
-            if (z<-32768) z=-32768;
-
-            U[j*rows+i] = (int)z;
-
-
-            for (k=0;k<rows;k++) {
-                PSIU[j*rows+k] -= ((long)U[j*rows+i]*PSIU[i*rows+k])>>15; //qAU*q15/q15=qAU
-            }
-
-            for (k=0;k<rows;k++) {
-                G[j*rows+k] -=  ((long)U[j*rows+i]*G[i*rows+k])>>15;
-            }
-
-        }
-        //show("U",U,25);
-        //show("G",G,25);
-        if (i==0) return;
-    }
-}
-
-void bierman_fast(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ) {
-    int alpha;
-    int beta,lambda;
-    int b[5]; // ok even for 4-dim state
-    int *a; // in [0,1] -> q15
-    unsigned int iy,j,i;
-
-    int *b_j,*b_i;
-    int *a_j;
-    int *D_j;
-    int *U_ij;
-    int *x_i;
-    int dzs;
+
+void bierman_fast(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *R, unsigned int16 dimy, unsigned int16 dimx )
+{
+    int16 alpha;
+    int16 beta,lambda;
+    int16 b[5]; // ok even for 4-dim state
+    int16 *a; // in [0,1] -> q15
+    unsigned int16 iy,j,i;
+
+    int16 *b_j,*b_i;
+    int16 *a_j;
+    int16 *D_j;
+    int16 *U_ij;
+    int16 *x_i;
+
+    int32 z_pom;
+    int16 z_pom_int16;
 
     a = U; // iyth row of U
@@ -202,7 +70,7 @@
         // a is a row
         for (j=0,a_j=a,b_j=b,D_j=D; j<dimx; j++,b_j++,D_j++,a_j++)
-            *b_j=((long)(*D_j)*(*a_j))>>15;
-
-        alpha = (long)R[iy]; //\alpha = R+vDv = R+a*b
+            *b_j=((int32)(*D_j)*(*a_j))>>15;
+
+        alpha = R[iy]; //\alpha = R+vDv = R+a*b
         // R in q15, a in q15, b=q15
 //		gamma = (1<<15)/alpha; //(in q15)
@@ -210,42 +78,53 @@
         //max gamma = 0.0061 => gamma_ref = q7
         for (j=0,a_j=a,b_j=b,D_j=D; j<dimx; j++,a_j++,b_j++,D_j++) {
-            beta   = alpha;
-            lambda = -((long)(*a_j)<<15)/beta;
-            alpha  += ((long)(*a_j)*(*b_j))>>15;
-            D[j] = ((((long)beta<<15)/alpha)*(*D_j))>>15; //gamma is long
+/*            beta=alpha;
+            lambda = -((int32)(*a_j)<<15)/beta;
+            alpha  += ((int32)(*a_j)*(*b_j))>>15;
+            D[j] = ((int32)beta**D_j)/alpha;*/
+/*xx*/
+            lambda=alpha;
+            alpha  += ((int32)(*a_j)*(*b_j))>>15;
+            D[j] = ((int32)lambda**D_j)/alpha;
+            z_pom_int16 = -((int32)(*a_j)<<15)/lambda;
+/*xx*/
+
             if (*D_j==0) *D_j=1;
 
             for (i=0,b_i=b,U_ij=U+j; i<j; i++, b_i++,U_ij+=dimx) {
                 beta   = *U_ij;
-                *U_ij +=  ((long)lambda*(*b_i))>>15;
-                *b_i  +=  ((long)beta*(*b_j))>>15;
-            }
-        }
-        dzs = (((long)difz[iy])<<15)/alpha;  // apply scaling to innovations
+//                *U_ij +=  ((int32)lambda*(*b_i))>>15;		// puvodni reseni
+                *U_ij -=  ((int32)(*a_j)*(*b_i))/lambda;         // pozadovane optimalni reseni
+//                *U_ij -=  ((int32)((int16)((int32)(*a_j)<<15)/lambda)**b_i)>>15;	// tohle funguje - problem je s tim pretypovanim na (int16)
+//				*U_ij -= (int16)((int32)(*a_j)*(*b_i))/lambda;
+//				z_pom = (((int32)(*a_j)*(*b_i))/lambda);
+/*  			    z_pom = (int32)(*U_ij)-(int16)((int32)(*a_j)*(*b_i))/lambda;
+				if (z_pom > 32767) z_pom = 32767;
+				if (z_pom < - 32768) z_pom = -32768;
+				*U_ij = z_pom;                      /**/
+//                *U_ij +=  ((int32)z_pom_int16*(*b_i))>>15;		// puvodni reseni - jen jina konstanta
+                *b_i  +=  ((int32)beta*(*b_j))>>15;
+            }
+        }
         // no shift due to gamma
         for (i=0,x_i=xp,b_i=b; i<dimx; i++,x_i++,b_i++) {
-            *x_i  += ((long)dzs*(*b_i))>>15; // multiply by unscaled Kalman gain
-        }
-
-        //cout << "Ub: " << U << endl;
-        //cout << "Db: " << D << endl <<endl;
-
-    }
-
-}
+            *x_i  += ((int32)difz[iy]*(*b_i))/alpha; // multiply by unscaled Kalman gain
+        }
+    }
+}
+
 
 // Thorton procedure - Kalman predictive variance in UD
-void thorton_fast(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int rows) {
-    unsigned int i,j,k;
+void thorton_fast(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 rows) {
+    unsigned int16 i,j,k;
     // copy D to Dold
-    int *Dold_i,*Dold_k;
-    int *D_i;
-    int *PSIU_ij,*PSIU_ik,*PSIU_jk;
-    int *Q_jj,*Q_ii,*Q_kk;
-    int *U_ji;
-    int *G_ik,*G_jk;
-    int irows,jrows;
-    long sigma; // in qAU+15!!
-    long z;
+    int16 *Dold_i,*Dold_k;
+    int16 *D_i;
+    int16 *PSIU_ij,*PSIU_ik,*PSIU_jk;
+    int16 *Q_jj,*Q_ii,*Q_kk;
+    int16 *U_ji;
+    int16 *G_ik,*G_jk;
+    int16 irows,jrows;
+    int32 sigma; // in qAU+15!!
+    int32 z;
 
     for (i=0,Dold_i=Dold,D_i=D;i<rows;i++,Dold_i++,D_i++) {
@@ -271,13 +150,12 @@
         for (k=0, PSIU_ik=PSIU+irows,Dold_k=Dold;
                 k<rows; k++, PSIU_ik++,Dold_k++) {
-			sigma += (((long)(*PSIU_ik)**PSIU_ik)>>(qAU))*(*Dold_k);
-        }
-        sigma += *(Q+i+irows)<<qAU;
+			sigma += (((int32)(*PSIU_ik)**PSIU_ik)>>(qAU))*(*Dold_k);
+        }
+        sigma += (int32)(*(Q+i+irows))<<qAU;
         for (j=i+1, G_ik=G+irows+i+1; j<rows; j++,G_ik++) {
-            sigma += (((long)(*G_ik)**G_ik)>>16)**(Q+j+j*rows);
-
-        }
-
-		if (sigma>((long)1<<(qAU+15))) {
+            sigma += (((int32)(*G_ik)**G_ik)>>16)**(Q+j+j*rows);
+        }
+
+		if (sigma>((int32)1<<(qAU+15))) {
 			*D_i = 32767;
 //			*(Dold+i)-=*(Q+i+irows);
@@ -294,10 +172,10 @@
                     k<rows; k++, PSIU_ik++, PSIU_jk++, Dold_k++) {
 
-				sigma += ((((long)(*PSIU_ik)**PSIU_jk)>>qAU)**Dold_k);
+				sigma += ((((int32)(*PSIU_ik)**PSIU_jk)>>qAU)**Dold_k);
             }
 
             for (k=i,G_ik=G+irows+i,G_jk=G+jrows+i,Q_kk=Q+k*rows+k;
                     k<rows;k++,G_ik++,G_jk++,Q_kk+=rows+1) {
-                sigma += ((((long)*G_ik)**G_jk)>>16)**Q_kk;
+                sigma += ((((int32)*G_ik)**G_jk)>>16)**Q_kk;
             }
 
@@ -307,15 +185,15 @@
 
             U_ji=U+jrows+i;
-            *U_ji = (int)z;
+            *U_ji = (int16)z;
 
 
             for (k=0,PSIU_ik=PSIU+irows,PSIU_jk=PSIU+jrows;
                     k<rows;k++,PSIU_ik++,PSIU_jk++) {
-                *PSIU_jk -= ((long)*U_ji**PSIU_ik)>>15;
+                *PSIU_jk -= ((int32)*U_ji**PSIU_ik)>>15;
             }
 
             for (k=0,G_jk=G+jrows,G_ik=G+irows;
                     k<rows;k++, G_jk++, G_ik++) {
-                *G_jk -=  ((long)*U_ji**G_ik)>>15;
+                *G_jk -=  ((int32)*U_ji**G_ik)>>15;
             }
 
@@ -323,50 +201,4 @@
 		if (i==0) return;
     }
-}
-
-void bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ) {
-    long alpha;
-    long gamma,beta,lambda;
-    int b[5]; // ok even for 4-dim state
-    int *a; // in [0,1] -> q15
-    unsigned int iy,j,i;
-
-    for (iy=0; iy<dimy; iy++) {
-        // a is a row
-        a = U+iy*dimx; // iyth row of U
-        for (j=0;j<dimx;j++) {
-            (j<iy)? b[j]=0: (j==iy)? b[j]=D[j] : b[j]=((long)D[j]*a[j])>>15;
-        }
-
-        alpha = (long)R[iy]; //\alpha = R+vDv = R+a*b
-        // R in q15, a in q15, b=q15
-//		gamma = (1<<15)/alpha; //(in q15)
-        //min alpha = R[iy] = 164
-        //max gamma = 0.0061 => gamma_ref = q7
-        for (j=0;j<dimx;j++) {
-            beta   = alpha;
-            lambda = -(long(a[j])<<15)/beta;
-            alpha  += ((long(a[j])*b[j])>>15);
-            D[j] = (((long(beta)<<15)/alpha)*D[j])>>15; //gamma is long
-            if (D[j]==0) D[j]=1;
-
-            // 			cout << "a: " << alpha << "g: " << gamma << endl;
-            for (i=0;i<j;i++) {
-                beta   = U[i*dimx+j];
-                U[i*dimx+j] +=  (lambda*b[i])>>15;
-                b[i]   +=  (beta*b[j])>>15;
-            }
-        }
-        int dzs = (long(difz[iy])<<15)/alpha;  // apply scaling to innovations
-        // no shift due to gamma
-        for (i=0; i<dimx; i++) {
-            xp[i]  += (long(dzs*b[i]))>>15; // multiply by unscaled Kalman gain
-        }
-
-        //cout << "Ub: " << U << endl;
-        //cout << "Db: " << D << endl <<endl;
-
-    }
-
 }
 
Index: /applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h
===================================================================
--- /applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h (revision 1230)
+++ /applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h (revision 1240)
@@ -12,29 +12,33 @@
 
 #define qAU 14
+#define qD 14
+
+#define int16 short int
+#define int32 int
 
 /* Matrix multiply Full matrix by upper diagonal matrix with unit diagonal; */
-extern void mmultAU(int *m1, int *up, int *result, unsigned int rows, unsigned int columns);
+extern void mmultAU(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns);
 
 /* Matrix multiply Full matrix by upper diagonal matrix; */
-extern void mmultACh(int *m1, int *up, int *result, unsigned int rows, unsigned int columns);
+extern void mmultACh(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns);
 
 /* perform Thorton update of UD matrix using PSI*U, Q, and temporaries G, Dold, for size dimx*/
-extern void thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx);
+extern void thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx);
 
 /* perform Bierman update of UD matrix using difz, R and xp, for size dimx*/
-extern void bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx );
+extern void bierman(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *R, unsigned int16 dimy, unsigned int16 dimx );
 
 /* perform Thorton update of UD matrix using PSI*U, Q, and temporaries G, Dold, for size dimx*/
-extern void thorton_fast(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx);
+extern void thorton_fast(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx);
 
 /* perform Bierman update of UD matrix using difz, R and xp, for size dimx*/
-extern void bierman_fast(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx );
+extern void bierman_fast(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *R, unsigned int16 dimy, unsigned int16 dimx );
 
 /* perform Householder update of Ch matrix using PSI*Ch , Q, */
-extern void householder(int *Ch /*= int *PSICh*/, int *Q, unsigned int dimx);
+extern void householder(int16 *Ch /*= int16 *PSICh*/, int16 *Q, unsigned int16 dimx);
 
 /* perform Givens update of Ch matrix using PSI*Ch , Q, */
-extern void givens(int *Ch /*= int *PSICh*/, int *Q, unsigned int dimx);
+extern void givens(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(int *difz, int *xp, int *Ch, int *R, unsigned int dimy, unsigned int dimx );
+extern void carlson(int16 *difz, int16 *xp, int16 *Ch, int16 *R, unsigned int16 dimy, unsigned int16 dimx );
