root/tests/testKF_QRexh.cpp @ 254

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

using namespace bdm;

//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;// read from matlab
        // input from Matlab
        it_file fin ( "testKF.it" );

        mat Dt, XQRt,eR,eQ;
        int Ndat;

        bool xxx= fin.seek ( "d" );

        if ( !xxx ) { it_error ( "testKF.it not found" );}

        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 );

        vec vQ1 = "0.01:0.04:1";
        vec vQ2 = "0.01:0.04:1";

        mat LL = zeros ( vQ1.length(),  vQ2.length() );

        Ndat = Dt.cols();

        RV rx ( "{x}","2" );
        RV ru ( "{u}","1" );
        RV ry ( "{y}","2" );
        //
        //
        Kalman<ldmat> KFtr ( rx,ry,ru );

        for ( int i =0; i<vQ1.length();i++ ) {

                for ( int j = 0; j<vQ2.length();j++ ) {
                        // KF with R unknown
                        mat Qj = Q;
                        Qj ( 0,0 ) = vQ1 ( i );
                        Qj ( 1,1 ) = vQ2 ( j );
                        KFtr.set_parameters ( A,B,C,D,ldmat ( R ),ldmat ( Qj ) );
                        KFtr.set_est ( mu0,ldmat ( P0 ) );

                        for ( int t=1;t<Ndat;t++ ) {
                                KFtr.bayes ( Dt.get_col ( t ) );
                                LL ( i,j ) += KFtr._ll();
                        }
                }
        }

        it_file fou ( "testKF_QR_exh.it" );

        fou << Name ( "LL" ) << LL;
        fou << Name ( "Q1" ) << vQ1;
        fou << Name ( "Q2" ) << vQ2;
        //Exit program:
        return 0;

}