root/tests/testKF_QR.cpp @ 290

#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);
        
        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");
        KF_QR.set_statistics(evolQR._e(), &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;

}