kalman.h

Go to the documentation of this file.

#ifndef KF_H
#define KF_H


#include "../math/functions.h"
#include "../stat/exp_family.h"
#include "../math/chmat.h"
#include "../base/user_info.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;
        }
        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:
        shared_ptr<diffbifn> pfxu;

        shared_ptr<diffbifn> phxu;

        enorm<fsqmat> E;
public:
        EKFfull ( );

        void set_parameters ( const shared_ptr<diffbifn> &pfxu, const shared_ptr<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 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:
        shared_ptr<diffbifn> pfxu;

        shared_ptr<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 ( const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const chmat Q0, const chmat R0 );

        void bayes ( const vec &dt );

        void from_setting ( const Setting &set );

        // TODO dodelat void to_setting( Setting &set ) const;

        const enorm<chmat>& posterior() {return est;}
};

UIREGISTER ( EKFCh );
SHAREDPTR ( EKFCh );


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 )->posterior().mean(), A ( 0 )->posterior()._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 )->posterior() ;
                        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() );
                }
        }
        const enorm<chmat>& posterior() const {
                return est;
        }

        void from_setting ( const Setting &set );

        // TODO dodelat void to_setting( Setting &set ) const;

};

UIREGISTER ( MultiModel );
SHAREDPTR ( MultiModel );



//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

---