root/library/tests/stresssuite/kalman_QR_stress.cpp

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

    Ndat = Dt.cols();
    XQRt = zeros ( 5, Ndat );
    mat Xt = zeros ( 2, Ndat );

//      cout << KF;
    RV rx ( "{x }", "2" );
    RV ru ( "{u }", "1" );
    RV ry ( "{y }", "1" );
    RV rQR ( "{Q,R }", "3" );
    //
    KFcondQR KF;
    // KF with R unknown
    KF.set_parameters ( A, B, C, D, ldmat ( R ), ldmat ( Q ) );
    KF.set_est ( mu0, ldmat ( P0 ) );
    //
    Kalman<ldmat> KFtr;
    // KF with R unknown
    KFtr.set_parameters ( A, B, C, D, ldmat ( R ), ldmat ( Q ) );
    KFtr.set_est ( mu0, ldmat ( P0 ) );

    mgamma evolQR;
    evolQR.set_parameters ( 10.0, "1 1 1" ); //sigma = 1/10 mu

    MPF<KFcondQR> KF_QR;
    KF_QR.set_parameters ( &evolQR, &evolQR, 100 );
    evolQR.condition ( "1 1 1" );
    egamma kfinit = egamma ( "10 10 10", "1 1 1" );
    KF_QR.set_statistics ( egamma ( "10 10 10", "1 1 1" ), &KF );
    const epdf& mpost = KF_QR.posterior();
    const epdf& mposttr = KFtr.posterior();

    XQRt.set_col ( 0, mpost.mean() );
    Xt.set_col ( 0, mposttr.mean() );
    for ( int t = 1; t < Ndat; t++ ) {
        KF_QR.bayes ( Dt.get_col ( t ) );
        KFtr.bayes ( Dt.get_col ( t ) );

        XQRt.set_col ( t, mpost.mean() );
        Xt.set_col ( t, mposttr.mean() );
    }

    it_file fou ( "kalman_stress_QR_res.it" );
    fou << Name ( "xqrth" ) << XQRt;
    fou << Name ( "xth" ) << Xt;
}