Index: /sts/pmsm_unkQpf.cpp
===================================================================
--- /tests/pmsm_unkQpf.cpp (revision 37)
+++  (revision )
@@ -1,116 +1,0 @@
-/*
-  \file
-  \brief Models for synchronous electric drive using IT++ and BDM
-  \author Vaclav Smidl.
-
-  -----------------------------------
-  BDM++ - C++ library for Bayesian Decision Making under Uncertainty
-
-  Using IT++ for numerical operations
-  -----------------------------------
-*/
-
-#include <itpp/itbase.h>
-#include <estim/libKF.h>
-#include <estim/libPF.h>
-#include <stat/libFN.h>
-
-#include "pmsm.h"
-
-using namespace itpp;
-
-//!Extended Kalman filter with unknown \c Q
-class EKF_unQ : public EKFCh , public BMcond {
-public:
-	//! Default constructor
-	EKF_unQ ( RV rx, RV ry,RV ru,RV rQ ) :EKFCh ( rx,ry,ru ),BMcond ( rQ ) {};
-	void condition ( const vec &Q0 ) {
-		Q.setD ( Q0,0 );
-		//from EKF
-		preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );
-	}; 
-};
-
-int main() {
-	// Kalman filter
-	int Ndat = 10000;
-
-//	cout << KF;
-	// internal model
-	IMpmsm fxu;
-	//                  Rs    Ls        dt       Fmag(Ypm) kp   p    J     Bf(Mz)
-	fxu.set_parameters ( 0.28, 0.003465, 20*1e-6, 0.1989,   1.5 ,4.0, 0.04, 0.0 );
-	// observation model
-	OMpmsm hxu;
-
-	vec mu0= "100 100 100 1";
-	vec Qdiag ( "0.1 0.1 0.01 0.00001" );
-	vec Rdiag ( "0.02 0.02" );
-	vec vQ = "0.01:0.01:100";
-
-	chmat Q ( Qdiag );
-	chmat R ( Rdiag );
-
-	RV rQ ( "100","{Q}","2","0" );
-	EKF_unQ KFE ( rx,ry,ru,rQ );
-	KFE.set_parameters ( &fxu,&hxu,Q,R );
-	KFE.set_est ( mu0, chmat ( 1000*ones ( 4 ) ) );
-
-	mgamma evolQ ( rQ,rQ );
-	//evolQ.set_parameters ( 10000.0 ); //sigma = 1/10 mu
-
-	MPF<EKF_unQ> M ( rx,rQ,evolQ,evolQ,100,KFE );
-
-	epdf& KFEep = KFE._epdf();
-	epdf& Mep = M._epdf();
-	// initialize
-	evolQ.set_parameters ( 1.0 ); //sigma = 1/10 mu
-	evolQ.condition ( "0.5 0.5" );
-	epdf& pfinit=evolQ._epdf();
-	M.set_est ( pfinit );
-	evolQ.set_parameters ( 1000.0 ); //sigma = 1/10 mu
-
-	//simulator values
-	vec dt ( 2 ); // output (isa isb)
-	vec wt ( 2 ); // noise on dt
-	vec ut ( 2 ); //
-	vec xt=mu0;   // initial state
-	vec et ( 4 ); // noise on xt
-
-	mat Xt=zeros ( 4,Ndat ); // True trajetory of xt
-	mat XtE=zeros ( 4,Ndat ); // Estimate of xt using EKF (known Q)
-	mat XtM=zeros ( 6,Ndat ); // Estimate of xt using EKF-MPF
-	Xt.set_col ( 0,mu0 );
-	XtM.set_col ( 0,Mep.mean() );
-
-	for ( int t=1;t<Ndat;t++ ) {
-		//simulator
-		// UniRNG.sample_vector ( 2,wt );
-
-		if ( rem ( t,500 ) <200 ) ut = rem ( t,500 ) *ones ( 2 );
-		else
-			ut=zeros ( 2 );
-
-		NorRNG.sample_vector ( 4,et );
-		NorRNG.sample_vector ( 2,wt );
-		xt = fxu.eval ( xt,ut ) + Q.sqrt_mult ( et );
-		dt = hxu.eval ( xt,ut ) + R.sqrt_mult ( wt );
-
-		//estimator
-		KFE.bayes ( concat ( dt,ut ) );
-		M.bayes ( concat ( dt,ut ) );
-
-		Xt.set_col ( t,xt );
-		XtE.set_col ( t,KFEep.mean() );
-		XtM.set_col ( t,Mep.mean() );
-	}
-
-	it_file fou ( "pmsm.it" );
-
-	fou << Name ( "xth" ) << Xt;
-	fou << Name ( "xthE" ) << XtE;
-	fou << Name ( "xthM" ) << XtM;
-	//Exit program:
-	return 0;
-
-}
Index: /sts/pmsm_sim2.cpp
===================================================================
--- /tests/pmsm_sim2.cpp (revision 63)
+++  (revision )
@@ -1,230 +1,0 @@
-/*
-  \file
-  \brief Models for synchronous electric drive using IT++ and BDM
-  \author Vaclav Smidl.
-
-  -----------------------------------
-  BDM++ - C++ library for Bayesian Decision Making under Uncertainty
-
-  Using IT++ for numerical operations
-  -----------------------------------
-*/
-
-#include <itpp/itbase.h>
-#include <estim/libKF.h>
-#include <estim/libPF.h>
-#include <stat/libFN.h>
-
-#include "pmsm.h"
-#include "simulator.h"
-
-#include "iopom.h"
-
-using namespace itpp;
-//!Extended Kalman filter with unknown \c Q
-class EKF_unQ : public EKFCh , public BMcond {
-public:
-	//! Default constructor
-	EKF_unQ ( RV rx, RV ry,RV ru,RV rQ ) :EKFCh ( rx,ry,ru ),BMcond ( rQ ) {};
-	void condition ( const vec &Q0 ) {
-		Q.setD ( Q0,0 );
-		//from EKF
-		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 ) );
-	}
-};
-
-void set_simulator_t(double &Ww) {
-
-        if (t>0.2) 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
-
-        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.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>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>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>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>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>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>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>9) Ww=2.*M_PI*0;
-        x[8]=0.0;
-}
-
-int main() {
-	// Kalman filter
-	int Ndat = 90000;
-	double h = 1e-6;
-	int Nsimstep = 125;
-	int Npart = 50;
-	
-	//StrSim:06:
-	vec SSAT(Ndat);
-
-	// internal model
-	IMpmsm 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 );
-	// observation model
-	OMpmsm hxu;
-
-	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 ( "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, chmat( 1*eye ( 4 ) ) );
-	KFE.set_parameters ( &fxu,&hxu,Q,R);
-
-	RV rQ ( "100","{Q}","4","0" );
-	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_unQful> M ( rx,rQ,evolQ,evolQ,Npart,KFEp );
-	// initialize
-	evolQ.set_parameters ( 1000.0 ,Qdiag, 0.5); //sigma = 1/10 mu
-	evolQ.condition ( Qdiag ); //Zdenek default
-	epdf& pfinit=evolQ._epdf();
-	M.set_est ( pfinit );
-	evolQ.set_parameters ( 100000.0, Qdiag, 0.9999 );
-
-	//
-
-	epdf& KFEep = KFE._epdf();
-	epdf& Mep = M._epdf();
-
-	mat Xt=zeros ( Ndat ,9 ); //true state from simulator
-	mat Dt=zeros ( Ndat,4+2 ); //observation
-	mat XtE=zeros ( Ndat, 4 );
-	mat XtM=zeros ( Ndat,4+4 ); //Q + x
-
-	// SET SIMULATOR
-	pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
-	double Ww=0.0;
-	vec dt ( 2 );
-	vec ut ( 2 );
-
-	for ( int tK=1;tK<Ndat;tK++ ) {
-		//Number of steps of a simulator for one step of Kalman
-		for ( int ii=0; ii<Nsimstep;ii++ ) {
-			//simulator
-			set_simulator_t(Ww);
-			pmsmsim_step ( Ww );
-		};
-		// collect data
-		ut ( 0 ) = KalmanObs[0];
-		ut ( 1 ) = KalmanObs[1];
-		dt ( 0 ) = KalmanObs[2];
-		dt ( 1 ) = KalmanObs[3];
-
-		//estimator
-		KFE.bayes ( concat ( dt,ut ) );
-		M.bayes ( concat ( dt,ut ) );
-		SSAT(tK) = M.SSAT;
-		
-		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))) ) );
-		XtE.set_row ( tK,KFEep.mean() );
-		XtM.set_row ( tK,Mep.mean() );
-	}
-
-/*	it_file fou ( "pmsm_sim.it" );
-
-	fou << Name ( "xth" ) << Xt;
-	fou << Name ( "Dt" ) << Dt;
-	fou << Name ( "xthE" ) << XtE;
-	fou << Name ( "xthM" ) << XtM;
-	fou << Name ( "SSAT" ) << SSAT;*/
-	//Exit program:
-
-	char tmpstr[200];
-	sprintf(tmpstr,"%s/%s","here/","format");
-	ofstream  form(tmpstr);
-	form << "# Experimetal header file"<< endl;
-	dirfile_write(form,"here/",Xt,"X","{isa isb om th }" );
-	dirfile_write ( form,"here",XtM,"XtM","{q1 q2 q3 q4 isa isb om th }" );
-	dirfile_write ( form,"here",XtE,"XE","{isa isb om th }" );
-	dirfile_write ( form,"here",Dt,"Dt","{isa isb ua ub }" );
-
-	////////////////
-	// Just Testing
-/*	NcFile nc ( "pmsm_sim2.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 q3 q4 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;
-}
-
Index: /sts/pmsm_sim3.cpp
===================================================================
--- /tests/pmsm_sim3.cpp (revision 63)
+++  (revision )
@@ -1,167 +1,0 @@
-/*
-  \file
-  \brief Models for synchronous electric drive using IT++ and BDM
-  \author Vaclav Smidl.
-
-  -----------------------------------
-  BDM++ - C++ library for Bayesian Decision Making under Uncertainty
-
-  Using IT++ for numerical operations
-  -----------------------------------
-*/
-
-#include <itpp/itbase.h>
-#include <estim/libKF.h>
-#include <estim/libPF.h>
-#include <stat/libFN.h>
-
-#include "pmsm.h"
-#include "simulator.h"
-
-#include "../simulator_zdenek/ekf_example/ekf_obj.h"
-#include "iopom.h"
-
-using namespace itpp;
-//!Extended Kalman filter with unknown \c Q
-
-void set_simulator_t(double &Ww) {
-
-        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
-
-        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.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>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>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>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>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>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>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
-        
-        x[8]=0.0;
-}
-
-int main() {
-	// Kalman filter
-	int Ndat = 90000;
-	double h = 1e-6;
-	int Nsimstep = 125;
-	int Npart = 100;
-	
-
-	vec mu0= "0.0 0.0 0.0 0.0";
-	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 );
-	chmat R ( Rdiag );
-	
-	RV rQ ( "100","{Q}","4","0" );
-	EKFfixed KFE ( rx, rQ);
-	KFE.init_ekf ( Nsimstep*h);
-
-
-///////////// Particles
-	mgamma_fix evolQ ( rQ,rQ );
-	MPF<EKFfixed> M ( rx,rQ,evolQ,evolQ,Npart,KFE );
-	// initialize
-	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 ( 500000.0, Qdiag, 0.9999 );
-
-
-	epdf& KFEep = KFE._epdf();
-	epdf& Mep = M._epdf();
-
-
-	mat Xt=zeros ( Ndat ,9 ); //true state from simulator
-	mat Dt=zeros ( Ndat,4 ); //observation
-	mat XtE=zeros ( Ndat, 4 );
-	mat XtM=zeros ( Ndat,4+4 ); //Q + x
-
-	// SET SIMULATOR
-	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 );
-
-	for ( int tK=1;tK<Ndat;tK++ ) {
-		//Number of steps of a simulator for one step of Kalman
-		for ( int ii=0; ii<Nsimstep;ii++ ) {
-			//simulator
-			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 );
-		};
-		// collect data
-		ut ( 0 ) = KalmanObs[0];
-		ut ( 1 ) = KalmanObs[1];
-		dt ( 0 ) = KalmanObs[2];
-		dt ( 1 ) = KalmanObs[3];
-		//estimator
-		KFE.bayes ( concat ( dt,ut ) );
-		M.bayes ( concat ( dt,ut ) );
-		
-		Xt.set_row ( tK,vec ( x,9 ) ); //vec from C-array
-		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() );
-	}
-
-	char tmpstr[200];
-	sprintf(tmpstr,"%s/%s","herez/","format");
-	ofstream  form(tmpstr);
-	form << "# Experimetal header file"<< endl;
-	dirfile_write(form,"herez/",Xt,"X","{isa isb om th }" );
-	dirfile_write ( form,"herez",XtM,"XtM","{q1 q2 q3 q4 isa isb om th }" );
-	dirfile_write ( form,"herez",XtE,"XE","{isa isb om th }" );
-	dirfile_write ( form,"herez",Dt,"Dt","{isa isb ua ub }" );
-
-	return 0;
-}
-
Index: /sts/pmsm_unkQ.cpp
===================================================================
--- /tests/pmsm_unkQ.cpp (revision 37)
+++  (revision )
@@ -1,107 +1,0 @@
-/*
-  \file
-  \brief Models for synchronous electric drive using IT++ and BDM
-  \author Vaclav Smidl.
-
-  -----------------------------------
-  BDM++ - C++ library for Bayesian Decision Making under Uncertainty
-
-  Using IT++ for numerical operations
-  -----------------------------------
-*/
-
-#include <itpp/itbase.h>
-#include <estim/libKF.h>
-#include <estim/libPF.h>
-#include <stat/libFN.h>
-
-#include "pmsm.h"
-
-using namespace itpp;
-/*
-// PMSM with Q on Ia and Ib given externally
-class EKF_unQ : public EKF<chmat> , public BMcond {
-public:
-	EKF_unQ( rx,ry,ru,rQ):EKF<chmat>(rx,ry,ru),BMcond(rQ){};
-	void condition(const vec &Q0){};
-};*/
-
-
-int main() {
-	// Kalman filter
-	int Ndat = 1000;
-
-//	cout << KF;
-	// internal model
-	IMpmsm fxu;
-	//                  Rs    Ls        dt       Fmag(Ypm) kp   p    J     Bf(Mz)
-	fxu.set_parameters ( 0.28, 0.003465, 20*1e-6, 0.1989,   1.5 ,4.0, 0.04, 0.0 );
-	// observation model
-	OMpmsm hxu;
-
-	vec mu0= "100 100 100 1";
-	vec Qdiag ( "0.1 0.1 0.01 0.01" );
-	vec Rdiag ( "0.02 0.02" );
-	vec vQ = "0.01:0.01:100";
-	chmat Q ( Qdiag );
-	chmat R ( Rdiag );
-	EKFCh KFE ( rx,ry,ru );
-	KFE.set_est ( mu0, chmat ( 1000*ones ( 4 ) ) );
-	KFE.set_parameters ( &fxu,&hxu,Q,R );
-
-	mat ll(100,Ndat);
-
-	EKFCh* kfArray[100];
-
-	for ( int i=0;i<100;i++ ) {
-		vec Qid ( Qdiag );
-		Qid ( 0 ) = vQ ( i ); Qid ( 1 ) = vQ ( i );
-		kfArray[i]= new EKFCh ( rx,ry,ru );
-		kfArray[i]->set_est ( mu0, chmat ( 1000*ones ( 4 ) ) );
-		kfArray[i]->set_parameters ( &fxu,&hxu,chmat ( Qid ),R );
-	}
-
-	epdf& KFEep = KFE._epdf();
-
-	//simulator values
-	vec dt ( 2 );
-	vec wt ( 2 );
-	vec ut ( 2 );
-	vec xt=mu0;
-	vec et ( 4 );
-
-	mat Xt=zeros ( 4,Ndat );
-	mat XtE=zeros ( 4,Ndat );
-	Xt.set_col ( 0,KFEep.mean() );
-
-	for ( int t=1;t<Ndat;t++ ) {
-		//simulator
-		UniRNG.sample_vector ( 2,wt );
-
-		if ( rem ( t,500 ) <200 ) ut = rem ( t,500 ) *ones ( 2 );
-		else
-			ut=zeros ( 2 );
-
-		NorRNG.sample_vector ( 4,et );
-		NorRNG.sample_vector ( 2,wt );
-		xt = fxu.eval ( xt,ut ) + Q.sqrt_mult ( et );
-		dt = hxu.eval ( xt,ut ) + R.sqrt_mult ( wt );
-
-		//estimator
-		KFE.bayes ( concat ( dt,ut ) );
-		for ( int i=0;i<100;i++ ) {kfArray[i]->bayes( concat ( dt,ut ) );ll(i,t)=ll(i,t-1) + kfArray[i]->_ll();
-		}
-		
-		Xt.set_col ( t,xt );
-		XtE.set_col ( t,KFEep.mean() );
-	}
-
-	it_file fou ( "pmsm.it" );
-
-	fou << Name ( "xth" ) << Xt;
-	fou << Name ( "xthE" ) << XtE;
-	fou << Name ( "ll" ) << ll;
-	//Exit program:
-	return 0;
-
-}
Index: /sts/pmsm.h
===================================================================
--- /tests/pmsm.h (revision 63)
+++  (revision )
@@ -1,136 +1,0 @@
-#ifndef PMSM_H
-#define PMSM_H
-
-#include <stat/libFN.h>
-
-
-//TODO hardcoded RVs!!!
-RV rx ( "1 2 3 4", "{ia, ib, om, th}", ones_i ( 4 ), zeros_i ( 4 ));
-RV ru ( "5 6", "{ua, ub}", ones_i ( 2 ) ,zeros_i ( 2 ));
-RV ry ( "7 8", "{oia, oib}", ones_i ( 2 ) ,zeros_i ( 2 ));
-
-//! 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;}
-
-	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 ) = 0.0;//kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
-		A ( 2,1 ) = 0.0;//kp*p*p * Ypm/J*dt* ( cos ( thm ) );
-		A ( 2,2 ) = 1.0;
-		A ( 2,3 ) = 0.0;//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" );};
-
-};
-
-//! Observation model for PMSM drive and its derivative with respect to \$x\$
-class OMpmsm: public diffbifn {
-public:
-	OMpmsm() :diffbifn (2, rx,ru ) {};
-
-	vec eval ( const vec &x0, const vec &u0 ) {
-		vec y ( 2 );
-		y ( 0 ) = x0 ( 0 );
-		y ( 1 ) = x0 ( 1 );
-		return y;
-	}
-
-	void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
-		A.clear();
-		A ( 0,0 ) = 1.0;
-		A ( 1,1 ) = 1.0;
-	}
-};
-
-#endif //PMSM_H
Index: /sts/pmsm_sim.cpp
===================================================================
--- /tests/pmsm_sim.cpp (revision 63)
+++  (revision )
@@ -1,130 +1,0 @@
-/*
-  \file
-  \brief Models for synchronous electric drive using IT++ and BDM
-  \author Vaclav Smidl.
-
-  -----------------------------------
-  BDM++ - C++ library for Bayesian Decision Making under Uncertainty
-
-  Using IT++ for numerical operations
-  -----------------------------------
-*/
-
-#include <itpp/itbase.h>
-#include <estim/libKF.h>
-#include <estim/libPF.h>
-#include <stat/libFN.h>
-
-#include "pmsm.h"
-#include "simulator.h"
-
-using namespace itpp;
-//!Extended Kalman filter with unknown \c Q
-class EKF_unQ : public EKFCh , public BMcond {
-public:
-	//! Default constructor
-	EKF_unQ ( RV rx, RV ry,RV ru,RV rQ ) :EKFCh ( rx,ry,ru ),BMcond ( rQ ) {};
-	void condition ( const vec &Q0 ) {
-		Q.setD ( Q0,0 );
-		//from EKF
-		preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );
-	};
-};
-
-
-int main() {
-	// Kalman filter
-	int Ndat = 30000;
-	double h = 1e-6;
-	int Nsimstep = 125;
-	int Npart = 1000;
-
-	// internal model
-	IMpmsm 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 );
-	// observation model
-	OMpmsm hxu;
-
-	vec mu0= "0.0 0.0 0.0 0.0";
-	vec Qdiag ( "0.01 0.01 0.00001 0.00001" ); //zdenek: 0.01 0.01 0.0001 0.0001
-	vec Rdiag ( "0.0005 0.0005" ); //var(diff(xth)) = "0.034 0.034"
-	chmat Q ( Qdiag );
-	chmat R ( Rdiag );
-	EKFCh KFE ( rx,ry,ru );
-	KFE.set_parameters ( &fxu,&hxu,Q,R );
-	KFE.set_est ( mu0, chmat ( 1*ones ( 4 ) ) );
-
-	RV rQ ( "100","{Q}","2","0" );
-	EKF_unQ KFEp ( rx,ry,ru,rQ );
-	KFEp.set_parameters ( &fxu,&hxu,Q,R );
-	KFEp.set_est ( mu0, chmat ( 1*ones ( 4 ) ) );
-
-	mgamma evolQ ( rQ,rQ );
-	MPF<EKF_unQ> M ( rx,rQ,evolQ,evolQ,Npart,KFEp );
-	// initialize
-	evolQ.set_parameters ( 100.0 ); //sigma = 1/10 mu
-	evolQ.condition ( "0.01 0.01" ); //Zdenek default
-	epdf& pfinit=evolQ._epdf();
-	M.set_est ( pfinit );
-	evolQ.set_parameters ( 1000.0 );
-
-	//
-
-	epdf& KFEep = KFE._epdf();
-	epdf& Mep = M._epdf();
-
-	mat Xt=zeros ( Ndat ,9 ); //true state from simulator
-	mat Dt=zeros ( Ndat,4+2 ); //observation
-	mat XtE=zeros ( Ndat, 4 );
-	mat XtM=zeros ( Ndat,6 ); //Q + x
-
-	// SET SIMULATOR
-	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 );
-
-	for ( int tK=1;tK<Ndat;tK++ ) {
-		//Number of steps of a simulator for one step of Kalman
-		for ( int ii=0; ii<Nsimstep;ii++ ) {
-			//simulator
-			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.; /* */
-
-			pmsmsim_step ( Ww );
-		};
-		// collect data
-		ut ( 0 ) = KalmanObs[0];
-		ut ( 1 ) = KalmanObs[1];
-		dt ( 0 ) = KalmanObs[2];
-		dt ( 1 ) = KalmanObs[3];
-
-		//estimator
-		KFE.bayes ( concat ( dt,ut ) );
-		M.bayes ( concat ( dt,ut ) );
-
-		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))) ) );
-		XtE.set_row ( tK,KFEep.mean() );
-		XtM.set_row ( tK,Mep.mean() );
-	}
-
-	it_file fou ( "pmsm_sim.it" );
-
-	fou << Name ( "xth" ) << Xt;
-	fou << Name ( "Dt" ) << Dt;
-	fou << Name ( "xthE" ) << XtE;
-	fou << Name ( "xthM" ) << XtM;
-	//Exit program:
-
-	return 0;
-}
-
Index: /sts/iopom.h
===================================================================
--- /tests/iopom.h (revision 65)
+++  (revision )
@@ -1,45 +1,0 @@
-//#include <netcdfcpp.h>
-#include <fstream.h>
-#include <fcntl.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 );
-// 	}
-// }
-
-
-void dirfile_write (std::ofstream &form, std::string Dir, mat &X, std::string Xn, Array<std::string> A ) {
-  char tmpstr[200];
-  char file[200];
-  const double* Dp=X._data();
-  int Len = X.rows();
-  
-	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 );
-		// save files == from kst-doc-datasource
-		sprintf(file,"%s/%s",Dir.c_str(),tmpstr);
-		
-		int fp;
-		fp = open(file, O_CREAT | O_WRONLY | O_TRUNC, 00644);
-		write(fp,&Dp[j*Len],Len*sizeof(double));
-		close(fp);
-  
-  		form << tmpstr << " RAW  d 1" << endl;
-	}
-}
Index: /pmsm/pmsm_unkQpf.cpp
===================================================================
--- /pmsm/pmsm_unkQpf.cpp (revision 72)
+++ /pmsm/pmsm_unkQpf.cpp (revision 72)
@@ -0,0 +1,116 @@
+/*
+  \file
+  \brief Models for synchronous electric drive using IT++ and BDM
+  \author Vaclav Smidl.
+
+  -----------------------------------
+  BDM++ - C++ library for Bayesian Decision Making under Uncertainty
+
+  Using IT++ for numerical operations
+  -----------------------------------
+*/
+
+#include <itpp/itbase.h>
+#include <estim/libKF.h>
+#include <estim/libPF.h>
+#include <stat/libFN.h>
+
+#include "pmsm.h"
+
+using namespace itpp;
+
+//!Extended Kalman filter with unknown \c Q
+class EKF_unQ : public EKFCh , public BMcond {
+public:
+	//! Default constructor
+	EKF_unQ ( RV rx, RV ry,RV ru,RV rQ ) :EKFCh ( rx,ry,ru ),BMcond ( rQ ) {};
+	void condition ( const vec &Q0 ) {
+		Q.setD ( Q0,0 );
+		//from EKF
+		preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );
+	}; 
+};
+
+int main() {
+	// Kalman filter
+	int Ndat = 10000;
+
+//	cout << KF;
+	// internal model
+	IMpmsm fxu;
+	//                  Rs    Ls        dt       Fmag(Ypm) kp   p    J     Bf(Mz)
+	fxu.set_parameters ( 0.28, 0.003465, 20*1e-6, 0.1989,   1.5 ,4.0, 0.04, 0.0 );
+	// observation model
+	OMpmsm hxu;
+
+	vec mu0= "100 100 100 1";
+	vec Qdiag ( "0.1 0.1 0.01 0.00001" );
+	vec Rdiag ( "0.02 0.02" );
+	vec vQ = "0.01:0.01:100";
+
+	chmat Q ( Qdiag );
+	chmat R ( Rdiag );
+
+	RV rQ ( "100","{Q}","2","0" );
+	EKF_unQ KFE ( rx,ry,ru,rQ );
+	KFE.set_parameters ( &fxu,&hxu,Q,R );
+	KFE.set_est ( mu0, chmat ( 1000*ones ( 4 ) ) );
+
+	mgamma evolQ ( rQ,rQ );
+	//evolQ.set_parameters ( 10000.0 ); //sigma = 1/10 mu
+
+	MPF<EKF_unQ> M ( rx,rQ,evolQ,evolQ,100,KFE );
+
+	epdf& KFEep = KFE._epdf();
+	epdf& Mep = M._epdf();
+	// initialize
+	evolQ.set_parameters ( 1.0 ); //sigma = 1/10 mu
+	evolQ.condition ( "0.5 0.5" );
+	epdf& pfinit=evolQ._epdf();
+	M.set_est ( pfinit );
+	evolQ.set_parameters ( 1000.0 ); //sigma = 1/10 mu
+
+	//simulator values
+	vec dt ( 2 ); // output (isa isb)
+	vec wt ( 2 ); // noise on dt
+	vec ut ( 2 ); //
+	vec xt=mu0;   // initial state
+	vec et ( 4 ); // noise on xt
+
+	mat Xt=zeros ( 4,Ndat ); // True trajetory of xt
+	mat XtE=zeros ( 4,Ndat ); // Estimate of xt using EKF (known Q)
+	mat XtM=zeros ( 6,Ndat ); // Estimate of xt using EKF-MPF
+	Xt.set_col ( 0,mu0 );
+	XtM.set_col ( 0,Mep.mean() );
+
+	for ( int t=1;t<Ndat;t++ ) {
+		//simulator
+		// UniRNG.sample_vector ( 2,wt );
+
+		if ( rem ( t,500 ) <200 ) ut = rem ( t,500 ) *ones ( 2 );
+		else
+			ut=zeros ( 2 );
+
+		NorRNG.sample_vector ( 4,et );
+		NorRNG.sample_vector ( 2,wt );
+		xt = fxu.eval ( xt,ut ) + Q.sqrt_mult ( et );
+		dt = hxu.eval ( xt,ut ) + R.sqrt_mult ( wt );
+
+		//estimator
+		KFE.bayes ( concat ( dt,ut ) );
+		M.bayes ( concat ( dt,ut ) );
+
+		Xt.set_col ( t,xt );
+		XtE.set_col ( t,KFEep.mean() );
+		XtM.set_col ( t,Mep.mean() );
+	}
+
+	it_file fou ( "pmsm.it" );
+
+	fou << Name ( "xth" ) << Xt;
+	fou << Name ( "xthE" ) << XtE;
+	fou << Name ( "xthM" ) << XtM;
+	//Exit program:
+	return 0;
+
+}
Index: /pmsm/pmsm.h
===================================================================
--- /pmsm/pmsm.h (revision 72)
+++ /pmsm/pmsm.h (revision 72)
@@ -0,0 +1,136 @@
+#ifndef PMSM_H
+#define PMSM_H
+
+#include <stat/libFN.h>
+
+
+//TODO hardcoded RVs!!!
+RV rx ( "1 2 3 4", "{ia, ib, om, th}", ones_i ( 4 ), zeros_i ( 4 ));
+RV ru ( "5 6", "{ua, ub}", ones_i ( 2 ) ,zeros_i ( 2 ));
+RV ry ( "7 8", "{oia, oib}", ones_i ( 2 ) ,zeros_i ( 2 ));
+
+//! 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;}
+
+	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 ) = 0.0;//kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
+		A ( 2,1 ) = 0.0;//kp*p*p * Ypm/J*dt* ( cos ( thm ) );
+		A ( 2,2 ) = 1.0;
+		A ( 2,3 ) = 0.0;//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" );};
+
+};
+
+//! Observation model for PMSM drive and its derivative with respect to \$x\$
+class OMpmsm: public diffbifn {
+public:
+	OMpmsm() :diffbifn (2, rx,ru ) {};
+
+	vec eval ( const vec &x0, const vec &u0 ) {
+		vec y ( 2 );
+		y ( 0 ) = x0 ( 0 );
+		y ( 1 ) = x0 ( 1 );
+		return y;
+	}
+
+	void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
+		A.clear();
+		A ( 0,0 ) = 1.0;
+		A ( 1,1 ) = 1.0;
+	}
+};
+
+#endif //PMSM_H
Index: /pmsm/pmsm_sim.cpp
===================================================================
--- /pmsm/pmsm_sim.cpp (revision 72)
+++ /pmsm/pmsm_sim.cpp (revision 72)
@@ -0,0 +1,130 @@
+/*
+  \file
+  \brief Models for synchronous electric drive using IT++ and BDM
+  \author Vaclav Smidl.
+
+  -----------------------------------
+  BDM++ - C++ library for Bayesian Decision Making under Uncertainty
+
+  Using IT++ for numerical operations
+  -----------------------------------
+*/
+
+#include <itpp/itbase.h>
+#include <estim/libKF.h>
+#include <estim/libPF.h>
+#include <stat/libFN.h>
+
+#include "pmsm.h"
+#include "simulator.h"
+
+using namespace itpp;
+//!Extended Kalman filter with unknown \c Q
+class EKF_unQ : public EKFCh , public BMcond {
+public:
+	//! Default constructor
+	EKF_unQ ( RV rx, RV ry,RV ru,RV rQ ) :EKFCh ( rx,ry,ru ),BMcond ( rQ ) {};
+	void condition ( const vec &Q0 ) {
+		Q.setD ( Q0,0 );
+		//from EKF
+		preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );
+	};
+};
+
+
+int main() {
+	// Kalman filter
+	int Ndat = 30000;
+	double h = 1e-6;
+	int Nsimstep = 125;
+	int Npart = 1000;
+
+	// internal model
+	IMpmsm 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 );
+	// observation model
+	OMpmsm hxu;
+
+	vec mu0= "0.0 0.0 0.0 0.0";
+	vec Qdiag ( "0.01 0.01 0.00001 0.00001" ); //zdenek: 0.01 0.01 0.0001 0.0001
+	vec Rdiag ( "0.0005 0.0005" ); //var(diff(xth)) = "0.034 0.034"
+	chmat Q ( Qdiag );
+	chmat R ( Rdiag );
+	EKFCh KFE ( rx,ry,ru );
+	KFE.set_parameters ( &fxu,&hxu,Q,R );
+	KFE.set_est ( mu0, chmat ( 1*ones ( 4 ) ) );
+
+	RV rQ ( "100","{Q}","2","0" );
+	EKF_unQ KFEp ( rx,ry,ru,rQ );
+	KFEp.set_parameters ( &fxu,&hxu,Q,R );
+	KFEp.set_est ( mu0, chmat ( 1*ones ( 4 ) ) );
+
+	mgamma evolQ ( rQ,rQ );
+	MPF<EKF_unQ> M ( rx,rQ,evolQ,evolQ,Npart,KFEp );
+	// initialize
+	evolQ.set_parameters ( 100.0 ); //sigma = 1/10 mu
+	evolQ.condition ( "0.01 0.01" ); //Zdenek default
+	epdf& pfinit=evolQ._epdf();
+	M.set_est ( pfinit );
+	evolQ.set_parameters ( 1000.0 );
+
+	//
+
+	epdf& KFEep = KFE._epdf();
+	epdf& Mep = M._epdf();
+
+	mat Xt=zeros ( Ndat ,9 ); //true state from simulator
+	mat Dt=zeros ( Ndat,4+2 ); //observation
+	mat XtE=zeros ( Ndat, 4 );
+	mat XtM=zeros ( Ndat,6 ); //Q + x
+
+	// SET SIMULATOR
+	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 );
+
+	for ( int tK=1;tK<Ndat;tK++ ) {
+		//Number of steps of a simulator for one step of Kalman
+		for ( int ii=0; ii<Nsimstep;ii++ ) {
+			//simulator
+			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.; /* */
+
+			pmsmsim_step ( Ww );
+		};
+		// collect data
+		ut ( 0 ) = KalmanObs[0];
+		ut ( 1 ) = KalmanObs[1];
+		dt ( 0 ) = KalmanObs[2];
+		dt ( 1 ) = KalmanObs[3];
+
+		//estimator
+		KFE.bayes ( concat ( dt,ut ) );
+		M.bayes ( concat ( dt,ut ) );
+
+		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))) ) );
+		XtE.set_row ( tK,KFEep.mean() );
+		XtM.set_row ( tK,Mep.mean() );
+	}
+
+	it_file fou ( "pmsm_sim.it" );
+
+	fou << Name ( "xth" ) << Xt;
+	fou << Name ( "Dt" ) << Dt;
+	fou << Name ( "xthE" ) << XtE;
+	fou << Name ( "xthM" ) << XtM;
+	//Exit program:
+
+	return 0;
+}
+
Index: /pmsm/pmsm_sim2.cpp
===================================================================
--- /pmsm/pmsm_sim2.cpp (revision 72)
+++ /pmsm/pmsm_sim2.cpp (revision 72)
@@ -0,0 +1,230 @@
+/*
+  \file
+  \brief Models for synchronous electric drive using IT++ and BDM
+  \author Vaclav Smidl.
+
+  -----------------------------------
+  BDM++ - C++ library for Bayesian Decision Making under Uncertainty
+
+  Using IT++ for numerical operations
+  -----------------------------------
+*/
+
+#include <itpp/itbase.h>
+#include <estim/libKF.h>
+#include <estim/libPF.h>
+#include <stat/libFN.h>
+
+#include "pmsm.h"
+#include "simulator.h"
+
+#include "iopom.h"
+
+using namespace itpp;
+//!Extended Kalman filter with unknown \c Q
+class EKF_unQ : public EKFCh , public BMcond {
+public:
+	//! Default constructor
+	EKF_unQ ( RV rx, RV ry,RV ru,RV rQ ) :EKFCh ( rx,ry,ru ),BMcond ( rQ ) {};
+	void condition ( const vec &Q0 ) {
+		Q.setD ( Q0,0 );
+		//from EKF
+		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 ) );
+	}
+};
+
+void set_simulator_t(double &Ww) {
+
+        if (t>0.2) 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
+
+        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.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>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>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>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>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>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>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>9) Ww=2.*M_PI*0;
+        x[8]=0.0;
+}
+
+int main() {
+	// Kalman filter
+	int Ndat = 90000;
+	double h = 1e-6;
+	int Nsimstep = 125;
+	int Npart = 50;
+	
+	//StrSim:06:
+	vec SSAT(Ndat);
+
+	// internal model
+	IMpmsm 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 );
+	// observation model
+	OMpmsm hxu;
+
+	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 ( "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, chmat( 1*eye ( 4 ) ) );
+	KFE.set_parameters ( &fxu,&hxu,Q,R);
+
+	RV rQ ( "100","{Q}","4","0" );
+	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_unQful> M ( rx,rQ,evolQ,evolQ,Npart,KFEp );
+	// initialize
+	evolQ.set_parameters ( 1000.0 ,Qdiag, 0.5); //sigma = 1/10 mu
+	evolQ.condition ( Qdiag ); //Zdenek default
+	epdf& pfinit=evolQ._epdf();
+	M.set_est ( pfinit );
+	evolQ.set_parameters ( 100000.0, Qdiag, 0.9999 );
+
+	//
+
+	epdf& KFEep = KFE._epdf();
+	epdf& Mep = M._epdf();
+
+	mat Xt=zeros ( Ndat ,9 ); //true state from simulator
+	mat Dt=zeros ( Ndat,4+2 ); //observation
+	mat XtE=zeros ( Ndat, 4 );
+	mat XtM=zeros ( Ndat,4+4 ); //Q + x
+
+	// SET SIMULATOR
+	pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
+	double Ww=0.0;
+	vec dt ( 2 );
+	vec ut ( 2 );
+
+	for ( int tK=1;tK<Ndat;tK++ ) {
+		//Number of steps of a simulator for one step of Kalman
+		for ( int ii=0; ii<Nsimstep;ii++ ) {
+			//simulator
+			set_simulator_t(Ww);
+			pmsmsim_step ( Ww );
+		};
+		// collect data
+		ut ( 0 ) = KalmanObs[0];
+		ut ( 1 ) = KalmanObs[1];
+		dt ( 0 ) = KalmanObs[2];
+		dt ( 1 ) = KalmanObs[3];
+
+		//estimator
+		KFE.bayes ( concat ( dt,ut ) );
+		M.bayes ( concat ( dt,ut ) );
+		SSAT(tK) = M.SSAT;
+		
+		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))) ) );
+		XtE.set_row ( tK,KFEep.mean() );
+		XtM.set_row ( tK,Mep.mean() );
+	}
+
+/*	it_file fou ( "pmsm_sim.it" );
+
+	fou << Name ( "xth" ) << Xt;
+	fou << Name ( "Dt" ) << Dt;
+	fou << Name ( "xthE" ) << XtE;
+	fou << Name ( "xthM" ) << XtM;
+	fou << Name ( "SSAT" ) << SSAT;*/
+	//Exit program:
+
+	char tmpstr[200];
+	sprintf(tmpstr,"%s/%s","here/","format");
+	ofstream  form(tmpstr);
+	form << "# Experimetal header file"<< endl;
+	dirfile_write(form,"here/",Xt,"X","{isa isb om th }" );
+	dirfile_write ( form,"here",XtM,"XtM","{q1 q2 q3 q4 isa isb om th }" );
+	dirfile_write ( form,"here",XtE,"XE","{isa isb om th }" );
+	dirfile_write ( form,"here",Dt,"Dt","{isa isb ua ub }" );
+
+	////////////////
+	// Just Testing
+/*	NcFile nc ( "pmsm_sim2.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 q3 q4 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;
+}
+
Index: /pmsm/pmsm_sim3.cpp
===================================================================
--- /pmsm/pmsm_sim3.cpp (revision 72)
+++ /pmsm/pmsm_sim3.cpp (revision 72)
@@ -0,0 +1,167 @@
+/*
+  \file
+  \brief Models for synchronous electric drive using IT++ and BDM
+  \author Vaclav Smidl.
+
+  -----------------------------------
+  BDM++ - C++ library for Bayesian Decision Making under Uncertainty
+
+  Using IT++ for numerical operations
+  -----------------------------------
+*/
+
+#include <itpp/itbase.h>
+#include <estim/libKF.h>
+#include <estim/libPF.h>
+#include <stat/libFN.h>
+
+#include "pmsm.h"
+#include "simulator.h"
+
+#include "../simulator_zdenek/ekf_example/ekf_obj.h"
+#include "iopom.h"
+
+using namespace itpp;
+//!Extended Kalman filter with unknown \c Q
+
+void set_simulator_t(double &Ww) {
+
+        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
+
+        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.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>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>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>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>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>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>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
+        
+        x[8]=0.0;
+}
+
+int main() {
+	// Kalman filter
+	int Ndat = 90000;
+	double h = 1e-6;
+	int Nsimstep = 125;
+	int Npart = 100;
+	
+
+	vec mu0= "0.0 0.0 0.0 0.0";
+	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 );
+	chmat R ( Rdiag );
+	
+	RV rQ ( "100","{Q}","4","0" );
+	EKFfixed KFE ( rx, rQ);
+	KFE.init_ekf ( Nsimstep*h);
+
+
+///////////// Particles
+	mgamma_fix evolQ ( rQ,rQ );
+	MPF<EKFfixed> M ( rx,rQ,evolQ,evolQ,Npart,KFE );
+	// initialize
+	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 ( 500000.0, Qdiag, 0.9999 );
+
+
+	epdf& KFEep = KFE._epdf();
+	epdf& Mep = M._epdf();
+
+
+	mat Xt=zeros ( Ndat ,9 ); //true state from simulator
+	mat Dt=zeros ( Ndat,4 ); //observation
+	mat XtE=zeros ( Ndat, 4 );
+	mat XtM=zeros ( Ndat,4+4 ); //Q + x
+
+	// SET SIMULATOR
+	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 );
+
+	for ( int tK=1;tK<Ndat;tK++ ) {
+		//Number of steps of a simulator for one step of Kalman
+		for ( int ii=0; ii<Nsimstep;ii++ ) {
+			//simulator
+			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 );
+		};
+		// collect data
+		ut ( 0 ) = KalmanObs[0];
+		ut ( 1 ) = KalmanObs[1];
+		dt ( 0 ) = KalmanObs[2];
+		dt ( 1 ) = KalmanObs[3];
+		//estimator
+		KFE.bayes ( concat ( dt,ut ) );
+		M.bayes ( concat ( dt,ut ) );
+		
+		Xt.set_row ( tK,vec ( x,9 ) ); //vec from C-array
+		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() );
+	}
+
+	char tmpstr[200];
+	sprintf(tmpstr,"%s/%s","herez/","format");
+	ofstream  form(tmpstr);
+	form << "# Experimetal header file"<< endl;
+	dirfile_write(form,"herez/",Xt,"X","{isa isb om th }" );
+	dirfile_write ( form,"herez",XtM,"XtM","{q1 q2 q3 q4 isa isb om th }" );
+	dirfile_write ( form,"herez",XtE,"XE","{isa isb om th }" );
+	dirfile_write ( form,"herez",Dt,"Dt","{isa isb ua ub }" );
+
+	return 0;
+}
+
Index: /pmsm/iopom.h
===================================================================
--- /pmsm/iopom.h (revision 72)
+++ /pmsm/iopom.h (revision 72)
@@ -0,0 +1,45 @@
+//#include <netcdfcpp.h>
+#include <fstream.h>
+#include <fcntl.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 );
+// 	}
+// }
+
+
+void dirfile_write (std::ofstream &form, std::string Dir, mat &X, std::string Xn, Array<std::string> A ) {
+  char tmpstr[200];
+  char file[200];
+  const double* Dp=X._data();
+  int Len = X.rows();
+  
+	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 );
+		// save files == from kst-doc-datasource
+		sprintf(file,"%s/%s",Dir.c_str(),tmpstr);
+		
+		int fp;
+		fp = open(file, O_CREAT | O_WRONLY | O_TRUNC, 00644);
+		write(fp,&Dp[j*Len],Len*sizeof(double));
+		close(fp);
+  
+  		form << tmpstr << " RAW  d 1" << endl;
+	}
+}
Index: /pmsm/pmsm_unkQ.cpp
===================================================================
--- /pmsm/pmsm_unkQ.cpp (revision 72)
+++ /pmsm/pmsm_unkQ.cpp (revision 72)
@@ -0,0 +1,107 @@
+/*
+  \file
+  \brief Models for synchronous electric drive using IT++ and BDM
+  \author Vaclav Smidl.
+
+  -----------------------------------
+  BDM++ - C++ library for Bayesian Decision Making under Uncertainty
+
+  Using IT++ for numerical operations
+  -----------------------------------
+*/
+
+#include <itpp/itbase.h>
+#include <estim/libKF.h>
+#include <estim/libPF.h>
+#include <stat/libFN.h>
+
+#include "pmsm.h"
+
+using namespace itpp;
+/*
+// PMSM with Q on Ia and Ib given externally
+class EKF_unQ : public EKF<chmat> , public BMcond {
+public:
+	EKF_unQ( rx,ry,ru,rQ):EKF<chmat>(rx,ry,ru),BMcond(rQ){};
+	void condition(const vec &Q0){};
+};*/
+
+
+int main() {
+	// Kalman filter
+	int Ndat = 1000;
+
+//	cout << KF;
+	// internal model
+	IMpmsm fxu;
+	//                  Rs    Ls        dt       Fmag(Ypm) kp   p    J     Bf(Mz)
+	fxu.set_parameters ( 0.28, 0.003465, 20*1e-6, 0.1989,   1.5 ,4.0, 0.04, 0.0 );
+	// observation model
+	OMpmsm hxu;
+
+	vec mu0= "100 100 100 1";
+	vec Qdiag ( "0.1 0.1 0.01 0.01" );
+	vec Rdiag ( "0.02 0.02" );
+	vec vQ = "0.01:0.01:100";
+	chmat Q ( Qdiag );
+	chmat R ( Rdiag );
+	EKFCh KFE ( rx,ry,ru );
+	KFE.set_est ( mu0, chmat ( 1000*ones ( 4 ) ) );
+	KFE.set_parameters ( &fxu,&hxu,Q,R );
+
+	mat ll(100,Ndat);
+
+	EKFCh* kfArray[100];
+
+	for ( int i=0;i<100;i++ ) {
+		vec Qid ( Qdiag );
+		Qid ( 0 ) = vQ ( i ); Qid ( 1 ) = vQ ( i );
+		kfArray[i]= new EKFCh ( rx,ry,ru );
+		kfArray[i]->set_est ( mu0, chmat ( 1000*ones ( 4 ) ) );
+		kfArray[i]->set_parameters ( &fxu,&hxu,chmat ( Qid ),R );
+	}
+
+	epdf& KFEep = KFE._epdf();
+
+	//simulator values
+	vec dt ( 2 );
+	vec wt ( 2 );
+	vec ut ( 2 );
+	vec xt=mu0;
+	vec et ( 4 );
+
+	mat Xt=zeros ( 4,Ndat );
+	mat XtE=zeros ( 4,Ndat );
+	Xt.set_col ( 0,KFEep.mean() );
+
+	for ( int t=1;t<Ndat;t++ ) {
+		//simulator
+		UniRNG.sample_vector ( 2,wt );
+
+		if ( rem ( t,500 ) <200 ) ut = rem ( t,500 ) *ones ( 2 );
+		else
+			ut=zeros ( 2 );
+
+		NorRNG.sample_vector ( 4,et );
+		NorRNG.sample_vector ( 2,wt );
+		xt = fxu.eval ( xt,ut ) + Q.sqrt_mult ( et );
+		dt = hxu.eval ( xt,ut ) + R.sqrt_mult ( wt );
+
+		//estimator
+		KFE.bayes ( concat ( dt,ut ) );
+		for ( int i=0;i<100;i++ ) {kfArray[i]->bayes( concat ( dt,ut ) );ll(i,t)=ll(i,t-1) + kfArray[i]->_ll();
+		}
+		
+		Xt.set_col ( t,xt );
+		XtE.set_col ( t,KFEep.mean() );
+	}
+
+	it_file fou ( "pmsm.it" );
+
+	fou << Name ( "xth" ) << Xt;
+	fou << Name ( "xthE" ) << XtE;
+	fou << Name ( "ll" ) << ll;
+	//Exit program:
+	return 0;
+
+}
Index: /pmsm/CMakeLists.txt
===================================================================
--- /pmsm/CMakeLists.txt (revision 72)
+++ /pmsm/CMakeLists.txt (revision 72)
@@ -0,0 +1,53 @@
+# Make sure the compiler can find include files from our Bdm library.
+include_directories (${BDM_SOURCE_DIR}/bdm)
+
+# Make sure the linker can find the Hello library once it is built.
+link_directories (${BDM_BINARY_DIR}/bdm)
+
+## Save all needed libraries in variable BdmLibs
+#SET(BdmLibs bdm itpp_debug)
+SET(BdmLibs bdm itpp)
+
+IF(WIN32)
+	SET(BdmLibs ${BdmLibs} libacml_dll)
+ENDIF(WIN32)
+
+# Add executable called "helloDemo" that is built from the source files
+# "demo.cxx" and "demo_b.cxx".  The extensions are automatically found.
+add_executable (test0 test0.cpp)
+add_executable (testKF testKF.cpp)
+add_executable (testPF testPF.cpp)
+add_executable (testSmp testSmp.cpp)
+add_executable (testEpdf testEpdf.cpp)
+add_executable (testResample testResample.cpp)
+add_executable (testKF_QR testKF_QR.cpp)
+add_executable (testKF_QRexh testKF_QRexh.cpp)
+add_executable (pmsm_unkQ pmsm_unkQ.cpp)
+add_executable (pmsm_unkQpf pmsm_unkQpf.cpp)
+
+
+# Link the executable to the Hello library.
+target_link_libraries (test0 ${BdmLibs})
+target_link_libraries (testKF ${BdmLibs})
+target_link_libraries (testPF ${BdmLibs})
+target_link_libraries (testSmp ${BdmLibs})
+target_link_libraries (testEpdf ${BdmLibs})
+target_link_libraries (testResample ${BdmLibs})
+target_link_libraries (testKF_QR ${BdmLibs})
+target_link_libraries (testKF_QRexh ${BdmLibs})
+target_link_libraries (pmsm_unkQ ${BdmLibs})
+target_link_libraries (pmsm_unkQpf ${BdmLibs})
+
+# PMSM stuff
+add_executable (testbidiff testbidiff.cpp)
+target_link_libraries (testbidiff ${BdmLibs})
+
+include_directories (${BDM_SOURCE_DIR}/simulator_zdenek)
+link_directories (${BDM_BINARY_DIR}/simulator_zdenek)
+link_directories (${BDM_BINARY_DIR}/simulator_zdenek/ekf_example)
+add_executable (pmsm_sim pmsm_sim.cpp)
+add_executable (pmsm_sim2 pmsm_sim2.cpp)
+add_executable (pmsm_sim3 pmsm_sim3.cpp)
+target_link_libraries (pmsm_sim ${BdmLibs} pmsmsim )
+target_link_libraries (pmsm_sim2 ${BdmLibs} pmsmsim )
+target_link_libraries (pmsm_sim3 ${BdmLibs} pmsmsim ekf_obj)
