Index: applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1341)
+++ applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (revision 1380)
@@ -3,5 +3,5 @@
 
 #include "ekf_obj.h"
-#include "../simulator.h"
+//#include "../simulator.h"
 
 double minQ(double Q){if (Q>1.0){ return 1.0;} else {return Q;};};
Index: applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (revision 1341)
+++ applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (revision 1380)
@@ -21,4 +21,5 @@
 #include "reference_Q15.h"
 #include "parametry_motoru.h"
+#include "mpf_double.h"
 
 using namespace bdm;
@@ -564,4 +565,34 @@
 
 
+class MPF_pmsm_red:public BM{
+
+
+public:
+		MPF_pmsm_red(){
+			dimy=2;
+			dimc=2;
+			mpf_init(1e-1, 1e-6, 1e-1);
+		};
+	void bayes ( const vec &val, const vec &cond ) {
+		const double &isa = val(0);
+		const double &isb = val(1);
+		const double &usa = cond(0);
+		const double &usb = cond(1);
+		mpf_bayes(isa,isb,usa,usb);
+	}
+	
+	class mp:public epdf{
+	public:
+		mp():epdf(){set_dim(N);}
+		vec sample() const {return zeros(N);}
+		double evallog(const vec &v) const {return 0.0;}
+		vec mean() const {vec tmp(N); mpf_th(tmp._data()); return tmp;}
+		vec variance() const {return zeros(N);}
+	};
+	
+	mp mypdf;
+	const mp& posterior() const {return mypdf;}
+};
+UIREGISTER(MPF_pmsm_red);
 
 
Index: applications/pmsm/simulator_zdenek/ekf_example/mpf_double.cpp
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/mpf_double.cpp (revision 1380)
+++ applications/pmsm/simulator_zdenek/ekf_example/mpf_double.cpp (revision 1380)
@@ -0,0 +1,229 @@
+#include "mpf_double.h"
+#include "math.h"
+#include <stdio.h>
+
+static double om[N];
+static double Pt[N];
+static double sth[N];
+static double cth[N];
+static double isdm[N];
+static double isqm[N];
+static double th[N];
+static double w[N];
+static double lwi[N];
+
+static double qth;
+static double qom;
+static double r;
+
+#define MAXrand 100
+#define MAXrandn 100
+static const double RArandu[MAXrand]={0.8147, 0.9058, 0.1270, 0.9134, 0.6324, 0.0975, 0.2785, 0.5469, 0.9575, 0.9649,
+                                      0.1576, 0.9706, 0.9572, 0.4854, 0.8003, 0.1419, 0.4218, 0.9157, 0.7922, 0.9595, 0.6557, 0.0357,
+                                      0.8491, 0.9340, 0.6787, 0.7577, 0.7431, 0.3922, 0.6555, 0.1712, 0.7060, 0.0318, 0.2769, 0.0462,
+                                      0.0971, 0.8235, 0.6948, 0.3171, 0.9502, 0.0344, 0.4387, 0.3816, 0.7655, 0.7952, 0.1869, 0.4898,
+                                      0.4456, 0.6463, 0.7094, 0.7547, 0.2760, 0.6797, 0.6551, 0.1626, 0.1190, 0.4984, 0.9597, 0.3404,
+                                      0.5853, 0.2238, 0.7513, 0.2551, 0.5060, 0.6991, 0.8909, 0.9593, 0.5472, 0.1386, 0.1493, 0.2575,
+                                      0.8407, 0.2543, 0.8143, 0.2435, 0.9293, 0.3500, 0.1966, 0.2511, 0.6160, 0.4733, 0.3517, 0.8308,
+                                      0.5853, 0.5497, 0.9172, 0.2858, 0.7572, 0.7537, 0.3804, 0.5678, 0.0759, 0.0540, 0.5308, 0.7792,
+                                      0.9340, 0.1299, 0.5688, 0.4694, 0.0119, 0.3371
+                                     };
+static const double *randu_i=&RArandu[0];
+const static double *randu_last=&RArandu[MAXrand-1];
+
+static const double RArandn[MAXrandn]={
+	0.6353, -0.6313, -1.0181,  0.4801,  0.4855,  0.9901, -0.5412, -1.0290,  1.0641, -0.0132,
+	-0.6014, -2.3252, -0.1821,  0.6682, -0.0050,  0.2189, -1.3335,  0.2431, -0.2454, -0.5803,
+	0.5512, -1.2316,  1.5210, -0.0783, -0.2762,  0.2617,  1.0727, -1.2566, -1.5175,  2.1363,
+	-1.0998,  1.0556, -0.0384,  0.8892,  1.2765,  1.2134, -0.7121, -0.3472,  0.0097, -0.2576,
+	0.0860, -0.1132,  1.2274,  2.3093,  1.8634, -0.2747, -0.0113, -0.9414,  0.0714, -1.4095,
+	-2.0046,  0.3792, -0.6962,  0.5246, -0.5226, -0.1331, -0.0008, -1.1746,  0.3165,  1.7701,
+	-0.4931,  0.9442,  0.0075, -0.0118,  0.1034, -1.2705, -0.2494, -1.0211,  0.4998,  0.3255,
+	0.4620, -2.1204, -0.7829,  0.9131, -0.8076, -1.6636,  0.3966, -0.4017,  1.2781, -1.1190,
+	-0.3210, -0.6447,  0.5869,  0.0559,  0.6804, -0.7036, -0.2640,  0.1737, -0.5478,  0.6204,
+	1.2366, -0.7043, -0.2512, -1.1071, -2.3646,  0.2809, -1.6640, -0.1161,  0.2608,  1.2698
+};
+static const double *randn_i=&RArandn[0];
+const static double *randn_last=&RArandn[MAXrandn-1];
+
+double randu() {
+	if ( randu_i==randu_last )
+		randu_i =&RArandu[0];
+	else
+		randu_i++;
+
+	return *randu_i;
+}
+
+double randn() {
+	if ( randn_i==randn_last )
+		randn_i =&RArandn[0];
+	else
+		randn_i++;
+
+	return *randn_i;
+}
+
+void resample() {
+	int N_babies[N];
+	double cumdist;
+	int i;
+	double ui;
+
+	double u0 = randu();
+
+	int j=0;
+	N_babies[0]=0;
+	cumdist = w[0];
+	for ( i = 0; i < N; i++ ) {
+		ui  = ( i + u0 ) / N;
+		while ( ui > cumdist ) {
+			j++;
+			N_babies[j]=0;
+			cumdist+=w[j];
+		}
+		N_babies [j] ++;
+	}
+	while (j++<=(N-1)) { // delete all N_babies after j
+		N_babies[j]=0; 
+	}
+
+	// COPY
+	int i_from=0;
+	int i_to=0;
+	while ( i_from<N ) {
+		while ( N_babies[i_from]>1 ) { // 1 baby stays where it is
+			
+			while ( N_babies[i_to]>=1 ) i_to++; // find first empty slot
+
+			//copy it
+			Pt[i_to]=Pt[i_from];
+			om[i_to]=om[i_from];
+			th[i_to]=th[i_from];
+			isdm[i_to]=isdm[i_from];
+			isqm[i_to]=isqm[i_from];
+
+			N_babies[i_to]++;
+			N_babies[i_from]--;
+		}
+		i_from++;
+	}
+}
+
+void mpf_bayes ( const double &isa, const double &isb , const double &usa, const double &usb ) {
+
+	double isd;
+	double isq;
+	double usd;
+	double usq;
+
+	double Cd;
+	double Cq;
+	double CC;
+	double oCC;
+
+	double difid;
+	double difiq;
+
+	double zeta;
+	double Kd;
+	double Kq;
+	double ro;
+	double ypd;
+	double ypq;
+	double detRy;
+	double ydiffd;
+	double ydiffq;
+	double ydC;
+
+	double maxlwi, sumlwi;
+	int i;
+// implementation starts here
+	for ( i=0; i<N; i++ ) {
+		Pt[i] = 1.0*1.0*Pt[i]+qom; // Pt is now predictive variance
+		th[i] =th[i] + om[i]*_dt+sqrt ( qth+_dt*_dt*Pt[i] ) *  randn() ;
+		while ( th[i]>M_PI ) th[i]=th[i]-2*M_PI;
+		while ( th[i]<-M_PI ) th[i]=th[i]+2*M_PI;
+
+		sth[i]=sin ( th[i] );
+		cth[i]=cos ( th[i] );
+
+		isd = cth[i]*isa+sth[i]*isb;
+		isq = -sth[i]*isa+cth[i]*isb;
+		usd = cth[i]*usa+sth[i]*usb;
+		usq = -sth[i]*usa+cth[i]*usb;
+
+		Cd = isq*_dt;
+		Cq = -_b - isd* _dt;
+
+		difid=isd- ( 1.0- ( _ad ) ) *isdm[i] - _cd *usd;
+		difiq=isq- ( 1.0- ( _aq ) ) *isqm[i] - _cq *usq;
+
+		CC=Cd*Cd+Cq*Cq;
+		zeta = Pt[i]/ ( r+Pt[i]*CC );
+		oCC = ( 1-zeta*CC );
+		ro = oCC/r;
+
+		Kd = Pt[i]*Cd*ro;
+		Kq = Pt[i]*Cq*ro;
+
+		Pt[i]=Pt[i]* ( 1- ( Kd*Cd+Kq*Cq ) );
+
+		ypd = Cd*om[i];
+		ypq = Cq*om[i];
+
+		detRy = ro/r;
+		om[i] = om[i] + Kd* ( difid - ypd ) +Kq* ( difiq-ypq );
+		ydiffd = ( ypd-difid );
+		ydiffq = ( ypq-difiq );
+		ydC = ydiffd*Cd + ydiffq*Cq;
+
+		lwi[i] = 0.5* ( log ( detRy ) + ( ydC*ydC*zeta -(ydiffd*ydiffd+ydiffq*ydiffq) ) /r ) ;
+		
+		isdm[i]=isd;
+		isqm[i]=isq;
+	}
+	maxlwi=-1e10;
+	for ( i=0;i<N;i++ ) {
+		if ( lwi[i]>maxlwi ) maxlwi=lwi[i];
+	}
+	for ( i=0;i<N;i++ ) lwi[i]-=maxlwi;
+	for ( i=0;i<N;i++ ) w[i]=exp ( lwi[i] ); // always resample -> wmin=1/n;
+
+	sumlwi=0.0;
+	for ( i=0;i<N;i++ ) sumlwi+=w[i];
+	for ( i=0;i<N;i++ ) w[i]/=sumlwi;
+	resample();
+}
+
+
+void mpf_init(double qom0, double qth0, double r0) {
+	r=r0;
+	qth=qth0;
+	qom=qom0;
+	int i;
+	for ( i=0; i<N; i++ ) {
+		th[i]=randu() *2*M_PI - M_PI;
+		om[i]=0;
+		Pt[i]=1;
+		isdm[i]=0;
+		isqm[i]=0;
+		w[i]=1.0/N;
+	}
+
+}
+void mpf_mean ( double Ecosth, double Esinth, double Eome ) {
+	int i;
+	Ecosth=0.0;
+	Esinth=0.0;
+	Eome=0.0;
+	for ( i=0;i<N;i++ ) {
+		Ecosth+=w[i]*cth[i];
+		Esinth+=w[i]*sth[i];
+		Eome+=w[i]*om[i];
+	}
+
+}
+void mpf_th(double th1[N]){
+	for (int i=0;i<N;i++) th1[i]=th[i];
+}
Index: applications/pmsm/simulator_zdenek/ekf_example/mpf_double.h
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/mpf_double.h (revision 1380)
+++ applications/pmsm/simulator_zdenek/ekf_example/mpf_double.h (revision 1380)
@@ -0,0 +1,22 @@
+// Bayesian Filtering for PMSM using Marginalized Particle Filter 
+
+#include "parametry_motoru.h"
+
+#define N 20
+
+#define Lsd Ls*1.1
+#define Lsq Ls
+#define _dt 0.000125
+#define _ad Rs*_dt/Lsd
+#define _aq Rs*_dt/Lsq
+#define _b  Fmag/Lsq*_dt
+#define _cd  _dt/Lsd
+#define _cq  _dt/Lsq
+
+double randn();
+double randu();
+
+void mpf_bayes ( const double &isa, const double &isb , const double &usa, const double &usb );
+void mpf_init(double qom0, double qth0, double r0);
+void mpf_mean(double Ecosth, double Esinth, double Eome);
+void mpf_th(double th1[N]);
Index: applications/pmsm/simulator_zdenek/ekf_example/CMakeLists.txt
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/CMakeLists.txt (revision 1174)
+++ applications/pmsm/simulator_zdenek/ekf_example/CMakeLists.txt (revision 1380)
@@ -2,3 +2,3 @@
 
 include_directories(../../bdm)
-add_library (ekf_obj ekf_obj.cpp fixed.cpp matrix.cpp matrix_vs.cpp)
+add_library (ekf_obj ekf_obj.cpp fixed.cpp matrix.cpp matrix_vs.cpp mpf_double.cpp)
