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

Timestamp:

03/16/12 16:46:40 (12 years ago)

Author:

smidl

Message:

Funkcni 1D Kalman

Files:

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

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

@@ -578 +578 @@
                 };
         void bayes ( const vec &val, const vec &cond ) {
-                const double &isa = val(0);
+/*              const double &isa = val(0);
                 const double &isb = val(1);
                 const double &usa = cond(0);
-                const double &usb = cond(1);
-                mpf_bayes(isa,isb,usa,usb);
+                const double &usb = cond(1);*/
+                mpf_bayes((floatx)val(0),(floatx)val(1),(floatx)cond(0), (floatx)cond(1));//isa,isb,usa,usb);
         }
         
         class mp:public epdf{
+                LOG_LEVEL(mp,logth);
         public:
-                mp():epdf(){set_dim(3);}
+                mp():epdf(){set_dim(3);         log_level[logth]=true;
+}
                 vec sample() const {return zeros(3);}
                 double evallog(const vec &v) const {return 0.0;}
-                vec mean() const {vec tmp(3); mpf_mean(tmp._data(), tmp._data()+1, tmp._data()+2); return tmp;}
+                vec mean() const {vec tmp(3); 
+                        floatx es,ec,eo;
+                        mpf_mean(&es, &ec, &eo); 
+                        tmp(0)=es;tmp(1)=ec;tmp(2)=eo;
+                        return tmp;
+                }
                 vec variance() const {return zeros(3);}
+                void log_register ( bdm::logger& L, const string& prefix ) {
+                        epdf::log_register ( L, prefix );
+                        if ( log_level[logth] ) {
+                                int th_dim = N; // dimension - dimension of cov
+                                L.add_vector( log_level, logth, RV ( th_dim ), prefix );
+                        }
+                }
+                void log_write() const {
+                        epdf::log_write();
+                        if ( log_level[logth] ) {
+                                floatx Th[N];
+                                mpf_th(Th);
+                                vec th(N); 
+                                for(int i=0;i<N;i++){th(i)=Th[i];}
+                                log_level.store( logth, th );
+                        }
+                }
         };
         
@@ -598 +622 @@
         
         void from_setting(const Setting &set){
-                BM::from_setting(set);
+            BM::from_setting(set);
+                UI::get ( log_level, set, "log_level", UI::optional );
+
                 UI::get(qom,set,"qom",UI::optional);
                 UI::get(qth,set,"qth",UI::optional);
@@ -604 +630 @@
         }
         void validate(){
-                mpf_init(qom,qth,r);
+                mpf_init((floatx)qom,(floatx)qth,(floatx)r);
                 
         }