root/tests/pmsm_sim.cpp @ 48

Revision 48, 3.6 kB (checked in by smidl, 17 years ago)

meziverze...

  • Property svn:eol-style set to native
RevLine 
[42]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 "pmsm.h"
19#include "simulator.h"
20
21using namespace itpp;
[48]22//!Extended Kalman filter with unknown \c Q
23class EKF_unQ : public EKFCh , public BMcond {
[42]24public:
[48]25        //! Default constructor
26        EKF_unQ ( RV rx, RV ry,RV ru,RV rQ ) :EKFCh ( rx,ry,ru ),BMcond ( rQ ) {};
27        void condition ( const vec &Q0 ) {
28                Q.setD ( Q0,0 );
29                //from EKF
30                preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );
31        }; 
32};
[42]33
34
35int main() {
36        // Kalman filter
37        int Ndat = 10000;
38        double h = 1e-6;
[48]39        int Nsimstep = 125;
40        int Npart = 100;
[42]41       
42        // internal model
43        IMpmsm fxu;
44        //                  Rs    Ls        dt       Fmag(Ypm) kp   p    J     Bf(Mz)
45        fxu.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989,   1.5 ,4.0, 0.04, 0.0 );
46        // observation model
47        OMpmsm hxu;
48
49        vec mu0= "0.0 0.0 0.0 0.0";
[48]50        vec Qdiag ( "0.05 0.05 0.001 0.001" ); //zdenek: 0.05 0.05 0.001 0.001
51        vec Rdiag ( "0.03 0.03" ); //var(diff(xth)) = "0.034 0.034"
[42]52        chmat Q ( Qdiag );
53        chmat R ( Rdiag );
54        EKFCh KFE ( rx,ry,ru );
[48]55        KFE.set_parameters ( &fxu,&hxu,Q,R );
[42]56        KFE.set_est ( mu0, chmat ( 1000*ones ( 4 ) ) );
57
[48]58        RV rQ ( "100","{Q}","4","0" );
59        EKF_unQ KFEp ( rx,ry,ru,rQ );
60        KFEp.set_parameters ( &fxu,&hxu,Q,R );
61        KFEp.set_est ( mu0, chmat ( 1000*ones ( 4 ) ) );
[42]62
[48]63        mgamma evolQ ( rQ,rQ );
64        MPF<EKF_unQ> M ( rx,rQ,evolQ,evolQ,Npart,KFEp );
65        // initialize
66        evolQ.set_parameters ( 100.0 ); //sigma = 1/10 mu
67        evolQ.condition ( "0.05 0.05 0.001 0.001" ); //Zdenek default
68        epdf& pfinit=evolQ._epdf();
69        M.set_est ( pfinit );
70        evolQ.set_parameters ( 1000.0 ); 
[42]71
[48]72        //
[42]73
74        epdf& KFEep = KFE._epdf();
[48]75        epdf& Mep = M._epdf();
[42]76
77        mat Xt=zeros ( 9,Ndat ); //true state from simulator
78        mat Dt=zeros ( 4,Ndat ); //observation
79        mat XtE=zeros ( 4,Ndat );
[48]80        mat XtM=zeros ( 8,Ndat ); //Q + x
[42]81
82        // SET SIMULATOR
83        pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
84        double Ww=0.0;
85        static int k_rampa=1;
86        static long k_rampa_tmp=0;
87        vec dt ( 2 );
[48]88        vec dtVS =zeros( 2 );
89        vec xtVS =zeros(4);
90        vec et ( 4 );
91        vec wt ( 2 );
[42]92        vec ut ( 2 );
[48]93        mat XtV=zeros ( 4,Ndat );
[42]94
95        for ( int tK=1;tK<Ndat;tK++ ) {
96                //Number of steps of a simulator for one step of Kalman
97                for ( int ii=0; ii<Nsimstep;ii++ ) {
98                        //simulator
99                        Ww+=k_rampa*2.*M_PI*2e-4;    //1000Hz/s
100                        if ( Ww>2.*M_PI*150. ) {
101                                Ww=2.*M_PI*150.;
102                                if ( k_rampa_tmp<500000 ) k_rampa_tmp++;
103                                else {k_rampa=-1;k_rampa_tmp=0;}
104                        };
105                        if ( Ww<-2.*M_PI*150. ) Ww=-2.*M_PI*150.; /* */
106
107                        pmsmsim_step ( Ww );
108                };
109                // collect data
110                ut ( 0 ) = KalmanObs[0];
111                ut ( 1 ) = KalmanObs[1];
112                dt ( 0 ) = KalmanObs[2];
113                dt ( 1 ) = KalmanObs[3];
[48]114               
115                // My own simulator for testing : Asuming ut is OK
116                NorRNG.sample_vector ( 4,et );
117                NorRNG.sample_vector ( 2,wt );
118                xtVS = fxu.eval ( xtVS,ut ) + Q.sqrt_mult ( et );
119                dtVS = hxu.eval ( xtVS,ut ) + R.sqrt_mult ( wt );
120
[42]121                //estimator
122                KFE.bayes ( concat ( dt,ut ) );
[48]123                M.bayes ( concat ( dt,ut ) );
[42]124
[48]125                Xt.set_col ( tK,vec ( x,9 ) ); //vec from C-array
[42]126                Dt.set_col ( tK, concat ( dt,ut ) );
127                XtE.set_col ( tK,KFEep.mean() );
[48]128                XtM.set_col ( tK,Mep.mean() );
129                XtV.set_col ( tK,xtVS );
[42]130        }
131
132        it_file fou ( "pmsm_sim.it" );
133
134        fou << Name ( "xth" ) << Xt;
135        fou << Name ( "Dt" ) << Dt;
136        fou << Name ( "xthE" ) << XtE;
[48]137        fou << Name ( "xthM" ) << XtM;
138        fou << Name ( "xthV" ) << XtV;
[42]139        //Exit program:
140        return 0;
141
142}
Note: See TracBrowser for help on using the browser.