root/pmsm/TR2245/wishart.cpp @ 312

Revision 295, 4.0 kB (checked in by smidl, 16 years ago)

new files

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/libPF.h>
16#include <estim/ekf_templ.h>
17#include <stat/libFN.h>
18
19#include <stat/loggers_ui.h>
20#include <stat/libEF_ui.h>
21
22#include "../pmsm.h"
23#include "simulator.h"
24#include "../sim_profiles.h"
25
26using namespace bdm;
27
28int main ( int argc, char* argv[] ) {
29        const char *fname;
30        if ( argc>1 ) {fname = argv[1]; }
31        else { fname = "unitsteps.cfg"; }
32        UIFile F ( fname );
33
34        int Ndat;
35        int Npart;
36        double h = 1e-6;
37        int Nsimstep = 125;
38
39        vec Qdiag;
40        vec Rdiag;
41
42//      mpdf* evolQ ;
43        try {
44                // Kalman filter
45                F.lookupValue ( "ndat", Ndat );
46                F.lookupValue ( "Npart",Npart );
47
48//              UIbuild ( F.lookup ( "Qrw" ),evolQ );
49                Qdiag= getvec ( F.lookup ( "dQ" ) ); //( "1e-6 1e-6 0.001 0.0001" ); //zdenek: 0.01 0.01 0.0001 0.0001
50                Rdiag=getvec ( F.lookup ( "dR" ) );// ( "1e-8 1e-8" ); //var(diff(xth)) = "0.034 0.034"
51        }
52        catch UICATCH;
53// internal model
54
55IMpmsm fxu;
56//                  Rs    Ls        dt       Fmag(Ypm)    kp   p    J     Bf(Mz)
57fxu.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989, 1.5 ,4.0, 0.04, 0.0 );
58        // observation model
59        OMpmsm hxu;
60
61        vec mu0= "0.0 0.0 0.0 0.0";
62        chmat Q ( Qdiag );
63        chmat R ( Rdiag );
64        EKFCh KFE ;
65        KFE.set_parameters ( &fxu,&hxu,Q,R );
66        KFE.set_est ( mu0, chmat ( zeros ( 4 ) ) );
67        KFE.set_rv ( rx );
68
69        RV rQ ( "{Q }","16" );
70        EKFCh_chQ KFEp ;
71        KFEp.set_parameters ( &fxu,&hxu,Q,R );
72        KFEp.set_est ( mu0, chmat ( zeros ( 4 ) ) );
73
74        rwiWishartCh* evolQw = new rwiWishartCh; 
75        evolQw->set_parameters(4, 0.1, sqrt(Qdiag),0.99);
76        MPF<EKFCh_chQ> M;
77        M.set_parameters ( evolQw,evolQw,Npart );
78        // initialize
79        chmat Ch0(diag(Qdiag));
80        evolQw->condition ( vec(Ch0._Ch()._data(),16) ); //Zdenek default
81        M.set_statistics ( evolQw->_e() , &KFEp );
82        //
83
84        M.set_rv ( concat ( rQ,rx ) );
85
86        dirfilelog *L; UIbuild ( F.lookup ( "logger" ), L );// ( "exp/mpf_test",100 );
87        int l_X = L->add ( rx, "xt" );
88        int l_D = L->add ( concat ( ry,ru ), "" );
89        int l_Q= L->add ( rQ, "" );
90        int l_fullQ= L->add ( rQ, "full" );
91
92        KFE.set_options ( "logbounds" );
93        KFE.log_add ( L,"KF" );
94        M.set_options ( "logbounds" );
95        M.log_add ( L,"M" );
96        L->init();
97
98        // SET SIMULATOR
99        pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
100        vec dt ( 2 );
101        vec ut ( 2 );
102        vec xt ( 4 );
103        vec xtm=zeros ( 4 );
104        double Ww=0.0;
105        vec vecW=getvec ( F.lookup ( "profile" ) );
106       
107        mat tQ=diag(Qdiag);
108        mat tChQ=chol(tQ);
109
110        for ( int tK=1;tK<Ndat;tK++ ) {
111                //Number of steps of a simulator for one step of Kalman
112                for ( int ii=0; ii<Nsimstep;ii++ ) {
113                        //simulator
114                        sim_profile_vec01t ( Ww,vecW );
115                        pmsmsim_step ( Ww );
116                };
117                ut ( 0 ) = KalmanObs[4];
118                ut ( 1 ) = KalmanObs[5];
119                xt = fxu.eval ( xtm,ut ) + tChQ.T() *randn ( 4 );
120                dt = hxu.eval ( xt,ut );
121                xtm = xt;
122
123                //Variances
124/*              if ( tK==1000 )  tQ ( 0,0 ) *=10;
125                if ( tK==2000 ) tQ ( 0,0 ) /=10;
126                if ( tK==3000 )  tQ( 1,1 ) *=10;
127                if ( tK==4000 ) tQ( 1,1 ) /=10;
128                if ( tK==5000 )  tQ( 2,2 ) *=10;
129                if ( tK==6000 ) tQ( 2,2 ) /=10;
130                if ( tK==7000 )  tQ( 3,3 ) *=10;
131                if ( tK==8000 ) tQ( 3,3 ) /=10;*/
132               
133                if (tK>1000) {tQ(0,1)=0.5*sqrt(tQ(0,0)*tQ(1,1));tQ(1,0)=tQ(0,1);}
134                if (tK>2000) {tQ(0,1)=0; tQ(1,0)=tQ(0,1);}
135               
136                if (tK>3000) {tQ(2,3)=-0.5*sqrt(tQ(2,2)*tQ(3,3)); tQ(3,2)=tQ(2,3);}
137                if (tK>4000) {tQ(2,3)=0; tQ(3,2)=tQ(2,3);}
138               
139                if (tK>5000) {tQ(0,2)=0.9*sqrt(tQ(0,0)*tQ(2,2)); tQ(2,0)=tQ(0,2);}
140                if (tK>6000) {tQ(0,2)=0; tQ(2,0)=tQ(0,2);}
141
142                tChQ=chol(tQ);
143               
144                //estimator
145                KFE.bayes ( concat ( dt,ut ) );
146                M.bayes ( concat ( dt,ut ) );
147
148                L->logit ( l_X,xt );
149                L->logit ( l_D,concat ( dt,ut ) );
150                mat Q=diag(Qdiag);
151                L->logit ( l_Q,vec(tQ._data(),16) );
152               
153                mat chQ(4,4);
154                copy_vector(16,M._e()->mean()._data(),chQ._data());
155                mat fQ=chQ.T()*chQ;
156                L->logit ( l_fullQ,vec(fQ._data(),16) );
157
158                KFE.logit ( L );
159                M.logit ( L );
160                L->step();
161        }
162        L->finalize();
163        //Exit program:
164
165        delete L;
166        return 0;
167}
Note: See TracBrowser for help on using the browser.