Changeset 1463

Timestamp:

06/21/12 09:49:24 (13 years ago)

Author:

smidl

Message:

Filter s odhadem kovariancnich matic pomoci VB

Files:

• applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h (modified) (2 diffs)

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

@@ -22 +22 @@
 #include "parametry_motoru.h"
 #include "mpf_double.h"
+#include "fast_exp.h"
 
 using namespace bdm;
@@ -636 +637 @@
 UIREGISTER(MPF_pmsm_red);
 
+//! EKF with covariance R estimated by the VB method
+class EKFvbR: public EKFfull{   
+    LOG_LEVEL(EKFvbR,logR,logQ);
+
+        //! Statistics of the R estimator
+        mat PsiR;
+        //! Statistics of the Q estimator
+        mat PsiQ;
+        //! forgetting factor
+        double phi;
+        //! number of VB iterations
+        int niter;
+        //! degrees of freedom
+        double nu;
+        //! stabilizing element
+        mat PsiR0;
+        //! stabilizing element
+        mat PsiQ0;
+        
+        void from_setting(const Setting &set){
+            EKFfull::from_setting(set);
+                if (!UI::get(phi,set,"phi",UI::optional)){
+                        phi = 0.99;
+                }
+                if (!UI::get(niter,set,"niter",UI::optional)){
+                        niter = 3;
+                }
+                PsiQ = Q;
+                PsiQ0 = Q;
+                PsiR = R;
+                PsiR0 = R;
+                nu = 3;
+    }   
+        void log_register ( logger &L, const string &prefix ) {
+                EKFfull::log_register(L,prefix);
+                L.add_vector(log_level, logR, RV("{R }",vec_1<int>(4)), prefix);
+                L.add_vector(log_level, logQ, RV("{Q }",vec_1<int>(4)), prefix);
+        }
+        void bayes ( const vec &val, const vec &cond ) {
+                vec diffx, diffy;
+                mat Psi_vbQ;
+                mat Psi_vbR;
+                nu = phi*nu + (1-phi)*2 + 1.0;
+                
+                //save initial values of posterior
+                vec mu0=est._mu();
+                fsqmat P0=est._R();
+                vec xpred = pfxu->eval(mu0,cond);
+                
+                for (int i=0; i<niter; i++){
+                        est._mu() = mu0;
+                        est._R() = P0;
+                        
+                        EKFfull::bayes(val,cond);
+
+                        diffy = val - fy._mu();
+                        Psi_vbR = phi*PsiR + (1-phi)*PsiR0+ outer_product(diffy,diffy)/*+C*mat(est._R())*C.T()*/;
+                        R = Psi_vbR/(nu-2);
+        
+                        diffx = est._mu() - xpred;
+                        Psi_vbQ = phi*PsiQ + (1-phi)*PsiQ0+ outer_product(diffx,diffx) /*+mat(est._R()) /*+ A*mat(P0)*A.T()*/;
+                        Q = Psi_vbQ/(nu-2);
+                }
+                PsiQ = Psi_vbQ;
+                PsiR = Psi_vbR;
+//              cout <<"==" << endl << Psi << endl << diff << endl << P0 << endl << ":" << Q;
+                log_level.store(logQ, vec(Q._M()._data(),4));
+                log_level.store(logR, vec(R._M()._data(),4));
+                
+        }
+};
+UIREGISTER(EKFvbR);
+
 
 #endif // KF_H