root/library/tests/test_kalman_QR.cpp @ 591

#include <estim/kalman.h>
#include <estim/particles.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 ) {
                bdm_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 );

        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 ( "testKF_QR_res.it" );
        fou << Name ( "xqrth" ) << XQRt;
        fou << Name ( "xth" ) << Xt;
        //Exit program:
        return 0;

}