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

Timestamp:

12/02/13 13:54:59 (10 years ago)

Author:

smidl

Message:

upravy rand

Files:

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

applications/pmsm/simulator_zdenek/ekf_example/mpf_double.cpp

@@ -1 @@
-#include "math.h"
+#include <math.h>
 #include <stdio.h>
 #include "fast_exp.h"
 #include "fastexp.h"
 #include "mpf_double.h"
+
+//Parameters of PMSM
+#define Lsd (3.465e-3*0.9)      //      *0.9)//H        ... zajimave - chodi to i pri Lsd = Lsq
+#define Lsq 3.465e-3//H
+//#define Ls  3.465e-3//H
+#define Rs 0.28//Ohm
+#define Fpm (0.1989*1.1)        //*1.15)        //V.s           // !!xx!!
+#define pp 4
+#define Ism 35.//40.
 
 static floatx om[N];
@@ -14 +23 @@
 static floatx w[N];
 static floatx lwi[N];
+static int i_best;
 
 static floatx qth;
@@ -20 +30 @@
 static floatx rinv;
 
-#if 1
-#include "RArandn.h"
-floatx qrandu() {
-        if ( randu_i==randu_last )
-                randu_i =&RArandu[0];
-        else
-                randu_i++;
-
-        return *randu_i;
-}
-
-floatx qrandn() {
-        if ( randn_i==randn_last )
-                randn_i =&RArandn[0];
-        else
-                randn_i++;
-
-        return *randn_i;
-}
-
-void rng_init()
-{
-randu_i=&RArandu[0];
-randn_i=&RArandn[0];
-}
-
-#else
-
-#include "rnor.h"
-floatx qrandu() {
-        return UNI;
-}
-
-floatx qrandn() {
-        return PolarRNG();
-}
-
-void rng_init()
-{
-zigset(86947731 );
-}
-
-
-#endif
+// staticke promenne pro model
+static float _ad;
+static float _aq;
+static float _b;
+static float _cd;
+static float _cq;
+static float _dd;
+static float _dq;
 
 
@@ -95 +69 @@
 
         while ( i_from<N ) {
+                w[i_from]=1.0/N;
                 while ( N_babies[i_from]>1 ) { // 1 baby stays where it is
                         
@@ -139 +114 @@
         floatx ydC;
 
-        floatx maxlwi, sumlwi;
+        floatx maxlwi, sumlwi,sumw2,neff_th;
         int i;
         
@@ -145 +120 @@
         floatx *w_i, *lw_i;
         floatx *sth_i, *cth_i;
+        floatx sumw_inv;
 // implementation starts here
         th_i = &th[0];
@@ -155 +131 @@
         for ( i=0; i<N; i++ ) {
 
+          /////////////////////////// HACK!!!!!!!!!!!
+          //om[i] = 0;
+          /////////////////////////// HACK!!!!!!!!!!!
+          
                 // time update
                 *Pt_i = 1.0*1.0*(*Pt_i)+qom; // Pt is now predictive variance
-                *th_i += *om_i*_dt+qth *  qrandn() ;
+                floatx ff =qrandn() ;
+//              printf("%f\n", ff);
+                *th_i += *om_i*_dt+qth * ff; 
                 while ( *th_i>M_PI ) *th_i -=2*M_PI;
                 while ( *th_i<-M_PI ) *th_i+=2*M_PI;
@@ -170 +152 @@
                 usq = -(*sth_i)*usa+(*cth_i)*usb;
 
-                Cd = isq*_dt;
-                Cq = -_b - isd* _dt;
+                Cd = isq*_dd;
+                Cq = -_b - isd* _dq;
 
                 difid=isd- _ad  * (*isdm_i) - _cd *(usd);
@@ -211 +193 @@
 
         }
-/*      maxlwi=-1e10;
+        maxlwi=-1e10;
         lw_i = &lwi[0];
+        i_best = 0;
         for ( i=0;i<N;i++,lw_i++ ) {
-                if ( *lw_i>maxlwi ) maxlwi=*lw_i;
-        }*/
+                if ( *lw_i>maxlwi ) {maxlwi=*lw_i;i_best=i;}
+        }
+/*!!*/
         lw_i = &lwi[0];
         w_i = &w[0];
         for ( i=0;i<N;i++,lw_i++,w_i++ ) {
-//              *lw_i-=maxlwi;
-                (*w_i)=fasterexp ( *lw_i ); // always resample -> wmin=1/n;
+                *lw_i-=maxlwi;
+                (*w_i)*=fasterexp ( *lw_i ); // do not always resample 
         }
 
         sumlwi=0.0;
+        sumw2=0.0;
         w_i = &w[0];
-        for ( i=0;i<N;i++,w_i++ ) sumlwi+=*w_i;
+        for ( i=0;i<N;i++,w_i++ ) {sumlwi+=*w_i;sumw2+=*w_i**w_i;}
+        sumw_inv = 1.0/sumlwi; // multiplication is faster than division!
         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();
+        for ( i=0;i<N;i++,w_i++ ) *w_i*=sumw_inv;
+        
+        neff_th = 0.8*N*sumw2*sumw_inv*sumw_inv;
+        if (1<neff_th)
+          resample();
 }
 
@@ -240 +223 @@
 void mpf_init(floatx qom0, floatx qth0, floatx r0) {
         
-        rng_init();
+//      rng_init();
         
         int i;
@@ -259 +242 @@
                 w[i]=1.0/N;
         }
+
+   // inicializace promennych
+   #if (VYBER_MODELU==0)
+    // plny model s respektovanim Lsd/Lsq
+    _ad = (1.-Rs*_dt/(Lsd));
+    _aq = (1.-Rs*_dt/(Lsq));
+    _b = Fpm/Lsq*_dt;
+    _cd = _dt/(Lsd);
+    _cq = _dt/(Lsq);
+    _dd = _dt*Lsq/Lsd;
+    _dq = _dt*Lsd/Lsq; /**/
+  #else
+  // parametry modelu identifikovane z namerenych dat z prototypu v EL204
+    _ad = 0.9880;
+    _aq = 0.9817;
+    _b = 0.0076;
+    _cd = 0.0289;
+    _cq = 0.0292;
+    _dd = _dt;
+    _dq = _dt;
+  #endif   
 
 }
@@ -277 +281 @@
 
 }
+
+void mpf_best( floatx *Ecosth, floatx *Esinth, floatx *Eome ) 
+{
+        *Ecosth=cth[i_best];
+        *Esinth=sth[i_best];
+        *Eome=om[i_best];
+}
+
+
 void mpf_th(floatx th1[N]){
         int i;
@@ -330 +343 @@
                 usq = -sth*usa+cth*usb;
 
-                Cd = isq*_dt;
-                Cq = -_b - isd* _dt;
+                Cd = isq*_dd;
+                Cq = -_b - isd* _dq;
 
                 difid=isd- _ad  *isdm - _cd *usd;