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

Timestamp:

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

Author:

smidl

Message:

Funkcni 1D Kalman

Files:

• applications/pmsm/simulator_zdenek/ekf_example/simulmpf.cpp (modified) (4 diffs)

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