libKF.h

Go to the documentation of this file.

#ifndef KF_H
#define KF_H


#include "../stat/libFN.h"
#include "../stat/libEF.h"
#include "../math/chmat.h"

namespace bdm
{

        class KalmanFull
        {
                protected:
                        int dimx, dimy, dimu;
                        mat A, B, C, D, R, Q;

                        //cache
                        mat _Pp, _Ry, _iRy, _K;
                public:
                        //posterior
                        vec mu;
                        mat P;

                        bool evalll;
                        double ll;
                public:
                        KalmanFull ( mat A, mat B, mat C, mat D, mat R, mat Q, mat P0, vec mu0 );
                        void bayes ( const vec &dt );
                        friend std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf );
                        KalmanFull() {};
        };


        template<class sq_T>

        class Kalman : public BM
        {
                protected:
                        RV rvy;
                        RV rvu;
                        int dimx;
                        int dimy;
                        int dimu;
                        mat A;
                        mat B;
                        mat C;
                        mat D;
                        sq_T Q;
                        sq_T R;

                        enorm<sq_T> est;
                        enorm<sq_T> fy;

                        mat _K;
                        vec& _yp;
                        sq_T& _Ry;
                        vec& _mu;
                        sq_T& _P;

                public:
                        Kalman ( );
                        Kalman ( const Kalman<sq_T> &K0 );
                        void set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const sq_T &Q0,const sq_T &R0 );
                        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 );
                        };

                        void bayes ( const vec &dt );
                        const epdf& posterior() const {return est;}
                        const enorm<sq_T>* _e() const {return &est;}
                        mat& __K() {return _K;}
                        vec _dP() {return _P->getD();}
        };

        class KalmanCh : public Kalman<chmat>
        {
                protected:
                        mat preA;
                        mat postA;

                public:
                        BM* _copy_() const
                        {
                                KalmanCh* K=new KalmanCh;
                                K->set_parameters ( A,B,C,D,Q,R );
                                K->set_statistics ( _mu,_P );
                                return K;
                        }
                        void set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const chmat &Q0,const chmat &R0 );
                        void set_statistics ( const vec &mu0, const chmat &P0 )
                        {
                                est.set_parameters ( mu0,P0 );
                        };


                        void bayes ( const vec &dt );
        };

        class EKFfull : public KalmanFull, public BM
        {
                protected:
                        diffbifn* pfxu;
                        diffbifn* phxu;

                        enorm<fsqmat> E;
                public:
                        EKFfull ( );
                        void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const mat Q0, const mat R0 );
                        void bayes ( const vec &dt );
                        void set_statistics ( vec mu0, mat P0 ) {mu=mu0;P=P0;};
                        const epdf& posterior() const{return E;};
                        const enorm<fsqmat>* _e() const{return &E;};
                        const mat _R() {return P;}
        };

        template<class sq_T>
        class EKF : public Kalman<fsqmat>
        {
                        diffbifn* pfxu;
                        diffbifn* phxu;
                public:
                        EKF ( RV rvx, RV rvy, RV rvu );
                        EKF<sq_T>* _copy_() const { return new EKF<sq_T> ( this ); }
                        void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const sq_T Q0, const sq_T R0 );
                        void bayes ( const vec &dt );
        };

        class EKFCh : public KalmanCh
        {
                protected:
                        diffbifn* pfxu;
                        diffbifn* phxu;
                public:
                        BM* _copy_() const
                        {
                                EKFCh* E=new EKFCh;
                                E->set_parameters ( pfxu,phxu,Q,R );
                                E->set_statistics ( _mu,_P );
                                return E;
                        }
                        void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const chmat Q0, const chmat R0 );
                        void bayes ( const vec &dt );
        };

        class KFcondQR : public Kalman<ldmat>
        {
//protected:
                public:
                        void condition ( const vec &QR )
                        {
                                it_assert_debug ( QR.length() == ( dimx+dimy ),"KFcondRQ: conditioning by incompatible vector" );

                                Q.setD ( QR ( 0, dimx-1 ) );
                                R.setD ( QR ( dimx, -1 ) );
                        };
        };

        class KFcondR : public Kalman<ldmat>
        {
//protected:
                public:
                        KFcondR ( ) : Kalman<ldmat> ( ) {};

                        void condition ( const vec &R0 )
                        {
                                it_assert_debug ( R0.length() == ( dimy ),"KFcondR: conditioning by incompatible vector" );

                                R.setD ( R0 );
                        };

        };


        template<class sq_T>
        Kalman<sq_T>::Kalman ( const Kalman<sq_T> &K0 ) : BM ( ),rvy ( K0.rvy ),rvu ( K0.rvu ),
                        dimx ( K0.dimx ), dimy ( K0.dimy ),dimu ( K0.dimu ),
                        A ( K0.A ), B ( K0.B ), C ( K0.C ), D ( K0.D ),
                        Q ( K0.Q ), R ( K0.R ),
                        est ( K0.est ), fy ( K0.fy ), _yp ( fy._mu() ),_Ry ( fy._R() ), _mu ( est._mu() ), _P ( est._R() )
        {

// copy values in pointers
//      _mu = K0._mu;
//      _P = K0._P;
//      _yp = K0._yp;
//      _Ry = K0._Ry;

        }

        template<class sq_T>
        Kalman<sq_T>::Kalman ( ) : BM (), est ( ), fy (),  _yp ( fy._mu() ), _Ry ( fy._R() ), _mu ( est._mu() ), _P ( est._R() )
        {
        };

        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 &Q0, const sq_T &R0 )
        {
                dimx = A0.rows();
                dimy = C0.rows();
                dimu = B0.cols();

                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;
                B = B0;
                C = C0;
                D = D0;
                R = R0;
                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" );

                sq_T iRy ( dimy );
                vec u = dt.get ( dimy,dimy+dimu-1 );
                vec y = dt.get ( 0,dimy-1 );
                //Time update
                _mu = A* _mu + B*u;
                //P  = A*P*A.transpose() + Q; in sq_T
                _P.mult_sym ( A );
                _P  +=Q;

                //Data update
                //_Ry = C*P*C.transpose() + R; in sq_T
                _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 )   //likelihood of observation y
                {
                        ll=fy.evallog ( y );
                }

//cout << "y: " << y-(*_yp) <<" R: " << _Ry->to_mat() << " iR: " << _iRy->to_mat() << " ll: " << ll <<endl;

        };

        class MultiModel: public BM
        {
                protected:
                        Array<EKFCh*> Models;
                        vec w;
                        vec _lls;
                        int policy;
                        enorm<chmat> est;
                public:
                        void set_parameters ( Array<EKFCh*> A, int pol0=1 )
                        {
                                Models=A;//TODO: test if evalll is set
                                w.set_length ( A.length() );
                                _lls.set_length ( A.length() );
                                policy=pol0;
                                
                                est.set_rv(RV("MM",A(0)->posterior().dimension(),0));
                                est.set_parameters(A(0)->_e()->mean(), A(0)->_e()->_R());
                        }
                        void bayes ( const vec &dt )
                        {
                                int n = Models.length();
                                int i;
                                for ( i=0;i<n;i++ )
                                {
                                        Models ( i )->bayes ( dt );
                                        _lls ( i ) = Models ( i )->_ll();
                                }
                                double mlls=max ( _lls );
                                w=exp ( _lls-mlls );
                                w/=sum ( w ); //normalization
                                //set statistics
                                switch ( policy )
                                {
                                        case 1:
                                        {
                                                int mi=max_index ( w );
                                                const enorm<chmat>* st=(Models(mi)->_e());
                                                est.set_parameters(st->mean(), st->_R());
                                        }
                                        break;
                                        default: it_error ( "unknown policy" );
                                }
                                // copy result to all models
                                for ( i=0;i<n;i++ )
                                {
                                                Models ( i )->set_statistics ( est.mean(),est._R());
                                }
                        }
                        //all posterior densities are equal => return the first one
                        const enorm<chmat>* _e() const {return &est;}
                                //all posterior densities are equal => return the first one
                        const enorm<chmat>& posterior() const {return est;}
        };


//TODO why not const pointer??

        template<class sq_T>
        EKF<sq_T>::EKF ( RV rvx0, RV rvy0, RV rvu0 ) : Kalman<sq_T> ( 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 );
                B.clear();
                phxu->dfdx_cond ( _mu,zeros ( dimu ),C,true );
//      phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true );
                D.clear();

                R = R0;
                Q = Q0;
        }

        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" );

                sq_T iRy ( dimy,dimy );
                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

                //P  = A*P*A.transpose() + Q; in sq_T
                _P.mult_sym ( A );
                _P +=Q;

                //Data update
                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;

                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 = phxu->eval ( _mu,u ); //y prediction
                ( _mu ) += _K* ( y-_yp );

                if ( evalll==true ) {ll+=fy.evallog ( y );}
        };


}
#endif // KF_H

---