Changeset 1439 for applications/pmsm/simulator_zdenek/ekf_example/mpf_double.cpp

Timestamp:

03/16/12 16:46:40 (12 years ago)

Author:

smidl

Message:

Funkcni 1D Kalman

Files:

• applications/pmsm/simulator_zdenek/ekf_example/mpf_double.cpp (modified) (9 diffs)

applications/pmsm/simulator_zdenek/ekf_example/mpf_double.cpp

@@ -2 +2 @@
 #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 +31 @@
                                       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 +46 @@
         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 +58 @@
 }
 
-double randn() {
+floatx qrandn() {
         if ( randn_i==randn_last )
                 randn_i =&RArandn[0];
@@ -67 +69 @@
 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 +94 @@
 
         // 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 +114 @@
 }
 
-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 +258 @@
 
 }
-void mpf_mean ( double *Ecosth, double *Esinth, double *Eome ) {
+void mpf_mean ( floatx *Ecosth, floatx *Esinth, floatx *Eome ) {
         int i;
         *Ecosth=0.0;
@@ -225 +270 @@
 
 }
-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;