Changeset 1168 for applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

Timestamp:

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

Author:

smidl

Message:

pmsm stuff fro bierman

Files:

• applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (modified) (1 diff)

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