Index: /tests/pmsm_sim2.cpp
===================================================================
--- /tests/pmsm_sim2.cpp (revision 62)
+++ /tests/pmsm_sim2.cpp (revision 63)
@@ -32,4 +32,36 @@
 		preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );
 	};
+	void bayes(const vec dt){
+	EKFCh::bayes(dt);
+		
+		vec xtrue(4);
+		//UGLY HACK!!! reliance on a predictor!!
+		xtrue(0)=x[0];
+		xtrue(1)=x[1];
+		xtrue(2)=x[2];
+		xtrue(3)=x[3];
+		
+		ll = -0.5* ( 4 * 1.83787706640935 +_P->logdet() +xtrue* ( inv(_P->to_mat())*xtrue ) );
+	}
+};
+class EKF_unQful : public EKFfull , public BMcond {
+public:
+	//! Default constructor
+	EKF_unQful ( RV rx, RV ry,RV ru,RV rQ ) :EKFfull ( rx,ry,ru ),BMcond ( rQ ) {};
+	void condition ( const vec &Q0 ) {
+		Q=diag(Q0);
+	};
+	void bayes(const vec dt){
+	EKFfull::bayes(dt);
+		
+		vec xtrue(4);
+		//UGLY HACK!!! reliance on a predictor!!
+		xtrue(0)=x[0];
+		xtrue(1)=x[1];
+		xtrue(2)=x[2];
+		xtrue(3)=x[3];
+		
+		BM::ll = -0.5* ( 4 * 1.83787706640935 +log(det(P)) +xtrue* ( inv(P)*xtrue ) );
+	}
 };
 
@@ -82,5 +114,6 @@
         if (t>8.5) x[8]=25.2;  // 21A
         
-//        x[8]=0.0;
+        if (t>9) Ww=2.*M_PI*0;
+        x[8]=0.0;
 }
 
@@ -90,5 +123,5 @@
 	double h = 1e-6;
 	int Nsimstep = 125;
-	int Npart = 100;
+	int Npart = 50;
 	
 	//StrSim:06:
@@ -104,19 +137,19 @@
 	vec mu0= "0.0 0.0 0.0 0.0";
 //	vec Qdiag ( "0.01 0.01 0.01 0.0001" ); //zdenek: 0.01 0.01 0.0001 0.0001
-	vec Qdiag ( "18 18 157.9 0.0001" ); //zdenek: 0.01 0.01 0.0001 0.0001
-	vec Rdiag ( "90 90" ); //var(diff(xth)) = "0.034 0.034"
+	vec Qdiag ( "10 10 10 0.001" ); //zdenek: 0.01 0.01 0.0001 0.0001
+	vec Rdiag ( "100 100" ); //var(diff(xth)) = "0.034 0.034"
 	chmat Q ( Qdiag );
 	chmat R ( Rdiag );
 	EKFCh KFE ( rx,ry,ru );
-	KFE.set_est ( mu0, ( 1*eye ( 4 ) ) );
-	KFE.set_parameters ( &fxu,&hxu,Qdiag,Rdiag);
+	KFE.set_est ( mu0, chmat( 1*eye ( 4 ) ) );
+	KFE.set_parameters ( &fxu,&hxu,Q,R);
 
 	RV rQ ( "100","{Q}","4","0" );
-	EKF_unQ KFEp ( rx,ry,ru,rQ );
-	KFEp.set_est ( mu0, chmat ( 1*ones ( 4 ) ) );
-	KFEp.set_parameters ( &fxu,&hxu,Q,R );
+	EKF_unQful KFEp ( rx,ry,ru,rQ );
+	KFEp.set_est ( mu0,  1*ones ( 4 ) );
+	KFEp.set_parameters ( &fxu,&hxu,diag(Qdiag),diag(Rdiag) );
 
 	mgamma_fix evolQ ( rQ,rQ );
-	MPF<EKF_unQ> M ( rx,rQ,evolQ,evolQ,Npart,KFEp );
+	MPF<EKF_unQful> M ( rx,rQ,evolQ,evolQ,Npart,KFEp );
 	// initialize
 	evolQ.set_parameters ( 1000.0 ,Qdiag, 0.5); //sigma = 1/10 mu
@@ -124,5 +157,5 @@
 	epdf& pfinit=evolQ._epdf();
 	M.set_est ( pfinit );
-	evolQ.set_parameters ( 5000.0, Qdiag, 0.9999 );
+	evolQ.set_parameters ( 100000.0, Qdiag, 0.9999 );
 
 	//
@@ -166,5 +199,5 @@
 	}
 
-	it_file fou ( "pmsm_sim.it" );
+/*	it_file fou ( "pmsm_sim.it" );
 
 	fou << Name ( "xth" ) << Xt;
@@ -172,5 +205,5 @@
 	fou << Name ( "xthE" ) << XtE;
 	fou << Name ( "xthM" ) << XtM;
-	fou << Name ( "SSAT" ) << SSAT;
+	fou << Name ( "SSAT" ) << SSAT;*/
 	//Exit program:
 
Index: /tests/pmsm_sim3.cpp
===================================================================
--- /tests/pmsm_sim3.cpp (revision 62)
+++ /tests/pmsm_sim3.cpp (revision 63)
@@ -27,5 +27,5 @@
 void set_simulator_t(double &Ww) {
 
-        if (t>0.2) x[8]=1.2;    // 1A //0.2ZP
+        if (t>0.0002) x[8]=1.2;    // 1A //0.2ZP
         if (t>0.4) x[8]=10.8;   // 9A
         if (t>0.6) x[8]=25.2;  // 21A
@@ -73,5 +73,5 @@
         if (t>8.5) x[8]=25.2;  // 21A
         
-//        x[8]=0.0;
+        x[8]=0.0;
 }
 
@@ -81,9 +81,9 @@
 	double h = 1e-6;
 	int Nsimstep = 125;
-	int Npart = 500;
+	int Npart = 100;
 	
 
 	vec mu0= "0.0 0.0 0.0 0.0";
-	vec Qdiag ( "0.01 0.01 0.0001 0.0001" ); //zdenek: 0.01 0.01 0.0001 0.0001
+	vec Qdiag ( "0.05 0.05 0.002 0.001" ); //zdenek: 0.01 0.01 0.0001 0.0001
 	vec Rdiag ( "0.05 0.05" ); //var(diff(xth)) = "0.034 0.034"
 	chmat Q ( Qdiag );
@@ -99,9 +99,9 @@
 	MPF<EKFfixed> M ( rx,rQ,evolQ,evolQ,Npart,KFE );
 	// initialize
-	evolQ.set_parameters ( 1000.0 ,Qdiag, 0.5); //sigma = 1/10 mu
+	evolQ.set_parameters ( 10.0 ,Qdiag, 1.0); //sigma = 1/10 mu
 	evolQ.condition ( Qdiag ); //Zdenek default
 	epdf& pfinit=evolQ._epdf();
 	M.set_est ( pfinit );
-	evolQ.set_parameters ( 100000.0, Qdiag, 0.99999 );
+	evolQ.set_parameters ( 500000.0, Qdiag, 0.9999 );
 
 
@@ -111,5 +111,5 @@
 
 	mat Xt=zeros ( Ndat ,9 ); //true state from simulator
-	mat Dt=zeros ( Ndat,4+2 ); //observation
+	mat Dt=zeros ( Ndat,4 ); //observation
 	mat XtE=zeros ( Ndat, 4 );
 	mat XtM=zeros ( Ndat,4+4 ); //Q + x
@@ -118,4 +118,7 @@
 	pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
 	double Ww=0.0;
+	static int k_rampa=1;
+	static long k_rampa_tmp=0;
+
 	vec dt ( 2 );
 	vec ut ( 2 );
@@ -125,5 +128,12 @@
 		for ( int ii=0; ii<Nsimstep;ii++ ) {
 			//simulator
-			set_simulator_t(Ww);
+			Ww+=k_rampa*2.*M_PI*2e-4;    //1000Hz/s
+			if ( Ww>2.*M_PI*150. ) {
+				Ww=2.*M_PI*150.;
+				if ( k_rampa_tmp<500000 ) k_rampa_tmp++;
+				else {k_rampa=-1;k_rampa_tmp=0;}
+			};
+			if ( Ww<-2.*M_PI*150. ) Ww=-2.*M_PI*150.; /* */
+//			set_simulator_t(Ww);
 			pmsmsim_step ( Ww );
 		};
@@ -138,6 +148,7 @@
 		
 		Xt.set_row ( tK,vec ( x,9 ) ); //vec from C-array
-		Dt.set_row ( tK, concat ( dt,ut,vec_1(sqrt(pow(ut(0),2)+pow(ut(1),2))), vec_1(sqrt(pow(dt(0),2)+pow(dt(1),2))) ) );
+		Dt.set_row ( tK, concat ( dt,ut));
 		XtE.set_row ( tK,KFEep.mean() );
+		double qNaN = std::numeric_limits<double>::quiet_NaN();
 		XtM.set_row ( tK,Mep.mean() );
 	}
Index: /tests/pmsm.h
===================================================================
--- /tests/pmsm.h (revision 62)
+++ /tests/pmsm.h (revision 63)
@@ -12,8 +12,59 @@
 //! State evolution model for a PMSM drive and its derivative with respect to \$x\$
 class IMpmsm : public diffbifn {
+protected:
 	double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
 
 public:
 	IMpmsm() :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 );
+
+		vec xk=zeros ( 4 );
+		//ia
+		xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
+		//ib
+		xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
+		//om
+		xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
+		//th
+		xk ( 3 ) = rem(thm + omm*dt,2*pi); // <0..2pi>
+		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" );};
+
+};
+
+class IMpmsmStat : public IMpmsm {
+	IMpmsmStat() :IMpmsm() {};
 	//! 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;}
Index: /tests/pmsm_sim.cpp
===================================================================
--- /tests/pmsm_sim.cpp (revision 54)
+++ /tests/pmsm_sim.cpp (revision 63)
@@ -18,24 +18,4 @@
 #include "pmsm.h"
 #include "simulator.h"
-
-#include <netcdfcpp.h>
-void write_to_nc ( NcFile &nc, mat &X, std::string Xn, Array<std::string> A ) {
-	char tmpstr[200];
-	int Len = X.rows();
-
-	sprintf ( tmpstr,"%s.length",Xn.c_str() );
-	NcDim* lengt = nc.add_dim ( tmpstr, ( long ) Len );
-	for ( int j=0; j<X.cols(); j++ ) {
-		if ( j<A.length() )
-			sprintf ( tmpstr,"%s_%s",Xn.c_str(), ( A ( j ) ).c_str() );
-		else
-			sprintf ( tmpstr,"%s_%d",Xn.c_str(),j );
-		// Create variables and their attributes
-		NcVar* P = nc.add_var ( tmpstr, ncDouble, lengt );
-		const double* Dp = X._data();
-		P->put ( &Dp[j*Len],Len );
-	}
-}
-
 
 using namespace itpp;
@@ -146,13 +126,4 @@
 	//Exit program:
 
-	////////////////
-	// Just Testing
-	NcFile nc ( "pmsm_sim.nc", NcFile::Replace ); // Create and leave in define mode
-	if ( ! nc.is_valid() ) {	std::cerr << "creation of NCFile failed."<<endl;}
-
-	write_to_nc ( nc,Xt,"X","{isa isb om th }" );
-	write_to_nc ( nc,XtM,"XtM","{q1 q2 isa isb om th }" );
-	write_to_nc ( nc,XtE,"XE","{isa isb om th }" );
-	write_to_nc ( nc,Dt,"Dt","{isa isb ua ub }" );
 	return 0;
 }
