Changeset 583 for library/bdm/estim

Show
Ignore:
Timestamp:
08/27/09 15:39:31 (15 years ago)
Author:
smidl
Message:

redesign of Kalman to use BM, new object StateSpace? created

Location:
library/bdm/estim
Files:
2 modified

Legend:

Unmodified
Added
Removed

  • library/bdm/estim/kalman.cpp

    r565 r583  
    66using std::endl; 
    77 
    8 KalmanFull::KalmanFull ( mat A0, mat B0, mat C0, mat D0, mat Q0, mat R0, mat P0, vec mu0 ) { 
    9         dimx = A0.rows(); 
    10         dimu = B0.cols(); 
    11         dimy = C0.rows(); 
    12  
    13         bdm_assert_debug ( A0.cols() == dimx, "KalmanFull: A is not square" ); 
    14         bdm_assert_debug ( B0.rows() == dimx, "KalmanFull: B is not compatible" ); 
    15         bdm_assert_debug ( C0.cols() == dimx, "KalmanFull: C is not square" ); 
    16         bdm_assert_debug ( ( D0.rows() == dimy ) || ( D0.cols() == dimu ), "KalmanFull: D is not compatible" ); 
    17         bdm_assert_debug ( ( Q0.cols() == dimx ) || ( Q0.rows() == dimx ), "KalmanFull: Q is not compatible" ); 
    18         bdm_assert_debug ( ( R0.cols() == dimy ) || ( R0.rows() == dimy ), "KalmanFull: R is not compatible" ); 
    19  
    20         A = A0; 
    21         B = B0; 
    22         C = C0; 
    23         D = D0; 
    24         R = R0; 
    25         Q = Q0; 
    26         mu = mu0; 
    27         P = P0; 
    28  
    29         ll = 0.0; 
    30         evalll = true; 
    31 } 
     8 
    329 
    3310void KalmanFull::bayes ( const vec &dt ) { 
    … …  
    3613        vec u = dt.get ( dimy, dimy + dimu - 1 ); 
    3714        vec y = dt.get ( 0, dimy - 1 ); 
     15        vec& mu = est->_mu(); 
     16        mat &P = est->_R(); 
     17        mat& _Ry = fy._R(); 
    3818        //Time update 
    3919        mu = A * mu + B * u; 
    40         P  = A * P * A.transpose() + Q; 
     20        P  = A * P * A.transpose() + (mat)Q; 
    4121 
    4222        //Data update 
    43         _Ry = C * P * C.transpose() + R; 
    44         _iRy = inv ( _Ry ); 
    45         _K = P * C.transpose() * _iRy; 
     23        _Ry = C * P * C.transpose() + (mat)R; 
     24        _K = P * C.transpose() * inv ( _Ry ); 
    4625        P -= _K * C * P; // P = P -KCP; 
    4726        mu += _K * ( y - C * mu - D * u ); 
    4827 
    4928        if ( evalll ) { 
    50                 //from enorm 
    51                 vec x = y - C * mu - D * u; 
    52                 ll = -0.5 * ( R.cols() * 1.83787706640935 + log ( det ( _Ry ) ) + x * ( _iRy * x ) ); 
     29                ll=est->evallog(y); 
    5330        } 
    5431}; 
    5532 
    56 std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf ) { 
    57         os << "mu:" << kf.mu << endl << "P:" << kf.P << endl; 
    58         return os; 
    59 } 
    60  
    6133 
    6234 
    6335/////////////////////////////// EKFS 
    64 EKFfull::EKFfull ( ) : BM (), E() {}; 
     36EKFfull::EKFfull ( ) : KalmanFull () {}; 
    6537 
    6638void EKFfull::set_parameters ( const shared_ptr<diffbifn> &pfxu0, const shared_ptr<diffbifn> &phxu0, const mat Q0, const mat R0 ) { 
    … …  
    7143        dimy = phxu0->dimension(); 
    7244        dimu = pfxu0->_dimu(); 
    73         E.epdf::set_parameters ( dimx ); 
     45        est->set_parameters( zeros(dimx), eye(dimx) ); 
    7446 
    7547        A.set_size ( dimx, dimx ); 
    7648        C.set_size ( dimy, dimx ); 
    7749        //initialize matrices A C, later, these will be only updated! 
    78         pfxu->dfdx_cond ( mu, zeros ( dimu ), A, true ); 
     50        pfxu->dfdx_cond ( est->_mu(), zeros ( dimu ), A, true ); 
    7951        B.clear(); 
    80         phxu->dfdx_cond ( mu, zeros ( dimu ), C, true ); 
     52        phxu->dfdx_cond ( est->_mu(), zeros ( dimu ), C, true ); 
    8153        D.clear(); 
    8254 
    … …  
    9062        vec u = dt.get ( dimy, dimy + dimu - 1 ); 
    9163        vec y = dt.get ( 0, dimy - 1 ); 
    92  
     64        vec &mu = est->_mu(); 
     65        mat &P = est->_R(); 
     66        mat& _Ry = fy._R(); 
     67         
    9368        pfxu->dfdx_cond ( mu, zeros ( dimu ), A, true ); 
    9469        phxu->dfdx_cond ( mu, zeros ( dimu ), C, true ); 
    … …  
    9671        //Time update 
    9772        mu = pfxu->eval ( mu, u );// A*mu + B*u; 
    98         P  = A * P * A.transpose() + Q; 
     73        P  = A * P * A.transpose() + (mat)Q; 
    9974 
    10075        //Data update 
    101         _Ry = C * P * C.transpose() + R; 
    102         _iRy = inv ( _Ry ); 
    103         _K = P * C.transpose() * _iRy; 
     76        _Ry = C * P * C.transpose() + (mat)R; 
     77        _K = P * C.transpose() * inv ( _Ry ); 
    10478        P -= _K * C * P; // P = P -KCP; 
    10579        vec yp = phxu->eval ( mu, u ); 
    10680        mu += _K * ( y - yp ); 
    10781 
    108         E.set_mu ( mu ); 
    109  
    11082        if ( BM::evalll ) { 
    111                 //from enorm 
    112                 vec x = y - yp; 
    113                 BM::ll = -0.5 * ( R.cols() * 1.83787706640935 + log ( det ( _Ry ) ) + x * ( _iRy * x ) ); 
     83                ll=fy.evallog(y); 
    11484        } 
    11585}; 
    … …  
    11989void KalmanCh::set_parameters ( const mat &A0, const mat &B0, const mat &C0, const mat &D0, const chmat &Q0, const chmat &R0 ) { 
    12090 
    121         ( ( Kalman<chmat>* ) this )->set_parameters ( A0, B0, C0, D0, Q0, R0 ); 
     91        ( ( StateSpace<chmat>* ) this )->set_parameters ( A0, B0, C0, D0, Q0, R0 ); 
     92         
     93        _K=zeros(dimx,dimy); 
    12294        // Cholesky special! 
     95        initialize(); 
     96} 
     97 
     98void KalmanCh::initialize(){ 
    12399        preA = zeros ( dimy + dimx + dimx, dimy + dimx ); 
    124100//      preA.clear(); 
    … …  
    127103} 
    128104 
    129  
    130105void KalmanCh::bayes ( const vec &dt ) { 
    131106 
    … …  
    134109        vec pom ( dimy ); 
    135110 
     111        chmat &_P=est->_R(); 
     112        vec &_mu = est->_mu(); 
     113        mat _K(dimx,dimy); 
     114        chmat &_Ry=fy._R(); 
     115        vec &_yp = fy._mu(); 
    136116        //TODO get rid of Q in qr()! 
    137117//      mat Q; 
    … …  
    174154        dimy = phxu0->dimension(); 
    175155        dimu = pfxu0->_dimu(); 
     156         
     157        vec &_mu = est->_mu(); 
    176158        // if mu is not set, set it to zeros, just for constant terms of A and C 
    177         if ( _mu.length() != dimx ) _mu = zeros ( dimx ); 
     159        if ( _mu.length() != dimx ) _mu = zeros ( dimx );  
    178160        A = zeros ( dimx, dimx ); 
    179161        C = zeros ( dimy, dimx ); 
    … …  
    203185        vec u = dt.get ( dimy, dimy + dimu - 1 ); 
    204186        vec y = dt.get ( 0, dimy - 1 ); 
    205  
     187        vec &_mu = est->_mu(); 
     188        chmat &_P = est->_R(); 
     189        chmat &_Ry = fy._R(); 
     190        vec &_yp = fy._mu(); 
     191         
    206192        pfxu->dfdx_cond ( _mu, u, A, false ); //update A by a derivative of fx 
    207193        phxu->dfdx_cond ( _mu, u, C, false ); //update A by a derivative of fx 

  • library/bdm/estim/kalman.h

    r565 r583  
    2020#include "../base/user_info.h" 
    2121 
    22 namespace bdm { 
    23  
    24 /*! 
    25 * \brief Basic Kalman filter with full matrices (education purpose only)! Will be deleted soon! 
    26 */ 
    27  
    28 class KalmanFull { 
    29 protected: 
    30         int dimx, dimy, dimu; 
    31         mat A, B, C, D, R, Q; 
    32  
    33         //cache 
    34         mat _Pp, _Ry, _iRy, _K; 
    35 public: 
    36         //posterior 
    37         //! Mean value of the posterior density 
    38         vec mu; 
    39         //! Variance of the posterior density 
    40         mat P; 
    41  
    42         bool evalll; 
    43         double ll; 
    44 public: 
    45         //! Full constructor 
    46         KalmanFull ( mat A, mat B, mat C, mat D, mat R, mat Q, mat P0, vec mu0 ); 
    47         //! Here dt = [yt;ut] of appropriate dimensions 
    48         void bayes ( const vec &dt ); 
    49         //! print elements of KF 
    50         friend std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf ); 
    51         //! For EKFfull; 
    52         KalmanFull() {}; 
    53 }; 
    54  
    55  
    56 /*! 
    57 * \brief Kalman filter with covariance matrices in square root form. 
     22namespace bdm 
     23{ 
     24 
     25/*! 
     26 * \brief Basic elements of linear state-space model 
    5827 
    5928Parameter evolution model:\f[ x_t = A x_{t-1} + B u_t + Q^{1/2} e_t \f] 
    6029Observation model: \f[ y_t = C x_{t-1} + C u_t + Q^{1/2} w_t. \f] 
    6130Where $e_t$ and $w_t$ are independent vectors Normal(0,1)-distributed disturbances. 
    62 */ 
     31 */ 
    6332template<class sq_T> 
    64  
    65 class Kalman : public BM { 
    66 protected: 
    67         //! Indetifier of output rv 
    68         RV rvy; 
    69         //! Indetifier of exogeneous rv 
    70         RV rvu; 
    71         //! cache of rv.count() 
    72         int dimx; 
    73         //! cache of rvy.count() 
    74         int dimy; 
    75         //! cache of rvu.count() 
    76         int dimu; 
    77         //! Matrix A 
    78         mat A; 
    79         //! Matrix B 
    80         mat B; 
    81         //! Matrix C 
    82         mat C; 
    83         //! Matrix D 
    84         mat D; 
    85         //! Matrix Q in square-root form 
    86         sq_T Q; 
    87         //! Matrix R in square-root form 
    88         sq_T R; 
    89  
    90         //!posterior density on $x_t$ 
    91         enorm<sq_T> est; 
    92         //!preditive density on $y_t$ 
    93         enorm<sq_T> fy; 
    94  
    95         //! placeholder for Kalman gain 
    96         mat _K; 
    97         //! cache of fy.mu 
    98         vec& _yp; 
    99         //! cache of fy.R 
    100         sq_T& _Ry; 
    101         //!cache of est.mu 
    102         vec& _mu; 
    103         //!cache of est.R 
    104         sq_T& _P; 
    105  
    106 public: 
    107         //! Default constructor 
    108         Kalman ( ); 
    109         //! Copy constructor 
    110         Kalman ( const Kalman<sq_T> &K0 ); 
    111         //! Set parameters with check of relevance 
    112         void set_parameters ( const mat &A0, const mat &B0, const mat &C0, const mat &D0, const sq_T &Q0, const sq_T &R0 ); 
    113         //! Set estimate values, used e.g. in initialization. 
    114         void set_est ( const vec &mu0, const sq_T &P0 ) { 
    115                 sq_T pom ( dimy ); 
    116                 est.set_parameters ( mu0, P0 ); 
    117                 P0.mult_sym ( C, pom ); 
    118                 fy.set_parameters ( C*mu0, pom ); 
    119         }; 
    120  
    121         //! Here dt = [yt;ut] of appropriate dimensions 
    122         void bayes ( const vec &dt ); 
    123         //!access function 
    124         const epdf& posterior() const { 
    125                 return est; 
    126         } 
    127         //!access function 
    128         mat& __K() { 
    129                 return _K; 
    130         } 
    131         //!access function 
    132         vec _dP() { 
    133                 return _P->getD(); 
    134         } 
    135 }; 
     33class StateSpace 
     34{ 
     35        protected: 
     36                //! cache of rv.count() 
     37                int dimx; 
     38                //! cache of rvy.count() 
     39                int dimy; 
     40                //! cache of rvu.count() 
     41                int dimu; 
     42                //! Matrix A 
     43                mat A; 
     44                //! Matrix B 
     45                mat B; 
     46                //! Matrix C 
     47                mat C; 
     48                //! Matrix D 
     49                mat D; 
     50                //! Matrix Q in square-root form 
     51                sq_T Q; 
     52                //! Matrix R in square-root form 
     53                sq_T R; 
     54        public: 
     55                StateSpace() : dimx (0), dimy (0), dimu (0), A(), B(), C(), D(), Q(), R() {} 
     56                void set_parameters (const mat &A0, const  mat &B0, const  mat &C0, const  mat &D0, const  sq_T &Q0, const sq_T &R0); 
     57                void validate(); 
     58                //! not virtual in this case 
     59                void from_setting (const Setting &set) { 
     60                        UI::get (A, set, "A", UI::compulsory); 
     61                        UI::get (B, set, "B", UI::compulsory); 
     62                        UI::get (C, set, "C", UI::compulsory); 
     63                        UI::get (D, set, "D", UI::compulsory); 
     64                        mat Qtm, Rtm; 
     65                        if(!UI::get(Qtm, set, "Q", UI::optional)){ 
     66                                vec dq; 
     67                                UI::get(dq, set, "dQ", UI::compulsory); 
     68                                Qtm=diag(dq); 
     69                        } 
     70                        if(!UI::get(Rtm, set, "R", UI::optional)){ 
     71                                vec dr; 
     72                                UI::get(dr, set, "dQ", UI::compulsory); 
     73                                Rtm=diag(dr); 
     74                        } 
     75                        R=Rtm; // automatic conversion 
     76                        Q=Qtm;  
     77                         
     78                        validate(); 
     79                }                
     80}; 
     81 
     82//! Common abstract base for Kalman filters  
     83template<class sq_T> 
     84class Kalman: public BM, public StateSpace<sq_T> 
     85{ 
     86        protected: 
     87                //! id of output 
     88                RV yrv; 
     89                //! id of input 
     90                RV urv; 
     91                //! Kalman gain 
     92                mat  _K; 
     93                //!posterior 
     94                shared_ptr<enorm<sq_T> > est; 
     95                //!marginal on data f(y|y) 
     96                enorm<sq_T>  fy; 
     97        public: 
     98                Kalman() : BM(), StateSpace<sq_T>(), yrv(),urv(), _K(),  est(new enorm<sq_T>){} 
     99                void set_statistics (const vec &mu0, const mat &P0) {est->set_parameters (mu0, P0); }; 
     100                void set_statistics (const vec &mu0, const sq_T &P0) {est->set_parameters (mu0, P0); }; 
     101                //! posterior 
     102                const enorm<sq_T>& posterior() const {return *est.get();} 
     103                //! shared posterior 
     104                shared_ptr<epdf> shared_posterior() {return est;} 
     105                //! load basic elements of Kalman from structure 
     106                void from_setting (const Setting &set) { 
     107                        StateSpace<sq_T>::from_setting(set); 
     108                                                 
     109                        mat P0; vec mu0; 
     110                        UI::get(mu0, set, "mu0", UI::optional); 
     111                        UI::get(P0, set,  "P0", UI::optional); 
     112                        set_statistics(mu0,P0); 
     113                        // Initial values 
     114                        UI::get (yrv, set, "yrv", UI::optional); 
     115                        UI::get (urv, set, "urv", UI::optional); 
     116                        set_drv(concat(yrv,urv)); 
     117                         
     118                        validate(); 
     119                } 
     120                void validate() { 
     121                        StateSpace<sq_T>::validate(); 
     122                        bdm_assert_debug(est->dimension(), "Statistics and model parameters mismatch"); 
     123                } 
     124}; 
     125/*! 
     126* \brief Basic Kalman filter with full matrices 
     127*/ 
     128 
     129class KalmanFull : public Kalman<fsqmat> 
     130{ 
     131        public: 
     132                //! For EKFfull; 
     133                KalmanFull() :Kalman<fsqmat>(){}; 
     134                //! Here dt = [yt;ut] of appropriate dimensions 
     135                void bayes (const vec &dt); 
     136}; 
     137 
     138 
    136139 
    137140/*! \brief Kalman filter in square root form 
    … …  
    140143\include kalman_simple.cpp 
    141144 
    142 */ 
    143 class KalmanCh : public Kalman<chmat> { 
    144 protected: 
    145 //! pre array (triangular matrix) 
    146         mat preA; 
    147 //! post array (triangular matrix) 
    148         mat postA; 
    149  
    150 public: 
    151         //! copy constructor 
    152         BM* _copy_() const { 
    153                 KalmanCh* K = new KalmanCh; 
    154                 K->set_parameters ( A, B, C, D, Q, R ); 
    155                 K->set_statistics ( _mu, _P ); 
    156                 return K; 
    157         } 
    158         //! Set parameters with check of relevance 
    159         void set_parameters ( const mat &A0, const mat &B0, const mat &C0, const mat &D0, const chmat &Q0, const chmat &R0 ); 
    160         void set_statistics ( const vec &mu0, const chmat &P0 ) { 
    161                 est.set_parameters ( mu0, P0 ); 
    162         }; 
    163  
    164  
    165         /*!\brief  Here dt = [yt;ut] of appropriate dimensions 
    166  
    167         The following equality hold::\f[ 
    168         \left[\begin{array}{cc} 
    169         R^{0.5}\\ 
    170         P_{t|t-1}^{0.5}C' & P_{t|t-1}^{0.5}CA'\\ 
    171         & Q^{0.5}\end{array}\right]<\mathrm{orth.oper.}>=\left[\begin{array}{cc} 
    172         R_{y}^{0.5} & KA'\\ 
    173         & P_{t+1|t}^{0.5}\\ 
    174         \\\end{array}\right]\f] 
    175  
    176         Thus this object evaluates only predictors! Not filtering densities. 
    177         */ 
    178         void bayes ( const vec &dt ); 
     145Complete constructor: 
     146*/ 
     147class KalmanCh : public Kalman<chmat> 
     148{ 
     149        protected: 
     150                //! @{ \name Internal storage - needs initialize() 
     151                //! pre array (triangular matrix) 
     152                mat preA; 
     153                //! post array (triangular matrix) 
     154                mat postA; 
     155                //!@} 
     156        public: 
     157                //! copy constructor 
     158                BM* _copy_() const { 
     159                        KalmanCh* K = new KalmanCh; 
     160                        K->set_parameters (A, B, C, D, Q, R); 
     161                        K->set_statistics (est->_mu(), est->_R()); 
     162                        return K; 
     163                } 
     164                //! set parameters for adapt from Kalman 
     165                void set_parameters (const mat &A0, const mat &B0, const mat &C0, const mat &D0, const chmat &Q0, const chmat &R0); 
     166                //! initialize internal parametetrs 
     167                void initialize(); 
     168 
     169                /*!\brief  Here dt = [yt;ut] of appropriate dimensions 
     170 
     171                The following equality hold::\f[ 
     172                \left[\begin{array}{cc} 
     173                R^{0.5}\\ 
     174                P_{t|t-1}^{0.5}C' & P_{t|t-1}^{0.5}CA'\\ 
     175                & Q^{0.5}\end{array}\right]<\mathrm{orth.oper.}>=\left[\begin{array}{cc} 
     176                R_{y}^{0.5} & KA'\\ 
     177                & P_{t+1|t}^{0.5}\\ 
     178                \\\end{array}\right]\f] 
     179 
     180                Thus this object evaluates only predictors! Not filtering densities. 
     181                */ 
     182                void bayes (const vec &dt); 
     183                 
     184                void from_settings(const Setting &set){ 
     185                        Kalman<chmat>::from_setting(set); 
     186                        initialize(); 
     187                } 
    179188}; 
    180189 
    … …  
    184193An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. 
    185194*/ 
    186 class EKFfull : public KalmanFull, public BM { 
    187 protected: 
    188         //! Internal Model f(x,u) 
    189         shared_ptr<diffbifn> pfxu; 
    190  
    191         //! Observation Model h(x,u) 
    192         shared_ptr<diffbifn> phxu; 
    193  
    194         enorm<fsqmat> E; 
    195 public: 
    196         //! Default constructor 
    197         EKFfull ( ); 
    198  
    199         //! Set nonlinear functions for mean values and covariance matrices. 
    200         void set_parameters ( const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const mat Q0, const mat R0 ); 
    201  
    202         //! Here dt = [yt;ut] of appropriate dimensions 
    203         void bayes ( const vec &dt ); 
    204         //! set estimates 
    205         void set_statistics ( vec mu0, mat P0 ) { 
    206                 mu = mu0; 
    207                 P = P0; 
    208         }; 
    209         //!dummy! 
    210         const epdf& posterior() const { 
    211                 return E; 
    212         }; 
    213         const mat _R() { 
    214                 return P; 
    215         } 
    216 }; 
    217  
    218 /*! 
    219 \brief Extended Kalman Filter 
     195class EKFfull : public KalmanFull 
     196{ 
     197        protected: 
     198                //! Internal Model f(x,u) 
     199                shared_ptr<diffbifn> pfxu; 
     200 
     201                //! Observation Model h(x,u) 
     202                shared_ptr<diffbifn> phxu; 
     203 
     204        public: 
     205                //! Default constructor 
     206                EKFfull (); 
     207 
     208                //! Set nonlinear functions for mean values and covariance matrices. 
     209                void set_parameters (const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const mat Q0, const mat R0); 
     210 
     211                //! Here dt = [yt;ut] of appropriate dimensions 
     212                void bayes (const vec &dt); 
     213                //! set estimates 
     214                void set_statistics (const vec &mu0, const mat &P0) { 
     215                        est->set_parameters (mu0, P0); 
     216                }; 
     217                const mat _R() { 
     218                        return est->_R().to_mat(); 
     219                } 
     220}; 
     221 
     222/*! 
     223\brief Extended Kalman Filter in Square root 
    220224 
    221225An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. 
    222226*/ 
     227 
     228class EKFCh : public KalmanCh 
     229{ 
     230        protected: 
     231                //! Internal Model f(x,u) 
     232                shared_ptr<diffbifn> pfxu; 
     233 
     234                //! Observation Model h(x,u) 
     235                shared_ptr<diffbifn> phxu; 
     236        public: 
     237                //! copy constructor duplicated - calls different set_parameters 
     238                BM* _copy_() const { 
     239                        EKFCh* E = new EKFCh; 
     240                        E->set_parameters (pfxu, phxu, Q, R); 
     241                        E->set_statistics (est->_mu(), est->_R()); 
     242                        return E; 
     243                } 
     244                //! Set nonlinear functions for mean values and covariance matrices. 
     245                void set_parameters (const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const chmat Q0, const chmat R0); 
     246 
     247                //! Here dt = [yt;ut] of appropriate dimensions 
     248                void bayes (const vec &dt); 
     249 
     250                void from_setting (const Setting &set); 
     251 
     252                // TODO dodelat void to_setting( Setting &set ) const; 
     253 
     254}; 
     255 
     256UIREGISTER (EKFCh); 
     257SHAREDPTR (EKFCh); 
     258 
     259 
     260//////// INstance 
     261 
     262/*! \brief (Switching) Multiple Model 
     263The model runs several models in parallel and evaluates thier weights (fittness). 
     264 
     265The statistics of the resulting density are merged using (geometric?) combination. 
     266 
     267The next step is performed with the new statistics for all models. 
     268*/ 
     269class MultiModel: public BM 
     270{ 
     271        protected: 
     272                //! List of models between which we switch 
     273                Array<EKFCh*> Models; 
     274                //! vector of model weights 
     275                vec w; 
     276                //! cache of model lls 
     277                vec _lls; 
     278                //! type of switching policy [1=maximum,2=...] 
     279                int policy; 
     280                //! internal statistics 
     281                enorm<chmat> est; 
     282        public: 
     283                void set_parameters (Array<EKFCh*> A, int pol0 = 1) { 
     284                        Models = A;//TODO: test if evalll is set 
     285                        w.set_length (A.length()); 
     286                        _lls.set_length (A.length()); 
     287                        policy = pol0; 
     288 
     289                        est.set_rv (RV ("MM", A (0)->posterior().dimension(), 0)); 
     290                        est.set_parameters (A (0)->posterior().mean(), A (0)->posterior()._R()); 
     291                } 
     292                void bayes (const vec &dt) { 
     293                        int n = Models.length(); 
     294                        int i; 
     295                        for (i = 0; i < n; i++) { 
     296                                Models (i)->bayes (dt); 
     297                                _lls (i) = Models (i)->_ll(); 
     298                        } 
     299                        double mlls = max (_lls); 
     300                        w = exp (_lls - mlls); 
     301                        w /= sum (w);   //normalization 
     302                        //set statistics 
     303                        switch (policy) { 
     304                                case 1: { 
     305                                        int mi = max_index (w); 
     306                                        const enorm<chmat> &st = Models (mi)->posterior() ; 
     307                                        est.set_parameters (st.mean(), st._R()); 
     308                                } 
     309                                break; 
     310                                default: 
     311                                        bdm_error ("unknown policy"); 
     312                        } 
     313                        // copy result to all models 
     314                        for (i = 0; i < n; i++) { 
     315                                Models (i)->set_statistics (est.mean(), est._R()); 
     316                        } 
     317                } 
     318                //! posterior density 
     319                const enorm<chmat>& posterior() const { 
     320                        return est; 
     321                } 
     322 
     323                void from_setting (const Setting &set); 
     324 
     325                // TODO dodelat void to_setting( Setting &set ) const; 
     326 
     327}; 
     328 
     329UIREGISTER (MultiModel); 
     330SHAREDPTR (MultiModel); 
     331 
     332/////////// INSTANTIATION 
     333 
    223334template<class sq_T> 
    224 class EKF : public Kalman<fsqmat> { 
    225         //! Internal Model f(x,u) 
    226         diffbifn* pfxu; 
    227         //! Observation Model h(x,u) 
    228         diffbifn* phxu; 
    229 public: 
    230         //! Default constructor 
    231         EKF ( RV rvx, RV rvy, RV rvu ); 
    232         //! copy constructor 
    233         EKF<sq_T>* _copy_() const { 
    234                 return new EKF<sq_T> ( this ); 
    235         } 
    236         //! Set nonlinear functions for mean values and covariance matrices. 
    237         void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const sq_T Q0, const sq_T R0 ); 
    238         //! Here dt = [yt;ut] of appropriate dimensions 
    239         void bayes ( const vec &dt ); 
    240 }; 
    241  
    242 /*! 
    243 \brief Extended Kalman Filter in Square root 
    244  
    245 An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. 
    246 */ 
    247  
    248 class EKFCh : public KalmanCh { 
    249 protected: 
    250         //! Internal Model f(x,u) 
    251         shared_ptr<diffbifn> pfxu; 
    252  
    253         //! Observation Model h(x,u) 
    254         shared_ptr<diffbifn> phxu; 
    255 public: 
    256         //! copy constructor duplicated - calls different set_parameters 
    257         BM* _copy_() const { 
    258                 EKFCh* E = new EKFCh; 
    259                 E->set_parameters ( pfxu, phxu, Q, R ); 
    260                 E->set_statistics ( _mu, _P ); 
    261                 return E; 
    262         } 
    263         //! Set nonlinear functions for mean values and covariance matrices. 
    264         void set_parameters ( const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const chmat Q0, const chmat R0 ); 
    265  
    266         //! Here dt = [yt;ut] of appropriate dimensions 
    267         void bayes ( const vec &dt ); 
    268  
    269         void from_setting ( const Setting &set ); 
    270  
    271         // TODO dodelat void to_setting( Setting &set ) const; 
    272  
    273         const enorm<chmat>& posterior() {return est;} 
    274 }; 
    275  
    276 UIREGISTER ( EKFCh ); 
    277 SHAREDPTR ( EKFCh ); 
    278  
    279  
    280 /*! 
    281 \brief Kalman Filter with conditional diagonal matrices R and Q. 
    282 */ 
    283  
    284 class KFcondQR : public Kalman<ldmat> { 
    285 //protected: 
    286 public: 
    287         void condition ( const vec &QR ) { 
    288                 bdm_assert_debug ( QR.length() == ( dimx + dimy ), "KFcondQR: conditioning by incompatible vector" ); 
    289  
    290                 Q.setD ( QR ( 0, dimx - 1 ) ); 
    291                 R.setD ( QR ( dimx, -1 ) ); 
    292         }; 
    293 }; 
    294  
    295 /*! 
    296 \brief Kalman Filter with conditional diagonal matrices R and Q. 
    297 */ 
    298  
    299 class KFcondR : public Kalman<ldmat> { 
    300 //protected: 
    301 public: 
    302         //!Default constructor 
    303         KFcondR ( ) : Kalman<ldmat> ( ) {}; 
    304  
    305         void condition ( const vec &R0 ) { 
    306                 bdm_assert_debug ( R0.length() == ( dimy ), "KFcondR: conditioning by incompatible vector" ); 
    307  
    308                 R.setD ( R0 ); 
    309         }; 
    310  
    311 }; 
    312  
    313 //////// INstance 
    314  
    315 template<class sq_T> 
    316 Kalman<sq_T>::Kalman ( const Kalman<sq_T> &K0 ) : BM ( ), rvy ( K0.rvy ), rvu ( K0.rvu ), 
    317                 dimx ( K0.dimx ), dimy ( K0.dimy ), dimu ( K0.dimu ), 
    318                 A ( K0.A ), B ( K0.B ), C ( K0.C ), D ( K0.D ), 
    319                 Q ( K0.Q ), R ( K0.R ), 
    320                 est ( K0.est ), fy ( K0.fy ), _yp ( fy._mu() ), _Ry ( fy._R() ), _mu ( est._mu() ), _P ( est._R() ) { 
    321  
    322 // copy values in pointers 
    323 //      _mu = K0._mu; 
    324 //      _P = K0._P; 
    325 //      _yp = K0._yp; 
    326 //      _Ry = K0._Ry; 
    327  
    328 } 
    329  
    330 template<class sq_T> 
    331 Kalman<sq_T>::Kalman ( ) : BM (), est ( ), fy (),  _yp ( fy._mu() ), _Ry ( fy._R() ), _mu ( est._mu() ), _P ( est._R() ) { 
    332 }; 
    333  
    334 template<class sq_T> 
    335 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 ) { 
     335void 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) 
     336{ 
    336337        dimx = A0.rows(); 
     338        dimu = B0.cols(); 
    337339        dimy = C0.rows(); 
    338         dimu = B0.cols(); 
    339  
    340         bdm_assert_debug ( A0.cols() == dimx, "Kalman: A is not square" ); 
    341         bdm_assert_debug ( B0.rows() == dimx, "Kalman: B is not compatible" ); 
    342         bdm_assert_debug ( C0.cols() == dimx, "Kalman: C is not square" ); 
    343         bdm_assert_debug ( ( D0.rows() == dimy ) || ( D0.cols() == dimu ),      "Kalman: D is not compatible" ); 
    344         bdm_assert_debug ( ( R0.cols() == dimy ) || ( R0.rows() == dimy ), "Kalman: R is not compatible" ); 
    345         bdm_assert_debug ( ( Q0.cols() == dimx ) || ( Q0.rows() == dimx ), "Kalman: Q is not compatible" ); 
    346340 
    347341        A = A0; 
    … …  
    351345        R = R0; 
    352346        Q = Q0; 
     347        validate(); 
    353348} 
    354349 
    355350template<class sq_T> 
    356 void Kalman<sq_T>::bayes ( const vec &dt ) { 
    357         bdm_assert_debug ( dt.length() == ( dimy + dimu ), "Kalman::bayes wrong size of dt" ); 
    358  
    359         sq_T iRy ( dimy ); 
    360         vec u = dt.get ( dimy, dimy + dimu - 1 ); 
    361         vec y = dt.get ( 0, dimy - 1 ); 
    362         //Time update 
    363         _mu = A * _mu + B * u; 
    364         //P  = A*P*A.transpose() + Q; in sq_T 
    365         _P.mult_sym ( A ); 
    366         _P  += Q; 
    367  
    368         //Data update 
    369         //_Ry = C*P*C.transpose() + R; in sq_T 
    370         _P.mult_sym ( C, _Ry ); 
    371         _Ry  += R; 
    372  
    373         mat Pfull = _P.to_mat(); 
    374  
    375         _Ry.inv ( iRy ); // result is in _iRy; 
    376         _K = Pfull * C.transpose() * ( iRy.to_mat() ); 
    377  
    378         sq_T pom ( ( int ) Pfull.rows() ); 
    379         iRy.mult_sym_t ( C*Pfull, pom ); 
    380         ( _P ) -= pom; // P = P -PC'iRy*CP; 
    381         ( _yp ) = C * _mu  + D * u; //y prediction 
    382         ( _mu ) += _K * ( y - _yp ); 
    383  
    384  
    385         if ( evalll == true ) { //likelihood of observation y 
    386                 ll = fy.evallog ( y ); 
    387         } 
    388  
    389 //cout << "y: " << y-(*_yp) <<" R: " << _Ry->to_mat() << " iR: " << _iRy->to_mat() << " ll: " << ll <<endl; 
    390  
    391 }; 
    392  
    393 /*! \brief (Switching) Multiple Model 
    394 The model runs several models in parallel and evaluates thier weights (fittness). 
    395  
    396 The statistics of the resulting density are merged using (geometric?) combination. 
    397  
    398 The next step is performed with the new statistics for all models. 
    399 */ 
    400 class MultiModel: public BM { 
    401 protected: 
    402         //! List of models between which we switch 
    403         Array<EKFCh*> Models; 
    404         //! vector of model weights 
    405         vec w; 
    406         //! cache of model lls 
    407         vec _lls; 
    408         //! type of switching policy [1=maximum,2=...] 
    409         int policy; 
    410         //! internal statistics 
    411         enorm<chmat> est; 
    412 public: 
    413         void set_parameters ( Array<EKFCh*> A, int pol0 = 1 ) { 
    414                 Models = A;//TODO: test if evalll is set 
    415                 w.set_length ( A.length() ); 
    416                 _lls.set_length ( A.length() ); 
    417                 policy = pol0; 
    418  
    419                 est.set_rv ( RV ( "MM", A ( 0 )->posterior().dimension(), 0 ) ); 
    420                 est.set_parameters ( A ( 0 )->posterior().mean(), A ( 0 )->posterior()._R() ); 
    421         } 
    422         void bayes ( const vec &dt ) { 
    423                 int n = Models.length(); 
    424                 int i; 
    425                 for ( i = 0; i < n; i++ ) { 
    426                         Models ( i )->bayes ( dt ); 
    427                         _lls ( i ) = Models ( i )->_ll(); 
    428                 } 
    429                 double mlls = max ( _lls ); 
    430                 w = exp ( _lls - mlls ); 
    431                 w /= sum ( w ); //normalization 
    432                 //set statistics 
    433                 switch ( policy ) { 
    434                 case 1: { 
    435                         int mi = max_index ( w ); 
    436                         const enorm<chmat> &st = Models ( mi )->posterior() ; 
    437                         est.set_parameters ( st.mean(), st._R() ); 
    438                 } 
    439                 break; 
    440                 default: 
    441                         bdm_error ( "unknown policy" ); 
    442                 } 
    443                 // copy result to all models 
    444                 for ( i = 0; i < n; i++ ) { 
    445                         Models ( i )->set_statistics ( est.mean(), est._R() ); 
    446                 } 
    447         } 
    448         //! posterior density 
    449         const enorm<chmat>& posterior() const { 
    450                 return est; 
    451         } 
    452  
    453         void from_setting ( const Setting &set ); 
    454  
    455         // TODO dodelat void to_setting( Setting &set ) const; 
    456  
    457 }; 
    458  
    459 UIREGISTER ( MultiModel ); 
    460 SHAREDPTR ( MultiModel ); 
    461  
    462  
    463  
    464 //TODO why not const pointer?? 
    465  
    466 template<class sq_T> 
    467 EKF<sq_T>::EKF ( RV rvx0, RV rvy0, RV rvu0 ) : Kalman<sq_T> ( rvx0, rvy0, rvu0 ) {} 
    468  
    469 template<class sq_T> 
    470 void EKF<sq_T>::set_parameters ( diffbifn* pfxu0,  diffbifn* phxu0, const sq_T Q0, const sq_T R0 ) { 
    471         pfxu = pfxu0; 
    472         phxu = phxu0; 
    473  
    474         //initialize matrices A C, later, these will be only updated! 
    475         pfxu->dfdx_cond ( _mu, zeros ( dimu ), A, true ); 
    476 //      pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true ); 
    477         B.clear(); 
    478         phxu->dfdx_cond ( _mu, zeros ( dimu ), C, true ); 
    479 //      phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true ); 
    480         D.clear(); 
    481  
    482         R = R0; 
    483         Q = Q0; 
     351void StateSpace<sq_T>::validate(){ 
     352        bdm_assert_debug (A.cols() == dimx, "KalmanFull: A is not square"); 
     353        bdm_assert_debug (B.rows() == dimx, "KalmanFull: B is not compatible"); 
     354        bdm_assert_debug (C.cols() == dimx, "KalmanFull: C is not square"); 
     355        bdm_assert_debug ( (D.rows() == dimy) || (D.cols() == dimu), "KalmanFull: D is not compatible"); 
     356        bdm_assert_debug ( (Q.cols() == dimx) || (Q.rows() == dimx), "KalmanFull: Q is not compatible"); 
     357        bdm_assert_debug ( (R.cols() == dimy) || (R.rows() == dimy), "KalmanFull: R is not compatible"); 
    484358} 
    485  
    486 template<class sq_T> 
    487 void EKF<sq_T>::bayes ( const vec &dt ) { 
    488         bdm_assert_debug ( dt.length() == ( dimy + dimu ), "EKF<>::bayes wrong size of dt" ); 
    489  
    490         sq_T iRy ( dimy, dimy ); 
    491         vec u = dt.get ( dimy, dimy + dimu - 1 ); 
    492         vec y = dt.get ( 0, dimy - 1 ); 
    493         //Time update 
    494         _mu = pfxu->eval ( _mu, u ); 
    495         pfxu->dfdx_cond ( _mu, u, A, false ); //update A by a derivative of fx 
    496  
    497         //P  = A*P*A.transpose() + Q; in sq_T 
    498         _P.mult_sym ( A ); 
    499         _P += Q; 
    500  
    501         //Data update 
    502         phxu->dfdx_cond ( _mu, u, C, false ); //update C by a derivative hx 
    503         //_Ry = C*P*C.transpose() + R; in sq_T 
    504         _P.mult_sym ( C, _Ry ); 
    505         ( _Ry ) += R; 
    506  
    507         mat Pfull = _P.to_mat(); 
    508  
    509         _Ry.inv ( iRy ); // result is in _iRy; 
    510         _K = Pfull * C.transpose() * ( iRy.to_mat() ); 
    511  
    512         sq_T pom ( ( int ) Pfull.rows() ); 
    513         iRy.mult_sym_t ( C*Pfull, pom ); 
    514         ( _P ) -= pom; // P = P -PC'iRy*CP; 
    515         _yp = phxu->eval ( _mu, u ); //y prediction 
    516         ( _mu ) += _K * ( y - _yp ); 
    517  
    518         if ( evalll == true ) { 
    519                 ll += fy.evallog ( y ); 
    520         } 
    521 }; 
    522  
    523359 
    524360}