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

Timestamp:

07/23/12 11:03:24 (12 years ago)

Author:

smidl

Message:

upravy choleskiho + nove qmath

Files:

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

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

@@ -23 +23 @@
 #include "mpf_double.h"
 #include "fast_exp.h"
+#include "ekf_mm.h"
+#include "qmath.h"
 
 using namespace bdm;
@@ -413 +415 @@
 class EKFfixedCh2 : public BM {
 public:
-        LOG_LEVEL(EKFfixedCh2,logCh, logA, logC, logP);
+        LOG_LEVEL(EKFfixedCh2,logCh, logA, logC, logP, logDet, logRem);
         
         void init_ekf2(double Tv);
@@ -470 +472 @@
                 L.add_vector ( log_level, logC, RV ("C2", 4 ), prefix );
                 L.add_vector ( log_level, logP, RV ("P2", 4 ), prefix );
-                
-        };
-        //void from_setting();
+                L.add_vector ( log_level, logDet, RV ("Det", 1 ), prefix );
+                L.add_vector ( log_level, logRem, RV ("Rem", 1 ), prefix );
+                
+        };
+        void from_setting ( const Setting &set ){
+                BM::from_setting(set);
+                vec dQ,dR;
+                UI::get ( dQ, set, "dQ", UI::optional );
+                UI::get ( dQ, set, "dR", UI::optional );
+                if (dQ.length()==2){
+                        Q[0]=prevod(dQ[0],15);      // 1e-3
+                        Q[3]=prevod(dQ[1],15);      // 1e-3
+                }
+                if (dR.length()==2){
+                        R[0]=prevod(dR[0],15);      // 1e-3
+                        R[3]=prevod(dR[1],15);      // 1e-3
+                }
+        }
 };
 
@@ -711 +728 @@
 
 
+class ekfChfix: public BM{
+        
+        ekf_data E;
+public:
+        ekfChfix(){
+                init_ekfCh2(&E,0.000125);set_dim(2);    dimc = 2;
+                dimy = 2;
+        } 
+        void bayes ( const vec &val, const vec &cond ) {
+                int16 ux,uy;
+                ux=prevod(cond[0]/Uref,15);
+                uy=prevod(cond[1]/Uref,15);
+                
+                int16 yx,yy;
+                // zadani mereni
+                yx=prevod(val[0]/Iref,15);
+                yy=prevod(val[1]/Iref,15);
+                
+                int16 detS, rem;
+                ekfCh2(&E, ux,uy,yx,yy, &detS, &rem);
+                
+                Est._mu()=vec_2(E.x_est[0]*Uref/32768., E.x_est[1]*Uref/32768.);
+                
+                ll = -0.5*qlog(detS)-0.5*rem;
+        }
+        const epdf& posterior() const {return Est;};
+        void from_setting ( const Setting &set ){
+                BM::from_setting(set);
+                vec dQ,dR;
+                UI::get ( dQ, set, "dQ", UI::optional );
+                UI::get ( dQ, set, "dR", UI::optional );
+                if (dQ.length()==2){
+                        E.Q[0]=prevod(dQ[0],15);      // 1e-3
+                        E.Q[3]=prevod(dQ[1],15);      // 1e-3
+                }
+                if (dR.length()==2){
+                        E.dR[0]=prevod(dR[0],15);      // 1e-3
+                        E.dR[1]=prevod(dR[1],15);      // 1e-3
+                }
+        }
+        
+        enorm<fsqmat> Est;
+        mat Ry;
+};
+UIREGISTER(ekfChfix);
+
 #endif // KF_H