root/applications/pmsm/TR2245/unitsteps.cpp

Revision 871, 3.5 kB (checked in by mido, 14 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 Toy Problem of mpf for Covariance Estimation
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 <base/loggers.h>
18
19
20#include "../pmsm.h"
21#include "simulator.h"
22#include "../sim_profiles.h"
23
24using namespace bdm;
25
26int main ( int argc, char* argv[] ) {
27        const char *fname;
28        if ( argc>1 ) {fname = argv[1]; }
29        else { fname = "unitsteps.cfg"; }
30        UIFile F ( fname );
31
32        double h = 1e-6;
33        int Nsimstep = 125;
34
35
36        // Kalman filter
37        int Ndat;
38        int Npart;
39        F.lookupValue ( "ndat", Ndat );
40        F.lookupValue ( "Npart",Npart );
41        shared_ptr<pdf> evolQ = UI::build<pdf>( F, "Qrw" );
42        vec Qdiag;
43        vec Rdiag;
44        UI::get( Qdiag, F, "dQ" ); //( "1e-6 1e-6 0.001 0.0001" ); //zdenek: 0.01 0.01 0.0001 0.0001
45        UI::get( Rdiag, F, "dR" );// ( "1e-8 1e-8" ); //var(diff(xth)) = "0.034 0.034"
46
47        // internal model
48
49        shared_ptr<IMpmsm> fxu= new IMpmsm;
50//                  Rs    Ls        dt       Fmag(Ypm)    kp   p    J     Bf(Mz)
51        fxu->set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989, 1.5 ,4.0, 0.04, 0.0 );
52        // observation model
53        shared_ptr<OMpmsm> hxu=new OMpmsm;
54
55        vec mu0= "0.0 0.0 0.0 0.0";
56        chmat Q ( Qdiag );
57        chmat R ( Rdiag );
58        EKFCh KFE ;
59        KFE.set_parameters ( fxu,hxu,Q,R );
60        KFE.set_statistics ( mu0, chmat ( zeros ( 4 ) ) );
61        KFE.set_rv ( rx );
62        KFE.validate();
63
64        RV rQ ( "{Q }","4" );
65        RV rU ("{u }","2");
66        RV rY ("{y }","2");
67        EKFCh_dQ KFEp ;
68        KFEp.set_parameters ( fxu,hxu,Q,R );
69        KFEp.set_statistics ( mu0, chmat ( zeros ( 4 ) ) );
70        KFEp.set_rv(rx);
71        KFEp.set_yrv(rY); 
72        KFEp.set_rvc(concat(rU, rQ)); 
73        KFEp.validate();
74
75        MPF M;
76        evolQ->set_rv(rQ);
77        M.set_pf ( evolQ,Npart );
78        M._pf().set_statistics(ones(Npart), euni(zeros(4),2*Qdiag));
79        M.set_BM(KFEp);
80        M.set_yrv ( rY   );
81        M.set_rvc ( rU   );
82        M.validate();
83
84        shared_ptr<dirfilelog> L = UI::build<dirfilelog>( F, "logger" );// ( "exp/mpf_test",100 );
85        int l_X = L->add_vector ( rx, "xt" );
86        int l_D = L->add_vector ( concat ( ry,ru ), "" );
87        int l_Q= L->add_vector ( rQ, "" );
88
89        KFE.log_level[ logbounds] = true;
90        KFE.log_register ( *L,"KF" );
91        M.log_level[logbounds] = true;
92        M.log_register ( *L,"M" );
93        L->init();
94
95        // SET SIMULATOR
96        pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
97        vec dt ( 2 );
98        vec ut ( 2 );
99        vec xt ( 4 );
100        vec xtm=zeros ( 4 );
101        double Ww=0.0;
102        vec vecW;
103        UI::get( vecW, F, "profile" );
104
105        for ( int tK=1;tK<Ndat;tK++ ) {
106                //Number of steps of a simulator for one step of Kalman
107                for ( int ii=0; ii<Nsimstep;ii++ ) {
108                        //simulator
109                        sim_profile_vec01t ( Ww,vecW );
110                        pmsmsim_step ( Ww );
111                };
112                ut ( 0 ) = KalmanObs[4];
113                ut ( 1 ) = KalmanObs[5];
114                xt = fxu->eval ( xtm,ut ) + diag ( sqrt ( Qdiag ) ) *randn ( 4 );
115                dt = hxu->eval ( xt,ut ) + diag(sqrt(Rdiag))*randn(2);
116                xtm = xt;
117
118                //Variances
119                if ( tK==1000 )  Qdiag ( 0 ) *=10;
120                if ( tK==2000 ) Qdiag ( 0 ) /=10;
121                if ( tK==3000 )  Qdiag ( 1 ) *=10;
122                if ( tK==4000 ) Qdiag ( 1 ) /=10;
123                if ( tK==5000 )  Qdiag ( 2 ) *=10;
124                if ( tK==6000 ) Qdiag ( 2 ) /=10;
125                if ( tK==7000 )  Qdiag ( 3 ) *=10;
126                if ( tK==8000 ) Qdiag ( 3 ) /=10;
127
128                //estimator
129                KFE.bayes (  dt,ut  );
130                M.bayes ( dt,ut );
131
132                L->log_vector ( l_X,xt );
133                L->log_vector ( l_D,concat ( dt,ut ) );
134                L->log_vector ( l_Q,Qdiag );
135
136                KFE.log_write ( );
137                M.log_write (  );
138                L->step();
139        }
140        L->finalize();
141        //Exit program:
142
143        return 0;
144}
Note: See TracBrowser for help on using the browser.