root/testKF.cpp @ 8

#include <itpp/itbase.h>
#include "libKF.h"
#include "libDC.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( "matlab/testKF.it" );
        mat Dt, Xt,Xt2;
        int Ndat;

        fin >> Name( "d" ) >> 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;
        
        Ndat = Dt.cols();
        Xt=zeros( 2,Ndat );
        Xt2=zeros( 2,Ndat );

        Kalman<ldmat> KF( A,B,C,D,ldmat(R),ldmat(Q),ldmat(P0),mu0 );
//      cout << KF;
        KalmanFull KF2( A,B,C,D,R,Q,P0,mu0 );

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

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

}