root/library/tests/stresssuite/kalman_QRexh_stress.cpp @ 1463

#include <estim/kalman.h>
#include <estim/particles.h>
#include "../mat_checks.h"

using namespace bdm;

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

TEST ( kalman_QRexh_stress ) {
    // Klaman filter
    mat A, B, C, D, R, Q, P0;
    vec mu0;
    mat Mu0;// read from matlab
    // input from Matlab
    it_file fin ( "kalman_stress.it" );

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

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

    if ( !xxx ) {
        bdm_error ( "kalman_stress.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;

    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 ( "kalman_stress_QR_exh.it" );

    fou << Name ( "LL" ) << LL;
    fou << Name ( "Q1" ) << vQ1;
    fou << Name ( "Q2" ) << vQ2;
}