Changeset 229 for bdm/estim

Timestamp:

01/15/09 10:53:54 (15 years ago)

Author:

smidl

Message:

epdf has a new function: variance()

Location:

bdm/estim

Files:

• ekf_templ.h (modified) (1 diff)
• libPF.h (modified) (1 diff)
• merger.h (modified) (2 diffs)

bdm/estim/ekf_templ.h

@@ -29 +29 @@
 };
 
+//!Extended Kalman filter in Choleski form with unknown \c Q
+class EKFCh_unQ : public EKFCh , public BMcond {
+public:
+        //! Default constructor
+        EKFCh_unQ ( RV rx, RV ry,RV ru,RV rQ ) :EKFCh ( rx,ry,ru ),BMcond ( rQ ) {};
+        void condition ( const vec &Q0 ) {
+                Q.setD ( Q0,0 );
+                //from EKF
+                preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );
+        };
+};
 
 #endif //EKF_TEMP_H

bdm/estim/libPF.h

@@ -88 +88 @@
                         // ugly
                         vec pom=zeros ( ( Coms ( 0 )->_rv() ).count() );
-
                         for ( int i=0; i<_w.length(); i++ ) {pom += Coms ( i )->mean() * _w ( i );}
-
                         return concat ( E.mean(),pom );
+                }
+                vec variance() const {
+                        // ugly
+                        vec pom=zeros ( ( Coms ( 0 )->_rv() ).count() );
+                        vec pom2=zeros ( ( Coms ( 0 )->_rv() ).count() );
+                        for ( int i=0; i<_w.length(); i++ ) {
+                                pom += Coms ( i )->mean() * _w ( i );
+                                pom2 += (Coms ( i )->variance() + pow(Coms(i)->mean(),2)) * _w ( i );}
+                        return concat ( E.variance(),pom2-pow(pom,2) );
                 }
 

bdm/estim/merger.h

@@ -102 +102 @@
                 return tmp;
         }
-        mat variance() const {
+        mat covariance() const {
                 const vec &w = eSmp._w();
                 const Array<vec> &S = eSmp._samples();
@@ -115 +115 @@
                 }
                 return Tmp-outer_product(mea,mea);
+        }
+        vec variance() const {
+                const vec &w = eSmp._w();
+                const Array<vec> &S = eSmp._samples();
+                
+                vec tmp=zeros(rv.count());
+                for ( int i=0; i<Ns; i++ ) {
+                        tmp+=w ( i ) *pow(S ( i ),2);
+                }
+                return tmp-pow(mean(),2);
         }
 //! for future use