Legend:
- Unmodified
- Added
- Removed
-
doprava/k1.cpp
r278 r283 12 12 13 13 14 #include <stat/libFN.h>15 14 #include <estim/ekf_templ.h> 16 #include <stat/loggers.h> 15 #include <stat/loggers_ui.h> 16 #include <stat/libDS_ui.h> 17 17 18 18 //include dopravni model … … 21 21 using namespace bdm; 22 22 23 int main() { 24 // Pocet dat 25 int Ndat = 900; 26 27 // Objekt pro ukladani vysledku 28 memlog L(Ndat); 29 23 int main( int argc, char* argv[] ) { 24 /* const char *fname; 25 if ( argc>1 ) {fname = argv[1]; } 26 else { fname = "k1.cfg"; } 27 UIFile F ( fname ); //protected by exceptions, should complain if not found*/ 28 29 FileDS DS("data.it","Data"); // Data Source 30 int ndat = DS.ndat(); // number of data 31 memlog L(ndat); // Logger 32 string resfile="k1_results.it"; // name of output file 33 vec dQ="0.1 0.1"; 34 vec dR="0.2 0.2"; // TODO: read from config file <== broken on windows 35 30 36 //model vyvoje stavu 31 37 IMk1 fxu; … … 37 43 38 44 // ESTIMATOR --- EKF 39 vec mu0= "0.0 0.0 0.0 0.0";40 45 // 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); 46 EKFCh Efix; //Extended KF s fix. variancemi 47 Efix.set_parameters ( &fxu,&hxu,diag(dQ),diag(dR)); 48 Efix.set_statistics ( zeros(fxu.dimension()), 100*eye ( fxu.dimension() ) ); // nulova 50 49 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 50 // Definovat co se bude logovat 51 Efix.log_add(&L,"E"); 58 52 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)); 53 54 vec dt; 55 for ( int t=1;t<ndat;t++ ) { 56 DS.getdata(dt); // dt is allocated 57 Efix.bayes(dt); //ESTIMATE 77 58 78 59 //LOG results 79 L.logit(L_mean, Efix.posterior().mean() ); 80 L.logit(L_xt, xt ); 81 L.logit(L_ut, ut ); 60 Efix.logit(&L); 82 61 83 62 L.step(); 63 DS.step(); 84 64 } 85 65 L.finalize(); 86 L.itsave( "k1.it");66 L.itsave(resfile.c_str()); 87 67 return 0; 88 68 } -
doprava/model.h
r278 r283 5 5 6 6 using namespace bdm; 7 8 //Tady se naplni "popis" jednotlivych nahodnych velicin na kterych se pracuje9 // Moznosti je velmi mnoho (viz doc/html/index.html): napriklad10 RV RVstav ( "{stav }", "4"); // Vyrabim stav velikosti 411 RV RVut ( "{ut }", "2"); // Vstup velikosti 212 RV RVpozor ( "{I1 O1 }"); //Vystup je intenzita a obsazenost13 7 14 8 //! Model stredni hodnoty vyvoje stavu pro k1 … … 19 13 public: 20 14 //! Constructor 21 IMk1() :diffbifn ( RVstav.count(), RVstav, RVut ) {};15 IMk1() :diffbifn () {dimy=4; dimx=3; dimu=1;}; 22 16 //! set CONSTANT parameters 23 17 void set_parameters ( double alp10, double alp20) {alp1=alp10; alp2=alp20;} … … 25 19 vec eval ( const vec &x0, const vec &u0 ) { 26 20 // napln stav nulami 27 vec xk=zeros ( RVstav.count());21 vec xk=zeros ( dimy ); 28 22 29 23 xk ( 0 ) = 0.2* x0(1) - 0.1* x0(2)+ u0(0); // vycucane z prstu … … 39 33 40 34 if (full) { // priznak full se nastavi na zacatku => je treba naplnit celou matici 41 A = eye( RVstav.count());35 A = eye(dimy); 42 36 A(0,1) = 0.2; 43 37 A(0,2) = -0.1; … … 54 48 public: 55 49 //! Constructor 56 OMk1() :diffbifn ( RVpozor.count(), RVstav, RVut ) {};50 OMk1() :diffbifn ( ) {dimy=2;dimx=3;dimu=2;}; //<======= TODO 57 51 // Model pozorovani je trivialni jen se zkopiruji stavy 58 52 vec eval(const vec &x0, const vec &u0 ){ 59 vec dt( RVpozor.count());53 vec dt(dimy); 60 54 // Pozoruji pouze prvni dva stavy 61 55 dt(0) = x0(0); … … 68 62 69 63 if (full) { // priznak full se nastavi na zacatku => je treba naplnit celou matici 70 A = zeros( RVpozor.count(),RVstav.count());64 A = zeros(dimy,dimx); 71 65 A(0,0)=1.0; 72 66 A(1,1)=1.0;
