Changeset 1439

Timestamp:

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

Author:

smidl

Message:

Funkcni 1D Kalman

Location:

applications/pmsm/simulator_zdenek/ekf_example

Files:

• data/kresli_odhad.m (modified) (1 diff)
• ekf_obj.cpp (modified) (1 diff)
• ekf_obj.h (modified) (3 diffs)
• mpf_double.cpp (modified) (9 diffs)
• mpf_double.h (modified) (1 diff)
• simulmpf.cpp (modified) (4 diffs)

applications/pmsm/simulator_zdenek/ekf_example/data/kresli_odhad.m

@@ -1 @@
-load graf1.txt -ascii
+%load graf1.txt -ascii
 load graf2.txt -ascii
 

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

@@ -1121 +1121 @@
         PSI[3]=0x7FFF;
 }
+

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

@@ -578 +578 @@
                 };
         void bayes ( const vec &val, const vec &cond ) {
-                const double &isa = val(0);
+/*              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);
+                const double &usb = cond(1);*/
+                mpf_bayes((floatx)val(0),(floatx)val(1),(floatx)cond(0), (floatx)cond(1));//isa,isb,usa,usb);
         }
         
         class mp:public epdf{
+                LOG_LEVEL(mp,logth);
         public:
-                mp():epdf(){set_dim(3);}
+                mp():epdf(){set_dim(3);         log_level[logth]=true;
+}
                 vec sample() const {return zeros(3);}
                 double evallog(const vec &v) const {return 0.0;}
-                vec mean() const {vec tmp(3); mpf_mean(tmp._data(), tmp._data()+1, tmp._data()+2); return tmp;}
+                vec mean() const {vec tmp(3); 
+                        floatx es,ec,eo;
+                        mpf_mean(&es, &ec, &eo); 
+                        tmp(0)=es;tmp(1)=ec;tmp(2)=eo;
+                        return tmp;
+                }
                 vec variance() const {return zeros(3);}
+                void log_register ( bdm::logger& L, const string& prefix ) {
+                        epdf::log_register ( L, prefix );
+                        if ( log_level[logth] ) {
+                                int th_dim = N; // dimension - dimension of cov
+                                L.add_vector( log_level, logth, RV ( th_dim ), prefix );
+                        }
+                }
+                void log_write() const {
+                        epdf::log_write();
+                        if ( log_level[logth] ) {
+                                floatx Th[N];
+                                mpf_th(Th);
+                                vec th(N); 
+                                for(int i=0;i<N;i++){th(i)=Th[i];}
+                                log_level.store( logth, th );
+                        }
+                }
         };
         
@@ -598 +622 @@
         
         void from_setting(const Setting &set){
-                BM::from_setting(set);
+            BM::from_setting(set);
+                UI::get ( log_level, set, "log_level", UI::optional );
+
                 UI::get(qom,set,"qom",UI::optional);
                 UI::get(qth,set,"qth",UI::optional);
@@ -604 +630 @@
         }
         void validate(){
-                mpf_init(qom,qth,r);
+                mpf_init((floatx)qom,(floatx)qth,(floatx)r);
                 
         }

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;
 
         

applications/pmsm/simulator_zdenek/ekf_example/mpf_double.h

@@ -2 +2 @@
 
 #include "parametry_motoru.h"
+#define Fpm 0.1989
+//#define pi 3.141596
 
-#define N 20
+#define N 1
 
 #define Lsd Ls*0.9
 #define Lsq Ls
 #define _dt 0.000125
-#define _ad (1.-Rs*_dt/Lsd)
-#define _aq (1.-Rs*_dt/Lsq)
-#define _b  Fmag/Lsq*_dt
-#define _cd  _dt/Lsd
-#define _cq  _dt/Lsq
+#define _ad (1.-Rs*_dt/(Lsd))
+#define _aq (1.-Rs*_dt/(Lsq))
+#define _b  Fpm/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]);
+# define floatx float
 
-double kalman_om ( const double &isa, const double &isb , const double &usa, const double &usb, const double &th );
+floatx qrandn();
+floatx qrandu();
+
+void mpf_bayes ( const floatx isa, const floatx isb , const floatx usa, const floatx usb );
+void mpf_init(floatx qom0, floatx qth0, floatx r0);
+void mpf_mean(floatx *Ecosth, floatx *Esinth, floatx *Eome);
+void mpf_th(floatx th1[N]);
+
+floatx kalman_om ( const floatx isa, const floatx isb , const floatx usa, const floatx usb, const floatx th );

applications/pmsm/simulator_zdenek/ekf_example/simulmpf.cpp

@@ -65 +65 @@
 
 // EKF - vysledek estimace
-static double mpf_estim[2]={0.,0.};     // w_est, theta_est
-static double Esinth, Ecosth;
+static floatx mpf_estim[2]={0.,0.};     // w_est, theta_est
+static floatx Esinth, Ecosth;
 
 FILE *fw;
@@ -115 +115 @@
   init_pmsm(param, REL1);
   init_regulace(param,h_reg);
-  mpf_init(1e0, 1e-5, 1e0);//VS:
+  mpf_init(1e0, 1e-3, 1e0);//VS:
 
   fw=fopen("data/graf2.txt","w");
@@ -161 +161 @@
 
         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.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>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>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>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>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>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>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>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
 
 
@@ -218 +218 @@
                 if (x[2]<-1615) x[2]=-1615;  /**/
                         
-                // KALMAN
-               Umk=*u*Uc/Ucn;
+       
+      // KALMAN (pouzij stara Usx, Usy!!
+          if (0){
+                mpf_bayes((floatx)Isx,(floatx)Isy,(floatx)Usx,(floatx)Usy);
+                mpf_mean(&Esinth,&Ecosth,&mpf_estim[0]);
+                mpf_estim[1]=atan2((floatx)Esinth,(floatx)Ecosth);
+          } else {
+                  mpf_estim[0]=kalman_om((floatx)Isx,(floatx)Isy,(floatx)Usx,(floatx)Usy,(floatx)x[3]);
+          }
+
+      Umk=*u*Uc/Ucn;
       ua=Umk*cos(*(u+1));
       ub=Umk*cos(*(u+1)-2./3.*M_PI);
       Usx=ua;                     // usx
       Usy=(ua+2.*ub)/sqrt(3.);    // usy
-       
-                mpf_bayes(Isx,Isy,Usx,Usy);
-                mpf_mean(&Esinth,&Ecosth,&mpf_estim[0]);
-                mpf_estim[1]=atan2(Esinth,Ecosth);
-
                 // vystup z EKF zaveden do regulatoru
                 // rychlost=ekf_estim[0]; theta=ekf_estim[1]; //VS