root/libKF.cpp @ 7

#include <itpp/itbase.h>
#include "libBM.h"
#include "libKF.h"

using namespace itpp;

using std::endl;

KalmanFull::KalmanFull( mat A0, mat B0, mat C0, mat D0, mat R0, mat Q0, mat P0, vec mu0 ) {
        dimx = A0.rows();
        dimu = B0.cols();
        dimy = C0.rows();

        it_assert_debug( A0.cols()==dimx, "KalmanFull: A is not square" );
        it_assert_debug( B0.rows()==dimx, "KalmanFull: B is not compatible" );
        it_assert_debug( C0.cols()==dimx, "KalmanFull: C is not square" );
        it_assert_debug(( D0.rows()==dimy ) || ( D0.cols()==dimu ),     "KalmanFull: D is not compatible" );
        it_assert_debug(( R0.cols()==dimy ) || ( R0.rows()==dimy ), "KalmanFull: R is not compatible" );
        it_assert_debug(( Q0.cols()==dimx ) || ( Q0.rows()==dimx ), "KalmanFull: Q is not compatible" );

        A = A0;
        B = B0;
        C = C0;
        D = D0;
        R = R0;
        Q = Q0;
        mu = mu0;
        P = P0;
}

void KalmanFull::bayes( const vec &dt, bool evalll ) {
        it_assert_debug( dt.length()==( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );

        vec u = dt.get( dimy,dimy+dimu-1 );
        vec y = dt.get( 0,dimy-1 );
        //Time update
        mu = A*mu + B*u;
        P  = A*P*A.transpose() + Q;

        //Data update
        _Ry = C*P*C.transpose() + R;
        _iRy = inv( _Ry );
        _K = P*C.transpose()*_iRy;
        P -= _K*C*P; // P = P -KCP;
        mu += _K*( y-C*mu-D*u );
};

std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf ) {
        os << "mu:" << kf.mu << endl << "P:" << kf.P <<endl;
}