Index: /pmsm/simulator_zdenek/simulator.cpp
===================================================================
--- /pmsm/simulator_zdenek/simulator.cpp (revision 239)
+++ /pmsm/simulator_zdenek/simulator.cpp (revision 240)
@@ -394,11 +394,11 @@
 
   // uprava velikosti vzhledem k Uc!=Ucn
-/*  kor_Uc=Ucn/230.;
+  kor_Uc=Ucn/230.;
   *u*=kor_Uc; /**/
 
   pwm(modulace);
 
-  *us=*u*cos(*(u+1));
-  *(us+1)=*u*sin(*(u+1));;
+//  *us=*u*cos(*(u+1));
+//  *(us+1)=*u*sin(*(u+1));;
 
   pmsm_model(5);
Index: /pmsm/sim_var.cpp
===================================================================
--- /pmsm/sim_var.cpp (revision 232)
+++ /pmsm/sim_var.cpp (revision 240)
@@ -40,8 +40,9 @@
 	double Ww = 0.0;
 	vec dt ( 2 );
-	vec ut ( 4 );
-	vec utm ( 4 );
+	vec ut ( 2 );
+	vec utm ( 2 );
 	vec dut ( 2 );
 	vec dit (2);
+	vec x2o(2);
 	vec xtm=zeros ( 4 );
 	vec xte=zeros ( 4 );
@@ -62,13 +63,13 @@
 	mat Q =diag( Qdiag );
 	mat R =diag ( Rdiag );
-	EKFfull Efix ( rx,ry,rU );
+	EKFfull Efix ( rx,ry,ru );
 	Efix.set_est ( mu0, 1*eye ( 4 )  );
 	Efix.set_parameters ( &fxu,&hxu,Q,R);
 
-	EKFfull Eop ( rx,ry,rU );
+	EKFfull Eop ( rx,ry,ru );
 	Eop.set_est ( mu0, 1*eye ( 4 ) );
 	Eop.set_parameters ( &fxu,&hxu,Q,R);
 
-	EKFfull Edi ( rx,ry,rU );
+	EKFfull Edi ( rx,ry,ru );
 	Edi.set_est ( mu0, 1*eye ( 4 ) );
 	Edi.set_parameters ( &fxu,&hxu,Q,R);
@@ -106,7 +107,6 @@
 		ut ( 0 ) = KalmanObs[4];
 		ut ( 1 ) = KalmanObs[5];
-		ut ( 2 ) = utm(0)-ut(0);
-		ut ( 3 ) = utm(1)-ut(1);
 		
+		x2o = fxu.eval2o(utm-ut);
 		utm = ut;
 		
@@ -156,5 +156,5 @@
 		//LOG
 		L.logit(X_log, 	vec(x,4)); //vec from C-array
-		L.logit(X2o_log, x2o_dbg); 
+		L.logit(X2o_log, x2o); 
 		L.logit(Xdf_log, xdif); 
 		L.logit(Efix_log, Efix_ep.mean() ); 
Index: /pmsm/mpf_u_delta.cpp
===================================================================
--- /pmsm/mpf_u_delta.cpp (revision 232)
+++ /pmsm/mpf_u_delta.cpp (revision 240)
@@ -63,5 +63,5 @@
 		xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ( ubm+ud ( 1 ) ) *dt/Ls;
 		//om
-		xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
+		xk ( 2 ) = omm;// + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
 		//th
 		xk ( 3 ) = thm + omm*dt; // <0..2pi>
@@ -93,10 +93,10 @@
 
 	vec mu0= "0.0 0.0 0.0 0.0";
-	vec Qdiag ( "0.4 0.4 0.001 0.000001" ); //zdenek: 0.01 0.01 0.0001 0.0001
+	vec Qdiag ( "0.4 0.4 0.1 0.000001" ); //zdenek: 0.01 0.01 0.0001 0.0001
 	vec Rdiag ( "0.1 0.1" ); //var(diff(xth)) = "0.034 0.034"
 	chmat Q ( Qdiag );
 	mat Q2o=diag(Qdiag);
 	chmat R ( Rdiag );
-	EKFCh KFE ( rx,ry,rU );
+	EKFCh KFE ( rx,ry,ru );
 	KFE.set_parameters ( &fxu0,&hxu,Q,R );
 	KFE.set_est ( mu0, chmat ( ones ( 4 ) ) );
@@ -111,10 +111,10 @@
 	MPF<EKFCh_du_kQ> M ( rx,rUd,evolUd,evolUd,Npart,KFEp );
 	// initialize
-	vec Ud0="0 0 100.0";
-	evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec( "1 1 10" )));
+	vec Ud0="0 0 1.0";
+	evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec( "1e-4 1e-4 10e-4" )));
 	evolUd.condition ( Ud0 );
 	epdf& pfinit=evolUd._epdf();
 	M.set_est ( pfinit );
-	evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec(" 9e-8 9e-8 9e-7" )));
+	evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec(" 5e-4 5e-4 1e-8" )));
 
 	mat Xt=zeros ( Ndat ,4 ); //true state from simulator
@@ -128,6 +128,6 @@
 	pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
 	vec dt ( 2 );
-	vec ut ( 4 );
-	vec utm ( 4 );
+	vec ut ( 2 );
+	vec utm ( 2 );
 	vec xt ( 4 );
 	vec xtm=zeros ( 4 );
@@ -145,17 +145,16 @@
 		ut ( 0 ) = KalmanObs[0];
 		ut ( 1 ) = KalmanObs[1];
-		ut ( 2 ) = utm(0)-ut(0);
-		ut ( 3 ) = utm(1)-ut(1);
 		dt ( 0 ) = KalmanObs[2]+0.3*Rnoise ( 0,t );
 		dt ( 1 ) = KalmanObs[3]+0.3*Rnoise ( 1,t );
 		xt = vec ( x,4 );
 
+		vec x2o = fxu0.eval2o(utm-ut);
 		utm =ut;
 		
-		Q2o*=0.9;
-		Q2o+=outer_product(x2o_dbg,x2o_dbg);
+		Q2o*=0.99;
+		Q2o+=outer_product(x2o,x2o);
 		//estimator
 		KFE.bayes ( concat ( dt,ut ) );
-		for (int ii=0; ii<Npart; ii++){dynamic_cast<EKFCh_du_kQ*>(M._BM(ii))->set_ref(chmat(Q2o));}
+		for (int ii=0; ii<Npart; ii++){dynamic_cast<EKFCh_du_kQ*>(M._BM(ii))->set_ref(chmat(elem_mult(Qdiag,Qdiag)));}
 		M.bayes ( concat ( dt,ut ) );
 
Index: /pmsm/pmsm.h
===================================================================
--- /pmsm/pmsm.h (revision 232)
+++ /pmsm/pmsm.h (revision 240)
@@ -12,11 +12,8 @@
 RV ru ( "{ua ub }");
 RV ry ( "{oia oib }");
-RV rU = concat(ru, RV("{dua dub }"));
 
 // class uipmsm : public uibase{
 // 	double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
 // };
-
-vec x2o_dbg(4);
 
 //! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
@@ -81,35 +78,31 @@
 	protected:
 		double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
-
+		//! store first derivatives for the use in second derivatives
+		double dia, dib, dom, dth;
+		//! d2t = dt^2/2, cth = cos(th), sth=sin(th)
+		double d2t, cth, sth;
+		double iam, ibm, omm, thm, uam, ubm;
 	public:
-		IMpmsm2o() :diffbifn (rx.count(), rx, rU ) {};
+		IMpmsm2o() :diffbifn (rx.count(), rx, ru ) {};
 	//! Set mechanical and electrical variables
-		void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;}
+		void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0; d2t=dt*dt/2;}
 
 		vec eval ( const vec &x0, const vec &u0 ) {
 		// last state
-			double iam = x0 ( 0 );
-			double ibm = x0 ( 1 );
-			double omm = x0 ( 2 );
-			double thm = x0 ( 3 );
-			double uam = u0 ( 0 );
-			double ubm = u0 ( 1 );
-			double dua = u0 ( 2 )/dt;
-			double dub = u0 ( 3 )/dt;
-
-			double cth = cos(thm);
-			double sth = sin(thm);
-			double d2t = dt*dt/2;
+			iam = x0 ( 0 );
+			ibm = x0 ( 1 );
+			omm = x0 ( 2 );
+			thm = x0 ( 3 );
+			uam = u0 ( 0 );
+			ubm = u0 ( 1 );
+
+			cth = cos(thm);
+			sth = sin(thm);
 			
-			double dia = (- Rs/Ls*iam +  Ypm/Ls*omm * sth + uam/Ls);
-			double dib = (- Rs/Ls*ibm -  Ypm/Ls*omm * cth + ubm/Ls);
-			double dom = kp*p*p * Ypm/J *( ibm * cth-iam * sth ) - p/J*Mz;
-			double dth = omm;
-			
-			double d2ia = (- Rs/Ls*dia +  Ypm/Ls*(dom * sth + omm*cth) + dua/Ls);
-			double d2ib = (- Rs/Ls*dib -  Ypm/Ls*(dom * cth - omm*sth) + dub/Ls);
-			double d2om = kp*p*p * Ypm/J *( dib * cth-ibm*sth - (dia * sth + iam *cth));
-			double d2th = dom;
-			
+			dia = (- Rs/Ls*iam +  Ypm/Ls*omm * sth + uam/Ls);
+			dib = (- Rs/Ls*ibm -  Ypm/Ls*omm * cth + ubm/Ls);
+			dom = kp*p*p * Ypm/J *( ibm * cth-iam * sth ) - p/J*Mz;
+			dth = omm;
+						
 			vec xk=zeros ( 4 );
 			xk ( 0 ) =  iam + dt*dia;// +d2t*d2ia;
@@ -118,9 +111,4 @@
 			xk ( 3 ) = thm + dt*dth;// +d2t*d2th; // <0..2pi>
 			
-			x2o_dbg(0)=d2t*d2ia;
-			x2o_dbg(1)=d2t*d2ib;
-			x2o_dbg(2)=d2t*d2om;
-			x2o_dbg(3)=d2t*d2th;
-			
 			if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
 			if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
@@ -128,9 +116,21 @@
 		}
 
+		//! eval 2nd order Taylor expansion, MUST be used only as a follow up AFTER eval()!!
+		vec eval2o(const vec &du){
+			double dua = du ( 0 )/dt;
+			double dub = du ( 1 )/dt;
+			
+			vec xth2o(4);
+			xth2o(0) = (- Rs/Ls*dia +  Ypm/Ls*(dom * sth + omm*cth) + dua/Ls);
+			xth2o(1) = (- Rs/Ls*dib -  Ypm/Ls*(dom * cth - omm*sth) + dub/Ls);
+			xth2o(2) = kp*p*p * Ypm/J *( dib * cth-ibm*sth - (dia * sth + iam *cth));
+			xth2o(3) = dom;
+			return xth2o;
+		}
 		void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
-			double iam = x0 ( 0 );
-			double ibm = x0 ( 1 );
-			double omm = x0 ( 2 );
-			double thm = x0 ( 3 );
+			 iam = x0 ( 0 );
+			 ibm = x0 ( 1 );
+			 omm = x0 ( 2 );
+			 thm = x0 ( 3 );
 		// d ia
 			A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
@@ -154,4 +154,5 @@
 //! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$, equation for \f$\omega\f$ is omitted.$
 class IMpmsmStat : public IMpmsm {
+	public:
 	IMpmsmStat() :IMpmsm() {};
 	//! Set mechanical and electrical variables
