Changeset 338 for bdm/estim

Show
Ignore:
Timestamp:
05/06/09 15:25:59 (15 years ago)
Author:
smidl
Message:

Multiple Models + changes in loggers

Location:
bdm/estim
Files:
2 modified

Legend:

Unmodified
Added
Removed

  • bdm/estim/KF_ui.h

    r332 r338  
    5151UIREGISTER ( UIEKF ); 
    5252 
     53class UIMultiModel: public UIbuilder { 
     54        public: 
     55                UIMultiModel():UIbuilder("MultiModel"){}; 
     56                bdmroot* build ( Setting &S ) const { 
     57                        Array<EKFCh*> A; 
     58                        MultiModel* MM; MM=new MultiModel; 
     59                                 
     60                        Setting& mod=S["models"]; 
     61                        A.set_length(mod.getLength()); 
     62                        for (int i=0;i<A.length();i++){ 
     63                                UIbuild(mod[i], A(i)); 
     64                        } 
     65                         
     66                        MM->set_parameters(A); 
     67                        MM->set_drv(A(0)->_drv()); 
     68                        //MM->set_rv(A(0)->_rv()); 
     69                 
     70                        if (S.exists("options")){MM->set_options(S["options"]);}; 
     71                 
     72                        return MM; 
     73                }        
     74}; 
     75UIREGISTER ( UIMultiModel ); 
    5376 

  • bdm/estim/libKF.h

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