Changeset 32 for bdm/estim

Timestamp:

03/03/08 13:00:32 (17 years ago)

Author:

smidl

Message:

test KF : estimation of R in KF is not possible! Likelihood of y_t is growing when R -> 0

Location:

bdm/estim

Files:

• libKF.cpp (modified) (4 diffs)
• libKF.h (modified) (8 diffs)
• libPF.cpp (modified) (1 diff)
• libPF.h (modified) (1 diff)

bdm/estim/libKF.cpp

@@ -6 +6 @@
 using std::endl;
 
-KalmanFull::KalmanFull( mat A0, mat B0, mat C0, mat D0, mat R0, mat Q0, mat P0, vec mu0 ) {
+KalmanFull::KalmanFull ( mat A0, mat B0, mat C0, mat D0, mat R0, mat Q0, mat P0, vec mu0 ) {
         dimx = A0.rows();
         dimu = B0.cols();
         dimy = C0.rows();
 
-        it_assert_debug( A0.cols()==dimx, "KalmanFull: A is not square" );
-        it_assert_debug( B0.rows()==dimx, "KalmanFull: B is not compatible" );
-        it_assert_debug( C0.cols()==dimx, "KalmanFull: C is not square" );
-        it_assert_debug(( D0.rows()==dimy ) || ( D0.cols()==dimu ),     "KalmanFull: D is not compatible" );
-        it_assert_debug(( R0.cols()==dimy ) || ( R0.rows()==dimy ), "KalmanFull: R is not compatible" );
-        it_assert_debug(( Q0.cols()==dimx ) || ( Q0.rows()==dimx ), "KalmanFull: Q is not compatible" );
+        it_assert_debug ( A0.cols() ==dimx, "KalmanFull: A is not square" );
+        it_assert_debug ( B0.rows() ==dimx, "KalmanFull: B is not compatible" );
+        it_assert_debug ( C0.cols() ==dimx, "KalmanFull: C is not square" );
+        it_assert_debug ( ( D0.rows() ==dimy ) || ( D0.cols() ==dimu ), "KalmanFull: D is not compatible" );
+        it_assert_debug ( ( R0.cols() ==dimy ) || ( R0.rows() ==dimy ), "KalmanFull: R is not compatible" );
+        it_assert_debug ( ( Q0.cols() ==dimx ) || ( Q0.rows() ==dimx ), "KalmanFull: Q is not compatible" );
 
         A = A0;
@@ -28 +28 @@
 }
 
-void KalmanFull::bayes( const vec &dt) {
-        it_assert_debug( dt.length()==( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
+void KalmanFull::bayes ( const vec &dt ) {
+        it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
 
-        vec u = dt.get( dimy,dimy+dimu-1 );
-        vec y = dt.get( 0,dimy-1 );
+        vec u = dt.get ( dimy,dimy+dimu-1 );
+        vec y = dt.get ( 0,dimy-1 );
         //Time update
         mu = A*mu + B*u;
@@ -39 +39 @@
         //Data update
         _Ry = C*P*C.transpose() + R;
-        _iRy = inv( _Ry );
-        _K = P*C.transpose()*_iRy;
+        _iRy = inv ( _Ry );
+        _K = P*C.transpose() *_iRy;
         P -= _K*C*P; // P = P -KCP;
-        mu += _K*( y-C*mu-D*u );
+        mu += _K* ( y-C*mu-D*u );
 };
 
@@ -50 +50 @@
 }
 
+void KFcondQR::condition ( const vec &QR ) {
+        it_assert_debug ( QR.length() == ( rvc.count() ),"KFcondRQ: conditioning by incompatible vector" );
+
+        Q.setD ( QR ( 0, dimx-1 ));
+        R.setD ( QR ( dimx, -1 ));
+};
+
+void KFcondR::condition ( const vec &R0 ) {
+        it_assert_debug ( R0.length() == ( rvc.count() ),"KFcondR: conditioning by incompatible vector" );
+
+        R.setD ( R0 );
+};

bdm/estim/libKF.h

@@ -24 +24 @@
 * \brief Basic Kalman filter with full matrices (education purpose only)! Will be deleted soon!
 */
-class KalmanFull : public BM { 
+
+class KalmanFull {
         int dimx, dimy, dimu;
         mat A, B, C, D, R, Q;
-        
-        //cache 
+
+        //cache
         mat _Pp, _Ry, _iRy, _K;
 public:
-        //posterior 
+        //posterior
         //! Mean value of the posterior density
         vec mu;
@@ -38 +39 @@
 
 public:
-        //! Full constructor 
-        KalmanFull ( mat A, mat B, mat C, mat D, mat R, mat Q, mat P0, vec mu0);
+        //! Full constructor
+        KalmanFull ( mat A, mat B, mat C, mat D, mat R, mat Q, mat P0, vec mu0 );
         //! Here dt = [yt;ut] of appropriate dimensions
-        void bayes(const vec &dt); 
-        epdf* _epdf(){return NULL;}; 
+        void bayes ( const vec &dt );
 
         friend std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf );
@@ -53 +53 @@
 */
 template<class sq_T>
-class Kalman : public BM { 
+
+class Kalman : public BM {
 protected:
+        RV rvy;
+        RV rvu;
         int dimx, dimy, dimu;
         mat A, B, C, D;
-        sq_T R, Q;
-        
-        //!posterior on $x_t$
+        sq_T Q,R;
+
+        //!posterior density on $x_t$
         enorm<sq_T> est;
         //!preditive density on $y_t$
         enorm<sq_T> fy;
-        
+
         mat _K;
         //cache of fy
-        vec* _yp; 
+        vec* _yp;
         sq_T* _Ry,*_iRy;
         //cache of est
         vec* _mu;
         sq_T* _P, *_iP;
-        
+
 public:
         //! Default constructor
-        Kalman (RV rvx0, RV rvy0, RV rvu0);
-        //! Set parameters with check of relevance 
-        void set_parameters (const mat &A0,const mat &B0,const mat &C0,const mat &D0,const sq_T &R0,const sq_T &Q0);
+        Kalman ( RV rvx0, RV rvy0, RV rvu0 );
+        //! Copy constructor
+        Kalman ( const Kalman<sq_T> &K0 );
+        //! Set parameters with check of relevance
+        void set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const sq_T &R0,const sq_T &Q0 );
         //! Set estimate values, used e.g. in initialization.
-        void set_est(const vec &mu0, const sq_T &P0 ){est.set_parameters(mu0,P0);};
+
+        void set_est ( const vec &mu0, const sq_T &P0 ) {
+                sq_T pom(dimy);
+                est.set_parameters ( mu0,P0 );
+                P0.mult_sym(C,pom);
+                fy.set_parameters ( C*mu0, pom );
+        };
+
         //! Here dt = [yt;ut] of appropriate dimensions
-        void bayes(const vec &dt); 
+        void bayes ( const vec &dt );
+        epdf& _epdf() {return est;}
 
 //      friend std::ostream &operator<< ( std::ostream &os, const Kalman<sq_T> &kf );
 
         //!access functions
-        enorm<sq_T>* _epdf(){return &est;} 
-};
-
-/*!
-\brief Extended Kalman Filter 
+};
+
+/*!
+\brief Extended Kalman Filter
 
 An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
 */
 template<class sq_T>
+
 class EKF : public Kalman<ldmat> {
         //! Internal Model f(x,u)
@@ -99 +112 @@
         //! Observation Model h(x,u)
         diffbifn* phxu;
-public: 
+public:
         //! Default constructor
-        EKF (RV rvx, RV rvy, RV rvu);
-        void set_parameters(diffbifn* pfxu, diffbifn* phxu, const sq_T Q0, const sq_T R0);
+        EKF ( RV rvx, RV rvy, RV rvu );
+        void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const sq_T Q0, const sq_T R0 );
         //! Here dt = [yt;ut] of appropriate dimensions
-        void bayes(const vec &dt);      
+        void bayes ( const vec &dt );
+};
+
+/*!
+\brief Kalman Filter with conditional diagonal matrices R and Q.
+*/
+
+class KFcondQR : public Kalman<ldmat>, public BMcond {
+//protected:
+public:
+        KFcondQR ( RV rvx, RV rvy, RV rvu, RV rvRQ ) : Kalman<ldmat> ( rvx, rvy,rvu ),BMcond ( rvRQ ) {};
+
+        void condition ( const vec &RQ );
+};
+
+/*!
+\brief Kalman Filter with conditional diagonal matrices R and Q.
+*/
+
+class KFcondR : public Kalman<ldmat>, public BMcond {
+//protected:
+public:
+        KFcondR ( RV rvx, RV rvy, RV rvu, RV rvR ) : Kalman<ldmat> ( rvx, rvy,rvu ),BMcond ( rvR ) {};
+
+        void condition ( const vec &R );
 };
 
@@ -110 +147 @@
 
 template<class sq_T>
-Kalman<sq_T>::Kalman(RV rvx, RV rvy, RV rvu): BM(),
-dimx(rvx.count()), dimy(rvy.count()),dimu(rvu.count()), 
- A(dimx,dimx), B(dimx,dimu), C(dimy,dimx), D(dimy,dimu), est(rvx), fy(rvy){
+Kalman<sq_T>::Kalman ( const Kalman<sq_T> &K0 ) : BM ( K0.rv ),rvy ( K0.rvy ),rvu ( K0.rvu ),
+                dimx ( rv.count() ), dimy ( rvy.count() ),dimu ( rvu.count() ),
+                A ( dimx,dimx ), B ( dimx,dimu ), C ( dimy,dimx ), D ( dimy,dimu ),est ( rv ), fy ( rvy ) {
+
+        this->set_parameters ( K0.A, K0.B, K0.C, K0.D, K0.R, K0.Q );
+
+//establish pointers
+        _mu = est._mu();
+        est._R ( _P,_iP );
+
+//fy
+        _yp = fy._mu();
+        fy._R ( _Ry,_iRy );
+
+//reset copy values in pointers
+        *_mu = *K0._mu;
+        *_P = *K0._P;
+        *_iP = *K0._iP;
+        *_yp = *K0._yp;
+        *_iRy = *K0._iRy;
+        *_Ry = *K0._Ry;
+
+}
+
+template<class sq_T>
+Kalman<sq_T>::Kalman ( RV rvx, RV rvy0, RV rvu0 ) : BM ( rvx ),rvy ( rvy0 ),rvu ( rvu0 ),
+                dimx ( rvx.count() ), dimy ( rvy.count() ),dimu ( rvu.count() ),
+                A ( dimx,dimx ), B ( dimx,dimu ), C ( dimy,dimx ), D ( dimy,dimu ),
+                Q(dimx), R (dimy),
+                est ( rvx ), fy ( rvy ) {
 //assign cache
 //est
-_mu = est._mu();
-est._R(_P,_iP);
+        _mu = est._mu();
+        est._R ( _P,_iP );
 
 //fy
-_yp = fy._mu();
-fy._R(_Ry,_iRy);
-};
-
-template<class sq_T>
-void Kalman<sq_T>::set_parameters(const mat &A0,const  mat &B0, const mat &C0, const mat &D0, const sq_T &R0, const sq_T &Q0) {
-        it_assert_debug( A0.cols()==dimx, "Kalman: A is not square" );
-        it_assert_debug( B0.rows()==dimx, "Kalman: B is not compatible" );
-        it_assert_debug( C0.cols()==dimx, "Kalman: C is not square" );
-        it_assert_debug(( D0.rows()==dimy ) || ( D0.cols()==dimu ),     "Kalman: D is not compatible" );
-        it_assert_debug(( R0.cols()==dimy ) || ( R0.rows()==dimy ), "Kalman: R is not compatible" );
-        it_assert_debug(( Q0.cols()==dimx ) || ( Q0.rows()==dimx ), "Kalman: Q is not compatible" );
+        _yp = fy._mu();
+        fy._R ( _Ry,_iRy );
+};
+
+template<class sq_T>
+void Kalman<sq_T>::set_parameters ( const mat &A0,const  mat &B0, const mat &C0, const mat &D0, const sq_T &R0, const sq_T &Q0 ) {
+        it_assert_debug ( A0.cols() ==dimx, "Kalman: A is not square" );
+        it_assert_debug ( B0.rows() ==dimx, "Kalman: B is not compatible" );
+        it_assert_debug ( C0.cols() ==dimx, "Kalman: C is not square" );
+        it_assert_debug ( ( D0.rows() ==dimy ) || ( D0.cols() ==dimu ), "Kalman: D is not compatible" );
+        it_assert_debug ( ( R0.cols() ==dimy ) || ( R0.rows() ==dimy ), "Kalman: R is not compatible" );
+        it_assert_debug ( ( Q0.cols() ==dimx ) || ( Q0.rows() ==dimx ), "Kalman: Q is not compatible" );
 
         A = A0;
@@ -137 +201 @@
         D = D0;
         R = R0;
-        Q = Q0; 
+        Q = Q0;
 }
 
 template<class sq_T>
-void Kalman<sq_T>::bayes( const vec &dt) {
-        it_assert_debug( dt.length()==( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
-
-        vec u = dt.get( dimy,dimy+dimu-1 );
-        vec y = dt.get( 0,dimy-1 );
+void Kalman<sq_T>::bayes ( const vec &dt ) {
+        it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
+
+        vec u = dt.get ( dimy,dimy+dimu-1 );
+        vec y = dt.get ( 0,dimy-1 );
         //Time update
-        *_mu = A*(*_mu) + B*u;
+        *_mu = A* ( *_mu ) + B*u;
         //P  = A*P*A.transpose() + Q; in sq_T
-        _P->mult_sym( A );
-        (*_P)+=Q;
+        _P->mult_sym ( A );
+        ( *_P ) +=Q;
 
         //Data update
         //_Ry = C*P*C.transpose() + R; in sq_T
-        _P->mult_sym( C, *_Ry);
-        (*_Ry)+=R;
+        _P->mult_sym ( C, *_Ry );
+        ( *_Ry ) +=R;
 
         mat Pfull = _P->to_mat();
-        
-        _Ry->inv( *_iRy ); // result is in _iRy;
-        _K = Pfull*C.transpose()*(_iRy->to_mat());
-        
-        sq_T pom((int)Pfull.rows());
-        _iRy->mult_sym_t(C*Pfull,pom);
-        (*_P) -= pom; // P = P -PC'iRy*CP;
-        (*_yp) = C*(*_mu)+D*u; //y prediction
-        (*_mu) += _K*( y-(*_yp) );
-        
-        if (evalll==true) {
-                ll+=fy.evalpdflog(*_yp);
+
+        _Ry->inv ( *_iRy ); // result is in _iRy;
+        fy._cached ( true );
+        _K = Pfull*C.transpose() * ( _iRy->to_mat() );
+
+        sq_T pom ( ( int ) Pfull.rows() );
+        _iRy->mult_sym_t ( C*Pfull,pom );
+        ( *_P ) -= pom; // P = P -PC'iRy*CP;
+        ( *_yp ) = C* ( *_mu ) +D*u; //y prediction
+        ( *_mu ) += _K* ( y- ( *_yp ) );
+
+
+        if ( evalll==true ) { //likelihood of observation y
+                ll=fy.evalpdflog ( y );
         }
+
+//cout << "y: " << y-(*_yp) <<" R: " << _Ry->to_mat() << " iR: " << _iRy->to_mat() << " ll: " << ll <<endl;
+
 };
 
@@ -176 +245 @@
 
 template<class sq_T>
-EKF<sq_T>::EKF(RV rvx0, RV rvy0, RV rvu0): Kalman<ldmat>(rvx0,rvy0,rvu0){}
-
-template<class sq_T>
-void EKF<sq_T>::set_parameters(diffbifn* pfxu0,  diffbifn* phxu0,const sq_T Q0,const sq_T R0) {
-                pfxu = pfxu0;
-                phxu = phxu0;
-                
-                //initialize matrices A C, later, these will be only updated!
-                pfxu->dfdx_cond(*_mu,zeros(dimu),A,true);
-                pfxu->dfdu_cond(*_mu,zeros(dimu),B,true);
-                phxu->dfdx_cond(*_mu,zeros(dimu),C,true);
-                phxu->dfdu_cond(*_mu,zeros(dimu),D,true);
-
-                R = R0;
-                Q = Q0;
+EKF<sq_T>::EKF ( RV rvx0, RV rvy0, RV rvu0 ) : Kalman<ldmat> ( rvx0,rvy0,rvu0 ) {}
+
+template<class sq_T>
+void EKF<sq_T>::set_parameters ( diffbifn* pfxu0,  diffbifn* phxu0,const sq_T Q0,const sq_T R0 ) {
+        pfxu = pfxu0;
+        phxu = phxu0;
+
+        //initialize matrices A C, later, these will be only updated!
+        pfxu->dfdx_cond ( *_mu,zeros ( dimu ),A,true );
+        pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true );
+        phxu->dfdx_cond ( *_mu,zeros ( dimu ),C,true );
+        phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true );
+
+        R = R0;
+        Q = Q0;
 
         using std::cout;
@@ -201 +270 @@
 
 template<class sq_T>
-void EKF<sq_T>::bayes( const vec &dt) {
-        it_assert_debug( dt.length()==( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
-
-        vec u = dt.get( dimy,dimy+dimu-1 );
-        vec y = dt.get( 0,dimy-1 );
+void EKF<sq_T>::bayes ( const vec &dt ) {
+        it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
+
+        vec u = dt.get ( dimy,dimy+dimu-1 );
+        vec y = dt.get ( 0,dimy-1 );
         //Time update
-        *_mu = pfxu->eval(*_mu, u);
-        pfxu->dfdx_cond(*_mu,u,A,false); //update A by a derivative of fx
-        
+        *_mu = pfxu->eval ( *_mu, u );
+        pfxu->dfdx_cond ( *_mu,u,A,false ); //update A by a derivative of fx
+
         //P  = A*P*A.transpose() + Q; in sq_T
-        _P->mult_sym( A );
-        (*_P)+=Q;
+        _P->mult_sym ( A );
+        ( *_P ) +=Q;
 
         //Data update
-        phxu->dfdx_cond(*_mu,u,C,false); //update C by a derivative hx
+        phxu->dfdx_cond ( *_mu,u,C,false ); //update C by a derivative hx
         //_Ry = C*P*C.transpose() + R; in sq_T
-        _P->mult_sym( C, *_Ry);
-        (*_Ry)+=R;
+        _P->mult_sym ( C, *_Ry );
+        ( *_Ry ) +=R;
 
         mat Pfull = _P->to_mat();
-        
-        _Ry->inv( *_iRy ); // result is in _iRy;
-        _K = Pfull*C.transpose()*(_iRy->to_mat());
-        (*_P) -= _K*C*Pfull; // P = P -KCP;
-        *_yp = phxu->eval(*_mu,u); //y prediction
-        (*_mu) += _K*( y-*_yp );
-        
-        if (evalll==true) {ll+=est.evalpdflog(y);}
+
+        _Ry->inv ( *_iRy ); // result is in _iRy;
+        _K = Pfull*C.transpose() * ( _iRy->to_mat() );
+
+        sq_T pom ( ( int ) Pfull.rows() );
+        _iRy->mult_sym_t ( C*Pfull,pom );
+        ( *_P ) -= pom; // P = P -PC'iRy*CP;
+        *_yp = phxu->eval ( *_mu,u ); //y prediction
+        ( *_mu ) += _K* ( y-*_yp );
+
+        if ( evalll==true ) {ll+=fy.evalpdflog ( y );}
 };
 

bdm/estim/libPF.cpp

@@ -5 +5 @@
 using std::endl;
 
-PF::PF(vec w0){w=w0/sum(w0); n=w.length();}
 
-ivec PF::resample( RESAMPLING_METHOD method ) {
-        ivec ind=zeros_i( n );
-        ivec N_babies = zeros_i( n );
-        vec cumDist = cumsum( w );
-        vec u( n );
-        int i,j,parent;
-        double u0;
+void PF::bayes ( const vec &dt ) {
+        int i;
+        vec lls ( n );
+        ivec ind;
+        double mlls=-std::numeric_limits<double>::infinity(), sum=0.0;
 
-        switch ( method ) {
-        case MULTINOMIAL:
-                u( n - 1 ) = pow( URNG.sample(), 1.0 / n );
-                for ( i = n - 2;i >= 0;i-- ) {
-                        u( i ) = u( i + 1 ) * pow( URNG.sample(), 1.0 / ( i + 1 ) );
-                }
-                break;
-        case STRATIFIED:
-                for ( i = 0;i < n;i++ ) {
-                        u( i ) = ( i + URNG.sample() ) / n;
-                }
-                break;
-        case SYSTEMATIC:
-                u0 = URNG.sample();
-                for ( i = 0;i < n;i++ ) {
-                        u( i ) = ( i + u0 ) / n;
-                }
-                break;
-        default:
-                it_error( "PF::resample(): Unknown resampling method" );
-        }
-        
-//      // <<<<<<<<<<<
-//      using std::cout;
-//      cout << "resampling:" << endl;
-//      cout << w <<endl;
-//      cout << cumDist <<endl;
-//      cout << u <<endl;
+        for ( i=0;i<n;i++ ) {
+                //generate new samples from paramater evolution model;
+                _samples ( i ) = par.samplecond ( _samples ( i ), lls ( i ) );
+                lls ( i ) *= obs.evalcond ( dt,_samples ( i ) );
 
-        // U is now full
-        j = 0;
-        for ( i = 0;i < n;i++ ) {
-                while ( u( i ) > cumDist( j ) ) j++;
-                N_babies( j )++;
+                if ( lls ( i ) >mlls ) mlls=lls ( i ); //find maximum
         }
 
-//      // <<<<<<<<<<<
-//      cout << N_babies <<endl;
+        // compute weights
+        for ( i=0;i<n;i++ ) {
+                _w ( i ) *= exp ( lls ( i ) - mlls ); // multiply w by likelihood
+        }
 
-        // We have assigned new babies for each Particle
-        // Now, we fill the resulting index such that:
-        // * particles with at least one baby should not move *
-        // This assures that reassignment can be done inplace;
+        //renormalize
+        for ( i=0;i<n;i++ ) {sum+=_w ( i );};
 
-        // find the first parent;
-        parent=0; while(N_babies(parent)==0) parent++;
-        // Build index
-        for ( i = 0;i < n;i++ ) {
-                if ( N_babies( i ) > 0 ){
-                        ind( i ) = i;
-                        N_babies( i ) --; //this index was now replicated;
-                        }
-                else {
-                        // test if the parent has been fully replicated 
-                        // if yes, find the next one
-                        while((N_babies(parent)==0) || (N_babies(parent)==1 && parent>i) ) parent++;
-                        // Replicate parent
-                        ind (i) = parent;
-                        N_babies( parent ) --; //this index was now replicated;
-                }
-/*                      // <<<<<<<<<<<
-                        cout <<ind << endl;*/
-        }
-        return ind;
+        _w ( i ) /=sum; //?
+
+        ind = est.resample();
+
 }
 
-TrivialPF::TrivialPF( mpdf &par0, mpdf &obs0, BM &prop0, int n0 ) : PF(1/n*ones(n)) {
-        is_proposal = true;
-        prop = &prop0;
-        par = &par0;
-        obs = &obs0;
-}
-TrivialPF::TrivialPF( mpdf &par0, mpdf &obs0,  int n0 ) : PF(1/n*ones(n)) {
-        is_proposal = false;
-        par = &par0;
-        obs = &obs0;
+void PF::set_est ( const epdf &epdf0 ) {
+        int i;
+
+        for ( i=0;i<n;i++ ) {
+                _samples ( i ) = epdf0.sample();
+        }
 }
 
-void TrivialPF::bayes( const vec &dt , bool evalll ) {
-        int i;
-        vec oldp;
-        double ll, gl, sum = 0.0;
-        Sort<double> S;
-        ivec ind, iw;
-        
-        //generate new samples
-        for ( i=0;i<n;i++ ) {
-                if(is_proposal) {
-                        epdf* prop_epdf;
-        //              prop_epdf = prop._epdf();
-                        prop->bayes(dt);
-                        ptcls( i ) = prop_epdf->sample();
-                        }
-//              gl = prop_cond.eval( ptcls( i ) );
-
-//              obs.evalcond( ptcls( i ), &obs_cond );
-//              ll = obs_cond.eval( dt );
-                w( i ) *= ll/gl;
-        }
-        //renormalize
-        for ( i=0;i<n;i++ ){sum+=w( i );};
-        w( i ) /=sum; //?
-        
-        ind = resample();
-        iw = S.sort_index( 0,n-1,w ); // the first one in iw is the strongest
-
-        for ( i=0;i<n;i++ ) {
-                ptcls( i ) = ptcls( i ); //potentionally dangerous!
-        }
-        
-}
+//MPF::MPF:{}

bdm/estim/libPF.h

@@ -15 +15 @@
 
 #include <itpp/itbase.h>
-#include "../stat/libBM.h"
+#include "../stat/libEF.h"
 #include "../math/libDC.h"
 
 using namespace itpp;
 
-enum RESAMPLING_METHOD { MULTINOMIAL = 0, STRATIFIED = 1, SYSTEMATIC = 3 };
+/*!
+* \brief Trivial particle filter with proposal density equal to parameter evolution model.
 
-/*!
-* \brief A Particle Filter prototype
+Posterior density is represented by a weighted empirical density (\c eEmp ).
+*/
 
-Bayesian Filtering equations hold.
-*/
-class PF : public BM { 
+class PF : public BM {
 protected:
-        int n; //number of particles
-        vec w; //particle weights
-        Uniform_RNG URNG; //used for resampling
-        
+        //!number of particles;
+        int n;
+        //!posterior density
+        eEmp est;
+        //! pointer into \c eEmp
+        vec &_w;
+        //! pointer into \c eEmp
+        Array<vec> &_samples;
+        //! Parameter evolution model
+        mpdf &par;
+        //! Observation model
+        mpdf &obs;
 public:
-        //! Returns indexes of particles that should be resampled. The ordering MUST guarantee inplace replacement. (Important for MPF.)
-        ivec resample(RESAMPLING_METHOD method = SYSTEMATIC);
-        PF (vec w);
+        PF ( const RV &rv0, mpdf &par0,  mpdf &obs0, int n0 ) :BM ( rv0 ),
+                        n ( n0 ),est ( rv0,n ),_w ( est._w() ),_samples ( est._samples() ),
+                        par ( par0 ), obs ( obs0 ) {};
+
+//      void set_parametres(mpdf &par0, mpdf &obs0) {par=&par0;obs=&obs0;};
+        void set_est ( const epdf &epdf0 );
         //TODO remove or implement bayes()!
-        void bayes(const vec &dt){};
-        epdf* _epdf(){return NULL;}
+        void bayes ( const vec &dt );
 };
 
 /*!
-* \brief Trivial particle filter with proposal density that is not conditioned on the data.
+\brief Marginalized Particle filter
 
-
+Trivial version: proposal $=$ parameter evolution, observation model is not used. (it is assumed to be part of BM).
 */
 
-class TrivialPF : public PF {
-        Array<vec> ptcls;
-        
-        bool is_proposal;
-        BM *prop;
-        mpdf *par;
-        mpdf *obs;
-        
+template<class BM_T>
+
+class MPF : public PF {
+        BM_T* Bms[1000];
+
+        //! internal class for MPDF providing composition of eEmp with external components
+
+class mpfepdf : public epdf  {
+        protected:
+                eEmp &E;
+                vec &_w;
+                Array<epdf*> Coms;
         public:
-        TrivialPF(mpdf &par, mpdf &obs, BM &prop, int n0);
-        TrivialPF(mpdf &par, mpdf &obs, int n0);
-        void bayes(const vec &dt, bool evalll);
+                mpfepdf ( eEmp &E0, const RV &rvc ) :
+                                epdf ( RV( ) ), E ( E0 ),  _w ( E._w() ),
+                                Coms ( _w.length() ) {
+                        rv.add ( E._rv() );
+                        rv.add ( rvc );
+                };
+
+                void set_elements ( int &i, double wi, epdf* ep )
+                {_w ( i ) =wi; Coms ( i ) =ep;};
+
+                vec mean() const {
+                        // 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 sample() const {it_error ( "Not implemented" );return 0;}
+
+                double evalpdflog ( const vec &val ) const {it_error ( "not implemented" ); return 0.0;}
+        };
+
+        //! estimate joining PF.est with conditional
+        mpfepdf jest;
+
+public:
+        //! Default constructor.
+        MPF ( const RV &rvlin, const RV &rvpf, mpdf &par0, mpdf &obs0, int n, const BM_T &BMcond0 ) : PF ( rvpf ,par0,obs0,n ),jest ( est,rvlin ) {
+                //
+                //TODO test if rv and BMcond.rv are compatible.
+                rv.add ( rvlin );
+                //
+
+                if ( n>1000 ) it_error ( "increase 1000 here!" );
+
+                for ( int i=0;i<n;i++ ) {
+                        Bms[i] = new BM_T ( BMcond0 ); //copy constructor
+                        epdf& pom=Bms[i]->_epdf();
+                        jest.set_elements ( i,1.0/n,&pom );
+                }
+        };
+
+        ~MPF() {
+        }
+
+        void bayes ( const vec &dt );
+        epdf& _epdf() {return jest;}
+
+        void set_est ( const epdf& epdf0 ) {
+                PF::set_est ( epdf0 );  // sample params in condition
+                // copy conditions to BMs
+
+                for ( int i=0;i<n;i++ ) {Bms[i]->condition ( _samples ( i ) );}
+        }
 };
 
-class MPF : public TrivialPF {
-        Array<BM> Bms;
-        public:
-        MPF(BM &B, BM &prop, mpdf &obs, mpdf &par);
-        void bayes(vec &dt);    
-};
+template<class BM_T>
+void MPF<BM_T>::bayes ( const vec &dt ) {
+        int i;
+        vec lls ( n );
+        ivec ind;
+        double mlls=-std::numeric_limits<double>::infinity();
+        double sum=0.0;
+
+        for ( i=0;i<n;i++ ) {
+                //generate new samples from paramater evolution model;
+                _samples ( i ) = par.samplecond ( _samples ( i ), lls ( i ) );
+                Bms[i]->condition ( _samples ( i ) );
+                Bms[i]->bayes ( dt );
+                lls ( i ) += Bms[i]->_ll();
+
+                if ( lls ( i ) >mlls ) mlls=lls ( i ); //find maximum likelihood (for numerical stability)
+        }
+
+        // compute weights
+        for ( i=0;i<n;i++ ) {
+                _w ( i ) *= exp ( lls ( i ) - mlls ); // multiply w by likelihood
+        }
+
+        //renormalize
+        for ( i=0;i<n;i++ ) {sum+=_w ( i );};
+
+        _w /=sum; //?
+
+        if ( ( _w*_w ) < ( 0.5*n ) ) {
+                ind = est.resample();
+                // Resample Bms!
+
+                for ( i=0;i<n;i++ ) {
+                        if ( ind ( i ) !=i ) {//replace the current Bm by a new one
+                                //fixme this would require new assignment operator
+                                // *Bms[i] = *Bms[ind ( i ) ];
+                                
+                                // poor-man's solution: replicate constructor here
+                                // copied from MPF::MPF
+                                delete Bms[i];
+                                Bms[i] = new BM_T ( *Bms[ind ( i ) ] ); //copy constructor
+                                epdf& pom=Bms[i]->_epdf();
+                                jest.set_elements ( i,1.0/n,&pom );
+                        }
+                };
+        }
+}
 
 #endif // KF_H