Index: /tests/pmsm.h
===================================================================
--- /tests/pmsm.h (revision 48)
+++ /tests/pmsm.h (revision 54)
@@ -1,4 +1,7 @@
 #ifndef PMSM_H
 #define PMSM_H
+
+#include <stat/libFN.h>
+
 
 //TODO hardcoded RVs!!!
Index: /tests/testbidiff.cpp
===================================================================
--- /tests/testbidiff.cpp (revision 54)
+++ /tests/testbidiff.cpp (revision 54)
@@ -0,0 +1,37 @@
+#include <itpp/itbase.h>
+#include "../bdm/stat/libBM.h"
+
+#include "pmsm.h"
+
+using namespace itpp;
+
+//These lines are needed for use of cout and endl
+using std::cout;
+using std::endl;
+
+int main()
+{
+	vec x0 = "100 120 100 2";
+	vec u0 = "1 1";
+	double h=1e-6;
+	
+	IMpmsm I;
+	I.set_parameters ( 0.28, 0.003465, h, 0.1989,   1.5 ,4.0, 0.04, 0.0 );
+	
+	vec x(x0);	x(0) += h;
+	cout << (I.eval(x,u0)-I.eval(x0,u0))/h <<endl;
+	x=x0;	x(1) += h;
+	cout << (I.eval(x,u0)-I.eval(x0,u0))/h <<endl;
+	x=x0;	x(2) += h;
+	cout << (I.eval(x,u0)-I.eval(x0,u0))/h <<endl;
+	x=x0;	x(3) += h;
+	cout << (I.eval(x,u0)-I.eval(x0,u0))/h <<endl;
+	cout << endl;
+	mat A(4,4);
+	I.dfdx_cond(x0,u0,A);
+	
+	cout <<A.T() << endl;
+	//Exit program:
+	return 0;
+
+}
Index: /tests/pmsm_sim.cpp
===================================================================
--- /tests/pmsm_sim.cpp (revision 48)
+++ /tests/pmsm_sim.cpp (revision 54)
@@ -19,4 +19,24 @@
 #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;
 //!Extended Kalman filter with unknown \c Q
@@ -29,5 +49,5 @@
 		//from EKF
 		preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );
-	}; 
+	};
 };
 
@@ -35,9 +55,9 @@
 int main() {
 	// Kalman filter
-	int Ndat = 10000;
+	int Ndat = 30000;
 	double h = 1e-6;
 	int Nsimstep = 125;
-	int Npart = 100;
-	
+	int Npart = 1000;
+
 	// internal model
 	IMpmsm fxu;
@@ -48,16 +68,16 @@
 
 	vec mu0= "0.0 0.0 0.0 0.0";
-	vec Qdiag ( "0.05 0.05 0.001 0.001" ); //zdenek: 0.05 0.05 0.001 0.001
-	vec Rdiag ( "0.03 0.03" ); //var(diff(xth)) = "0.034 0.034"
+	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 ( 1000*ones ( 4 ) ) );
+	KFE.set_est ( mu0, chmat ( 1*ones ( 4 ) ) );
 
-	RV rQ ( "100","{Q}","4","0" );
+	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 ( 1000*ones ( 4 ) ) );
+	KFEp.set_est ( mu0, chmat ( 1*ones ( 4 ) ) );
 
 	mgamma evolQ ( rQ,rQ );
@@ -65,8 +85,8 @@
 	// initialize
 	evolQ.set_parameters ( 100.0 ); //sigma = 1/10 mu
-	evolQ.condition ( "0.05 0.05 0.001 0.001" ); //Zdenek default
+	evolQ.condition ( "0.01 0.01" ); //Zdenek default
 	epdf& pfinit=evolQ._epdf();
 	M.set_est ( pfinit );
-	evolQ.set_parameters ( 1000.0 ); 
+	evolQ.set_parameters ( 1000.0 );
 
 	//
@@ -75,8 +95,8 @@
 	epdf& Mep = M._epdf();
 
-	mat Xt=zeros ( 9,Ndat ); //true state from simulator
-	mat Dt=zeros ( 4,Ndat ); //observation
-	mat XtE=zeros ( 4,Ndat );
-	mat XtM=zeros ( 8,Ndat ); //Q + x
+	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
@@ -86,10 +106,5 @@
 	static long k_rampa_tmp=0;
 	vec dt ( 2 );
-	vec dtVS =zeros( 2 );
-	vec xtVS =zeros(4);
-	vec et ( 4 );
-	vec wt ( 2 );
 	vec ut ( 2 );
-	mat XtV=zeros ( 4,Ndat );
 
 	for ( int tK=1;tK<Ndat;tK++ ) {
@@ -112,10 +127,4 @@
 		dt ( 0 ) = KalmanObs[2];
 		dt ( 1 ) = KalmanObs[3];
-		
-		// My own simulator for testing : Asuming ut is OK
-		NorRNG.sample_vector ( 4,et );
-		NorRNG.sample_vector ( 2,wt );
-		xtVS = fxu.eval ( xtVS,ut ) + Q.sqrt_mult ( et );
-		dtVS = hxu.eval ( xtVS,ut ) + R.sqrt_mult ( wt );
 
 		//estimator
@@ -123,9 +132,8 @@
 		M.bayes ( concat ( dt,ut ) );
 
-		Xt.set_col ( tK,vec ( x,9 ) ); //vec from C-array
-		Dt.set_col ( tK, concat ( dt,ut ) );
-		XtE.set_col ( tK,KFEep.mean() );
-		XtM.set_col ( tK,Mep.mean() );
-		XtV.set_col ( tK,xtVS );
+		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() );
 	}
 
@@ -136,7 +144,16 @@
 	fou << Name ( "xthE" ) << XtE;
 	fou << Name ( "xthM" ) << XtM;
-	fou << Name ( "xthV" ) << XtV;
 	//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;
+}
 
-}
Index: /tests/CMakeLists.txt
===================================================================
--- /tests/CMakeLists.txt (revision 42)
+++ /tests/CMakeLists.txt (revision 54)
@@ -40,6 +40,9 @@
 
 # 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)
 add_executable (pmsm_sim pmsm_sim.cpp)
-target_link_libraries (pmsm_sim ${BdmLibs} pmsmsim)
+target_link_libraries (pmsm_sim ${BdmLibs} pmsmsim netcdf_c++)
