[254] | 1 | /*! |
---|
| 2 | \file |
---|
| 3 | \brief Models for synchronous electric drive using IT++ and BDM |
---|
| 4 | \author Vaclav Smidl. |
---|
| 5 | |
---|
| 6 | ----------------------------------- |
---|
| 7 | BDM++ - C++ library for Bayesian Decision Making under Uncertainty |
---|
| 8 | |
---|
| 9 | Using IT++ for numerical operations |
---|
| 10 | ----------------------------------- |
---|
| 11 | */ |
---|
| 12 | |
---|
| 13 | #include <itpp/itbase.h> |
---|
| 14 | #include <stat/libFN.h> |
---|
| 15 | #include <estim/ekf_templ.h> |
---|
| 16 | #include <stat/loggers.h> |
---|
| 17 | |
---|
| 18 | //include dopravni model |
---|
| 19 | #include "model.h" |
---|
| 20 | |
---|
| 21 | using namespace bdm; |
---|
| 22 | |
---|
| 23 | int main() { |
---|
| 24 | // Pocet dat |
---|
| 25 | int Ndat = 900; |
---|
| 26 | |
---|
| 27 | // Objekt pro ukladani vysledku |
---|
| 28 | memlog L(Ndat); |
---|
| 29 | |
---|
| 30 | //model vyvoje stavu |
---|
| 31 | IMk1 fxu; |
---|
| 32 | fxu.set_parameters (0.5, 0.5); // pokud nejake budou |
---|
| 33 | |
---|
| 34 | //model pozorovani |
---|
| 35 | OMk1 hxu; |
---|
| 36 | //hxu.set_parameters (); // odkomentovat pokud budou |
---|
| 37 | |
---|
| 38 | // ESTIMATOR --- EKF |
---|
| 39 | vec mu0= "0.0 0.0 0.0 0.0"; |
---|
| 40 | // Priprava covariancnich matic pro EKF |
---|
| 41 | vec Qdiag ( "1.0 10. 10 10" ); |
---|
| 42 | vec Rdiag ( "1 1" ); //var(diff(xth)) = "0.034 0.034" |
---|
| 43 | mat Q =diag( Qdiag ); |
---|
| 44 | mat R =diag ( Rdiag ); |
---|
| 45 | EKFfull Efix ( RVstav,RVpozor,RVut ); |
---|
| 46 | // pocatecni podminky |
---|
| 47 | Efix.set_est ( mu0, 1*eye ( 4 ) ); // nulova |
---|
| 48 | // nastaveni modelu pro EKF |
---|
| 49 | Efix.set_parameters ( &fxu,&hxu,Q,R); |
---|
| 50 | |
---|
| 51 | int L_xt = L.add(RVstav, "xt"); // Tady se rika jak velky vektor (pomoci obj. RV) se bude logovat |
---|
| 52 | // A jak se bude jsmenovat vysledek |
---|
| 53 | int L_ut = L.add(RVut, "ut"); // Tady se rika jak velky vektor (pomoci obj. RV) se bude logovat |
---|
| 54 | // A jak se bude jsmenovat vysledek |
---|
| 55 | int L_mean = L.add(RVstav, "odh_xt"); // Tady se rika jak velky vektor (pomoci obj. RV) se bude logovat |
---|
| 56 | // A jak se bude jsmenovat vysledek |
---|
| 57 | |
---|
| 58 | L.init(); // <<==== allocate memory for results |
---|
| 59 | // Priprava poli pro simulaci |
---|
| 60 | vec ut(RVut.count()); |
---|
| 61 | vec xt(RVstav.count()); |
---|
| 62 | vec dt(RVpozor.count()); |
---|
| 63 | // minuly stav |
---|
| 64 | vec xtm=zeros(RVstav.count()); // nulovy pocatecni stav |
---|
| 65 | for ( int t=1;t<Ndat;t++ ) { |
---|
| 66 | // Nastaveni vstupu |
---|
| 67 | ut(0) = 1+sin((double)t/10); // V ut jsou same jednicky |
---|
| 68 | ut(1) = 1+cos((double)t/10); // V ut jsou same jednicky |
---|
| 69 | |
---|
| 70 | // Generovani DAT modelem |
---|
| 71 | xt = fxu.eval(xtm,ut); |
---|
| 72 | dt = hxu.eval(xt,ut); |
---|
| 73 | xtm = xt; //save xt for the next step |
---|
| 74 | |
---|
| 75 | //ESTIMATE |
---|
| 76 | Efix.bayes(concat(dt,ut)); |
---|
| 77 | |
---|
| 78 | //LOG results |
---|
| 79 | L.logit(L_mean, Efix._epdf().mean() ); |
---|
| 80 | L.logit(L_xt, xt ); |
---|
| 81 | L.logit(L_ut, ut ); |
---|
| 82 | |
---|
| 83 | L.step(); |
---|
| 84 | } |
---|
| 85 | L.finalize(); |
---|
| 86 | L.itsave("k1.it"); |
---|
| 87 | return 0; |
---|
| 88 | } |
---|