root/pmsm/pmsm_sim2.cpp @ 147

Revision 117, 3.6 kB (checked in by smidl, 16 years ago)

uklid: vse prevedeno na loggery, a sim3 se nepreklada

  • Property svn:eol-style set to native
Line 
1/*
2  \file
3  \brief Models for synchronous electric drive using IT++ and BDM
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#include <itpp/itbase.h>
14#include <estim/libKF.h>
15#include <estim/libPF.h>
16#include <stat/libFN.h>
17
18#include <stat/loggers.h>
19
20#include "pmsm.h"
21#include "simulator.h"
22#include "sim_profiles.h"
23
24using namespace itpp;
25//!Extended Kalman filter with unknown \c Q
26class EKF_unQ : public EKFCh , public BMcond {
27public:
28        //! Default constructor
29        EKF_unQ ( RV rx, RV ry,RV ru,RV rQ ) :EKFCh ( rx,ry,ru ),BMcond ( rQ ) {};
30        void condition ( const vec &Q0 ) {
31                Q.setD ( Q0,0 );
32                //from EKF
33                preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );
34        };
35        void bayes(const vec dt){
36        EKFCh::bayes(dt);
37               
38                vec xtrue(4);
39                //UGLY HACK!!! reliance on a predictor!!
40                xtrue(0)=x[0];
41                xtrue(1)=x[1];
42                xtrue(2)=x[2];
43                xtrue(3)=x[3];
44               
45                ll = -0.5* ( 4 * 1.83787706640935 +_P.logdet() +_P.invqform(xtrue));
46        }
47};
48class EKF_unQful : public EKFfull , public BMcond {
49public:
50        //! Default constructor
51        EKF_unQful ( RV rx, RV ry,RV ru,RV rQ ) :EKFfull ( rx,ry,ru ),BMcond ( rQ ) {};
52        void condition ( const vec &Q0 ) {
53                Q=diag(Q0);
54        };
55        void bayes(const vec dt){
56        EKFfull::bayes(dt);
57               
58                vec xtrue(4);
59                //UGLY HACK!!! reliance on a predictor!!
60                xtrue(0)=x[0];
61                xtrue(1)=x[1];
62                xtrue(2)=x[2];
63                xtrue(3)=x[3];
64               
65                BM::ll = -0.5* ( 4 * 1.83787706640935 +log(det(P)) +xtrue* ( inv(P)*xtrue ) );
66        }
67};
68
69int main() {
70        // Kalman filter
71        int Ndat = 90000;
72        double h = 1e-6;
73        int Nsimstep = 125;
74        int Npart = 50;
75       
76        dirfilelog L("exp/pmsm_sim2",1000);
77       
78        // internal model
79        IMpmsm fxu;
80        //                  Rs    Ls        dt           Fmag(Ypm) kp   p    J     Bf(Mz)
81        fxu.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989,   1.5 ,4.0, 0.04, 0.0 );
82        // observation model
83        OMpmsm hxu;
84
85        vec mu0= "0.0 0.0 0.0 0.0";
86//      vec Qdiag ( "0.01 0.01 0.01 0.0001" ); //zdenek: 0.01 0.01 0.0001 0.0001
87        vec Qdiag ( "10 10 10 0.001" ); //zdenek: 0.01 0.01 0.0001 0.0001
88        vec Rdiag ( "100 100" ); //var(diff(xth)) = "0.034 0.034"
89        chmat Q ( Qdiag );
90        chmat R ( Rdiag );
91        EKFCh KFE ( rx,ry,ru );
92        KFE.set_est ( mu0, chmat( 1*eye ( 4 ) ) );
93        KFE.set_parameters ( &fxu,&hxu,Q,R);
94
95        RV rQ ( "100","{Q}","4","0" );
96        EKF_unQful KFEp ( rx,ry,ru,rQ );
97        KFEp.set_est ( mu0,  1*ones ( 4 ) );
98        KFEp.set_parameters ( &fxu,&hxu,diag(Qdiag),diag(Rdiag) );
99
100        mgamma_fix evolQ ( rQ,rQ );
101        MPF<EKF_unQful> M ( rx,rQ,evolQ,evolQ,Npart,KFEp );
102        // initialize
103        evolQ.set_parameters ( 1000.0 ,Qdiag, 0.5); //sigma = 1/10 mu
104        evolQ.condition ( Qdiag ); //Zdenek default
105        epdf& pfinit=evolQ._epdf();
106        M.set_est ( pfinit );
107        evolQ.set_parameters ( 100000.0, Qdiag, 0.9999 );
108        //
109        epdf& KFEep = KFE._epdf();
110        epdf& Mep = M._epdf();
111
112        int X_log = L.add(rx,"X");
113        int Efix_log = L.add(rx,"XF");
114        int M_log = L.add(concat(rQ,rx),"M");
115        L.init();
116
117        // SET SIMULATOR
118        pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
119        double Ww=0.0;
120        vec dt ( 2 );
121        vec ut ( 2 );
122
123        for ( int tK=1;tK<Ndat;tK++ ) {
124                //Number of steps of a simulator for one step of Kalman
125                for ( int ii=0; ii<Nsimstep;ii++ ) {
126                        //simulator
127                        sim_profile_steps1(Ww);
128                        pmsmsim_step ( Ww );
129                };
130                // collect data
131                ut ( 0 ) = KalmanObs[0];
132                ut ( 1 ) = KalmanObs[1];
133                dt ( 0 ) = KalmanObs[2];
134                dt ( 1 ) = KalmanObs[3];
135
136                //estimator
137                KFE.bayes ( concat ( dt,ut ) );
138                M.bayes ( concat ( dt,ut ) );
139               
140                L.logit(X_log,  vec(x,4)); //vec from C-array
141                L.logit(Efix_log, KFEep.mean() ); 
142                L.logit(M_log,  Mep.mean() ); 
143               
144                L.step(false);
145        }
146
147        L.step(true); //final
148       
149        return 0;
150}
Note: See TracBrowser for help on using the browser.