Index: applications/pmsm/simulator_zdenek/ekf_example/simulmpf.cpp
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/simulmpf.cpp (revision 1437)
+++ applications/pmsm/simulator_zdenek/ekf_example/simulmpf.cpp (revision 1439)
@@ -65,6 +65,6 @@
 
 // EKF - vysledek estimace
-static double mpf_estim[2]={0.,0.};     // w_est, theta_est
-static double Esinth, Ecosth;
+static floatx mpf_estim[2]={0.,0.};     // w_est, theta_est
+static floatx Esinth, Ecosth;
 
 FILE *fw;
@@ -115,5 +115,5 @@
   init_pmsm(param, REL1);
   init_regulace(param,h_reg);
-  mpf_init(1e0, 1e-5, 1e0);//VS:
+  mpf_init(1e0, 1e-3, 1e0);//VS:
 
   fw=fopen("data/graf2.txt","w");
@@ -161,44 +161,44 @@
 
         if (t>0.7) Ww=2.*M_PI*10.;
-        if (t>1.0) x[8]=1.2;    // 1A
-        if (t>1.2) x[8]=10.8;   // 9A
-        if (t>1.4) x[8]=25.2;  // 21A
+//         if (t>1.0) x[8]=1.2;    // 1A
+//         if (t>1.2) x[8]=10.8;   // 9A
+//         if (t>1.4) x[8]=25.2;  // 21A
 
         if (t>1.6) Ww=2.*M_PI*50.;
-        if (t>1.9) x[8]=1.2;    // 1A
-        if (t>2.1) x[8]=10.8;   // 9A
-        if (t>2.3) x[8]=25.2;  // 21A
+//         if (t>1.9) x[8]=1.2;    // 1A
+//         if (t>2.1) x[8]=10.8;   // 9A
+//         if (t>2.3) x[8]=25.2;  // 21A
 
         if (t>2.5) Ww=2.*M_PI*100;
-        if (t>2.8) x[8]=1.2;    // 1A
-        if (t>3.0) x[8]=10.8;   // 9A
-        if (t>3.2) x[8]=25.2;  // 21A
+//         if (t>2.8) x[8]=1.2;    // 1A
+//         if (t>3.0) x[8]=10.8;   // 9A
+//         if (t>3.2) x[8]=25.2;  // 21A
 
         if (t>3.4) Ww=2.*M_PI*150;
-        if (t>3.7) x[8]=1.2;    // 1A
-        if (t>3.9) x[8]=10.8;   // 9A
-        if (t>4.1) x[8]=25.2;  // 21A
+//         if (t>3.7) x[8]=1.2;    // 1A
+//         if (t>3.9) x[8]=10.8;   // 9A
+//         if (t>4.1) x[8]=25.2;  // 21A
 
         if (t>4.3) Ww=2.*M_PI*0;
-        if (t>4.8) x[8]=-1.2;    // 1A
-        if (t>5.0) x[8]=-10.8;   // 9A
-        if (t>5.2) x[8]=-25.2;  // 21A
+//         if (t>4.8) x[8]=-1.2;    // 1A
+//         if (t>5.0) x[8]=-10.8;   // 9A
+//         if (t>5.2) x[8]=-25.2;  // 21A
 
         if (t>5.4) Ww=2.*M_PI*(-10.);
-        if (t>5.7) x[8]=-1.2;    // 1A
-        if (t>5.9) x[8]=-10.8;   // 9A
-        if (t>6.1) x[8]=-25.2;  // 21A
+//         if (t>5.7) x[8]=-1.2;    // 1A
+//         if (t>5.9) x[8]=-10.8;   // 9A
+//         if (t>6.1) x[8]=-25.2;  // 21A
 
         if (t>6.3) Ww=2.*M_PI*(-50.);
-        if (t>6.7) x[8]=-1.2;    // 1A
-        if (t>6.9) x[8]=-10.8;   // 9A
-        if (t>7.1) x[8]=-25.2;  // 21A
+//         if (t>6.7) x[8]=-1.2;    // 1A
+//         if (t>6.9) x[8]=-10.8;   // 9A
+//         if (t>7.1) x[8]=-25.2;  // 21A
 
         if (t>7.3) Ww=2.*M_PI*(-100.);
-        if (t>7.7) x[8]=-1.2;    // 1A
-        if (t>7.9) x[8]=-10.8;   // 9A
-        if (t>8.1) x[8]=-25.2;  // 21A
-        if (t>8.3) x[8]=10.8;   // 9A
-        if (t>8.5) x[8]=25.2;  // 21A
+//         if (t>7.7) x[8]=-1.2;    // 1A
+//         if (t>7.9) x[8]=-10.8;   // 9A
+//         if (t>8.1) x[8]=-25.2;  // 21A
+//         if (t>8.3) x[8]=10.8;   // 9A
+//         if (t>8.5) x[8]=25.2;  // 21A
 
 
@@ -218,15 +218,19 @@
 		if (x[2]<-1615) x[2]=-1615;  /**/
 			
-                // KALMAN
-               Umk=*u*Uc/Ucn;
+       
+      // KALMAN (pouzij stara Usx, Usy!!
+	  if (0){
+		mpf_bayes((floatx)Isx,(floatx)Isy,(floatx)Usx,(floatx)Usy);
+		mpf_mean(&Esinth,&Ecosth,&mpf_estim[0]);
+		mpf_estim[1]=atan2((floatx)Esinth,(floatx)Ecosth);
+	  } else {
+		  mpf_estim[0]=kalman_om((floatx)Isx,(floatx)Isy,(floatx)Usx,(floatx)Usy,(floatx)x[3]);
+	  }
+
+      Umk=*u*Uc/Ucn;
       ua=Umk*cos(*(u+1));
       ub=Umk*cos(*(u+1)-2./3.*M_PI);
       Usx=ua;                     // usx
       Usy=(ua+2.*ub)/sqrt(3.);    // usy
-       
-		mpf_bayes(Isx,Isy,Usx,Usy);
-		mpf_mean(&Esinth,&Ecosth,&mpf_estim[0]);
-		mpf_estim[1]=atan2(Esinth,Ecosth);
-
                 // vystup z EKF zaveden do regulatoru
                 // rychlost=ekf_estim[0]; theta=ekf_estim[1]; //VS
