root/library/bdm/estim/kalman.h @ 586

/*!
  \file
  \brief Bayesian Filtering for linear Gaussian models (Kalman Filter) and extensions
  \author Vaclav Smidl.

  -----------------------------------
  BDM++ - C++ library for Bayesian Decision Making under Uncertainty

  Using IT++ for numerical operations
  -----------------------------------
*/

#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
{

/*!
 * \brief Basic elements of linear state-space model

Parameter evolution model:\f[ x_t = A x_{t-1} + B u_t + Q^{1/2} e_t \f]
Observation model: \f[ y_t = C x_{t-1} + C u_t + Q^{1/2} w_t. \f]
Where $e_t$ and $w_t$ are independent vectors Normal(0,1)-distributed disturbances.
 */
template<class sq_T>
class StateSpace
{
        protected:
                //! cache of rv.count()
                int dimx;
                //! cache of rvy.count()
                int dimy;
                //! cache of rvu.count()
                int dimu;
                //! Matrix A
                mat A;
                //! Matrix B
                mat B;
                //! Matrix C
                mat C;
                //! Matrix D
                mat D;
                //! Matrix Q in square-root form
                sq_T Q;
                //! Matrix R in square-root form
                sq_T R;
        public:
                StateSpace() : dimx (0), dimy (0), dimu (0), A(), B(), C(), D(), Q(), R() {}
                void set_parameters (const mat &A0, const  mat &B0, const  mat &C0, const  mat &D0, const  sq_T &Q0, const sq_T &R0);
                void validate();
                //! not virtual in this case
                void from_setting (const Setting &set) {
                        UI::get (A, set, "A", UI::compulsory);
                        UI::get (B, set, "B", UI::compulsory);
                        UI::get (C, set, "C", UI::compulsory);
                        UI::get (D, set, "D", UI::compulsory);
                        mat Qtm, Rtm;
                        if(!UI::get(Qtm, set, "Q", UI::optional)){
                                vec dq;
                                UI::get(dq, set, "dQ", UI::compulsory);
                                Qtm=diag(dq);
                        }
                        if(!UI::get(Rtm, set, "R", UI::optional)){
                                vec dr;
                                UI::get(dr, set, "dQ", UI::compulsory);
                                Rtm=diag(dr);
                        }
                        R=Rtm; // automatic conversion
                        Q=Qtm; 
                        
                        validate();
                }               
                //! access function
                int _dimx(){return dimx;}
                //! access function
                int _dimy(){return dimy;}
                //! access function
                int _dimu(){return dimu;}
                //! access function
                mat _A(){return A;}
                //! access function
                mat _B(){return B;}
                //! access function
                mat _C(){return C;}
                //! access function
                mat _D(){return D;}
                //! access function
                sq_T _Q(){return Q;}
                //! access function
                sq_T _R(){return R;}
};

//! Common abstract base for Kalman filters 
template<class sq_T>
class Kalman: public BM, public StateSpace<sq_T>
{
        protected:
                //! id of output
                RV yrv;
                //! id of input
                RV urv;
                //! Kalman gain
                mat  _K;
                //!posterior
                shared_ptr<enorm<sq_T> > est;
                //!marginal on data f(y|y)
                enorm<sq_T>  fy;
        public:
                Kalman() : BM(), StateSpace<sq_T>(), yrv(),urv(), _K(),  est(new enorm<sq_T>){}
                void set_statistics (const vec &mu0, const mat &P0) {est->set_parameters (mu0, P0); };
                void set_statistics (const vec &mu0, const sq_T &P0) {est->set_parameters (mu0, P0); };
                //! posterior
                const enorm<sq_T>& posterior() const {return *est.get();}
                //! shared posterior
                shared_ptr<epdf> shared_posterior() {return est;}
                //! load basic elements of Kalman from structure
                void from_setting (const Setting &set) {
                        StateSpace<sq_T>::from_setting(set);
                                                
                        mat P0; vec mu0;
                        UI::get(mu0, set, "mu0", UI::optional);
                        UI::get(P0, set,  "P0", UI::optional);
                        set_statistics(mu0,P0);
                        // Initial values
                        UI::get (yrv, set, "yrv", UI::optional);
                        UI::get (urv, set, "urv", UI::optional);
                        set_drv(concat(yrv,urv));
                        
                        validate();
                }
                void validate() {
                        StateSpace<sq_T>::validate();
                        bdm_assert_debug(est->dimension(), "Statistics and model parameters mismatch");
                }
};
/*!
* \brief Basic Kalman filter with full matrices
*/

class KalmanFull : public Kalman<fsqmat>
{
        public:
                //! For EKFfull;
                KalmanFull() :Kalman<fsqmat>(){};
                //! Here dt = [yt;ut] of appropriate dimensions
                void bayes (const vec &dt);
};
UIREGISTER(KalmanFull);


/*! \brief Kalman filter in square root form

Trivial example:
\include kalman_simple.cpp

Complete constructor:
*/
class KalmanCh : public Kalman<chmat>
{
        protected:
                //! @{ \name Internal storage - needs initialize()
                //! pre array (triangular matrix)
                mat preA;
                //! post array (triangular matrix)
                mat postA;
                //!@}
        public:
                //! copy constructor
                BM* _copy_() const {
                        KalmanCh* K = new KalmanCh;
                        K->set_parameters (A, B, C, D, Q, R);
                        K->set_statistics (est->_mu(), est->_R());
                        return K;
                }
                //! set parameters for adapt from Kalman
                void set_parameters (const mat &A0, const mat &B0, const mat &C0, const mat &D0, const chmat &Q0, const chmat &R0);
                //! initialize internal parametetrs
                void initialize();

                /*!\brief  Here dt = [yt;ut] of appropriate dimensions

                The following equality hold::\f[
                \left[\begin{array}{cc}
                R^{0.5}\\
                P_{t|t-1}^{0.5}C' & P_{t|t-1}^{0.5}CA'\\
                & Q^{0.5}\end{array}\right]<\mathrm{orth.oper.}>=\left[\begin{array}{cc}
                R_{y}^{0.5} & KA'\\
                & P_{t+1|t}^{0.5}\\
                \\\end{array}\right]\f]

                Thus this object evaluates only predictors! Not filtering densities.
                */
                void bayes (const vec &dt);
                
                void from_setting(const Setting &set){
                        Kalman<chmat>::from_setting(set);
                        initialize();
                }
};
UIREGISTER(KalmanCh);

/*!
\brief Extended Kalman Filter in full matrices

An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
*/
class EKFfull : public KalmanFull
{
        protected:
                //! Internal Model f(x,u)
                shared_ptr<diffbifn> pfxu;

                //! Observation Model h(x,u)
                shared_ptr<diffbifn> phxu;

        public:
                //! Default constructor
                EKFfull ();

                //! Set nonlinear functions for mean values and covariance matrices.
                void set_parameters (const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const mat Q0, const mat R0);

                //! Here dt = [yt;ut] of appropriate dimensions
                void bayes (const vec &dt);
                //! set estimates
                void set_statistics (const vec &mu0, const mat &P0) {
                        est->set_parameters (mu0, P0);
                };
                const mat _R() {
                        return est->_R().to_mat();
                }
};
UIREGISTER(EKFfull);


/*!
\brief Extended Kalman Filter in Square root

An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
*/

class EKFCh : public KalmanCh
{
        protected:
                //! Internal Model f(x,u)
                shared_ptr<diffbifn> pfxu;

                //! Observation Model h(x,u)
                shared_ptr<diffbifn> phxu;
        public:
                //! copy constructor duplicated - calls different set_parameters
                BM* _copy_() const {
                        EKFCh* E = new EKFCh;
                        E->set_parameters (pfxu, phxu, Q, R);
                        E->set_statistics (est->_mu(), est->_R());
                        return E;
                }
                //! Set nonlinear functions for mean values and covariance matrices.
                void set_parameters (const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const chmat Q0, const chmat R0);

                //! Here dt = [yt;ut] of appropriate dimensions
                void bayes (const vec &dt);

                void from_setting (const Setting &set);

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

};

UIREGISTER (EKFCh);
SHAREDPTR (EKFCh);


//////// INstance

/*! \brief (Switching) Multiple Model
The model runs several models in parallel and evaluates thier weights (fittness).

The statistics of the resulting density are merged using (geometric?) combination.

The next step is performed with the new statistics for all models.
*/
class MultiModel: public BM
{
        protected:
                //! List of models between which we switch
                Array<EKFCh*> Models;
                //! vector of model weights
                vec w;
                //! cache of model lls
                vec _lls;
                //! type of switching policy [1=maximum,2=...]
                int policy;
                //! internal statistics
                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:
                                        bdm_error ("unknown policy");
                        }
                        // copy result to all models
                        for (i = 0; i < n; i++) {
                                Models (i)->set_statistics (est.mean(), est._R());
                        }
                }
                //! posterior density
                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);

//! conversion of outer ARX model (mlnorm) to state space model 
/*!
The model is constructed as:
\f[ x_{t+1} = Ax_t + B u_t + R^{1/2} e_t, y_t=Cx_t+Du_t + R^{1/2}w_t, \f]
For example, for:
\f[ y_t = a y_{t-1} + b u_{t-1}\f]
the state \f$ x_t = [y_{t-1}, u_{t-1}] \f$
*/
template<class sq_T>
shared_ptr<StateSpace<fsqmat> > to_state_space(const mlnorm<sq_T > &ml, ivec &theta_in_A, ivec &theta_in_C){
        //get ids of yrv
                        
                        ivec yids= unique(ml._rv()._ids()); //yrv is now stored in _yrv
                        ivec dids= unique(ml._rvc()._ids()); //yrv is now stored in _yrv
                        ivec uids=unique_complement(dids, yids);
        
                        RV yrv_uni = RV(yids,zeros_i(yids.length()));
                        RV urv_uni = RV(uids,zeros_i(yids.length()));
                        //We can do only 1d now... :(
                        bdm_assert_debug(yrv_uni._dsize()==urv_uni._dsize()==1, "Only for SISO so far..." );
                                        
                        RV xrv; //empty
                        RV Crv; //empty
                        int td=ml._rvc().mintd();
                        for (int t=-1;t>=td;t--){
                                xrv.add(yrv_uni);
                                Crv.add(urv_uni);
                        }
                        
                        int dimx = xrv._dsize();
                        
                        theta_in_A = ml._rv().dataind(xrv);
                        theta_in_C = ml._rvc().dataind(xrv);
                        // some chcek of corretness
                        
                        vec A1row = zeros(xrv._dsize());
                        vec C1row = zeros(xrv._dsize());
                        vec theta = ml._A().get_row(0); // this 
                        set_subvector( A1row, theta_in_A, theta);
                        set_subvector( C1row, theta_in_C, theta);

                        StateSpace<fsqmat> stsp=new StateSpace<fsqmat>();
                        mat A=zeros(dimx,dimx);
                        A.set_row(0,A1row);
                        for (int j=1; j<dimx; j++){A(j,j-1)=1.0;} // off diagonal
                        mat B=zeros(dimx,1);
                        B(0) = 1.0;
                        mat C=zeros(1,dimx);
                        C.set_row(0,C1row);
                        stsp.set_parameters(A,B,C,zeros(1,1), zeros(dimx,dimx), ml._R());
                        return stsp;
}

/////////// INSTANTIATION

template<class sq_T>
void StateSpace<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();
        dimu = B0.cols();
        dimy = C0.rows();

        A = A0;
        B = B0;
        C = C0;
        D = D0;
        R = R0;
        Q = Q0;
        validate();
}

template<class sq_T>
void StateSpace<sq_T>::validate(){
        bdm_assert_debug (A.cols() == dimx, "KalmanFull: A is not square");
        bdm_assert_debug (B.rows() == dimx, "KalmanFull: B is not compatible");
        bdm_assert_debug (C.cols() == dimx, "KalmanFull: C is not square");
        bdm_assert_debug ( (D.rows() == dimy) || (D.cols() == dimu), "KalmanFull: D is not compatible");
        bdm_assert_debug ( (Q.cols() == dimx) || (Q.rows() == dimx), "KalmanFull: Q is not compatible");
        bdm_assert_debug ( (R.cols() == dimy) || (R.rows() == dimy), "KalmanFull: R is not compatible");
}

}
#endif // KF_H