Index: applications/pmsm/simulator_zdenek/ekf_example/mpf_double.cpp
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/mpf_double.cpp (revision 1438)
+++ applications/pmsm/simulator_zdenek/ekf_example/mpf_double.cpp (revision 1439)
@@ -2,22 +2,24 @@
 #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;
+//#include <libconfig/lib/grammar.y>
+
+static floatx om[N];
+static floatx Pt[N];
+static floatx sth[N];
+static floatx cth[N];
+static floatx isdm[N];
+static floatx isqm[N];
+static floatx th[N];
+static floatx w[N];
+static floatx lwi[N];
+
+static floatx qth;
+static floatx qom;
+static floatx 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,
+static const floatx 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,
@@ -29,8 +31,8 @@
                                       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]={
+static const floatx *randu_i=&RArandu[0];
+const static floatx *randu_last=&RArandu[MAXrand-1];
+
+static const floatx 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,
@@ -44,8 +46,8 @@
 	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() {
+static const floatx *randn_i=&RArandn[0];
+const static floatx *randn_last=&RArandn[MAXrandn-1];
+
+floatx qrandu() {
 	if ( randu_i==randu_last )
 		randu_i =&RArandu[0];
@@ -56,5 +58,5 @@
 }
 
-double randn() {
+floatx qrandn() {
 	if ( randn_i==randn_last )
 		randn_i =&RArandn[0];
@@ -67,9 +69,11 @@
 void resample() {
 	int N_babies[N];
-	double cumdist;
-	int i;
-	double ui;
-
-	double u0 = randu();
+	floatx cumdist;
+	int i;
+	floatx ui;
+	int i_from=0;
+	int i_to=0;
+
+	floatx u0 = qrandu();
 
 	int j=0;
@@ -90,6 +94,5 @@
 
 	// 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
@@ -111,100 +114,142 @@
 }
 
-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;
+void mpf_bayes ( const floatx isa, const floatx isb , const floatx usa, const floatx usb ) {
+
+	floatx isd;
+	floatx isq;
+	floatx usd;
+	floatx usq;
+
+	floatx Cd;
+	floatx Cq;
+	floatx CC;
+	floatx oCC;
+
+	floatx difid;
+	floatx difiq;
+
+	floatx zeta;
+	floatx Kd;
+	floatx Kq;
+	floatx ro;
+	floatx ypd;
+	floatx ypq;
+	floatx detRy;
+	floatx ydiffd;
+	floatx ydiffq;
+	floatx ydC;
+
+	floatx maxlwi, sumlwi;
+	int i;
+	
+	floatx *th_i, *Pt_i, *om_i, *isdm_i, *isqm_i;
+	floatx *w_i, *lw_i;
+	floatx *sth_i, *cth_i;
 // implementation starts here
+	th_i = &th[0];
+	Pt_i = &Pt[0];
+	om_i = &om[0];
+	isdm_i = &isdm[0];
+	isqm_i = &isqm[0];
+	sth_i = &sth[0];
+	cth_i = &cth[0];
 	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;
+
+		// time update
+		*Pt_i = 1.0*1.0*(*Pt_i)+qom; // Pt is now predictive variance
+		*th_i += *om_i*_dt+qth *  qrandn() ;
+		while ( *th_i>M_PI ) *th_i -=2*M_PI;
+		while ( *th_i<-M_PI ) *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;
+		// process old voltage
+		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;
+		difid=isd- _ad  * (*isdm_i) - _cd *(usd);
+		difiq=isq- _aq  * (*isqm_i) - _cq *(usq);
 
 		CC=Cd*Cd+Cq*Cq;
-		zeta = Pt[i]/ ( r+Pt[i]*CC );
+		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 );
+		Kd = *Pt_i*Cd*ro;
+		Kq = *Pt_i*Cq*ro;
+
+		(*Pt_i)=(*Pt_i)*( 1.0- ( Kd*Cd+Kq*Cq ) );
+
+		ypd = Cd**om_i;
+		ypq = Cq**om_i;
+
+		//detRy = ro/r;
+		*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;
+		//lwi[i] = 0.5* ( log ( detRy ) + ( ydC*ydC*zeta -(ydiffd*ydiffd+ydiffq*ydiffq) ) /r ) ;
+		lwi[i] = 0.5*( ydC*ydC*zeta -(ydiffd*ydiffd+ydiffq*ydiffq) ) /r ;
+		
+		*isdm_i=isd;
+		*isqm_i=isq;
+		
+		// shift all counters
+		th_i++;
+		Pt_i++;
+		om_i++;
+		isdm_i++;
+		isqm_i++;
+		sth_i++;
+		cth_i++;
+
 	}
 	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;
+	lw_i = &lwi[0];
+	for ( i=0;i<N;i++,lw_i++ ) {
+		if ( *lw_i>maxlwi ) maxlwi=*lw_i;
+	}
+	lw_i = &lwi[0];
+	w_i = &w[0];
+	for ( i=0;i<N;i++,lw_i++,w_i++ ) {
+		*lw_i-=maxlwi;
+		(*w_i)=exp ( *lw_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;
+	w_i = &w[0];
+	for ( i=0;i<N;i++,w_i++ ) sumlwi+=*w_i;
+	w_i = &w[0];
+	for ( i=0;i<N;i++,w_i++ ) *w_i/=sumlwi;
+	
+	// NEFF
+	floatx sw2=0.0;
+	for ( i=0;i<N;i++ ) sw2+=w[i]*w[i];
+	printf("%f\n",1./sw2*N);
+	
 	resample();
 }
 
 
-void mpf_init(double qom0, double qth0, double r0) {
+void mpf_init(floatx qom0, floatx qth0, floatx r0) {
+	int i;
 	r=r0;
 	qth=qth0;
 	qom=qom0;
-	int i;
+
 	for ( i=0; i<N; i++ ) {
-		th[i]=randu() *2*M_PI - M_PI;
+		th[i]=qrandu() *2*M_PI - M_PI;
+		sth[i]=sin ( th[i] );
+		cth[i]=cos ( th[i] );
+		
 		om[i]=0;
-		Pt[i]=1;
+		Pt[i]=10.;
 		isdm[i]=0;
 		isqm[i]=0;
@@ -213,5 +258,5 @@
 
 }
-void mpf_mean ( double *Ecosth, double *Esinth, double *Eome ) {
+void mpf_mean ( floatx *Ecosth, floatx *Esinth, floatx *Eome ) {
 	int i;
 	*Ecosth=0.0;
@@ -225,38 +270,39 @@
 
 }
-void mpf_th(double th1[N]){
-	for (int i=0;i<N;i++) th1[i]=th[i];
-}
-
-double kalman_om( const double &isa, const double &isb , const double &usa, const double &usb, const double &th ) {
-
-	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;
-
-	static double Pt;
-	static double om;
-	double cth, sth;
-	static double isdm, isqm;
+void mpf_th(floatx th1[N]){
+	int i;
+	for (i=0;i<N;i++) th1[i]=th[i];
+}
+
+floatx kalman_om( const floatx isa, const floatx isb , const floatx usa, const floatx usb, const floatx th ) {
+
+	floatx isd;
+	floatx isq;
+	floatx usd;
+	floatx usq;
+
+	floatx Cd;
+	floatx Cq;
+	floatx CC;
+	floatx oCC;
+
+	floatx difid;
+	floatx difiq;
+
+	floatx zeta;
+	floatx Kd;
+	floatx Kq;
+	floatx ro;
+	floatx ypd;
+	floatx ypq;
+	floatx detRy;
+	floatx ydiffd;
+	floatx ydiffq;
+	floatx ydC;
+
+	static floatx Pt;
+	static floatx om;
+	floatx cth, sth;
+	static floatx isdm, isqm;
 
 	
