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

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

adaptation of /applications to new version of LOG_LEVEL
also, a cosmetic change made in enumerations: logub -> logubound, loglb -> loglbound

  • Property svn:eol-style set to native
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/particles.h>
16#include <estim/ekf_template.h>
17#include <stat/functions.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        UIFile 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
51        // internal model
52        IMpmsm fxu;
53        //                  Rs    Ls        dt       Fmag(Ypm)    kp   p    J     Bf(Mz)
54        fxu.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989, 1.5 ,4.0, 0.04, 0.0 );
55        // observation model
56        OMpmsm hxu;
57
58        vec mu0= "0.0 0.0 0.0 0.0";
59        chmat Q ( Qdiag );
60        chmat R ( Rdiag );
61        EKFCh KFE ;
62        KFE.set_parameters ( &fxu,&hxu,Q,R );
63        KFE.set_est ( mu0, chmat ( 0.01*eye( 4 ) ) );
64        KFE.set_rv ( rx );
65
66        RV rQ ( "{Q }","16" );
67        EKFCh_chQ KFEp ;
68        KFEp.set_parameters ( &fxu,&hxu,Q,R );
69        KFEp.set_est ( mu0, chmat (0.01* eye( 4 ) ) );
70
71        rwiWishartCh* evolQw = new rwiWishartCh; 
72        evolQw->set_parameters(4, k, sqrt(Qdiag),l);
73        MPF<EKFCh_chQ> M;
74        M.set_parameters ( evolQw,evolQw,Npart );
75        // initialize
76        chmat Ch0(diag(Qdiag));
77        evolQw->condition ( vec(Ch0._Ch()._data(),16) ); //Zdenek default
78        M.set_statistics ( evolQw->_e() , &KFEp );
79        //
80
81        M.set_rv ( concat ( rQ,rx ) );
82
83        dirfilelog *L = UI::build<dirfilelog> ( F, "logger" );// ( "exp/mpf_test",100 );
84       
85        KFE.log_level[logbounds] = true;
86        KFE.log_add ( *L,"KF" );
87        M.log_level[logbounds] = true;
88        M.log_add ( *L,"M" );
89        DS->log_add(*L);
90        L->init();
91
92        // SET SIMULATOR
93        pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
94        vec dt ( 2 );
95        vec ut ( 2 );
96        vec xt ( 4 );
97        vec xtm=zeros ( 4 );
98//      double Ww=0.0;
99       
100        vec obs(6);
101        for ( int tK=1;tK<Ndat;tK++ ) {
102                //Number of steps of a simulator for one step of Kalman
103                DS->step();
104                DS->getdata(obs);
105               
106                dt=obs.left(2);
107                ut=obs.right(2);
108                //estimator
109                KFE.bayes ( concat ( dt,ut ) );
110                M.bayes ( concat ( dt,ut ) );
111
112                DS->logit (*L);
113
114                KFE.logit ( *L );
115                M.logit ( *L );
116                L->step();
117        }
118        L->finalize();
119        //Exit program:
120
121        delete L;
122        return 0;
123}
Note: See TracBrowser for help on using the browser.