Changeset 1168

Timestamp:

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

Author:

smidl

Message:

pmsm stuff fro bierman

Files:

• applications/pmsm/CMakeLists.txt (modified) (1 diff)
• applications/pmsm/pmsm_estim.cpp (modified) (1 diff)
• applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (modified) (3 diffs)
• applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (modified) (1 diff)
• library/bdm/estim/kalman.cpp (modified) (3 diffs)
• library/bdm/estim/kalman.h (modified) (4 diffs)

applications/pmsm/CMakeLists.txt

@@ -22 +22 @@
 ## Build simulator
 add_subdirectory (simulator_zdenek)
-#add_subdirectory (simulator_zdenek/ekf_example)
+add_subdirectory (simulator_zdenek/ekf_example)
 
 ## Use it
 include_directories (./simulator_zdenek)
 link_directories (./simulator_zdenek)
-#link_directories (./simulator_zdenek/ekf_example)
+link_directories (./simulator_zdenek/ekf_example)
 
-EXEC (pmsm_estim pmsmsim)
+EXEC (pmsm_estim pmsmsim ekf_obj)
 #EXEC (sim pmsmsim)
 #EXEC (sim_var pmsmsim)

applications/pmsm/pmsm_estim.cpp

@@ -13 +13 @@
 #include "filters.h"
 #include "base/datasources.h"
-
+#include "simulator_zdenek/ekf_example/ekf_obj.h"
 
 using namespace bdm;

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

library/bdm/estim/kalman.cpp

@@ -513 +513 @@
                 }
         }
+        
+        if ( log_level[logU] ){
+                // transformed U
+                mat tU;
+                mat P= U*diag(D)*U.T();
+                
+                vec xref(5);
+                xref(0)= 30.0*1.4142;
+                xref(1)= 30.0*1.4142;
+                xref(2)= 6.283185*200.;
+                xref(3) = 3.141593;
+                xref(4) = 34.0;
+                
+                mat T = diag(1.0/(xref));
+                mat Pf = T*P*T;
+                
+                ldmat Pld(Pf);
+                
+                //vec tmp=vec(U._data(),dimension()*dimension());
+                vec tmp=vec(Pld._L()._data(),dimension()*dimension());
+                log_level.store(logU,round(((int)1<<14)*tmp));
+        }
+        if ( log_level[logG] ){
+                vec tmp=vec(G._data(),dimension()*dimension());
+                log_level.store(logG,tmp);
+        }
+        {
+        }
         //cout << "Ut: " << U << endl;
         //cout << "Dt: " << D << endl;
@@ -535 +563 @@
                         D(j) = beta*gamma*D(j); 
 
-                        cout << "a: " << alpha << "g: " << gamma << endl;
+//                      cout << "a: " << alpha << "g: " << gamma << endl;
                         for (i=0;i<j;i++){
                                 beta   = U(i,j); 
@@ -584 +612 @@
         UI::get ( dR, set, "dR", UI::compulsory );
         set_parameters ( IM, OM, diag ( dQ ), dR  );
-}
-
-}
+        
+        UI::get(log_level, set, "log_level", UI::optional);
+}
+
+}

library/bdm/estim/kalman.h

@@ -429 +429 @@
 class EKF_UD : public BM {
         protected:
+                //! logger
+                LOG_LEVEL(EKF_UD,logU, logG);
                 //! Internal Model f(x,u)
                 shared_ptr<diffbifn> pfxu;
@@ -447 +449 @@
                 enorm<ldmat> est;
         public:
+                
                 //! copy constructor duplicated 
                 EKF_UD* _copy() const {
@@ -458 +461 @@
                 }
                 
-
                 EKF_UD(){}
+                
                 
                 EKF_UD(const EKF_UD &E0): pfxu(E0.pfxu),phxu(E0.phxu), U(E0.U), D(E0.D){}
@@ -469 +472 @@
                 void bayes ( const vec &yt, const vec &cond = empty_vec );
                 
+                void log_register ( bdm::logger& L, const string& prefix ){
+                        BM::log_register ( L, prefix );
+                                                
+                        if ( log_level[logU] )
+                                L.add_vector ( log_level, logU, RV ( dimension()*dimension() ), prefix );
+                        if ( log_level[logG] )
+                                L.add_vector ( log_level, logG, RV ( dimension()*dimension() ), prefix );
+                        
+                }
                 /*! Create object from the following structure