Index: /simulator_zdenek/ekf_example/ekf_obj.cpp
===================================================================
--- /simulator_zdenek/ekf_example/ekf_obj.cpp (revision 61)
+++ /simulator_zdenek/ekf_example/ekf_obj.cpp (revision 61)
@@ -0,0 +1,146 @@
+#include <itpp/itbase.h>
+#include <estim/libKF.h>
+
+#include "ekf_obj.h"
+
+double minQ(double Q){if (Q>1.0){ return 1.0;} else {return Q;};};
+
+///////////////
+void EKFfixed::bayes(const vec &dt){
+	ekf(dt(2),dt(3),dt(0),dt(1));
+	
+	if ( BM::evalll ) {
+		//from enorm
+		vec ydif(2);
+		ydif(0)=dt(0)-zprevod(x_pred[0],Qm)*Iref;
+		ydif(1)=dt(1)-zprevod(x_pred[1],Qm)*Iref;
+		
+		BM::ll = -0.5* ( 2 * 1.83787706640935 +log ( det ( Ry ) ) +ydif* ( inv(Ry)*ydif ) );
+	}
+};
+
+
+void EKFfixed::update_psi(void)
+{
+  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);
+
+  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;
+
+  // 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_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;
+  x_pred[2]=x_est[2];
+  x_pred[3]=(((long)x_est[3]<<15)+(long)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)
+{
+  int Y_error[2];
+  long 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(.01,15);       // 0.05
+  Q[5]=Q[0];
+  Q[10]=prevod(0.0001,15);      // 1e-3
+  Q[15]=prevod(0.0001,15);      // 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*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;
+}
Index: /simulator_zdenek/ekf_example/ekf_obj.h
===================================================================
--- /simulator_zdenek/ekf_example/ekf_obj.h (revision 61)
+++ /simulator_zdenek/ekf_example/ekf_obj.h (revision 61)
@@ -0,0 +1,102 @@
+/*!
+  \file
+  \brief Bayesian Filtering for linear Gaussian models (Kalman Filter) and extensions
+  \author Vaclav Smidl.
+
+  -----------------------------------
+  BDM++ - C++ library for Bayesian Decision Making under Uncertainty
+
+  Using IT++ for numerical operations
+  -----------------------------------
+*/
+
+#ifndef EKFfix_H
+#define EKFfix_H
+
+#include <itpp/itbase.h>
+#include <estim/libKF.h>
+#include "fixed.h"
+#include "matrix.h"
+#include "reference.h"
+#include "parametry_motoru.h"
+
+using namespace itpp;
+
+double minQ(double Q);
+
+/*!
+\brief Extended Kalman Filter in full matrices
+
+An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
+*/
+class EKFfixed : public BM, public BMcond {
+public:
+void init_ekf(double Tv);
+void ekf(double ux, double uy, double isxd, double isyd);
+
+/* Declaration of local functions */
+void prediction(int *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 */
+ enorm<fsqmat> E;
+ mat Ry;
+  
+public:
+	//! Default constructor
+	EKFfixed ( RV rvx,RV rvc ):BM(rvx),BMcond(rvc),E(rvx),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<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;}
+};
+	//! Here dt = [yt;ut] of appropriate dimensions
+	void bayes ( const vec &dt );
+	//!dummy!
+	epdf& _epdf(){return E;};
+	void condition ( const vec &Q0 ) {
+		
+		Q[0]=prevod(minQ(Q0(0)),15);       // 0.05
+		Q[5]=prevod(minQ(Q0(1)),15);
+		Q[10]=prevod(minQ(Q0(2)),15);      // 1e-3
+		Q[15]=prevod(minQ(Q0(3)),15);      // 1e-3
+
+	}
+};
+
+
+#endif // KF_H
+
+
Index: /simulator_zdenek/ekf_example/matrix.cpp
===================================================================
--- /simulator_zdenek/ekf_example/matrix.cpp (revision 50)
+++ /simulator_zdenek/ekf_example/matrix.cpp (revision 61)
@@ -395,4 +395,5 @@
 
   det=((long)*matrix**(matrix+3)-(long)*(matrix+1)**(matrix+2))>>15;
+  if (det==0) det=1;
 
   *result++=(((long)*(matrix+3))<<15)/det;
Index: /simulator_zdenek/ekf_example/CMakeLists.txt
===================================================================
--- /simulator_zdenek/ekf_example/CMakeLists.txt (revision 53)
+++ /simulator_zdenek/ekf_example/CMakeLists.txt (revision 61)
@@ -1,1 +1,3 @@
 add_executable (simulace fixed.cpp matrix.cpp pmsm_mod.cpp pwm.cpp regulace.cpp ekf.cpp simul.cpp )
+
+add_library (ekf_obj ekf_obj.cpp fixed.cpp matrix.cpp)
