root/applications/pmsm/TR2245/pmsm_wishart.cpp @ 376

Revision 357, 2.7 kB (checked in by mido, 15 years ago)

mnoho zmen:
1) presun FindXXX modulu do \system
2) zalozeni dokumentace \doc\local\library_structure.dox
3) presun obsahu \tests\UI primo do \tests
4) namisto \INSTALL zalozen \install.html, je to vhodnejsi pro uzivatele WINDOWS, a snad i obecne
5) snaha o predelani veskerych UI podle nove koncepce, soubory pmsm_ui.h, arx_ui.h, KF_ui.h, libDS_ui.h, libEF_ui.h a loggers_ui.h ponechavam
jen zdokumentacnich duvodu, nic by na nich jiz nemelo zaviset, a po zkontrolovani spravnosti provedenych uprav by mely byt smazany
6) predelani estimatoru tak, aby fungoval s novym UI konceptem
7) vytazeni tridy bdmroot do samostatneho souboru \bdm\bdmroot.h
8) pridana dokumentace pro zacleneni programu ASTYLE do Visual studia, ASTYLE pridan do instalacniho balicku pro Windows

Line 
1/*!
2  \file
3  \brief TR 2525 file for testing Wishart random walk on PMSM simulator
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
14
15#include <estim/libPF.h>
16#include <estim/ekf_templ.h>
17#include <stat/libFN.h>
18
19#include "user_info.h"
20#include "../pmsmDS.h"
21
22using namespace bdm;
23
24int main ( int argc, char* argv[] ) {
25        const char *fname;
26        if ( argc>1 ) {fname = argv[1]; }
27        else { fname = "pmsm_wishart.cfg"; }
28        UI_File F ( fname );
29
30        double h = 1e-6;
31        int Nsimstep = 125;
32
33        // Kalman filter
34        double k;
35        double l;
36        F.lookupValue ( "k",k);
37        F.lookupValue ( "l",l);
38
39        int Ndat;
40        int Npart;
41        F.lookupValue ( "ndat", Ndat );
42        F.lookupValue ( "Npart", Npart );
43
44        vec Qdiag;
45        vec Rdiag;
46        UI::get( Qdiag, F, "dQ" ); //( "1e-6 1e-6 0.001 0.0001" ); //zdenek: 0.01 0.01 0.0001 0.0001
47        UI::get( Rdiag, F, "dR" );// ( "1e-8 1e-8" ); //var(diff(xth)) = "0.034 0.034"
48               
49        pmsmDS* DS = UI::build<pmsmDS>(F,"system");
50        //      mpdf* evolQ = UI::build<mpdf>( F, "Qrw" );
51
52        // internal model
53        IMpmsm fxu;
54        //                  Rs    Ls        dt       Fmag(Ypm)    kp   p    J     Bf(Mz)
55        fxu.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989, 1.5 ,4.0, 0.04, 0.0 );
56        // observation model
57        OMpmsm hxu;
58
59        vec mu0= "0.0 0.0 0.0 0.0";
60        chmat Q ( Qdiag );
61        chmat R ( Rdiag );
62        EKFCh KFE ;
63        KFE.set_parameters ( &fxu,&hxu,Q,R );
64        KFE.set_est ( mu0, chmat ( 0.01*eye( 4 ) ) );
65        KFE.set_rv ( rx );
66
67        RV rQ ( "{Q }","16" );
68        EKFCh_chQ KFEp ;
69        KFEp.set_parameters ( &fxu,&hxu,Q,R );
70        KFEp.set_est ( mu0, chmat (0.01* eye( 4 ) ) );
71
72        rwiWishartCh* evolQw = new rwiWishartCh; 
73        evolQw->set_parameters(4, k, sqrt(Qdiag),l);
74        MPF<EKFCh_chQ> M;
75        M.set_parameters ( evolQw,evolQw,Npart );
76        // initialize
77        chmat Ch0(diag(Qdiag));
78        evolQw->condition ( vec(Ch0._Ch()._data(),16) ); //Zdenek default
79        M.set_statistics ( evolQw->_e() , &KFEp );
80        //
81
82        M.set_rv ( concat ( rQ,rx ) );
83
84        dirfilelog *L = UI::build<dirfilelog> ( F, "logger" );// ( "exp/mpf_test",100 );
85       
86        KFE.set_options ( "logbounds" );
87        KFE.log_add ( *L,"KF" );
88        M.set_options ( "logbounds" );
89        M.log_add ( *L,"M" );
90        DS->log_add(*L);
91        L->init();
92
93        // SET SIMULATOR
94        pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
95        vec dt ( 2 );
96        vec ut ( 2 );
97        vec xt ( 4 );
98        vec xtm=zeros ( 4 );
99//      double Ww=0.0;
100       
101        vec obs(6);
102        for ( int tK=1;tK<Ndat;tK++ ) {
103                //Number of steps of a simulator for one step of Kalman
104                DS->step();
105                DS->getdata(obs);
106               
107                dt=obs.left(2);
108                ut=obs.right(2);
109                //estimator
110                KFE.bayes ( concat ( dt,ut ) );
111                M.bayes ( concat ( dt,ut ) );
112
113                DS->logit (*L);
114
115                KFE.logit ( *L );
116                M.logit ( *L );
117                L->step();
118        }
119        L->finalize();
120        //Exit program:
121
122        delete L;
123        return 0;
124}
Note: See TracBrowser for help on using the browser.