Index: /pmsm/sim_var.cpp
===================================================================
--- /pmsm/sim_var.cpp (revision 224)
+++ /pmsm/sim_var.cpp (revision 232)
@@ -32,6 +32,7 @@
 	int Nsimstep = 125;
 
-	dirfilelog L("exp/sim_var",1000);
+	dirfilelog L("exp/sim_var2",1000);
 	//memlog L(Ndat);
+	
 	
 	// SET SIMULATOR
@@ -39,5 +40,6 @@
 	double Ww = 0.0;
 	vec dt ( 2 );
-	vec ut ( 2 );
+	vec ut ( 4 );
+	vec utm ( 4 );
 	vec dut ( 2 );
 	vec dit (2);
@@ -47,5 +49,5 @@
 	vec xt ( 4 );
 	vec ddif=zeros(2);
-	IMpmsm fxu;
+	IMpmsm2o fxu;
 	//                  Rs    Ls        dt           Fmag(Ypm) kp   p    J     Bf(Mz)
 	fxu.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989,   1.5 ,4.0, 0.04, 0.0 );
@@ -60,13 +62,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);
@@ -80,7 +82,9 @@
 	RV rR( "{R }", "4");
 	RV rUD( "{u_isa u_isb i_isa i_isb }", ones_i(4));
-	RV rDu("{dux duy duxf duyf }",ones_i(4));
+	RV rDu("{dux duy }",ones_i(2));
 	RV rDi("{disa disb }",ones_i(2));
 	int X_log = L.add(rx,"X");
+	int X2o_log = L.add(rx,"X2o");
+	int Xdf_log = L.add(rx,"Xdf");
 	int Efix_log = L.add(rx,"XF");
 	int Eop_log = L.add(rx,"XO");
@@ -100,6 +104,11 @@
 		};
 		// simulation via deterministic model
-		ut ( 0 ) = KalmanObs[0];
-		ut ( 1 ) = KalmanObs[1];
+		ut ( 0 ) = KalmanObs[4];
+		ut ( 1 ) = KalmanObs[5];
+		ut ( 2 ) = utm(0)-ut(0);
+		ut ( 3 ) = utm(1)-ut(1);
+		
+		utm = ut;
+		
 		dt ( 0 ) = KalmanObs[2];
 		dt ( 1 ) = KalmanObs[3];
@@ -147,4 +156,6 @@
 		//LOG
 		L.logit(X_log, 	vec(x,4)); //vec from C-array
+		L.logit(X2o_log, x2o_dbg); 
+		L.logit(Xdf_log, xdif); 
 		L.logit(Efix_log, Efix_ep.mean() ); 
 		L.logit(Eop_log, 	Eop_ep.mean() ); 
Index: /pmsm/mpf_u_delta.cpp
===================================================================
--- /pmsm/mpf_u_delta.cpp (revision 230)
+++ /pmsm/mpf_u_delta.cpp (revision 232)
@@ -84,5 +84,5 @@
 
 	// internal model
-	IMpmsm fxu0;
+	IMpmsm2o fxu0;
 	IMpmsm_delta fxu;
 	//                  Rs    Ls        dt       Fmag(Ypm)    kp   p    J     Bf(Mz)
@@ -94,8 +94,9 @@
 	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 Rdiag ( "0.2 0.2" ); //var(diff(xth)) = "0.034 0.034"
+	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 ) ) );
@@ -110,10 +111,10 @@
 	MPF<EKFCh_du_kQ> M ( rx,rUd,evolUd,evolUd,Npart,KFEp );
 	// initialize
-	vec Ud0="0 0 1.0";
-	evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec( "1 1 0.0001" )));
+	vec Ud0="0 0 100.0";
+	evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec( "1 1 10" )));
 	evolUd.condition ( Ud0 );
 	epdf& pfinit=evolUd._epdf();
 	M.set_est ( pfinit );
-	evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec(" 4e-3 4e-3 1e-3" )));
+	evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec(" 9e-8 9e-8 9e-7" )));
 
 	mat Xt=zeros ( Ndat ,4 ); //true state from simulator
@@ -127,5 +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 ( 2 );
+	vec ut ( 4 );
+	vec utm ( 4 );
 	vec xt ( 4 );
 	vec xtm=zeros ( 4 );
@@ -143,14 +145,21 @@
 		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 );
 
+		utm =ut;
+		
+		Q2o*=0.9;
+		Q2o+=outer_product(x2o_dbg,x2o_dbg);
 		//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));}
 		M.bayes ( concat ( dt,ut ) );
 
 		Xt.set_row ( tK, xt ); //vec from C-array
-		Dt.set_row ( tK, concat ( dt,ut, vec_2 ( KalmanObs[4],KalmanObs[5] ) ) );
+		Dt.set_row ( tK, concat ( dt,ut.left(2), vec_2 ( KalmanObs[4],KalmanObs[5] ) ) );
 		Qtr.set_row ( tK, Qdiag );
 		XtE.set_row ( tK,KFE._e()->mean() );
Index: /pmsm/pmsm.h
===================================================================
--- /pmsm/pmsm.h (revision 224)
+++ /pmsm/pmsm.h (revision 232)
@@ -12,8 +12,11 @@
 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$
@@ -74,4 +77,79 @@
 };
 
+//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
+class IMpmsm2o : public diffbifn {
+	protected:
+		double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
+
+	public:
+		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;}
+
+		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;
+			
+			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;
+			
+			vec xk=zeros ( 4 );
+			xk ( 0 ) =  iam + dt*dia;// +d2t*d2ia;
+			xk ( 1 ) = ibm + dt*dib;// +d2t*d2ib;
+			xk ( 2 ) = omm +dt*dom;// +d2t*d2om;
+			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;
+			return xk;
+		}
+
+		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 );
+		// d ia
+			A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
+			A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
+		// d ib
+			A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
+			A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
+		// d om
+			A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
+			A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
+			A ( 2,2 ) = 1.0;
+			A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
+		// d th
+			A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
+		}
+
+		void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
+
+};
+
 //! 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 {
