[7] | 1 | #include <itpp/itbase.h> |
---|
| 2 | #include "libBM.h" |
---|
| 3 | #include "libKF.h" |
---|
| 4 | |
---|
| 5 | using namespace itpp; |
---|
| 6 | |
---|
| 7 | using std::endl; |
---|
| 8 | |
---|
| 9 | KalmanFull::KalmanFull( mat A0, mat B0, mat C0, mat D0, mat R0, mat Q0, mat P0, vec mu0 ) { |
---|
| 10 | dimx = A0.rows(); |
---|
| 11 | dimu = B0.cols(); |
---|
| 12 | dimy = C0.rows(); |
---|
| 13 | |
---|
| 14 | it_assert_debug( A0.cols()==dimx, "KalmanFull: A is not square" ); |
---|
| 15 | it_assert_debug( B0.rows()==dimx, "KalmanFull: B is not compatible" ); |
---|
| 16 | it_assert_debug( C0.cols()==dimx, "KalmanFull: C is not square" ); |
---|
| 17 | it_assert_debug(( D0.rows()==dimy ) || ( D0.cols()==dimu ), "KalmanFull: D is not compatible" ); |
---|
| 18 | it_assert_debug(( R0.cols()==dimy ) || ( R0.rows()==dimy ), "KalmanFull: R is not compatible" ); |
---|
| 19 | it_assert_debug(( Q0.cols()==dimx ) || ( Q0.rows()==dimx ), "KalmanFull: Q is not compatible" ); |
---|
| 20 | |
---|
| 21 | A = A0; |
---|
| 22 | B = B0; |
---|
| 23 | C = C0; |
---|
| 24 | D = D0; |
---|
| 25 | R = R0; |
---|
| 26 | Q = Q0; |
---|
| 27 | mu = mu0; |
---|
| 28 | P = P0; |
---|
| 29 | } |
---|
| 30 | |
---|
| 31 | void KalmanFull::bayes( const vec &dt, bool evalll ) { |
---|
| 32 | it_assert_debug( dt.length()==( dimy+dimu ),"KalmanFull::bayes wrong size of dt" ); |
---|
| 33 | |
---|
| 34 | vec u = dt.get( dimy,dimy+dimu-1 ); |
---|
| 35 | vec y = dt.get( 0,dimy-1 ); |
---|
| 36 | //Time update |
---|
| 37 | mu = A*mu + B*u; |
---|
| 38 | P = A*P*A.transpose() + Q; |
---|
| 39 | |
---|
| 40 | //Data update |
---|
| 41 | _Ry = C*P*C.transpose() + R; |
---|
| 42 | _iRy = inv( _Ry ); |
---|
| 43 | _K = P*C.transpose()*_iRy; |
---|
| 44 | P -= _K*C*P; // P = P -KCP; |
---|
| 45 | mu += _K*( y-C*mu-D*u ); |
---|
| 46 | }; |
---|
| 47 | |
---|
| 48 | std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf ) { |
---|
| 49 | os << "mu:" << kf.mu << endl << "P:" << kf.P <<endl; |
---|
| 50 | } |
---|