root/doprava/k1.cpp @ 254

/*!
  \file
  \brief Models for synchronous electric drive using IT++ and BDM
  \author Vaclav Smidl.

  -----------------------------------
  BDM++ - C++ library for Bayesian Decision Making under Uncertainty

  Using IT++ for numerical operations
  -----------------------------------
*/

#include <itpp/itbase.h>
#include <stat/libFN.h>
#include <estim/ekf_templ.h>
#include <stat/loggers.h>

//include dopravni model
#include "model.h"

using namespace bdm;

int main() {
        // Pocet dat
        int Ndat = 900;

        // Objekt pro ukladani vysledku
        memlog L(Ndat);

        //model vyvoje stavu
        IMk1 fxu;
        fxu.set_parameters (0.5, 0.5); // pokud nejake budou

        //model pozorovani
        OMk1 hxu;
        //hxu.set_parameters (); // odkomentovat pokud budou

        // ESTIMATOR --- EKF
        vec mu0= "0.0 0.0 0.0 0.0";
        // Priprava covariancnich matic pro EKF
        vec Qdiag ( "1.0 10. 10 10" ); 
        vec Rdiag ( "1 1" ); //var(diff(xth)) = "0.034 0.034"
        mat Q =diag( Qdiag );
        mat R =diag ( Rdiag );
        EKFfull Efix ( RVstav,RVpozor,RVut );
        // pocatecni podminky
        Efix.set_est ( mu0, 1*eye ( 4 )  ); // nulova 
        // nastaveni modelu pro EKF
        Efix.set_parameters ( &fxu,&hxu,Q,R);

        int L_xt = L.add(RVstav, "xt"); // Tady se rika jak velky vektor (pomoci obj. RV) se bude logovat 
                                                                                // A jak se bude jsmenovat vysledek
        int L_ut = L.add(RVut, "ut"); // Tady se rika jak velky vektor (pomoci obj. RV) se bude logovat 
                                                                                // A jak se bude jsmenovat vysledek
        int L_mean = L.add(RVstav, "odh_xt"); // Tady se rika jak velky vektor (pomoci obj. RV) se bude logovat 
                                                                                // A jak se bude jsmenovat vysledek

        L.init(); // <<==== allocate memory for results
        // Priprava poli pro simulaci
        vec ut(RVut.count()); 
        vec xt(RVstav.count()); 
        vec dt(RVpozor.count()); 
        // minuly stav
        vec xtm=zeros(RVstav.count()); // nulovy pocatecni stav
        for ( int t=1;t<Ndat;t++ ) {
                // Nastaveni vstupu
                ut(0) = 1+sin((double)t/10); // V ut jsou same jednicky
                ut(1) = 1+cos((double)t/10); // V ut jsou same jednicky
                
                // Generovani DAT modelem
                xt = fxu.eval(xtm,ut);
                dt = hxu.eval(xt,ut);
                xtm = xt; //save xt for the next step

                //ESTIMATE
                Efix.bayes(concat(dt,ut));

                //LOG results
                L.logit(L_mean, Efix._epdf().mean() ); 
                L.logit(L_xt, xt ); 
                L.logit(L_ut, ut ); 
                
                L.step();
        }
        L.finalize();
        L.itsave("k1.it");
        return 0;
}