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

Revision 737, 18.4 kB (checked in by mido, 14 years ago)

ASTYLER RUN OVER THE WHOLE LIBRARY, JUPEE

  • Property svn:eol-style set to native
Line 
1/*!
2  \file
3  \brief Bayesian Filtering for linear Gaussian models (Kalman Filter) and extensions
4  \author Vaclav Smidl.
5
6  -----------------------------------
7  BDM++ - C++ library for Bayesian Decision Making under Uncertainty
8
9  Using IT++ for numerical operations
10  -----------------------------------
11*/
12
13#ifndef KF_H
14#define KF_H
15
16
17#include "../math/functions.h"
18#include "../stat/exp_family.h"
19#include "../math/chmat.h"
20#include "../base/user_info.h"
21//#include <../applications/pmsm/simulator_zdenek/ekf_example/pmsm_mod.h>
22
23namespace bdm {
24
25/*!
26 * \brief Basic elements of linear state-space model
27
28Parameter evolution model:\f[ x_{t+1} = A x_{t} + B u_t + Q^{1/2} e_t \f]
29Observation model: \f[ y_t = C x_{t} + C u_t + R^{1/2} w_t. \f]
30Where $e_t$ and $w_t$ are mutually independent vectors of Normal(0,1)-distributed disturbances.
31 */
32template<class sq_T>
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
104template<class sq_T>
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        }
156};
157/*!
158* \brief Basic Kalman filter with full matrices
159*/
160
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 );
175
176
177/*! \brief Kalman filter in square root form
178
179Trivial example:
180\include kalman_simple.cpp
181
182Complete constructor:
183*/
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 );
231
232/*!
233\brief Extended Kalman Filter in full matrices
234
235An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
236*/
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 );
291//                      pfxu = UI::build<diffbifn>(set, "IM", UI::compulsory);
292//                      phxu = UI::build<diffbifn>(set, "OM", UI::compulsory);
293//
294//                      mat R0;
295//                      UI::get(R0, set, "R",UI::compulsory);
296//                      mat Q0;
297//                      UI::get(Q0, set, "Q",UI::compulsory);
298//
299//
300//                      mat P0; vec mu0;
301//                      UI::get(mu0, set, "mu0", UI::optional);
302//                      UI::get(P0, set,  "P0", UI::optional);
303//                      set_statistics(mu0,P0);
304//                      // Initial values
305//                      UI::get (yrv, set, "yrv", UI::optional);
306//                      UI::get (urv, set, "urv", UI::optional);
307//                      set_drv(concat(yrv,urv));
308//
309//                      // setup StateSpace
310//                      pfxu->dfdu_cond(mu0, zeros(pfxu->_dimu()), A,true);
311//                      phxu->dfdu_cond(mu0, zeros(pfxu->_dimu()), C,true);
312//
313                validate();
314        }
315        void validate() {
316                // check stats and IM and OM
317        }
318};
319UIREGISTER ( EKFfull );
320
321
322/*!
323\brief Extended Kalman Filter in Square root
324
325An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
326*/
327
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 );
358
359
360//////// INstance
361
362/*! \brief (Switching) Multiple Model
363The model runs several models in parallel and evaluates thier weights (fittness).
364
365The statistics of the resulting density are merged using (geometric?) combination.
366
367The next step is performed with the new statistics for all models.
368*/
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
433/*!
434The model is constructed as:
435\f[ x_{t+1} = Ax_t + B u_t + R^{1/2} e_t, y_t=Cx_t+Du_t + R^{1/2}w_t, \f]
436For example, for:
437Using Frobenius form, see [].
438
439For easier use in the future, indeces theta_in_A and theta_in_C are set. TODO - explain
440*/
441//template<class sq_T>
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        }
520};
521/*!
522State-Space representation of multivariate autoregressive model.
523The original model:
524\f[ y_t = \theta [\ldots y_{t-k}, \ldots u_{t-l}, \ldots z_{t-m}]' + \Sigma^{-1/2} e_t \f]
525where \f$ k,l,m \f$ are maximum delayes of corresponding variables in the regressor.
526
527The transformed state is:
528\f[ x_t = [y_{t} \ldots y_{t-k-1}, u_{t} \ldots u_{t-l-1}, z_{t} \ldots z_{t-m-1}]\f]
529
530The state accumulates all delayed values starting from time \f$ t \f$ .
531
532
533*/
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
585                        }
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 );
637                        }
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        }
650};
651
652/////////// INSTANTIATION
653
654template<class sq_T>
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 ) {
656
657        A = A0;
658        B = B0;
659        C = C0;
660        D = D0;
661        R = R0;
662        Q = Q0;
663        validate();
664}
665
666template<class sq_T>
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" );
674}
675
676}
677#endif // KF_H
678
Note: See TracBrowser for help on using the browser.