Changeset 229

Timestamp:

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

Author:

smidl

Message:

epdf has a new function: variance()

Files:

• bdm/estim/ekf_templ.h (modified) (1 diff)
• bdm/estim/libPF.h (modified) (1 diff)
• bdm/estim/merger.h (modified) (2 diffs)
• bdm/stat/emix.h (modified) (2 diffs)
• bdm/stat/libBM.h (modified) (1 diff)
• bdm/stat/libEF.h (modified) (9 diffs)
• matlab/pmsm/mpf_test_disp.m (modified) (4 diffs)
• pmsm/mpf_test.cpp (modified) (7 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

bdm/stat/emix.h

@@ -104 +104 @@
                 for ( i = 0;i < w.length();i++ ) {mu += w ( i ) * Coms ( i )->mean(); }
                 return mu;
+        }
+        vec variance() const {
+                //non-central moment
+                vec mom2 = zeros(rv.count());
+                for ( int i = 0;i < w.length();i++ ) {mom2 += w ( i ) * pow(Coms ( i )->mean(),2); }
+                //central moment
+                return mom2-pow(mean(),2);
         }
         double evallog ( const vec &val ) const {
@@ -243 +250 @@
                 return tmp;
         }
+        vec variance() const {
+                vec tmp ( rv.count() ); //second moment
+                for ( int i=0;i<epdfs.length();i++ ) {
+                        vec pom = epdfs ( i )->mean();
+                        dls ( i )->fill_val ( tmp, pow(pom,2) );
+                }
+                return tmp-pow(mean(),2);
+        }
         vec sample() const {
                 vec tmp ( rv.count() );

bdm/stat/libBM.h

@@ -197 +197 @@
         virtual vec mean() const =0;
 
+        //! return expected variance (not covariance!)
+        virtual vec variance() const = 0;
+        
         //! Destructor for future use;
         virtual ~epdf() {};

bdm/stat/libEF.h

@@ -132 +132 @@
         double lognc () const;
         vec mean() const {return mu;}
+        vec variance() const {return diag(R.to_mat());}
 //      mlnorm<sq_T>* condition ( const RV &rvn ) const ;
         mpdf* condition ( const RV &rvn ) const ;
@@ -192 +193 @@
         vec sample() const;
         vec mean() const;
+        vec variance() const{it_error("Not implemented"); return vec(0);};
         void mean_mat ( mat &M, mat&R ) const;
         //! In this instance, val= [theta, r]. For multivariate instances, it is stored columnwise val = [theta_1 theta_2 ... r_1 r_2 ]
@@ -220 +222 @@
         //!sufficient statistics
         vec beta;
+        //!speedup variable
+        double gamma;
 public:
         //!Default constructor
@@ -226 +230 @@
         eDirich ( const eDirich &D0 ) : eEF ( D0.rv ),beta ( D0.beta ) {};
         vec sample() const {it_error ( "Not implemented" );return vec_1 ( 0.0 );};
-        vec mean() const {return beta/sum ( beta );};
+        vec mean() const {return beta/gamma;};
+        vec variance() const {return elem_mult(beta,(beta+1))/ (gamma*(gamma+1));}
         //! In this instance, val is ...
         double evallog_nn ( const vec &val ) const {double tmp; tmp=( beta-1 ) *log ( val );            it_assert_debug(std::isfinite(tmp),"Infinite value");
@@ -248 +253 @@
                 }
                 beta= beta0;
+                gamma = sum(beta);
         }
 };
@@ -327 +333 @@
         //! Returns poiter to alpha and beta. Potentially dengerous: use with care!
         void _param ( vec* &a, vec* &b ) {a=&alpha;b=&beta;};
-        vec mean() const {vec pom ( alpha ); pom/=beta; return pom;}
+        vec mean() const {return elem_div(alpha,beta);}
+        vec variance() const {return elem_div(alpha,elem_mult(beta,beta)); }
 };
 
@@ -365 +372 @@
                 void _param ( vec* &a, vec* &b ) {a=alpha;b=beta;};
                 vec mean() const {return elem_div(*beta,*alpha-1);}
+                vec variance() const {vec mea=mean(); return elem_div(elem_mult(mea,mea),*alpha-2);}
 };
 /*
@@ -416 +424 @@
                 lnk = log ( nk );
         }
-        vec mean() const {vec pom=high; pom-=low; pom/=2.0; return pom;}
+        vec mean() const {return (high-low)/2.0;}
+        vec variance() const {return (pow(high,2)+pow(low,2)+elem_mult(high,low))/3.0;}
 };
 
@@ -646 +655 @@
                 return pom;
         }
+        vec variance() const {
+                vec pom=zeros ( rv.count() );
+                for ( int i=0;i<n;i++ ) {pom+=pow(samples ( i ),2) *w ( i );}
+                return pom-pow(mean(),2);
+        }
 };
 

matlab/pmsm/mpf_test_disp.m

@@ -8 +8 @@
 
 figure(2)
+StdX = 2*sqrt(VarE);
 for i=1:4
         subplot(4,1,i)
@@ -13 +14 @@
         hold on;
         plot(xthE(:,i)','r')
+    plot(xthE(:,i) - StdX(:,i),'r:');
+    plot(xthE(:,i) + StdX(:,i),'r:');
         plot(xthM(:,i+4)','g')
 %       plot(xthV(i,:)','m');
@@ -18 +21 @@
 
 figure(3)
+StdM = 2*sqrt(VarM);
 for i=1:4
         subplot(4,1,i)
         plot(xthM(:,i)')
+    hold on
+    plot(xthM(:,i)-StdM(:,i),':')
+    plot(xthM(:,i)+StdM(:,i),':')
     if i<4
         %set(gca,'XTick',[]);
@@ -27 +34 @@
     end
     ylabel(['Q(' num2str(i) ',' num2str(i) ')']);
-    hold on
-        plot(Qtr(:,i)','--')
+    plot(Qtr(:,i)','--')
 end
 

pmsm/mpf_test.cpp

@@ -15 +15 @@
 #include <estim/libKF.h>
 #include <estim/libPF.h>
+#include <estim/ekf_templ.h>
 #include <stat/libFN.h>
 
@@ -22 +23 @@
 
 using namespace itpp;
-//!Extended Kalman filter with unknown \c Q
-class EKF_unQ : public EKFCh , public BMcond {
-public:
-        //! Default constructor
-        EKF_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() );
-        };
-};
-
 
 int main() {
@@ -59 +48 @@
 
         RV rQ ( "{Q }","4" );
-        EKF_unQ KFEp ( rx,ry,ru,rQ );
+        EKFCh_unQ KFEp ( rx,ry,ru,rQ );
         KFEp.set_parameters ( &fxu,&hxu,Q,R );
         KFEp.set_est ( mu0, chmat ( zeros ( 4 ) ) );
@@ -65 +54 @@
         //mgamma_fix evolQ ( rQ,rQ );
         migamma_fix evolQ ( rQ,rQ );
-        MPF<EKF_unQ> M ( rx,rQ,evolQ,evolQ,Npart,KFEp );
+        MPF<EKFCh_unQ> M ( rx,rQ,evolQ,evolQ,Npart,KFEp );
         // initialize
         evolQ.set_parameters ( 0.1, Qdiag, 1.0); //sigma = 1/10 mu
@@ -85 +74 @@
         mat Dt=zeros ( Ndat,2+2 ); //observation
         mat XtE=zeros ( Ndat, 4 );
+        mat VarE=zeros ( Ndat, 4 );
         mat Qtr=zeros ( Ndat, 4 );
         mat XtM=zeros ( Ndat,4+4 ); //Q + x
+        mat VarM=zeros ( Ndat,4+4 ); //Q + x
 
         // SET SIMULATOR
@@ -129 +120 @@
                 Qtr.set_row ( tK, Qdiag);
                 XtE.set_row ( tK,KFEep.mean() );
+                VarE.set_row ( tK,KFEep.variance() );
                 XtM.set_row ( tK,Mep.mean() );
+                VarM.set_row ( tK,Mep.variance() );
         }
 
@@ -139 +132 @@
         fou << Name ( "xthE" ) << XtE;
         fou << Name ( "xthM" ) << XtM;
+        fou << Name ( "VarE" ) << VarE;
+        fou << Name ( "VarM" ) << VarM;
         //Exit program: