#include <estim/kalman.h>

#include "ekf_obj.h"
//#include "../simulator.h"

double minQ(double Q){if (Q>1.0){ return 1.0;} else {return Q;};};

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_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){
	ekf(yt(0),yt(1),ut(0),ut(1));
	
	vec xhat(4);	
	//UGLY HACK!!! reliance on a predictor!!
	xhat(0)=zprevod(x_est[0],Qm)*Iref;
	xhat(1)=zprevod(x_est[1],Qm)*Iref;
	xhat(2)=zprevod(x_est[2],Qm)*Wref;
	xhat(3)=zprevod(x_est[3],15)*Thetaref;
	
	E.set_mu(xhat);
	
	if ( BM::evalll ) {
/*		//from enorm
		vec xdif(x,4);//first 4 of x
		//UGLY HACK!!! reliance on a predictor!!
/*		xdif(0)=x[0]-zprevod(x_pred[0],Qm)*Iref;
		xdif(1)=x[1]-zprevod(x_pred[1],Qm)*Iref;
		xdif(2)=x[2]-zprevod(x_pred[2],Qm)*Wref;
		xdif(3)=x[3]-zprevod(x_pred[3],15);*/
		
//		xdif -=xhat; //(xdif=x-xhat)
		
		mat Pfull(4,4);
		double* Pp=Pfull._data();
		for(int16 i=0;i<16;i++){*(Pp++) = zprevod(P_est[i],15);}
		
		E._R()._M()=Pfull;
		
		
//		BM::ll = -0.5* ( 4 * 1.83787706640935 +log ( det ( Pfull ) ) +xdif* ( inv(Pfull)*xdif ) );*/
	}
};


void EKFfixed::update_psi(void)
{
  int16 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);

  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
  //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=((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]=(((int32)x_est[3]<<15)+(int32)cG*x_est[2])>>15;

  update_psi();

  mmult(PSI,P_est,temp15a,3,3,3);
//  mtrans(PSI,temp15b,5,5);
  mmultt(temp15a,PSI,P_pred,3,3,3);
  maddD(P_pred,Q,3,3);
}

void EKFfixed::correction(void)
{
  int16 Y_error[2];
  int32 temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */

  choice_P(P_pred,temp15a,3);
  maddD(temp15a,R,1,1);
  minv2(temp15a,temp30a);
  	Ry(0,0)=zprevod(temp15a[0],15);
  	Ry(0,1)=zprevod(temp15a[1],15);
  	Ry(1,0)=zprevod(temp15a[2],15);
  	Ry(1,1)=zprevod(temp15a[3],15);
  
  mmultDr(P_pred,temp15a,3,3,1,1);
  mmult1530(temp15a,temp30a,Kalm,3,1,1);


  /* estimate the state system */
  choice_x(x_pred, temp15a);
  msub(Y_mes,temp15a,Y_error,1,0);
  mmult(Kalm,Y_error,temp15a,3,1,0);
  madd(x_pred,temp15a,x_est,3,0);

  /* matrix of covariances - version without MMULTDL() */

/* Version with MMULTDL() */
  mmultDl(P_pred,temp15a,1,3,3,1);

  mmult(Kalm,temp15a,P_est,3,1,3);
  msub(P_pred,P_est,P_est,3,3);
/* END */
}


void EKFfixed::ekf(double ux, double uy, double isxd, double isyd)
{
  // vypocet napeti v systemu (x,y)
  ukalm[0]=prevod(ux/Uref,Qm);
  ukalm[1]=prevod(uy/Uref,Qm);

  // zadani mereni
  Y_mes[0]=prevod(isxd/Iref,Qm);
  Y_mes[1]=prevod(isyd/Iref,Qm);

  ////// vlastni rutina EKF /////////////////////////
  prediction(ukalm);
  correction();

  // 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)(3)=zprevod(x_est[3],15)*Thetaref;
}

void EKFfixed::init_ekf(double Tv)
{
  // Tuning of matrix Q
  Q[0]=prevod(.05,15);       // 0.05
  Q[5]=Q[0];
  Q[10]=prevod(0.0002,15);      // 1e-3
  Q[15]=prevod(0.001,15);      // 1e-3

  // Tuning of matrix R
  R[0]=prevod(0.1,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*4/Thetaref,15);
//  cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
  cH=prevod(Tv*Wref*Fmag/Iref/Ls,17); //cB in q
  //  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;
  
  P_est[0]=0x7FFF;
  P_est[5]=0x7FFF;
  P_est[10]=0x7FFF;
  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 ) {
	pfxu = pfxu0;
	phxu = phxu0;
	
	set_dim ( pfxu0->_dimx() );
	dimy = phxu0->dimension();
	dimc = pfxu0->_dimu();
	
	vec &_mu = est._mu();
	// if mu is not set, set it to zeros, just for constant terms of A and C
	if ( _mu.length() != dimension() ) _mu = zeros ( dimension() );
	A = zeros ( dimension(), dimension() );
	C = zeros ( dimy, dimension() );
	
	//initialize matrices A C, later, these will be only updated!
	pfxu->dfdx_cond ( _mu, zeros ( dimc ), A, true );
	//	pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true );
	phxu->dfdx_cond ( _mu, zeros ( dimc ), C, true );
	//	phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true );
	
	R = R0;
	Q = Q0;
	
	// 
}
// aux fnc 
void UDtof(const mat &U, const vec &D, imat &Uf, ivec &Df, const vec &xref){
	mat P= U*diag(D)*U.T();
	mat T = diag(1.0/(xref));
	mat Pf = T*P*T;
	
	ldmat Pld(Pf);
	
	mat Ut=Pld._L().T()*(1<<15); // U is in q15 -- diagonal is 0!!!
	Uf=round_i(Ut);
	Df=round_i(Pld._D()*(1<<15));
	ivec zer=find(Df==0);
	for(int16 i=0; i<zer.length(); i++) Df(zer(i))=1;
}


void EKF_UDfix::bayes ( const vec &yt, const vec &cond ) {
	//preparatory
	vec &_mu=est._mu();
	const vec &u=cond;
	int16 dim = dimension();
	///// !!!!!!!!!!!!!!!!
	U = est._R()._L().T();
	D =  est._R()._D();
	
	////////////
	
	pfxu->dfdx_cond ( _mu, u, A, false ); //update A by a derivative of fx
	phxu->dfdx_cond ( _mu, u, C, false ); //update A by a derivative of fx
	
	mat PhiU = A*U;

	//////
/*	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;
	
	
	vec Din = D;
	int16 i,j,k;
	double sigma;
	mat G = eye(dim);
	//////// thorton
	
	//move mean;
	_mu = pfxu->eval(_mu,u);
	
	for (i=dim-1; i>=0;i--){
		sigma = 0.0; 
		for (j=0; j<dim; j++) {
			sigma += PhiU(i,j)*PhiU(i,j) *Din(j); 
			sigma += G(i,j)*G(i,j) * Q(j,j);
		}
		
/*		double sigma2= 0.0;
		for (j=0; j<dim; j++) {
			sigma2 += PhiU(i,j)*PhiU(i,j) *Din(j); 
		}
		sigma2 +=Q(i,i);//*G(i,i)=1.0
		for (j=i+1; j<dim; j++) {
			sigma2 += G(i,j)*G(i,j) * Q(j,j);
		}*/
		D(i) = sigma; 
		
	/*	UDtof(U,D,Utf,Dtf,xref);
		cout << "d=sig"<<endl;
		cout << Dtf << endl;
	*/	
		for (j=0;j<i;j++){ 
// 			cout << i << "," << j << endl;
			sigma = 0.0; 
			for (k=0;k<dim;k++){ 
				sigma += PhiU(i,k)*Din(k)*PhiU(j,k); 
			}
			for (k=0;k<dim;k++){ 
				sigma += G(i,k)*Q(k,k)*G(j,k); 
			}
			//
			U(j,i) = sigma/D(i); 
			
/*			cout << "U=sig/D"<<endl;
			UDtof(U,D,Utf,Dtf,xref);
			cout << Utf << endl << Dtf << endl;
			cout << G << endl << Din << endl<<endl;*/
			
			for (k=0;k<dim;k++){ 
				PhiU(j,k) = PhiU(j,k) - U(j,i)*PhiU(i,k); 
			}
			for (k=0;k<dim;k++){ 
				G(j,k) = G(j,k) - U(j,i)*G(i,k); 
			}
		
		}
	}

	// bierman
	
	double dz,alpha,gamma,beta,lambda;
	vec a;
	vec b;
	vec yp = phxu->eval(_mu,u);
	vec xp=_mu; // used in bierman
	
	
	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. 
		dz    = yt(iy) - yp(iy); 
		
		alpha = R(iy); 
		gamma = 1/alpha; 
		for (j=0;j<dim;j++){
			beta   = alpha; 
			alpha  = alpha + a(j)*b(j); 
			lambda = -a(j)*gamma; 
			gamma  = 1.0/alpha; 
			D(j) = beta*gamma*D(j); 
			
			// 			cout << "a: " << alpha << "g: " << gamma << endl;
			for (i=0;i<j;i++){
				beta   = U(i,j); 
				U(i,j) = beta + b(i)*lambda; 
				b(i)   = b(i) + b(j)*beta; 
			}
		}
		double dzs = gamma*dz;  // apply scaling to innovations 
		_mu   = _mu + dzs*b; // multiply by unscaled Kalman gain 
		//cout << "Ub: " << U << endl;
		//cout << "Db: " << D << endl <<endl;
		
	}

	
	/////
	est._R().__L()=U.T();
	est._R().__D()=D;
	
	if ( evalll == true ) { //likelihood of observation y
	}
}

void EKF_UDfix::from_setting ( const Setting &set ) {
	BM::from_setting ( set );
	shared_ptr<diffbifn> IM = UI::build<diffbifn> ( set, "IM", UI::compulsory );
	shared_ptr<diffbifn> OM = UI::build<diffbifn> ( set, "OM", UI::compulsory );
	
	//statistics
	int16 dim = IM->dimension();
	vec mu0;
	if ( !UI::get ( mu0, set, "mu0" ) )
		mu0 = zeros ( dim );
	
	mat P0;
	vec dP0;
	if ( UI::get ( dP0, set, "dP0" ) )
		P0 = diag ( dP0 );
	else if ( !UI::get ( P0, set, "P0" ) )
		P0 = eye ( dim );
	
	est._mu()=mu0;
	est._R()=ldmat(P0);
	
	//parameters
	vec dQ, dR;
	UI::get ( dQ, set, "dQ", UI::compulsory );
	UI::get ( dR, set, "dR", UI::compulsory );
	set_parameters ( IM, OM, diag ( dQ ), dR  );
	
	UI::get(log_level, set, "log_level", UI::optional);
}


void EKFfixedUD::bayes(const itpp::vec& yt, const itpp::vec& ut)
{
ekf(ut[0],ut[1],yt[0],yt[1]);
}


void EKFfixedUD::ekf(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;
	
	// 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];
	//               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);
	
	//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);
		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);
	
	 {
		 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];
		log_level.store(logD,get_from_ivec(dd));
	}
	
	//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];
	
	// 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 EKFfixedUD::init_ekf(double Tv)
{
	// Tuning of matrix Q
	Q[0]=prevod(.001,15);       // 0.05
	Q[5]=Q[0];
	Q[10]=prevod(0.0005,15);      // 1e-3
	Q[15]=prevod(0.0001,15);      // 1e-3

	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
	
	Df[0]=1<<14;
	Df[1]=1<<14;
	Df[2]=1<<14;
	Df[3]=1<<14;
	
	// 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); // < ===========
	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;
	
}

void EKFfixedUD2::bayes(const itpp::vec& yt, const itpp::vec& ut)
{
	ekf2(ut[0],ut[1],yt[0],yt[1]);
}


void EKFfixedUD2::ekf2(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;
	
	x_est[0]=x_est[0];
	//               q15              + q15*q13
	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);
//	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)
	{
		int Ai[4];
		for (int i=0;i<4; i++) Ai[i]=(int)(PSI[i]);
		ivec Ad(Ai,4);
		log_level.store(logA,get_from_ivec(Ad));
	}
	
	//thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx);
	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(Ud._data(),Ud._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])+(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];
	{
		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));
	}
	
	//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,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)*Wref;
	(mu)(1)=zprevod(x_est[1],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 EKFfixedUD2::init_ekf2(double Tv)
{
	// Tuning of matrix Q
	Q[0]=prevod(0.01,15);      // 1e-3
	Q[3]=prevod(0.01,15);      // 1e-3
	
	Uf[0]=0x7FFF;// !       // 0.05
	Uf[1]=Uf[2]=0;
	Uf[3]=0x7FFF;//!
	
	Df[0]=1<<14;
	Df[1]=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]=cG;
	PSI[3]=0x7FFF;
}

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)
{
	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)
	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, 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=((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];
	//               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);
	
	//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;
	}
	{
/*		ivec Ad(PSI,16);
		log_level.store(logA,get_from_ivec(Ad));*/
	}
	
	///////// end of copy ///////////////
	mmultACh(PSI,Chf,PSICh,4,4);
	//thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx);
	//householder(PSICh,Q,4);
	givens_fast(PSICh,Q,4);
	// COPY 
	for (int16 ii=0; ii<16; ii++){Chf[ii]=PSICh[ii];}
	
	{
		
		int Chi[16]; for (int i=0;i<16;i++) Chi[i]=Chf[i];
		ivec Chd(Chi,16);
		log_level.store(logCh,get_from_ivec(Chd));
		imat mCh(Chd._data(), 4,4);
		imat P = mCh*mCh.T();
		ivec iP(P._data(),16);
		log_level.store(logP,get_from_ivec(iP));
	}
	
	
	int16 difz[2];
	difz[0]=(Y_mes[0]-x_est[0]); // shift to q15!!
	difz[1]=(Y_mes[1]-x_est[1]);//<<2;
	
	
	//
	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_fast(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.0005,15);      // 1e-3
	Q[15]=prevod(0.001,15);      // 1e-3
	
	Chf[0]=0x3FFF;// !       // 0.05
	Chf[1]=Chf[2]=Chf[3]=Chf[4]=0;
	Chf[5]=0x3FFF;//!
	Chf[6]=Chf[6]=Chf[8]=Chf[9]=0;
	Chf[10]=0x3FFF;//!      // 1e-3
	Chf[11]=Chf[12]=Chf[13]=Chf[4]=0;
	Chf[15]=0x3FFF;      // 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); //  <=======
	//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 */
	PSI[0]=cA;
	PSI[5]=PSI[0];
	PSI[10]=0x7FFF;
	PSI[14]=cG;
	PSI[15]=0x7FFF;
	
}

void EKFfixedCh2::bayes(const itpp::vec& yt, const itpp::vec& ut)
{
	ekf2(ut[0],ut[1],yt[0],yt[1]);
}


void EKFfixedCh2::ekf2(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;
	
	x_est[0]=x_est[0];
	//               q15              + q15*q13
	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 ///////////////
	mmultACh(PSI,Chf,PSICh,2,2);
	
	//void EKFfixed::update_psi(void)
	{
		int Ai[4];
		for (int i=0;i<4; i++) Ai[i]=(int)(PSICh[i]);
		ivec Ad(Ai,4);
		log_level.store(logA,get_from_ivec(Ad));
	}
	
	//thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx);
	givens_fast(PSICh,Q,2);
	for (int i=0;i<4;i++) Chf[i]=PSICh[i];	
	
	// 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 Chi[4]; 	for (int i=0;i<4; i++) Chi[i]=(int)Chf[i];
//		int Ci[4]; 	for (int i=0;i<4; i++) Ci[i]=(int)C[i];
		
		imat Chd(Chi,2,2);
	//	imat Cd(Ci,2,2);
		imat CCh = Chd;
		log_level.store(logCh,get_from_ivec(ivec(CCh._data(),CCh._datasize())));
	}
	
	
	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 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));
	}
	
	//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];  
	carlson_fastC(difz,x_est,Chf,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)*Wref;
	(mu)(1)=zprevod(x_est[1],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 EKFfixedCh2::init_ekf2(double Tv)
{
	// Tuning of matrix Q
	Q[0]=prevod(0.005,15);      // 1e-3
	Q[3]=prevod(0.001,15);      // 1e-3
	
	Chf[0]=0x1FFF;// !       // 0.05
	Chf[1]=Chf[2]=0;
	Chf[3]=0x1FFF;//!
		
	// Tuning of matrix R
	R[0]=prevod(0.001,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/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[2]=cG;
	PSI[3]=0x7FFF;
}