Index: /applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp
===================================================================
--- /applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1168)
+++ /applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1174)
@@ -6,4 +6,17 @@
 
 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++){
+			*I++ = M(i,j);
+		}
+	}
+}
+void vec_to_int(const ivec &v, int *I){
+	for (int i=0;i<v.length(); i++){
+			*I++ = v(i);
+	}
+}
 
 ///////////////
@@ -167,2 +180,265 @@
   PSI[15]=0x7FFF;
 }
+
+
+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(int 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;
+	int 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(5);
+	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;
+	
+	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); 
+	
+	// A*U == PSI*U
+	mmultAU(PSI,Uf,PSIU,5,5);	
+/*	cout << ivec(PSIU,25) << endl;
+	cout << (Af*Utf)/(1<<15) <<endl;*/
+	
+	vec Din = D;
+	int 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);
+		}*/
+		
+/*		UDtof(U,D,Utf,Dtf,xref);
+		cout << "d=sig"<<endl;
+		cout << Utf << endl << Dtf << endl;
+		cout << G << endl << Din << endl<<endl;*/
+		
+		D(i) = sigma; 
+		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); 
+			}
+		
+/*			cout << "end"<<endl;
+			UDtof(U,D,Utf,Dtf,xref);
+			cout << G << endl << Din << endl;
+			cout << Utf << endl << Dtf << endl<<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);
+
+	thorton(Uf,Df,PSIU,Qf,Gf,Dfold,5);
+	
+	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
+	
+	double dz,alpha,gamma,beta,lambda;
+	vec a;
+	vec b;
+	vec yp = phxu->eval(_mu,u);
+	vec xp=_mu; // used in bierman
+	
+	for (int 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;
+		
+	}
+
+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;
+	for (i=0; i<25;i++) cout << Uf[i] << ","; cout <<endl;
+	for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl;
+	for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;
+
+	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);
+	for (i=0; i<25;i++) cout << Uf[i] << ","; cout <<endl;
+	for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl;
+	for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;
+	cout << 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
+	int 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);
+}
Index: /applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h
===================================================================
--- /applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (revision 1168)
+++ /applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (revision 1174)
@@ -18,4 +18,5 @@
 #include "fixed.h"
 #include "matrix.h"
+#include "matrix_vs.h"
 #include "reference.h"
 #include "parametry_motoru.h"
@@ -24,4 +25,7 @@
 
 double minQ(double Q);
+
+void mat_to_int(const imat &M, int *I);
+void vec_to_int(const ivec &v, int *I);
 
 /*!
@@ -97,4 +101,100 @@
 UIREGISTER(EKFfixed);
 
+//! EKF for comparison of EKF_UD with its fixed-point implementation
+class EKF_UDfix : public BM {
+	protected:
+		//! logger
+		LOG_LEVEL(EKF_UDfix,logU, logG);
+		//! Internal Model f(x,u)
+		shared_ptr<diffbifn> pfxu;
+		
+		//! Observation Model h(x,u)
+		shared_ptr<diffbifn> phxu;
+		
+		//! U part
+		mat U;
+		//! D part
+		vec D;
+		
+		int Uf[25];
+		int Df[5];
+		int Dfold[5];
+		
+		mat A;
+		mat C;
+		mat Q;
+		vec R;
+		
+		int PSI[25];
+		int PSIU[25];
+		int Gf[25];
+
+		enorm<ldmat> est;
+		
+		
+	public:
+		
+		//! copy constructor duplicated 
+		EKF_UDfix* _copy() const {
+			return new EKF_UDfix(*this);
+		}
+		
+		const enorm<ldmat>& posterior()const{return est;};
+		
+		enorm<ldmat>& prior() {
+			return const_cast<enorm<ldmat>&>(posterior());
+		}
+		
+		EKF_UDfix(){}
+		
+		
+		EKF_UDfix(const EKF_UDfix &E0): pfxu(E0.pfxu),phxu(E0.phxu), U(E0.U), D(E0.D){}
+		
+		//! Set nonlinear functions for mean values and covariance matrices.
+		void set_parameters ( const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const mat Q0, const vec R0 );
+		
+		//! Here dt = [yt;ut] of appropriate dimensions
+		void bayes ( const vec &yt, const vec &cond = empty_vec );
+		
+		void log_register ( bdm::logger& L, const string& prefix ){
+			BM::log_register ( L, prefix );
+			
+			if ( log_level[logU] )
+				L.add_vector ( log_level, logU, RV ( dimension()*dimension() ), prefix );
+			if ( log_level[logG] )
+				L.add_vector ( log_level, logG, RV ( dimension()*dimension() ), prefix );
+			
+		}
+		/*! Create object from the following structure
+		
+		\code
+		class = 'EKF_UD';
+		OM = configuration of bdm::diffbifn;    % any offspring of diffbifn, bdm::diffbifn::from_setting
+		IM = configuration of bdm::diffbifn;    % any offspring of diffbifn, bdm::diffbifn::from_setting
+		dQ = [...];                             % vector containing diagonal of Q
+		dR = [...];                             % vector containing diagonal of R
+		--- optional fields ---
+		mu0 = [...];                            % vector of statistics mu0
+		dP0 = [...];                            % vector containing diagonal of P0
+		-- or --
+		P0 = [...];                             % full matrix P0
+		--- inherited fields ---
+		bdm::BM::from_setting
+		\endcode
+		If the optional fields are not given, they will be filled as follows:
+		\code
+		mu0 = [0,0,0,....];                     % empty statistics
+		P0 = eye( dim );              
+		\endcode
+		*/
+		void from_setting ( const Setting &set );
+		
+		void validate() {};
+		// TODO dodelat void to_setting( Setting &set ) const;
+		
+};
+UIREGISTER(EKF_UDfix);
+
+
 #endif // KF_H
 
Index: /applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp
===================================================================
--- /applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp (revision 1174)
+++ /applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp (revision 1174)
@@ -0,0 +1,179 @@
+/************************************
+        Extended Kalman Filter
+        Matrix operations
+
+        V. Smidl
+
+Rev. 30.8.2010
+
+30.8.2010      Prvni verze
+
+*************************************/
+#include "fixed.h"
+#include "stdio.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;
+  int *m2pom;
+
+  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)(*m1++)**m2pom;
+        m2pom+=columns;
+      }
+      // add the missing A(i,j)
+		tmp_sum +=(long)(*m1)<<15; // no need to shift
+		m1-=(j); // shift back to first element
+
+      // saturation effect
+      tmp_sum=tmp_sum>>15;
+      if (tmp_sum>32767) tmp_sum=32767;
+      if (tmp_sum<-32768) tmp_sum=-32768;
+
+      *result++=tmp_sum;
+
+      tmp_sum=0;
+    }
+    m1+=(columns);
+  }
+};
+
+void UDprt(int *U, int *D){
+	return;
+	for (int i=0;i<5;i++){
+		for (int j=0;j<5;j++){
+			printf("%d,",U[i*5+j]);
+		}
+		printf("\n");
+	}
+	for (int i=0;i<5;i++){printf("%d,",D[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 q15
+	for (i=rows-1; i>=0;i--){ // check i==0 at the END!
+		sigma = 0;
+		
+		for (j=0;j<rows; j++){
+			sigma += (((long)PSIU[i+j*rows]*PSIU[i+j*rows])>>15)*(Dold[i]);
+		}
+		sigma += Q[i+i*rows];
+		for (j=i+1;j<rows; j++){
+			sigma += (((long)G[i+j*rows]*G[i+j*rows])>>13)*Q[j+j*rows];
+		}
+		
+		*(D+i)=sigma>>15;
+		if (D[i]==0) D[i]=1;
+
+/*		printf("d=sig\n");
+		UDprt(U,D);
+		UDprt(G,Dold);*/
+		
+		for (j=0;j<i;j++){
+// 			printf("\n%d,%d\n",i,j);
+			sigma =0;
+			for (k=0;k<rows;k++){
+				sigma += ((((long)PSIU[i*rows+k])*PSIU[j*rows+k])>>15)*Dold[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;
+
+/*			printf("U=sig/D\n");
+			UDprt(U,D);
+			UDprt(G,Dold);*/
+			
+			for (k=0;k<rows;k++){ 
+				PSIU[j*rows+k] -= ((long)U[j*rows+i]*PSIU[i*rows+k])>>15; 
+			}
+			for (k=0;k<rows;k++){ 
+				G[j*rows+k] -=  ((long)U[j*rows+i]*G[i*rows+k])>>15; 
+			}
+			
+/*			printf("end\n");
+			UDprt(U,D);
+			UDprt(G,Dold);
+			printf("\n");	*/
+		}
+		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];
+	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++)
+			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; 
+			alpha  += ((long)(a[j])*b[j]>>15); 
+			lambda = -(a[j])*gamma>>15; 
+			gamma  = (1<<15)/alpha; // in q15 now
+			int mpl=((long)beta*gamma); // no-shift, max_gamma=2^15
+			D[j] = (long)mpl*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 = (gamma*(difz[iy]))>>15;  // 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 1174)
+++ /applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h (revision 1174)
@@ -0,0 +1,19 @@
+/************************************
+        Extended Kalman Filter
+        Matrix operations
+
+        V. Smidl
+
+Rev. 30.8.2010
+
+30.8.2010      Prvni verze
+
+*************************************/
+/* Matrix multiply Full matrix by upper diagonal matrix; */
+extern void mmultAU(int *m1, int *up, int *result, unsigned int rows, unsigned int 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);
+
+/* 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 );
Index: /applications/pmsm/simulator_zdenek/ekf_example/CMakeLists.txt
===================================================================
--- /applications/pmsm/simulator_zdenek/ekf_example/CMakeLists.txt (revision 66)
+++ /applications/pmsm/simulator_zdenek/ekf_example/CMakeLists.txt (revision 1174)
@@ -2,3 +2,3 @@
 
 include_directories(../../bdm)
-add_library (ekf_obj ekf_obj.cpp fixed.cpp matrix.cpp)
+add_library (ekf_obj ekf_obj.cpp fixed.cpp matrix.cpp matrix_vs.cpp)
Index: /applications/pmsm/pmsm_estim.cpp
===================================================================
--- /applications/pmsm/pmsm_estim.cpp (revision 1168)
+++ /applications/pmsm/pmsm_estim.cpp (revision 1174)
@@ -3,6 +3,5 @@
 \file
 \brief Multi-Estimator (developped for PMSM)
-
-
+ 
  */
 
