Show
Ignore:
Timestamp:
11/25/09 12:14:38 (15 years ago)
Author:
mido
Message:

ASTYLER RUN OVER THE WHOLE LIBRARY, JUPEE

Files:
1 modified

Legend:

Unmodified
Added
Removed

  • library/bdm/estim/kalman.h

    r723 r737  
    2121//#include <../applications/pmsm/simulator_zdenek/ekf_example/pmsm_mod.h> 
    2222 
    23 namespace bdm 
    24 { 
     23namespace bdm { 
    2524 
    2625/*! 
    … …  
    3231 */ 
    3332template<class sq_T> 
    34 class StateSpace 
    35 { 
    36         protected: 
    37                 //! Matrix A 
    38                 mat A; 
    39                 //! Matrix B 
    40                 mat B; 
    41                 //! Matrix C 
    42                 mat C; 
    43                 //! Matrix D 
    44                 mat D; 
    45                 //! Matrix Q in square-root form 
    46                 sq_T Q; 
    47                 //! Matrix R in square-root form 
    48                 sq_T R; 
    49         public: 
    50                 StateSpace() :  A(), B(), C(), D(), Q(), R() {} 
    51                 //!copy constructor 
    52                 StateSpace(const StateSpace<sq_T> &S0) :  A(S0.A), B(S0.B), C(S0.C), D(S0.D), Q(S0.Q), R(S0.R) {} 
    53                 //! set all matrix parameters 
    54                 void set_parameters (const mat &A0, const  mat &B0, const  mat &C0, const  mat &D0, const  sq_T &Q0, const sq_T &R0); 
    55                 //! validation 
    56                 void validate(); 
    57                 //! not virtual in this case 
    58                 void from_setting (const Setting &set) { 
    59                         UI::get (A, set, "A", UI::compulsory); 
    60                         UI::get (B, set, "B", UI::compulsory); 
    61                         UI::get (C, set, "C", UI::compulsory); 
    62                         UI::get (D, set, "D", UI::compulsory); 
    63                         mat Qtm, Rtm; // full matrices 
    64                         if(!UI::get(Qtm, set, "Q", UI::optional)){ 
    65                                 vec dq; 
    66                                 UI::get(dq, set, "dQ", UI::compulsory); 
    67                                 Qtm=diag(dq); 
    68                         } 
    69                         if(!UI::get(Rtm, set, "R", UI::optional)){ 
    70                                 vec dr; 
    71                                 UI::get(dr, set, "dQ", UI::compulsory); 
    72                                 Rtm=diag(dr); 
    73                         } 
    74                         R=Rtm; // automatic conversion to square-root form 
    75                         Q=Qtm;  
    76                          
    77                         validate(); 
    78                 }                
    79                 //! access function 
    80                 const mat& _A() const {return A;} 
    81                 //! access function 
    82                 const mat& _B()const {return B;} 
    83                 //! access function 
    84                 const mat& _C()const {return C;} 
    85                 //! access function 
    86                 const mat& _D()const {return D;} 
    87                 //! access function 
    88                 const sq_T& _Q()const {return Q;} 
    89                 //! access function 
    90                 const sq_T& _R()const {return R;} 
    91 }; 
    92  
    93 //! Common abstract base for Kalman filters  
     33class StateSpace { 
     34protected: 
     35        //! Matrix A 
     36        mat A; 
     37        //! Matrix B 
     38        mat B; 
     39        //! Matrix C 
     40        mat C; 
     41        //! Matrix D 
     42        mat D; 
     43        //! Matrix Q in square-root form 
     44        sq_T Q; 
     45        //! Matrix R in square-root form 
     46        sq_T R; 
     47public: 
     48        StateSpace() :  A(), B(), C(), D(), Q(), R() {} 
     49        //!copy constructor 
     50        StateSpace ( const StateSpace<sq_T> &S0 ) :  A ( S0.A ), B ( S0.B ), C ( S0.C ), D ( S0.D ), Q ( S0.Q ), R ( S0.R ) {} 
     51        //! set all matrix parameters 
     52        void set_parameters ( const mat &A0, const  mat &B0, const  mat &C0, const  mat &D0, const  sq_T &Q0, const sq_T &R0 ); 
     53        //! validation 
     54        void validate(); 
     55        //! not virtual in this case 
     56        void from_setting ( const Setting &set ) { 
     57                UI::get ( A, set, "A", UI::compulsory ); 
     58                UI::get ( B, set, "B", UI::compulsory ); 
     59                UI::get ( C, set, "C", UI::compulsory ); 
     60                UI::get ( D, set, "D", UI::compulsory ); 
     61                mat Qtm, Rtm; // full matrices 
     62                if ( !UI::get ( Qtm, set, "Q", UI::optional ) ) { 
     63                        vec dq; 
     64                        UI::get ( dq, set, "dQ", UI::compulsory ); 
     65                        Qtm = diag ( dq ); 
     66                } 
     67                if ( !UI::get ( Rtm, set, "R", UI::optional ) ) { 
     68                        vec dr; 
     69                        UI::get ( dr, set, "dQ", UI::compulsory ); 
     70                        Rtm = diag ( dr ); 
     71                } 
     72                R = Rtm; // automatic conversion to square-root form 
     73                Q = Qtm; 
     74 
     75                validate(); 
     76        } 
     77        //! access function 
     78        const mat& _A() const { 
     79                return A; 
     80        } 
     81        //! access function 
     82        const mat& _B() const { 
     83                return B; 
     84        } 
     85        //! access function 
     86        const mat& _C() const { 
     87                return C; 
     88        } 
     89        //! access function 
     90        const mat& _D() const { 
     91                return D; 
     92        } 
     93        //! access function 
     94        const sq_T& _Q() const { 
     95                return Q; 
     96        } 
     97        //! access function 
     98        const sq_T& _R() const { 
     99                return R; 
     100        } 
     101}; 
     102 
     103//! Common abstract base for Kalman filters 
    94104template<class sq_T> 
    95 class Kalman: public BM, public StateSpace<sq_T> 
    96 { 
    97         protected: 
    98                 //! id of output 
    99                 RV yrv; 
    100                 //! Kalman gain 
    101                 mat  _K; 
    102                 //!posterior 
    103                 enorm<sq_T> est; 
    104                 //!marginal on data f(y|y) 
    105                 enorm<sq_T>  fy; 
    106         public: 
    107                 Kalman<sq_T>() : BM(), StateSpace<sq_T>(), yrv(), _K(),  est(){} 
    108                 //! Copy constructor 
    109                 Kalman<sq_T>(const Kalman<sq_T> &K0) : BM(K0), StateSpace<sq_T>(K0), yrv(K0.yrv), _K(K0._K),  est(K0.est), fy(K0.fy){} 
    110                 //!set statistics of the posterior 
    111                 void set_statistics (const vec &mu0, const mat &P0) {est.set_parameters (mu0, P0); }; 
    112                 //!set statistics of the posterior 
    113                 void set_statistics (const vec &mu0, const sq_T &P0) {est.set_parameters (mu0, P0); }; 
    114                 //! return correctly typed posterior (covariant return) 
    115                 const enorm<sq_T>& posterior() const {return est;} 
    116                 //! load basic elements of Kalman from structure 
    117                 void from_setting (const Setting &set) { 
    118                         StateSpace<sq_T>::from_setting(set); 
    119                                                  
    120                         mat P0; vec mu0; 
    121                         UI::get(mu0, set, "mu0", UI::optional); 
    122                         UI::get(P0, set,  "P0", UI::optional); 
    123                         set_statistics(mu0,P0); 
    124                         // Initial values 
    125                         UI::get (yrv, set, "yrv", UI::optional); 
    126                         UI::get (rvc, set, "urv", UI::optional); 
    127                         set_yrv(concat(yrv,rvc)); 
    128                          
    129                         validate(); 
    130                 } 
    131                 //! validate object 
    132                 void validate() { 
    133                         StateSpace<sq_T>::validate(); 
    134                         dimy = this->C.rows(); 
    135                         dimc = this->B.cols(); 
    136                         set_dim(this->A.rows()); 
    137                          
    138                         bdm_assert(est.dimension(), "Statistics and model parameters mismatch"); 
    139                 } 
     105class Kalman: public BM, public StateSpace<sq_T> { 
     106protected: 
     107        //! id of output 
     108        RV yrv; 
     109        //! Kalman gain 
     110        mat  _K; 
     111        //!posterior 
     112        enorm<sq_T> est; 
     113        //!marginal on data f(y|y) 
     114        enorm<sq_T>  fy; 
     115public: 
     116        Kalman<sq_T>() : BM(), StateSpace<sq_T>(), yrv(), _K(),  est() {} 
     117        //! Copy constructor 
     118        Kalman<sq_T> ( const Kalman<sq_T> &K0 ) : BM ( K0 ), StateSpace<sq_T> ( K0 ), yrv ( K0.yrv ), _K ( K0._K ),  est ( K0.est ), fy ( K0.fy ) {} 
     119        //!set statistics of the posterior 
     120        void set_statistics ( const vec &mu0, const mat &P0 ) { 
     121                est.set_parameters ( mu0, P0 ); 
     122        }; 
     123        //!set statistics of the posterior 
     124        void set_statistics ( const vec &mu0, const sq_T &P0 ) { 
     125                est.set_parameters ( mu0, P0 ); 
     126        }; 
     127        //! return correctly typed posterior (covariant return) 
     128        const enorm<sq_T>& posterior() const { 
     129                return est; 
     130        } 
     131        //! load basic elements of Kalman from structure 
     132        void from_setting ( const Setting &set ) { 
     133                StateSpace<sq_T>::from_setting ( set ); 
     134 
     135                mat P0; 
     136                vec mu0; 
     137                UI::get ( mu0, set, "mu0", UI::optional ); 
     138                UI::get ( P0, set,  "P0", UI::optional ); 
     139                set_statistics ( mu0, P0 ); 
     140                // Initial values 
     141                UI::get ( yrv, set, "yrv", UI::optional ); 
     142                UI::get ( rvc, set, "urv", UI::optional ); 
     143                set_yrv ( concat ( yrv, rvc ) ); 
     144 
     145                validate(); 
     146        } 
     147        //! validate object 
     148        void validate() { 
     149                StateSpace<sq_T>::validate(); 
     150                dimy = this->C.rows(); 
     151                dimc = this->B.cols(); 
     152                set_dim ( this->A.rows() ); 
     153 
     154                bdm_assert ( est.dimension(), "Statistics and model parameters mismatch" ); 
     155        } 
    140156}; 
    141157/*! 
    … …  
    143159*/ 
    144160 
    145 class KalmanFull : public Kalman<fsqmat> 
    146 { 
    147         public: 
    148                 //! For EKFfull; 
    149                 KalmanFull() :Kalman<fsqmat>(){}; 
    150                 //! Here dt = [yt;ut] of appropriate dimensions 
    151                 void bayes (const vec &yt, const vec &cond=empty_vec); 
    152                 BM* _copy_() const { 
    153                         KalmanFull* K = new KalmanFull; 
    154                         K->set_parameters (A, B, C, D, Q, R); 
    155                         K->set_statistics (est._mu(), est._R()); 
    156                         return K; 
    157                 } 
    158 }; 
    159 UIREGISTER(KalmanFull); 
     161class KalmanFull : public Kalman<fsqmat> { 
     162public: 
     163        //! For EKFfull; 
     164        KalmanFull() : Kalman<fsqmat>() {}; 
     165        //! Here dt = [yt;ut] of appropriate dimensions 
     166        void bayes ( const vec &yt, const vec &cond = empty_vec ); 
     167        BM* _copy_() const { 
     168                KalmanFull* K = new KalmanFull; 
     169                K->set_parameters ( A, B, C, D, Q, R ); 
     170                K->set_statistics ( est._mu(), est._R() ); 
     171                return K; 
     172        } 
     173}; 
     174UIREGISTER ( KalmanFull ); 
    160175 
    161176 
    … …  
    167182Complete constructor: 
    168183*/ 
    169 class KalmanCh : public Kalman<chmat> 
    170 { 
    171         protected: 
    172                 //! @{ \name Internal storage - needs initialize() 
    173                 //! pre array (triangular matrix) 
    174                 mat preA; 
    175                 //! post array (triangular matrix) 
    176                 mat postA; 
    177                 //!@} 
    178         public: 
    179                 //! copy constructor 
    180                 BM* _copy_() const { 
    181                         KalmanCh* K = new KalmanCh; 
    182                         K->set_parameters (A, B, C, D, Q, R); 
    183                         K->set_statistics (est._mu(), est._R()); 
    184                         K->validate(); 
    185                         return K; 
    186                 } 
    187                 //! set parameters for adapt from Kalman 
    188                 void set_parameters (const mat &A0, const mat &B0, const mat &C0, const mat &D0, const chmat &Q0, const chmat &R0); 
    189                 //! initialize internal parametetrs 
    190                 void initialize(); 
    191  
    192                 /*!\brief  Here dt = [yt;ut] of appropriate dimensions 
    193  
    194                 The following equality hold::\f[ 
    195                 \left[\begin{array}{cc} 
    196                 R^{0.5}\\ 
    197                 P_{t|t-1}^{0.5}C' & P_{t|t-1}^{0.5}CA'\\ 
    198                 & Q^{0.5}\end{array}\right]<\mathrm{orth.oper.}>=\left[\begin{array}{cc} 
    199                 R_{y}^{0.5} & KA'\\ 
    200                 & P_{t+1|t}^{0.5}\\ 
    201                 \\\end{array}\right]\f] 
    202  
    203                 Thus this object evaluates only predictors! Not filtering densities. 
    204                 */ 
    205                 void bayes (const vec &yt, const vec &cond=empty_vec); 
    206  
    207                 void from_setting(const Setting &set){ 
    208                         Kalman<chmat>::from_setting(set); 
    209                         validate(); 
    210                 } 
    211                 void validate() { 
    212                         Kalman<chmat>::validate(); 
    213                         initialize(); 
    214                 } 
    215 }; 
    216 UIREGISTER(KalmanCh); 
     184class KalmanCh : public Kalman<chmat> { 
     185protected: 
     186        //! @{ \name Internal storage - needs initialize() 
     187        //! pre array (triangular matrix) 
     188        mat preA; 
     189        //! post array (triangular matrix) 
     190        mat postA; 
     191        //!@} 
     192public: 
     193        //! copy constructor 
     194        BM* _copy_() const { 
     195                KalmanCh* K = new KalmanCh; 
     196                K->set_parameters ( A, B, C, D, Q, R ); 
     197                K->set_statistics ( est._mu(), est._R() ); 
     198                K->validate(); 
     199                return K; 
     200        } 
     201        //! set parameters for adapt from Kalman 
     202        void set_parameters ( const mat &A0, const mat &B0, const mat &C0, const mat &D0, const chmat &Q0, const chmat &R0 ); 
     203        //! initialize internal parametetrs 
     204        void initialize(); 
     205 
     206        /*!\brief  Here dt = [yt;ut] of appropriate dimensions 
     207 
     208        The following equality hold::\f[ 
     209        \left[\begin{array}{cc} 
     210        R^{0.5}\\ 
     211        P_{t|t-1}^{0.5}C' & P_{t|t-1}^{0.5}CA'\\ 
     212        & Q^{0.5}\end{array}\right]<\mathrm{orth.oper.}>=\left[\begin{array}{cc} 
     213        R_{y}^{0.5} & KA'\\ 
     214        & P_{t+1|t}^{0.5}\\ 
     215        \\\end{array}\right]\f] 
     216 
     217        Thus this object evaluates only predictors! Not filtering densities. 
     218        */ 
     219        void bayes ( const vec &yt, const vec &cond = empty_vec ); 
     220 
     221        void from_setting ( const Setting &set ) { 
     222                Kalman<chmat>::from_setting ( set ); 
     223                validate(); 
     224        } 
     225        void validate() { 
     226                Kalman<chmat>::validate(); 
     227                initialize(); 
     228        } 
     229}; 
     230UIREGISTER ( KalmanCh ); 
    217231 
    218232/*! 
    … …  
    221235An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. 
    222236*/ 
    223 class EKFfull : public KalmanFull 
    224 { 
    225         protected: 
    226                 //! Internal Model f(x,u) 
    227                 shared_ptr<diffbifn> pfxu; 
    228  
    229                 //! Observation Model h(x,u) 
    230                 shared_ptr<diffbifn> phxu; 
    231  
    232         public: 
    233                 //! Default constructor 
    234                 EKFfull (); 
    235  
    236                 //! Set nonlinear functions for mean values and covariance matrices. 
    237                 void set_parameters (const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const mat Q0, const mat R0); 
    238  
    239                 //! Here dt = [yt;ut] of appropriate dimensions 
    240                 void bayes (const vec &yt, const vec &cond=empty_vec); 
    241                 //! set estimates 
    242                 void set_statistics (const vec &mu0, const mat &P0) { 
    243                         est.set_parameters (mu0, P0); 
    244                 }; 
    245                 //! access function 
    246                 const mat _R() { 
    247                         return est._R().to_mat(); 
    248                 } 
    249                 void from_setting (const Setting &set) {                         
    250                         BM::from_setting(set); 
    251                         shared_ptr<diffbifn> IM = UI::build<diffbifn> ( set, "IM", UI::compulsory ); 
    252                         shared_ptr<diffbifn> OM = UI::build<diffbifn> ( set, "OM", UI::compulsory ); 
    253                          
    254                         //statistics 
    255                         int dim = IM->dimension(); 
    256                         vec mu0; 
    257                         if ( !UI::get ( mu0, set, "mu0" ) ) 
    258                                 mu0 = zeros ( dim ); 
    259                          
    260                         mat P0; 
    261                         vec dP0; 
    262                         if ( UI::get ( dP0, set, "dP0" ) ) 
    263                                 P0 = diag ( dP0 ); 
    264                         else if ( !UI::get ( P0, set, "P0" ) ) 
    265                                 P0 = eye ( dim ); 
    266                          
    267                         set_statistics ( mu0, P0 ); 
    268                          
    269                         //parameters 
    270                         vec dQ, dR; 
    271                         UI::get ( dQ, set, "dQ", UI::compulsory ); 
    272                         UI::get ( dR, set, "dR", UI::compulsory ); 
    273                         set_parameters ( IM, OM, diag ( dQ ), diag ( dR ) ); 
    274                                                  
    275                         string options; 
    276                         if ( UI::get ( options, set, "options" ) ) 
    277                                 set_options ( options ); 
     237class EKFfull : public KalmanFull { 
     238protected: 
     239        //! Internal Model f(x,u) 
     240        shared_ptr<diffbifn> pfxu; 
     241 
     242        //! Observation Model h(x,u) 
     243        shared_ptr<diffbifn> phxu; 
     244 
     245public: 
     246        //! Default constructor 
     247        EKFfull (); 
     248 
     249        //! Set nonlinear functions for mean values and covariance matrices. 
     250        void set_parameters ( const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const mat Q0, const mat R0 ); 
     251 
     252        //! Here dt = [yt;ut] of appropriate dimensions 
     253        void bayes ( const vec &yt, const vec &cond = empty_vec ); 
     254        //! set estimates 
     255        void set_statistics ( const vec &mu0, const mat &P0 ) { 
     256                est.set_parameters ( mu0, P0 ); 
     257        }; 
     258        //! access function 
     259        const mat _R() { 
     260                return est._R().to_mat(); 
     261        } 
     262        void from_setting ( const Setting &set ) { 
     263                BM::from_setting ( set ); 
     264                shared_ptr<diffbifn> IM = UI::build<diffbifn> ( set, "IM", UI::compulsory ); 
     265                shared_ptr<diffbifn> OM = UI::build<diffbifn> ( set, "OM", UI::compulsory ); 
     266 
     267                //statistics 
     268                int dim = IM->dimension(); 
     269                vec mu0; 
     270                if ( !UI::get ( mu0, set, "mu0" ) ) 
     271                        mu0 = zeros ( dim ); 
     272 
     273                mat P0; 
     274                vec dP0; 
     275                if ( UI::get ( dP0, set, "dP0" ) ) 
     276                        P0 = diag ( dP0 ); 
     277                else if ( !UI::get ( P0, set, "P0" ) ) 
     278                        P0 = eye ( dim ); 
     279 
     280                set_statistics ( mu0, P0 ); 
     281 
     282                //parameters 
     283                vec dQ, dR; 
     284                UI::get ( dQ, set, "dQ", UI::compulsory ); 
     285                UI::get ( dR, set, "dR", UI::compulsory ); 
     286                set_parameters ( IM, OM, diag ( dQ ), diag ( dR ) ); 
     287 
     288                string options; 
     289                if ( UI::get ( options, set, "options" ) ) 
     290                        set_options ( options ); 
    278291//                      pfxu = UI::build<diffbifn>(set, "IM", UI::compulsory); 
    279292//                      phxu = UI::build<diffbifn>(set, "OM", UI::compulsory); 
    280 //                       
     293// 
    281294//                      mat R0; 
    282295//                      UI::get(R0, set, "R",UI::compulsory); 
    283296//                      mat Q0; 
    284297//                      UI::get(Q0, set, "Q",UI::compulsory); 
    285 //                       
    286 //                       
     298// 
     299// 
    287300//                      mat P0; vec mu0; 
    288301//                      UI::get(mu0, set, "mu0", UI::optional); 
    … …  
    293306//                      UI::get (urv, set, "urv", UI::optional); 
    294307//                      set_drv(concat(yrv,urv)); 
    295 //  
     308// 
    296309//                      // setup StateSpace 
    297310//                      pfxu->dfdu_cond(mu0, zeros(pfxu->_dimu()), A,true); 
    298311//                      phxu->dfdu_cond(mu0, zeros(pfxu->_dimu()), C,true); 
    299 //                       
    300                         validate(); 
    301                 } 
    302                 void validate() { 
    303                         // check stats and IM and OM 
    304                 } 
    305 }; 
    306 UIREGISTER(EKFfull); 
     312// 
     313                validate(); 
     314        } 
     315        void validate() { 
     316                // check stats and IM and OM 
     317        } 
     318}; 
     319UIREGISTER ( EKFfull ); 
    307320 
    308321 
    … …  
    313326*/ 
    314327 
    315 class EKFCh : public KalmanCh 
    316 { 
    317         protected: 
    318                 //! Internal Model f(x,u) 
    319                 shared_ptr<diffbifn> pfxu; 
    320  
    321                 //! Observation Model h(x,u) 
    322                 shared_ptr<diffbifn> phxu; 
    323         public: 
    324                 //! copy constructor duplicated - calls different set_parameters 
    325                 BM* _copy_() const { 
    326                         EKFCh* E = new EKFCh; 
    327                         E->set_parameters (pfxu, phxu, Q, R); 
    328                         E->set_statistics (est._mu(), est._R()); 
    329                         return E; 
    330                 } 
    331                 //! Set nonlinear functions for mean values and covariance matrices. 
    332                 void set_parameters (const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const chmat Q0, const chmat R0); 
    333  
    334                 //! Here dt = [yt;ut] of appropriate dimensions 
    335                 void bayes (const vec &yt, const vec &cond=empty_vec); 
    336  
    337                 void from_setting (const Setting &set); 
    338  
    339                 void validate(){}; 
    340                 // TODO dodelat void to_setting( Setting &set ) const; 
    341  
    342 }; 
    343  
    344 UIREGISTER (EKFCh); 
    345 SHAREDPTR (EKFCh); 
     328class EKFCh : public KalmanCh { 
     329protected: 
     330        //! Internal Model f(x,u) 
     331        shared_ptr<diffbifn> pfxu; 
     332 
     333        //! Observation Model h(x,u) 
     334        shared_ptr<diffbifn> phxu; 
     335public: 
     336        //! copy constructor duplicated - calls different set_parameters 
     337        BM* _copy_() const { 
     338                EKFCh* E = new EKFCh; 
     339                E->set_parameters ( pfxu, phxu, Q, R ); 
     340                E->set_statistics ( est._mu(), est._R() ); 
     341                return E; 
     342        } 
     343        //! Set nonlinear functions for mean values and covariance matrices. 
     344        void set_parameters ( const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const chmat Q0, const chmat R0 ); 
     345 
     346        //! Here dt = [yt;ut] of appropriate dimensions 
     347        void bayes ( const vec &yt, const vec &cond = empty_vec ); 
     348 
     349        void from_setting ( const Setting &set ); 
     350 
     351        void validate() {}; 
     352        // TODO dodelat void to_setting( Setting &set ) const; 
     353 
     354}; 
     355 
     356UIREGISTER ( EKFCh ); 
     357SHAREDPTR ( EKFCh ); 
    346358 
    347359 
    … …  
    355367The next step is performed with the new statistics for all models. 
    356368*/ 
    357 class MultiModel: public BM 
    358 { 
    359         protected: 
    360                 //! List of models between which we switch 
    361                 Array<EKFCh*> Models; 
    362                 //! vector of model weights 
    363                 vec w; 
    364                 //! cache of model lls 
    365                 vec _lls; 
    366                 //! type of switching policy [1=maximum,2=...] 
    367                 int policy; 
    368                 //! internal statistics 
    369                 enorm<chmat> est; 
    370         public: 
    371                 //! set internal parameters 
    372                 void set_parameters (Array<EKFCh*> A, int pol0 = 1) { 
    373                         Models = A;//TODO: test if evalll is set 
    374                         w.set_length (A.length()); 
    375                         _lls.set_length (A.length()); 
    376                         policy = pol0; 
    377  
    378                         est.set_rv (RV ("MM", A (0)->posterior().dimension(), 0)); 
    379                         est.set_parameters (A (0)->posterior().mean(), A (0)->posterior()._R()); 
    380                 } 
    381                 void bayes (const vec &yt, const vec &cond=empty_vec) { 
    382                         int n = Models.length(); 
    383                         int i; 
    384                         for (i = 0; i < n; i++) { 
    385                                 Models (i)->bayes (yt); 
    386                                 _lls (i) = Models (i)->_ll(); 
    387                         } 
    388                         double mlls = max (_lls); 
    389                         w = exp (_lls - mlls); 
    390                         w /= sum (w);   //normalization 
    391                         //set statistics 
    392                         switch (policy) { 
    393                                 case 1: { 
    394                                         int mi = max_index (w); 
    395                                         const enorm<chmat> &st = Models (mi)->posterior() ; 
    396                                         est.set_parameters (st.mean(), st._R()); 
    397                                 } 
    398                                 break; 
    399                                 default: 
    400                                         bdm_error ("unknown policy"); 
    401                         } 
    402                         // copy result to all models 
    403                         for (i = 0; i < n; i++) { 
    404                                 Models (i)->set_statistics (est.mean(), est._R()); 
    405                         } 
    406                 } 
    407                 //! return correctly typed posterior (covariant return) 
    408                 const enorm<chmat>& posterior() const { 
    409                         return est; 
    410                 } 
    411  
    412                 void from_setting (const Setting &set); 
    413  
    414                 // TODO dodelat void to_setting( Setting &set ) const; 
    415  
    416 }; 
    417  
    418 UIREGISTER (MultiModel); 
    419 SHAREDPTR (MultiModel); 
    420  
    421 //! conversion of outer ARX model (mlnorm) to state space model  
     369class MultiModel: public BM { 
     370protected: 
     371        //! List of models between which we switch 
     372        Array<EKFCh*> Models; 
     373        //! vector of model weights 
     374        vec w; 
     375        //! cache of model lls 
     376        vec _lls; 
     377        //! type of switching policy [1=maximum,2=...] 
     378        int policy; 
     379        //! internal statistics 
     380        enorm<chmat> est; 
     381public: 
     382        //! set internal parameters 
     383        void set_parameters ( Array<EKFCh*> A, int pol0 = 1 ) { 
     384                Models = A;//TODO: test if evalll is set 
     385                w.set_length ( A.length() ); 
     386                _lls.set_length ( A.length() ); 
     387                policy = pol0; 
     388 
     389                est.set_rv ( RV ( "MM", A ( 0 )->posterior().dimension(), 0 ) ); 
     390                est.set_parameters ( A ( 0 )->posterior().mean(), A ( 0 )->posterior()._R() ); 
     391        } 
     392        void bayes ( const vec &yt, const vec &cond = empty_vec ) { 
     393                int n = Models.length(); 
     394                int i; 
     395                for ( i = 0; i < n; i++ ) { 
     396                        Models ( i )->bayes ( yt ); 
     397                        _lls ( i ) = Models ( i )->_ll(); 
     398                } 
     399                double mlls = max ( _lls ); 
     400                w = exp ( _lls - mlls ); 
     401                w /= sum ( w ); //normalization 
     402                //set statistics 
     403                switch ( policy ) { 
     404                case 1: { 
     405                        int mi = max_index ( w ); 
     406                        const enorm<chmat> &st = Models ( mi )->posterior() ; 
     407                        est.set_parameters ( st.mean(), st._R() ); 
     408                } 
     409                break; 
     410                default: 
     411                        bdm_error ( "unknown policy" ); 
     412                } 
     413                // copy result to all models 
     414                for ( i = 0; i < n; i++ ) { 
     415                        Models ( i )->set_statistics ( est.mean(), est._R() ); 
     416                } 
     417        } 
     418        //! return correctly typed posterior (covariant return) 
     419        const enorm<chmat>& posterior() const { 
     420                return est; 
     421        } 
     422 
     423        void from_setting ( const Setting &set ); 
     424 
     425        // TODO dodelat void to_setting( Setting &set ) const; 
     426 
     427}; 
     428 
     429UIREGISTER ( MultiModel ); 
     430SHAREDPTR ( MultiModel ); 
     431 
     432//! conversion of outer ARX model (mlnorm) to state space model 
    422433/*! 
    423434The model is constructed as: 
    … …  
    429440*/ 
    430441//template<class sq_T> 
    431 class StateCanonical: public StateSpace<fsqmat>{ 
    432         protected: 
    433                 //! remember connection from theta ->A 
    434                 datalink_part th2A; 
    435                 //! remember connection from theta ->C 
    436                 datalink_part th2C; 
    437                 //! remember connection from theta ->D 
    438                 datalink_part th2D; 
    439                 //!cached first row of A 
    440                 vec A1row; 
    441                 //!cached first row of C 
    442                 vec C1row; 
    443                 //!cached first row of D 
    444                 vec D1row; 
    445                  
    446         public: 
    447                 //! set up this object to match given mlnorm 
    448                 void connect_mlnorm(const mlnorm<fsqmat> &ml){ 
    449                         //get ids of yrv                                 
    450                         const RV &yrv = ml._rv(); 
    451                         //need to determine u_t - it is all in _rvc that is not in ml._rv() 
    452                         RV rgr0 = ml._rvc().remove_time(); 
    453                         RV urv = rgr0.subt(yrv);  
    454                          
    455                         //We can do only 1d now... :( 
    456                         bdm_assert(yrv._dsize()==1, "Only for SISO so far..." ); 
    457  
    458                         // create names for  
    459                         RV xrv; //empty 
    460                         RV Crv; //empty 
    461                         int td=ml._rvc().mint(); 
    462                         // assuming strictly proper function!!! 
    463                         for (int t=-1;t>=td;t--){ 
    464                                 xrv.add(yrv.copy_t(t)); 
    465                                 Crv.add(urv.copy_t(t)); 
    466                         } 
    467                                                  
    468                         // get mapp 
    469                         th2A.set_connection(xrv, ml._rvc()); 
    470                         th2C.set_connection(Crv, ml._rvc()); 
    471                         th2D.set_connection(urv, ml._rvc()); 
    472  
    473                         //set matrix sizes 
    474                         this->A=zeros(xrv._dsize(),xrv._dsize()); 
    475                         for (int j=1; j<xrv._dsize(); j++){A(j,j-1)=1.0;} // off diagonal 
    476                                 this->B=zeros(xrv._dsize(),1); 
    477                                 this->B(0) = 1.0; 
    478                                 this->C=zeros(1,xrv._dsize()); 
    479                                 this->D=zeros(1,urv._dsize()); 
    480                                 this->Q = zeros(xrv._dsize(),xrv._dsize()); 
    481                         // R is set by update 
    482                          
    483                         //set cache 
    484                         this->A1row = zeros(xrv._dsize()); 
    485                         this->C1row = zeros(xrv._dsize()); 
    486                         this->D1row = zeros(urv._dsize()); 
    487                          
    488                         update_from(ml); 
    489                         validate(); 
    490                 }; 
    491                 //! fast function to update parameters from ml - not checked for compatibility!! 
    492                 void update_from(const mlnorm<fsqmat> &ml){ 
    493                          
    494                         vec theta = ml._A().get_row(0); // this  
    495                          
    496                         th2A.filldown(theta,A1row); 
    497                         th2C.filldown(theta,C1row); 
    498                         th2D.filldown(theta,D1row); 
    499  
    500                         R = ml._R(); 
    501  
    502                         A.set_row(0,A1row); 
    503                         C.set_row(0,C1row+D1row(0)*A1row); 
    504                         D.set_row(0,D1row); 
    505                          
    506                 } 
     442class StateCanonical: public StateSpace<fsqmat> { 
     443protected: 
     444        //! remember connection from theta ->A 
     445        datalink_part th2A; 
     446        //! remember connection from theta ->C 
     447        datalink_part th2C; 
     448        //! remember connection from theta ->D 
     449        datalink_part th2D; 
     450        //!cached first row of A 
     451        vec A1row; 
     452        //!cached first row of C 
     453        vec C1row; 
     454        //!cached first row of D 
     455        vec D1row; 
     456 
     457public: 
     458        //! set up this object to match given mlnorm 
     459        void connect_mlnorm ( const mlnorm<fsqmat> &ml ) { 
     460                //get ids of yrv 
     461                const RV &yrv = ml._rv(); 
     462                //need to determine u_t - it is all in _rvc that is not in ml._rv() 
     463                RV rgr0 = ml._rvc().remove_time(); 
     464                RV urv = rgr0.subt ( yrv ); 
     465 
     466                //We can do only 1d now... :( 
     467                bdm_assert ( yrv._dsize() == 1, "Only for SISO so far..." ); 
     468 
     469                // create names for 
     470                RV xrv; //empty 
     471                RV Crv; //empty 
     472                int td = ml._rvc().mint(); 
     473                // assuming strictly proper function!!! 
     474                for ( int t = -1; t >= td; t-- ) { 
     475                        xrv.add ( yrv.copy_t ( t ) ); 
     476                        Crv.add ( urv.copy_t ( t ) ); 
     477                } 
     478 
     479                // get mapp 
     480                th2A.set_connection ( xrv, ml._rvc() ); 
     481                th2C.set_connection ( Crv, ml._rvc() ); 
     482                th2D.set_connection ( urv, ml._rvc() ); 
     483 
     484                //set matrix sizes 
     485                this->A = zeros ( xrv._dsize(), xrv._dsize() ); 
     486                for ( int j = 1; j < xrv._dsize(); j++ ) { 
     487                        A ( j, j - 1 ) = 1.0;    // off diagonal 
     488                } 
     489                this->B = zeros ( xrv._dsize(), 1 ); 
     490                this->B ( 0 ) = 1.0; 
     491                this->C = zeros ( 1, xrv._dsize() ); 
     492                this->D = zeros ( 1, urv._dsize() ); 
     493                this->Q = zeros ( xrv._dsize(), xrv._dsize() ); 
     494                // R is set by update 
     495 
     496                //set cache 
     497                this->A1row = zeros ( xrv._dsize() ); 
     498                this->C1row = zeros ( xrv._dsize() ); 
     499                this->D1row = zeros ( urv._dsize() ); 
     500 
     501                update_from ( ml ); 
     502                validate(); 
     503        }; 
     504        //! fast function to update parameters from ml - not checked for compatibility!! 
     505        void update_from ( const mlnorm<fsqmat> &ml ) { 
     506 
     507                vec theta = ml._A().get_row ( 0 ); // this 
     508 
     509                th2A.filldown ( theta, A1row ); 
     510                th2C.filldown ( theta, C1row ); 
     511                th2D.filldown ( theta, D1row ); 
     512 
     513                R = ml._R(); 
     514 
     515                A.set_row ( 0, A1row ); 
     516                C.set_row ( 0, C1row + D1row ( 0 ) *A1row ); 
     517                D.set_row ( 0, D1row ); 
     518 
     519        } 
    507520}; 
    508521/*! 
    509522State-Space representation of multivariate autoregressive model. 
    510523The original model: 
    511 \f[ y_t = \theta [\ldots y_{t-k}, \ldots u_{t-l}, \ldots z_{t-m}]' + \Sigma^{-1/2} e_t \f]  
     524\f[ y_t = \theta [\ldots y_{t-k}, \ldots u_{t-l}, \ldots z_{t-m}]' + \Sigma^{-1/2} e_t \f] 
    512525where \f$ k,l,m \f$ are maximum delayes of corresponding variables in the regressor. 
    513526 
    … …  
    519532 
    520533*/ 
    521 class StateFromARX: public StateSpace<chmat>{ 
    522         protected: 
    523                 //! remember connection from theta ->A 
    524                 datalink_part th2A; 
    525                 //! remember connection from theta ->B 
    526                 datalink_part th2B; 
    527                 //!function adds n diagonal elements from given starting point r,c 
    528                 void diagonal_part(mat &A, int r, int c, int n){ 
    529                         for(int i=0; i<n; i++){A(r,c)=1.0; r++;c++;} 
    530                 }; 
    531                 //! similar to ARX.have_constant 
    532                 bool have_constant; 
    533         public: 
    534                 //! set up this object to match given mlnorm 
    535                 //! Note that state-space and common mpdf use different meaning of \f$ _t \f$ in \f$ u_t \f$.  
    536                 //!While mlnorm typically assumes that \f$ u_t \rightarrow y_t \f$ in state space it is \f$ u_{t-1} \rightarrow y_t \f$ 
    537                 //! For consequences in notation of internal variable xt see arx2statespace_notes.lyx. 
    538                 void connect_mlnorm(const mlnorm<chmat> &ml, RV &xrv, RV &urv){ 
    539  
    540                         //get ids of yrv                                 
    541                         const RV &yrv = ml._rv(); 
    542                         //need to determine u_t - it is all in _rvc that is not in ml._rv() 
    543                         const RV &rgr = ml._rvc(); 
    544                         RV rgr0 = rgr.remove_time(); 
    545                         urv = rgr0.subt(yrv);  
    546                                                  
    547                         // create names for state variables 
    548                         xrv=yrv;  
    549                          
    550                         int y_multiplicity = -rgr.mint(yrv); 
    551                         int y_block_size = yrv.length()*(y_multiplicity); // current yt + all delayed yts 
    552                         for (int m=0;m<y_multiplicity - 1;m++){ // ========= -1 is important see arx2statespace_notes 
    553                                 xrv.add(yrv.copy_t(-m-1)); //add delayed yt 
     534class StateFromARX: public StateSpace<chmat> { 
     535protected: 
     536        //! remember connection from theta ->A 
     537        datalink_part th2A; 
     538        //! remember connection from theta ->B 
     539        datalink_part th2B; 
     540        //!function adds n diagonal elements from given starting point r,c 
     541        void diagonal_part ( mat &A, int r, int c, int n ) { 
     542                for ( int i = 0; i < n; i++ ) { 
     543                        A ( r, c ) = 1.0; 
     544                        r++; 
     545                        c++; 
     546                } 
     547        }; 
     548        //! similar to ARX.have_constant 
     549        bool have_constant; 
     550public: 
     551        //! set up this object to match given mlnorm 
     552        //! Note that state-space and common mpdf use different meaning of \f$ _t \f$ in \f$ u_t \f$. 
     553        //!While mlnorm typically assumes that \f$ u_t \rightarrow y_t \f$ in state space it is \f$ u_{t-1} \rightarrow y_t \f$ 
     554        //! For consequences in notation of internal variable xt see arx2statespace_notes.lyx. 
     555        void connect_mlnorm ( const mlnorm<chmat> &ml, RV &xrv, RV &urv ) { 
     556 
     557                //get ids of yrv 
     558                const RV &yrv = ml._rv(); 
     559                //need to determine u_t - it is all in _rvc that is not in ml._rv() 
     560                const RV &rgr = ml._rvc(); 
     561                RV rgr0 = rgr.remove_time(); 
     562                urv = rgr0.subt ( yrv ); 
     563 
     564                // create names for state variables 
     565                xrv = yrv; 
     566 
     567                int y_multiplicity = -rgr.mint ( yrv ); 
     568                int y_block_size = yrv.length() * ( y_multiplicity ); // current yt + all delayed yts 
     569                for ( int m = 0; m < y_multiplicity - 1; m++ ) { // ========= -1 is important see arx2statespace_notes 
     570                        xrv.add ( yrv.copy_t ( -m - 1 ) ); //add delayed yt 
     571                } 
     572                //! temporary RV for connection to ml.rvc, since notation of xrv and ml.rvc does not match 
     573                RV xrv_ml = xrv.copy_t ( -1 ); 
     574 
     575                // add regressors 
     576                ivec u_block_sizes ( urv.length() ); // no_blocks = yt + unique rgr 
     577                for ( int r = 0; r < urv.length(); r++ ) { 
     578                        RV R = urv.subselect ( vec_1 ( r ) ); //non-delayed element of rgr 
     579                        int r_size = urv.size ( r ); 
     580                        int r_multiplicity = -rgr.mint ( R ); 
     581                        u_block_sizes ( r ) = r_size * r_multiplicity ; 
     582                        for ( int m = 0; m < r_multiplicity; m++ ) { 
     583                                xrv.add ( R.copy_t ( -m - 1 ) ); //add delayed yt 
     584                                xrv_ml.add ( R.copy_t ( -m - 1 ) ); //add delayed yt 
    554585                        } 
    555                         //! temporary RV for connection to ml.rvc, since notation of xrv and ml.rvc does not match 
    556                         RV xrv_ml = xrv.copy_t(-1); 
    557                          
    558                         // add regressors 
    559                         ivec u_block_sizes(urv.length()); // no_blocks = yt + unique rgr 
    560                         for (int r=0;r<urv.length();r++){ 
    561                                 RV R=urv.subselect(vec_1(r)); //non-delayed element of rgr 
    562                                 int r_size=urv.size(r); 
    563                                 int r_multiplicity=-rgr.mint(R); 
    564                                 u_block_sizes(r)= r_size*r_multiplicity ; 
    565                                 for(int m=0;m<r_multiplicity;m++){  
    566                                         xrv.add(R.copy_t(-m-1)); //add delayed yt 
    567                                         xrv_ml.add(R.copy_t(-m-1)); //add delayed yt 
    568                                 } 
     586                } 
     587                // add constant 
     588                if ( any ( ml._mu_const() != 0.0 ) ) { 
     589                        have_constant = true; 
     590                        xrv.add ( RV ( "bdm_reserved_constant_one", 1 ) ); 
     591                } else { 
     592                        have_constant = false; 
     593                } 
     594 
     595 
     596                // get mapp 
     597                th2A.set_connection ( xrv_ml, ml._rvc() ); 
     598                th2B.set_connection ( urv, ml._rvc() ); 
     599 
     600                //set matrix sizes 
     601                this->A = zeros ( xrv._dsize(), xrv._dsize() ); 
     602                //create y block 
     603                diagonal_part ( this->A, yrv._dsize(), 0, y_block_size - yrv._dsize() ); 
     604 
     605                this->B = zeros ( xrv._dsize(), urv._dsize() ); 
     606                //add diagonals for rgr 
     607                int active_x = y_block_size; 
     608                for ( int r = 0; r < urv.length(); r++ ) { 
     609                        diagonal_part ( this->A, active_x + urv.size ( r ), active_x, u_block_sizes ( r ) - urv.size ( r ) ); 
     610                        this->B.set_submatrix ( active_x, 0, eye ( urv.size ( r ) ) ); 
     611                        active_x += u_block_sizes ( r ); 
     612                } 
     613                this->C = zeros ( yrv._dsize(), xrv._dsize() ); 
     614                this->C.set_submatrix ( 0, 0, eye ( yrv._dsize() ) ); 
     615                this->D = zeros ( yrv._dsize(), urv._dsize() ); 
     616                this->R.setCh ( zeros ( yrv._dsize(), yrv._dsize() ) ); 
     617                this->Q.setCh ( zeros ( xrv._dsize(), xrv._dsize() ) ); 
     618                // Q is set by update 
     619 
     620                update_from ( ml ); 
     621                validate(); 
     622        }; 
     623        //! fast function to update parameters from ml - not checked for compatibility!! 
     624        void update_from ( const mlnorm<chmat> &ml ) { 
     625 
     626                vec Arow = zeros ( A.cols() ); 
     627                vec Brow = zeros ( B.cols() ); 
     628                //  ROW- WISE EVALUATION ===== 
     629                for ( int i = 0; i < ml._rv()._dsize(); i++ ) { 
     630 
     631                        vec theta = ml._A().get_row ( i ); 
     632 
     633                        th2A.filldown ( theta, Arow ); 
     634                        if ( have_constant ) { 
     635                                // constant is always at the end no need for datalink 
     636                                Arow ( A.cols() - 1 ) = ml._mu_const() ( i ); 
    569637                        } 
    570                         // add constant 
    571                         if (any(ml._mu_const()!=0.0)){ 
    572                                 have_constant=true; 
    573                                 xrv.add(RV("bdm_reserved_constant_one",1)); 
    574                         } else {have_constant=false;} 
    575                                  
    576                  
    577                         // get mapp 
    578                         th2A.set_connection(xrv_ml, ml._rvc()); 
    579                         th2B.set_connection(urv, ml._rvc()); 
    580                          
    581                         //set matrix sizes 
    582                         this->A=zeros(xrv._dsize(),xrv._dsize()); 
    583                         //create y block 
    584                         diagonal_part(this->A, yrv._dsize(), 0, y_block_size-yrv._dsize() ); 
    585                          
    586                         this->B=zeros(xrv._dsize(), urv._dsize()); 
    587                         //add diagonals for rgr 
    588                         int active_x=y_block_size; 
    589                         for (int r=0; r<urv.length(); r++){ 
    590                                 diagonal_part( this->A, active_x+urv.size(r),active_x, u_block_sizes(r)-urv.size(r)); 
    591                                 this->B.set_submatrix(active_x, 0, eye(urv.size(r))); 
    592                                 active_x+=u_block_sizes(r); 
    593                         } 
    594                         this->C=zeros(yrv._dsize(), xrv._dsize()); 
    595                         this->C.set_submatrix(0,0,eye(yrv._dsize())); 
    596                         this->D=zeros(yrv._dsize(),urv._dsize()); 
    597                         this->R.setCh(zeros(yrv._dsize(),yrv._dsize())); 
    598                         this->Q.setCh(zeros(xrv._dsize(),xrv._dsize())); 
    599                         // Q is set by update 
    600                                                          
    601                         update_from(ml); 
    602                         validate(); 
    603                 }; 
    604                 //! fast function to update parameters from ml - not checked for compatibility!! 
    605                 void update_from(const mlnorm<chmat> &ml){ 
    606  
    607                         vec Arow=zeros(A.cols()); 
    608                         vec Brow=zeros(B.cols()); 
    609                         //  ROW- WISE EVALUATION ===== 
    610                         for(int i=0;i<ml._rv()._dsize(); i++){  
    611                                  
    612                                 vec theta = ml._A().get_row(i);  
    613                                  
    614                                 th2A.filldown(theta,Arow); 
    615                                 if (have_constant){ 
    616                                         // constant is always at the end no need for datalink 
    617                                         Arow(A.cols()-1) = ml._mu_const()(i); 
    618                                 } 
    619                                 this->A.set_row(i,Arow); 
    620                                  
    621                                 th2B.filldown(theta,Brow); 
    622                                 this->B.set_row(i,Brow); 
    623                         } 
    624                         this->Q._Ch().set_submatrix(0,0,ml.__R()._Ch()); 
    625                          
    626                 }; 
    627                 //! access function 
    628                 bool _have_constant() const {return have_constant;} 
     638                        this->A.set_row ( i, Arow ); 
     639 
     640                        th2B.filldown ( theta, Brow ); 
     641                        this->B.set_row ( i, Brow ); 
     642                } 
     643                this->Q._Ch().set_submatrix ( 0, 0, ml.__R()._Ch() ); 
     644 
     645        }; 
     646        //! access function 
     647        bool _have_constant() const { 
     648                return have_constant; 
     649        } 
    629650}; 
    630651 
    … …  
    632653 
    633654template<class sq_T> 
    634 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) 
    635 { 
     655void 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 ) { 
    636656 
    637657        A = A0; 
    … …  
    645665 
    646666template<class sq_T> 
    647 void StateSpace<sq_T>::validate(){ 
    648         bdm_assert (A.cols() == A.rows(), "KalmanFull: A is not square"); 
    649         bdm_assert (B.rows() == A.rows(), "KalmanFull: B is not compatible"); 
    650         bdm_assert (C.cols() == A.rows(), "KalmanFull: C is not compatible"); 
    651         bdm_assert ( (D.rows() == C.rows()) && (D.cols() == B.cols()), "KalmanFull: D is not compatible"); 
    652         bdm_assert ( (Q.cols() == A.rows()) && (Q.rows() == A.rows()), "KalmanFull: Q is not compatible"); 
    653         bdm_assert ( (R.cols() == C.rows()) && (R.rows() == C.rows()), "KalmanFull: R is not compatible"); 
     667void StateSpace<sq_T>::validate() { 
     668        bdm_assert ( A.cols() == A.rows(), "KalmanFull: A is not square" ); 
     669        bdm_assert ( B.rows() == A.rows(), "KalmanFull: B is not compatible" ); 
     670        bdm_assert ( C.cols() == A.rows(), "KalmanFull: C is not compatible" ); 
     671        bdm_assert ( ( D.rows() == C.rows() ) && ( D.cols() == B.cols() ), "KalmanFull: D is not compatible" ); 
     672        bdm_assert ( ( Q.cols() == A.rows() ) && ( Q.rows() == A.rows() ), "KalmanFull: Q is not compatible" ); 
     673        bdm_assert ( ( R.cols() == C.rows() ) && ( R.rows() == C.rows() ), "KalmanFull: R is not compatible" ); 
    654674} 
    655675