Changeset 1381 for applications

Timestamp:

08/02/11 16:02:37 (13 years ago)

Author:

smidl

Message:

simulator

Location:

applications/pmsm/simulator_zdenek/ekf_example

Files:

• CMakeLists.txt (modified) (1 diff)
• ekf_obj.h (modified) (3 diffs)
• mpf_double.cpp (modified) (1 diff)
• mpf_double.h (modified) (1 diff)
• simul.cpp (modified) (1 diff)
• simulmpf.cpp (copied) (copied from applications/pmsm/simulator_zdenek/ekf_example/simul.cpp) (6 diffs)

applications/pmsm/simulator_zdenek/ekf_example/CMakeLists.txt

@@ -1 +1 @@
 add_executable (simulace fixed.cpp matrix.cpp pmsm_mod.cpp pwm.cpp regulace.cpp ekf.cpp simul.cpp )
+add_executable (simulmpf pmsm_mod.cpp pwm.cpp regulace.cpp mpf_double.cpp simulmpf.cpp )
 
 include_directories(../../bdm)

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

@@ -566 +566 @@
 
 class MPF_pmsm_red:public BM{
+                double qom, qth, r;
 
 
@@ -572 +573 @@
                         dimy=2;
                         dimc=2;
-                        mpf_init(1e-1, 1e-6, 1e-1);
+                        qom=1e-1;
+                        qth=1e-6;
+                        r=1e-1;
                 };
         void bayes ( const vec &val, const vec &cond ) {
@@ -584 +587 @@
         class mp:public epdf{
         public:
-                mp():epdf(){set_dim(N);}
-                vec sample() const {return zeros(N);}
+                mp():epdf(){set_dim(3);}
+                vec sample() const {return zeros(3);}
                 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);}
+                vec mean() const {vec tmp(3); mpf_mean(tmp._data(), tmp._data()+1, tmp._data()+2); return tmp;}
+                vec variance() const {return zeros(3);}
         };
         
         mp mypdf;
         const mp& posterior() const {return mypdf;}
+        
+        void from_setting(const Setting &set){
+                BM::from_setting(set);
+                UI::get(qom,set,"qom",UI::optional);
+                UI::get(qth,set,"qth",UI::optional);
+                UI::get(r,set,"r",UI::optional);
+        }
+        void validate(){
+                mpf_init(qom,qth,r);
+                
+        }
 };
 UIREGISTER(MPF_pmsm_red);

applications/pmsm/simulator_zdenek/ekf_example/mpf_double.cpp

@@ -213 +213 @@
 
 }
-void mpf_mean ( double Ecosth, double Esinth, double Eome ) {
-        int i;
-        Ecosth=0.0;
-        Esinth=0.0;
-        Eome=0.0;
+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];
+                *Ecosth+=(w[i]*(cth[i]));
+                *Esinth+=(w[i]*(sth[i]));
+                *Eome+=(w[i]*(om[i]));
         }
 

applications/pmsm/simulator_zdenek/ekf_example/mpf_double.h

@@ -19 +19 @@
 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_mean(double *Ecosth, double *Esinth, double *Eome);
 void mpf_th(double th1[N]);

applications/pmsm/simulator_zdenek/ekf_example/simul.cpp

@@ -243 +243 @@
 
     if (t>=t_sense)
-      if (print_counter>199)
+      if (print_counter>125)
       { fprintf(fw,"%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f \n",t,x[0],x[1],x[6],*ladeni_regulace,x[7],*(ladeni_regulace+2),x[4],x[5],x[3],x[2],Ww,*(ladeni_regulace+1),Urm_max,*(ladeni_regulace+4),*(ladeni_regulace+5),x[10],*(ladeni_regulace+6),Uc, *(ladeni_regulace+7),ladeni_pila,ladeni_ur,*(ladeni_regulace+8),*(ladeni_regulace+9),ekf_estim[0],ekf_estim[1]);
       // t, Isx, Isy, Isd, Isdw, Isq, Isqw, M, Fs, poloha, rychlost, rychlost_w, Urm, Urm_max,

applications/pmsm/simulator_zdenek/ekf_example/simulmpf.cpp

@@ -17 +17 @@
 #include "pwm.h"
 #include "regulace.h"
-#include "ekf.h"
+#include "mpf_double.h"
 
 #define Qm      13
@@ -56 +56 @@
 
 static double Isx, Isy;
+static double Usx, Usy, ua, ub, Umk;
 static double theta;
 static double Uc_mer;
@@ -64 +65 @@
 
 // EKF - vysledek estimace
-static double ekf_estim[2]={0.,0.};     // w_est, theta_est
+static double mpf_estim[2]={0.,0.};     // w_est, theta_est
+static double Esinth, Ecosth;
 
 FILE *fw;
@@ -113 +115 @@
   init_pmsm(param, REL1);
   init_regulace(param,h_reg);
-  init_ekf(h_reg,param);
-
-  fw=fopen("data/graf1.txt","w");
+  mpf_init(1e0, 1e-5, 1e0);//VS:
+
+  fw=fopen("data/graf2.txt","w");
 
   Idwf=0.;
@@ -215 +217 @@
                 if (x[2]>1615) {x[2]=1615; if (k_rampa_tmp<24000) k_rampa_tmp++; else {k_rampa=-1;k_rampa_tmp=0;}}
                 if (x[2]<-1615) x[2]=-1615;  /**/
-
+                        
                 // KALMAN
-                ekf(ekf_estim,*u,*(u+1),Ucn,Uc_mer,Isx,Isy);
+               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]=atan(Esinth/Ecosth);
 
                 // vystup z EKF zaveden do regulatoru
@@ -243 +253 @@
 
     if (t>=t_sense)
-      if (print_counter>199)
-      { fprintf(fw,"%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f \n",t,x[0],x[1],x[6],*ladeni_regulace,x[7],*(ladeni_regulace+2),x[4],x[5],x[3],x[2],Ww,*(ladeni_regulace+1),Urm_max,*(ladeni_regulace+4),*(ladeni_regulace+5),x[10],*(ladeni_regulace+6),Uc, *(ladeni_regulace+7),ladeni_pila,ladeni_ur,*(ladeni_regulace+8),*(ladeni_regulace+9),ekf_estim[0],ekf_estim[1]);
-      // t, Isx, Isy, Isd, Isdw, Isq, Isqw, M, Fs, poloha, rychlost, rychlost_w, Urm, Urm_max,
+      if (print_counter>125)//sampling period
+      { fprintf(fw,"%f %f %f %f %f %f %f \n", t,x[0],x[1],Usx,Usy,mpf_estim[0],mpf_estim[1]); // t, Isx, Isy, Isd, Isdw, Isq, Isqw, M, Fs, poloha, rychlost, rychlost_w, Urm, Urm_max,
       // Fs_model, M_model, zatezny uhel (beta), zatezny uhel vypocteny z napeti (zbeta), Uc
         print_counter=0;