Changeset 66

Timestamp:

04/09/08 15:39:48 (16 years ago)

Author:

smidl

Message:

oprava simulatoru

Location:

simulator_zdenek/ekf_example

Files:

• CMakeLists.txt (modified) (1 diff)
• ekf_obj.cpp (modified) (2 diffs)
• simul.cpp (modified) (1 diff)

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 )
 
+include_directories(../../bdm)
 add_library (ekf_obj ekf_obj.cpp fixed.cpp matrix.cpp)

simulator_zdenek/ekf_example/ekf_obj.cpp

@@ -3 +3 @@
 
 #include "ekf_obj.h"
+#include "../simulator.h"
 
 double minQ(double Q){if (Q>1.0){ return 1.0;} else {return Q;};};
@@ -9 +10 @@
 void EKFfixed::bayes(const vec &dt){
         ekf(dt(2),dt(3),dt(0),dt(1));
+        vec xhat(4);    
+        //UGLY HACK!!! reliance on a predictor!!
+        xhat(0)=zprevod(x_est[0],Qm)*Iref;
+        xhat(1)=zprevod(x_est[1],Qm)*Iref;
+        xhat(2)=zprevod(x_est[2],Qm)*Wref;
+        xhat(3)=zprevod(x_est[3],15);
+        
+        E.set_mu(xhat);
         
         if ( BM::evalll ) {
                 //from enorm
-                vec ydif(2);
-                ydif(0)=dt(0)-zprevod(x_pred[0],Qm)*Iref;
-                ydif(1)=dt(1)-zprevod(x_pred[1],Qm)*Iref;
+                vec xdif(x,4);//first 4 of x
+                //UGLY HACK!!! reliance on a predictor!!
+/*              xdif(0)=x[0]-zprevod(x_pred[0],Qm)*Iref;
+                xdif(1)=x[1]-zprevod(x_pred[1],Qm)*Iref;
+                xdif(2)=x[2]-zprevod(x_pred[2],Qm)*Wref;
+                xdif(3)=x[3]-zprevod(x_pred[3],15);*/
                 
-                BM::ll = -0.5* ( 2 * 1.83787706640935 +log ( det ( Ry ) ) +ydif* ( inv(Ry)*ydif ) );
+                xdif -=xhat; //(xdif=x-xhat)
+                
+                mat Pfull(4,4);
+                double* Pp=Pfull._data();
+                for(int i=0;i<16;i++){*(Pp++) = zprevod(P_est[i],15);}
+                
+                BM::ll = -0.5* ( 4 * 1.83787706640935 +log ( det ( Pfull ) ) +xdif* ( inv(Pfull)*xdif ) );
         }
 };

simulator_zdenek/ekf_example/simul.cpp

@@ -1 @@
-/* Hlavni simulacni program
+/* Hlavni simulacni program
 
    Simulace tramvaje s PMSM v plovouci radove carce