Changeset 1168 for applications/pmsm/simulator_zdenek

Timestamp:

08/27/10 17:05:55 (14 years ago)

Author:

smidl

Message:

pmsm stuff fro bierman

Location:

applications/pmsm/simulator_zdenek/ekf_example

Files:

• ekf_obj.cpp (modified) (3 diffs)
• ekf_obj.h (modified) (1 diff)

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

@@ -8 +8 @@
 
 ///////////////
-void EKFfixed::bayes(const vec &dt){
-        ekf(dt(2),dt(3),dt(0),dt(1));
+void EKFfixed::bayes(const vec &yt, const vec &ut){
+        ekf(yt(0),yt(1),ut(0),ut(1));
+        
         vec xhat(4);    
         //UGLY HACK!!! reliance on a predictor!!
@@ -15 +16 @@
         xhat(1)=zprevod(x_est[1],Qm)*Iref;
         xhat(2)=zprevod(x_est[2],Qm)*Wref;
-        xhat(3)=zprevod(x_est[3],15);
+        xhat(3)=zprevod(x_est[3],15)*Thetaref;
         
         E.set_mu(xhat);
@@ -33 +34 @@
                 double* Pp=Pfull._data();
                 for(int i=0;i<16;i++){*(Pp++) = zprevod(P_est[i],15);}
+                
+                E._R()._M()=Pfull;
+                
                 
                 BM::ll = -0.5* ( 4 * 1.83787706640935 +log ( det ( Pfull ) ) +xdif* ( inv(Pfull)*xdif ) );

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

@@ -65 +65 @@
         //! Default constructor
         EKFfixed ():BM(),E(),Ry(2,2){
-        int i;
- for(i=0;i<16;i++){Q[i]=0;}
- for(i=0;i<4;i++){R[i]=0;}
+                int i;
+                for(i=0;i<16;i++){Q[i]=0;}
+                for(i=0;i<4;i++){R[i]=0;}
 
- for(i=0;i<4;i++){x_est[i]=0;}
- for(i=0;i<4;i++){x_pred[i]=0;}
- for(i=0;i<16;i++){P_pred[i]=0;}
- for(i=0;i<16;i++){P_est[i]=0;}
- P_est[0]=0x7FFF;
- P_est[5]=0x7FFF;
- P_est[10]=0x7FFF;
- P_est[15]=0x7FFF;
- for(i=0;i<2;i++){Y_mes[i]=0;}
- for(i=0;i<2;i++){ukalm[i]=0;}
- for(i=0;i<8;i++){Kalm[i]=0;}
+                for(i=0;i<4;i++){x_est[i]=0;}
+                for(i=0;i<4;i++){x_pred[i]=0;}
+                for(i=0;i<16;i++){P_pred[i]=0;}
+                for(i=0;i<16;i++){P_est[i]=0;}
+                P_est[0]=0x7FFF;
+                P_est[5]=0x7FFF;
+                P_est[10]=0x7FFF;
+                P_est[15]=0x7FFF;
+                for(i=0;i<2;i++){Y_mes[i]=0;}
+                for(i=0;i<2;i++){ukalm[i]=0;}
+                for(i=0;i<8;i++){Kalm[i]=0;}
 
- for(i=0;i<16;i++){PSI[i]=0;}
-};
+                for(i=0;i<16;i++){PSI[i]=0;}
+
+                set_dim(4);
+                E._mu()=zeros(4);
+                E._R()=zeros(4,4);
+                init_ekf(0.000125);
+        };
         //! Here dt = [yt;ut] of appropriate dimensions
-        void bayes ( const vec &dt );
+        void bayes ( const vec &yt, const vec &ut );
         //!dummy!
-        epdf& posterior(){return E;};
-        void condition ( const vec &Q0 ) {
-                
-                Q[0]=prevod(minQ(Q0(0)),15);       // 0.05
-                Q[5]=prevod(minQ(Q0(1)),15);
-                Q[10]=prevod(minQ(Q0(2)),15);      // 1e-3
-                Q[15]=prevod(minQ(Q0(3)),15);      // 1e-3
-
-        }
+        const epdf& posterior() const {return E;};
+        
 };
 
+UIREGISTER(EKFfixed);
 
 #endif // KF_H