root/tests/testKF.cpp @ 25

#include <itpp/itbase.h>
#include <estim/libKF.h>

using namespace itpp;

//These lines are needed for use of cout and endl
using std::cout;
using std::endl;

int main() {


        // Klaman filter
        mat A, B,C,D,R,Q,P0;
        vec mu0;
        mat Mu0;;
        // input from Matlab
        it_file fin( "testKF.it" );

        mat Dt, Xt,Xt2,XtE;
        int Ndat;

        bool xxx= fin.seek( "d" );
        fin >>Dt;
        fin.seek( "A" ); 
        fin >> A;
        fin.seek( "B" ); 
        fin >> B;
        fin.seek( "C" ); 
        fin >> C;
        fin.seek( "D" ); 
        fin >> D;
        fin.seek( "R" ); 
        fin >> R;
        fin.seek( "Q" ); fin >> Q;
        fin.seek( "P0" ); fin >> P0;
        fin.seek( "mu0" ); fin >> Mu0; 
        mu0=Mu0.get_col(0);
        
        Ndat = Dt.cols();
        Xt=zeros( 2,Ndat );
        Xt2=zeros( 2,Ndat );
        XtE=zeros( 2,Ndat );

        Kalman<fsqmat> KF( A,B,C,D,fsqmat(R),fsqmat(Q),fsqmat(P0),mu0 );
//      cout << KF;
        KalmanFull KF2( A,B,C,D,R,Q,P0,mu0 );
        RV rx("1","{x}","2","0","0");
        RV ru("2","{u}","1","0","0");
        RV ry("3","{y}","1","0","0");
        bilinfn fxu(rx,ru,A,B);
        bilinfn hxu(rx,ru,C,D);
        EKF<fsqmat> KFE(&fxu,&hxu,Q,R,mu0,P0);

        Xt.set_col( 0,KF.mu );
        Xt2.set_col( 0,KF.mu );
        XtE.set_col( 0,KF.mu );
        for ( int t=1;t<Ndat;t++ ) {
                KF.bayes( Dt.get_col( t ));
                KF2.bayes( Dt.get_col( t ));
                KFE.bayes( Dt.get_col( t ));
                Xt.set_col(t,KF.mu);
                Xt2.set_col(t,KF2.mu);
                XtE.set_col(t,KFE.mu);
//              Kmu = KF.mu;
//              cout <<  "t:" <<t<< "  " << dt<<"  "<<Kmu <<endl;
        }

        it_file fou( "testKF_res.it" );
        fou << Name("xth") << Xt;
        fou << Name("xth2") << Xt2;
        fou << Name("xthE") << XtE;
        //Exit program:
        return 0;

}