Index: applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (revision 1464)
+++ applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (revision 1466)
@@ -28,84 +28,12 @@
 using namespace bdm;
 
-double minQ(double Q);
+double minQ(double Q); 
 
 void mat_to_int16(const imat &M, int16 *I);
 void vec_to_int16(const ivec &v, int16 *I);
+void int16_to_mat(int16 *I, imat &M, int rows, int cols);
+void int16_to_vec(int16 *I, ivec &v, int len);
 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 point16 arithmetic
-
-An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
-*/
-class EKFfixed : public BM {
-public:
-void init_ekf(double Tv);
-void ekf(double ux, double uy, double isxd, double isyd);
-
-/* Declaration of local functions */
-void prediction(int16 *ux);
-void correction(void);
-void update_psi(void);
-
-/* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
- 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;
-  
-public:
-	//! Default constructor
-	EKFfixed ():BM(),E(),Ry(2,2){
-		int16 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<4;i++){x_pred[i]=0;}
-		for(i=0;i<16;i++){P_pred[i]=0;}
-		for(i=0;i<16;i++){P_est[i]=0;}
-		P_est[0]=0x7FFF;
-		P_est[5]=0x7FFF;
-		P_est[10]=0x7FFF;
-		P_est[15]=0x7FFF;
-		for(i=0;i<2;i++){Y_mes[i]=0;}
-		for(i=0;i<2;i++){ukalm[i]=0;}
-		for(i=0;i<8;i++){Kalm[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;};
-	
-};
-
-UIREGISTER(EKFfixed);
-
-#endif
 
 //! EKF for testing q44
@@ -121,4 +49,54 @@
 };
 UIREGISTER(EKFtest);
+
+
+class EKFscale: public EKFCh{
+	LOG_LEVEL(EKFscale, logCh, logA, logC);
+public:
+	mat Tx;
+	mat Ty;
+	
+	void from_setting ( const Setting &set ){
+		EKFCh::from_setting(set);
+		vec v;
+		UI::get(v, set, "xmax", UI::compulsory);
+		Tx = diag(1./v);
+		UI::get(v, set, "ymax", UI::compulsory);
+		Ty = diag(1./v);
+		
+		UI::get(log_level, set, "log_level", UI::optional);
+	};
+	
+	void log_register(logger &L, const string &prefix){
+		BM::log_register ( L, prefix );
+		
+		L.add_vector ( log_level, logCh, RV ("Ch", dimension()*dimension() ), prefix );
+		L.add_vector ( log_level, logA, RV ("A", dimension()*dimension() ), prefix );
+		L.add_vector ( log_level, logC, RV ("C", dimensiony()*dimension() ), prefix );
+	};
+	
+	void log_write() const{
+		BM::log_write();
+	    if ( log_level[logCh] ) {
+			mat Chsc=Tx*(est._R()._Ch());
+			vec v(Chsc._data(), dimension()*dimension());
+			if (v(0)<0)
+				v= -v;
+			log_level.store( logCh, round(v*32767));
+		}
+		if (log_level[logA]){
+			mat Asc = Tx*A*inv(Tx);
+			vec v(Asc._data(), dimension()*dimension());
+			log_level.store( logA, round(v*32767));
+		}
+		if (log_level[logC]){
+			mat Csc = Ty*C*inv(Tx);
+			vec v(Csc._data(), dimensiony()*dimension());
+			log_level.store( logC, round(v*32767));
+		}
+	}
+	
+};
+UIREGISTER(EKFscale);
 
 /*!
@@ -400,6 +378,5 @@
 		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 );
-		
+		L.add_vector ( log_level, logP, RV ("P", 16 ), prefix );		
 	};
 	//void from_setting();
@@ -656,5 +633,5 @@
 //! EKF with covariance R estimated by the VB method
 class EKFvbR: public EKFfull{	
-    LOG_LEVEL(EKFvbR,logR,logQ);
+    LOG_LEVEL(EKFvbR,logR,logQ,logP);
 
 	//! Statistics of the R estimator
@@ -691,4 +668,5 @@
 		L.add_vector(log_level, logR, RV("{R }",vec_1<int>(4)), prefix);
 		L.add_vector(log_level, logQ, RV("{Q }",vec_1<int>(4)), prefix);
+		L.add_vector(log_level, logP, RV("{P }",vec_1<int>(4)), prefix);
 	}
 	void bayes ( const vec &val, const vec &cond ) {
@@ -709,10 +687,13 @@
 			EKFfull::bayes(val,cond);
 
-			diffy = val - fy._mu();
-			Psi_vbR = phi*PsiR + (1-phi)*PsiR0+ outer_product(diffy,diffy)/*+C*mat(est._R())*C.T()*/;
-			R = Psi_vbR/(nu-2);
+//			diffy = val - fy._mu();
+//			Psi_vbR = phi*PsiR + (1-phi)*PsiR0+ outer_product(diffy,diffy)/*+C*mat(est._R())*C.T()*/;
+//			R = Psi_vbR/(nu-2);
 	
 			diffx = est._mu() - xpred;
-			Psi_vbQ = phi*PsiQ + (1-phi)*PsiQ0+ outer_product(diffx,diffx) /*+mat(est._R()) /*+ A*mat(P0)*A.T()*/;
+			Psi_vbQ = phi*PsiQ + (1-phi)*PsiQ0+ outer_product(diffx,diffx);/*mat(est._R());  A*mat(P0)*A.T();*/
+			//
+			Psi_vbQ(0,1) = 0.0;
+			Psi_vbQ(1,0) = 0.0;
 			Q = Psi_vbQ/(nu-2);
 		}
@@ -722,5 +703,9 @@
 		log_level.store(logQ, vec(Q._M()._data(),4));
 		log_level.store(logR, vec(R._M()._data(),4));
-		
+		{
+			mat Ch(2,2);
+			Ch=chol(est._R()._M());
+			log_level.store(logP, vec(Ch._data(),4));
+		}
 	}
 };
@@ -729,5 +714,4 @@
 
 class ekfChfix: public BM{
-	
 	ekf_data E;
 public:
@@ -758,5 +742,5 @@
 		vec dQ,dR;
 		UI::get ( dQ, set, "dQ", UI::optional );
-		UI::get ( dQ, set, "dR", UI::optional );
+		UI::get ( dR, set, "dR", UI::optional );
 		if (dQ.length()==2){
 			E.Q[0]=prevod(dQ[0],15);      // 1e-3
@@ -774,4 +758,108 @@
 UIREGISTER(ekfChfix);
 
+
+
+class ekfChfixQ: public BM{
+	LOG_LEVEL(ekfChfixQ,logQ,logCh,logC,logRes)
+	ekf_data E;
+	int64 PSI_Q0, PSI_Q1, PSI_Q0_reg, PSI_Q1_reg;
+	int Q_ni;
+	int phi_Q;
+public:
+	ekfChfixQ(){
+		init_ekfCh2(&E,0.000125);set_dim(2);	dimc = 2;
+		dimy = 2;
+		Q_ni = 7;
+		phi_Q = ((1<<Q_ni) -1)<<(15-Q_ni);
+
+		PSI_Q0_reg = ((int64)((1<<15)-phi_Q)*E.Q[0])<<Q_ni;
+		PSI_Q1_reg = ((int64)((1<<15)-phi_Q)*E.Q[3])<<Q_ni;
+		PSI_Q0 = ((int64)E.Q[0])<<Q_ni;
+		PSI_Q1 = ((int64)E.Q[3])<<Q_ni;
+	} 
+	void bayes ( const vec &val, const vec &cond ) {
+		int16 ux,uy;
+		ux=prevod(cond[0]/Uref,15);
+		uy=prevod(cond[1]/Uref,15);
+		
+		int16 yx,yy;
+		// zadani mereni
+		yx=prevod(val[0]/Iref,15);
+		yy=prevod(val[1]/Iref,15);
+		
+		int16 detS, rem;
+		ekfCh2(&E, ux,uy,yx,yy, &detS, &rem);
+		
+		Est._mu()=vec_2(E.x_est[0]*Uref/32768., E.x_est[1]*Uref/32768.);
+		
+		int16 xerr0 = E.x_est[0]-E.x_pred[0];
+		PSI_Q0 = ((int64)(phi_Q*PSI_Q0) + (((int64)xerr0*xerr0)<<15) + PSI_Q0_reg)>>15;		
+		E.Q[0] = PSI_Q0>>Q_ni;
+
+		xerr0 = E.x_est[1]-E.x_pred[1];
+		PSI_Q1 = ((int64)(phi_Q*PSI_Q1) + (((int64)xerr0*xerr0)<<15) + PSI_Q1_reg)>>15;		
+		E.Q[3] = PSI_Q1>>Q_ni;
+		
+		cout << E.Q[0] << "," << E.Q[3] <<endl;
+		ll = -0.5*qlog(detS)-0.5*rem;
+	}
+	const epdf& posterior() const {return Est;};
+	void from_setting ( const Setting &set ){
+		BM::from_setting(set);
+		vec dQ,dR;
+		UI::get ( dQ, set, "dQ", UI::optional );
+		UI::get ( dR, set, "dR", UI::optional );
+		if (dQ.length()==2){
+			E.Q[0]=prevod(dQ[0],15);      // 1e-3
+			E.Q[3]=prevod(dQ[1],15);      // 1e-3
+		}
+		if (dR.length()==2){
+			E.dR[0]=prevod(dR[0],15);      // 1e-3
+			E.dR[1]=prevod(dR[1],15);      // 1e-3
+		}
+
+		UI::get(Q_ni, set, "Q_ni", UI::optional);
+		{ // zmena Q!!
+			phi_Q = ((1<<Q_ni) -1)<<(15-Q_ni);
+
+			PSI_Q0_reg = ((int64)((1<<15)-phi_Q)*E.Q[0])<<Q_ni;
+			PSI_Q1_reg = ((int64)((1<<15)-phi_Q)*E.Q[3])<<Q_ni;
+			PSI_Q0 = ((int64)E.Q[0])<<Q_ni;
+			PSI_Q1 = ((int64)E.Q[3])<<Q_ni;
+
+		}
+		UI::get(log_level, set, "log_level", UI::optional);
+
+	}
+	void log_register ( logger &L, const string &prefix ) {
+		BM::log_register(L,prefix);
+		L.add_vector(log_level, logQ, RV("{Q }",vec_1<int>(2)), prefix);
+		L.add_vector(log_level, logCh, RV("{Ch }",vec_1<int>(3)), prefix);
+		L.add_vector(log_level, logC, RV("{C }",vec_1<int>(4)), prefix);
+		L.add_vector(log_level, logRes, RV("{dy }",vec_1<int>(2)), prefix);
+	}
+	void log_write() const {
+		BM::log_write();
+		if ( log_level[logQ] ) {
+			log_level.store( logQ, vec_2((double)E.Q[0],(double)E.Q[3] ));
+		}
+		if ( log_level[logCh] ) {
+			log_level.store( logCh, vec_3((double)E.Chf[0],(double)E.Chf[1], (double)E.Chf[3]));
+		}
+		if ( log_level[logRes] ) {
+			log_level.store( logRes, vec_2((double)E.difz[0],(double)E.difz[1]));
+		}
+		if ( log_level[logC] ) {
+			vec v(4);
+			for (int i=0;i<4;i++) v(i)=E.C[i];
+			log_level.store( logC, v);
+		}
+	}
+	
+	enorm<fsqmat> Est;
+	mat Ry;
+};
+UIREGISTER(ekfChfixQ);
+
 #endif // KF_H
 
