Index: /applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp
===================================================================
--- /applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1174)
+++ /applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1179)
@@ -443,2 +443,122 @@
 	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)
+	int uf[2];
+	uf[0]=prevod(ux/Uref,Qm);
+	uf[1]=prevod(uy/Uref,Qm);
+	
+	int Y_mes[2];
+	// zadani mereni
+	Y_mes[0]=prevod(isxd/Iref,Qm);
+	Y_mes[1]=prevod(isyd/Iref,Qm);
+	
+	////// 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);
+	
+	tmp=((long)cB*x_est[2])>>15;
+	x_est[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*uf[0])>>15;
+	x_est[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*uf[1])>>15;
+	x_est[2]=x_est[2];
+	x_est[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15;
+	
+	//void EKFfixed::update_psi(void)
+	{		
+		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;
+	}
+	
+	///////// 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(Uf,Df,PSIU,Q,G,Dfold,4);
+	
+	int difz[2];
+	difz[0]=Y_mes[0]-x_est[0];
+	difz[1]=Y_mes[1]-x_est[1];
+	//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];
+	bierman_fast(difz,x_est,Uf,Df,dR,2,4);
+	
+	// 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;
+	
+//	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(.01,15);       // 0.05
+	Q[5]=Q[0];
+	Q[10]=prevod(0.0001,15);      // 1e-3
+	Q[15]=prevod(0.0001,15);      // 1e-3
+
+	Uf[0]=0x7FFF;       // 0.05
+	Uf[5]=0x7FFF;
+	Uf[10]=0x7FFF;      // 1e-3
+	Uf[15]=0x7FFF;      // 1e-3
+	
+	Df[0]=0x7FFF;
+	Df[1]=0x7FFF;
+	Df[2]=0x7FFF;
+	Df[3]=0x7FFF;
+	
+	// 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*4/Thetaref,15);
+	cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,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;
+	
+	//////////////////////// =================
+	///// 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;
+}
+
Index: /applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h
===================================================================
--- /applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (revision 1174)
+++ /applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (revision 1179)
@@ -100,4 +100,62 @@
 
 UIREGISTER(EKFfixed);
+
+/*!
+\brief Extended Kalman Filter with UD matrices in fixed point arithmetic
+
+An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
+*/
+class EKFfixedUD : public BM {
+	public:
+		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 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
+		
+		enorm<fsqmat> E;
+		mat Ry;
+		
+	public:
+		//! Default constructor
+		EKFfixedUD ():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++){Uf[i]=0;}
+			for(i=0;i<4;i++){Df[i]=0;}
+			for(i=0;i<16;i++){G[i]=0;}
+			for(i=0;i<4;i++){Dfold[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(EKFfixedUD);
 
 //! EKF for comparison of EKF_UD with its fixed-point implementation
@@ -197,4 +255,6 @@
 
 
+
+
 #endif // KF_H
 
Index: /applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp
===================================================================
--- /applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp (revision 1178)
+++ /applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp (revision 1179)
@@ -13,165 +13,322 @@
 #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
+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
     {
-      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);
-  }
+        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]);
+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*rows+j]*PSIU[i*rows+j])>>15)*(Dold[i]);
+			printf("-- %d,%d --", PSIU[i*rows+j], Dold[i]);
 		}
-		printf("\n");
-	}
-	for (int i=0;i<5;i++){printf("%d,",D[i]);}
-	printf("\n");
-}
- 
+		sigma += Q[i*rows+i];
+        for (j=i+1;j<rows; j++) {
+            sigma += (((long)G[i*rows+j]*G[i*rows+j])>>13)*Q[j*rows+j];
+			printf("-- %d,%d --", G[i*rows+j], Q[j*rows+j]);
+		}
+        printf("%d\n",sigma);
+		
+        *(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];
+		   }
+            printf("%d\n",sigma);
+			for (k=0;k<rows;k++) {
+                sigma += ((((long)G[i*rows+k])*G[j*rows+k])>>13)*Q[k*rows+k];
+            }
+            printf("%d\n",sigma);
+			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(int x=0;x<16;x++) printf("%d,", PSIU[x]);
+			printf("\n");
+			
+            for (k=0;k<rows;k++) {
+                G[j*rows+k] -=  ((long)U[j*rows+i]*G[i*rows+k])>>15;
+            }
+            for(int x=0;x<16;x++) printf("%d,", G[x]);
+			printf("\n");
+			
+            /*			printf("end\n");
+            			UDprt(U,D);
+            			UDprt(G,Dold);
+            			printf("\n");	*/
+        }
+        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;
+    a = U; // iyth row of U
+    for (iy=0; iy<dimy; iy++, a+=dimx) {
+        // 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
+        // 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,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
+            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 +=  (lambda*(*b_i))>>15;
+                *b_i  +=  ((long)beta*(*b_j))>>15;
+            }
+        }
+        int dzs = (((long)difz[iy])<<15)/alpha;  // apply scaling to innovations
+        // 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;
+
+    }
+
+}
+
 // 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; 
+void thorton_fast(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_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 q15
+
+    for (i=0,Dold_i=Dold,D_i=D;i<rows;i++,Dold_i++,D_i++) {
+        *Dold_i=*D_i;
+    }
+
+    // initialize G = eye()
+    G_ik= G;
+    *G_ik++=1<<14;
+    for (i=0;i<rows-1;i++) {
+        // clean elem before diag
+        for (k=0; k<rows; k++) {
+            *G_ik++=0;
+        }
+        *G_ik++=1<<14;
+    }
+    // eye created
+
+    for (i=rows-1, Dold_i=Dold+i, D_i=D+i;
+            true; i--, Dold_i--,D_i--) { // stop if i==0 at the END!
+        irows=i*rows;
+        sigma = 0;
+        for (k=0, PSIU_ik=PSIU+irows;
+           k<rows; k++, PSIU_ik++) {//Dold_i=
+            sigma += (((long)(*PSIU_ik)**PSIU_ik)>>15)*(*Dold_i);
+			printf("-- %d,%d --", *PSIU_ik, *Dold_i);
 		}
-		*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;
+		sigma += *(Q+i+irows);
+        for (j=i+1, G_ik=G+irows+i+1; j<rows; j++,G_ik++) {
+            sigma += (((long)(*G_ik)**G_ik)>>13)**(Q+j+j*rows);
+			printf("++ %d,%d ++", *G_ik,  *(Q+j+j*rows));
+			
+        }
+        printf("%d\n",sigma);
 		
-		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; 
-			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;
-		
-	}
-	
-}
+        *D_i=sigma>>15;
+        if (*D_i==0) *D_i=1;
+
+
+        for (j=0;j<i;j++) {
+            jrows = j*rows;
+
+            sigma =0;
+            for (k=0, PSIU_ik=PSIU+irows, PSIU_jk=PSIU+jrows, Dold_k=Dold;
+                    k<rows; k++, PSIU_ik++, PSIU_jk++, Dold_k++) {
+				
+                sigma += ((((long)*PSIU_ik)**PSIU_jk)>>15)**Dold_k;
+            }
+            printf("%d\n",sigma);
+			
+            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)>>13)**Q_kk;
+            }
+            printf("%d\n",sigma);
+			
+			long z=sigma/(*D_i); // shift by 15
+            if (z>32767) z=32767;
+            if (z<-32768) z=-32768;
+
+            U_ji=U+jrows+i;
+            *U_ji = (int)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;
+            }
+            for(int x=0;x<16;x++) printf("%d,", PSIU[x]);
+			printf("\n");
+			
+            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;
+            }
+            for(int x=0;x<16;x++) printf("%d,", G[x]);
+			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]; // 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++)
+            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 1174)
+++ /applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h (revision 1179)
@@ -18,2 +18,8 @@
 /* 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 );
+
+/* 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);
+
+/* 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 );
