root/pmsm/pmsm_sim.cpp @ 266

Revision 262, 3.1 kB (checked in by smidl, 16 years ago)

cleanup of include files

  • 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
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#include "sim_profiles.h"
21
22using namespace bdm;
23//!Extended Kalman filter with unknown \c Q
24class EKF_unQ : public EKFCh , public BMcond {
25public:
26        //! Default constructor
27        EKF_unQ ( RV rx, RV ry,RV ru,RV rQ ) :EKFCh ( rx,ry,ru ),BMcond ( rQ ) {};
28        void condition ( const vec &Q0 ) {
29                Q.setD ( Q0,0 );
30                //from EKF
31                preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );
32        };
33};
34
35
36int main() {
37        // Kalman filter
38        int Ndat = 9000;
39        double h = 1e-6;
40        int Nsimstep = 125;
41        int Npart = 200;
42
43        // internal model
44        IMpmsm fxu;
45        //                  Rs    Ls        dt       Fmag(Ypm)    kp   p    J     Bf(Mz)
46        fxu.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989, 1.5 ,4.0, 0.04, 0.0 );
47        // observation model
48        OMpmsm hxu;
49
50        vec mu0= "0.0 0.0 0.0 0.0";
51        vec Qdiag ( "0.001 0.001 1e-6 1e-10" ); //zdenek: 0.01 0.01 0.0001 0.0001
52        vec Rdiag ( "1e-10 1e-10" ); //var(diff(xth)) = "0.034 0.034"
53        chmat Q ( Qdiag );
54        chmat R ( Rdiag );
55        EKFCh KFE ( rx,ry,ru );
56        KFE.set_parameters ( &fxu,&hxu,Q,R );
57        KFE.set_est ( mu0, chmat ( 1*ones ( 4 ) ) );
58
59        RV rQ ( "{Q }","2" );
60        EKF_unQ KFEp ( rx,ry,ru,rQ );
61        KFEp.set_parameters ( &fxu,&hxu,Q,R );
62        KFEp.set_est ( mu0, chmat ( zeros ( 4 ) ) );
63
64        mgamma evolQ ( rQ,rQ );
65        MPF<EKF_unQ> M ( rx,rQ,evolQ,evolQ,Npart,KFEp );
66        // initialize
67        evolQ.set_parameters ( 10.0 ); //sigma = 1/10 mu
68        evolQ.condition ( "0.01 0.01" ); //Zdenek default
69        epdf& pfinit=evolQ._epdf();
70        M.set_est ( pfinit );
71        evolQ.set_parameters ( 10.0 );
72
73        //
74
75        const epdf& KFEep = KFE._epdf();
76        const epdf& Mep = M._epdf();
77
78        mat Xt=zeros ( Ndat ,9 ); //true state from simulator
79        mat Dt=zeros ( Ndat,4+2 ); //observation
80        mat XtE=zeros ( Ndat, 4 );
81        mat XtM=zeros ( Ndat,2+4 ); //Q + x
82
83        // SET SIMULATOR
84        pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
85        double Ww=0.0;
86        static int k_rampa=1;
87        static long k_rampa_tmp=0;
88        vec dt ( 2 );
89        vec ut ( 2 );
90        vec vecW="0.1 0.2 0.4 0.9 0.4 0.2 0.0 -0.4 -0.9 -1.6 -0.4 0.0 0.0";
91
92        for ( int tK=1;tK<Ndat;tK++ ) {
93                //Number of steps of a simulator for one step of Kalman
94                for ( int ii=0; ii<Nsimstep;ii++ ) {
95                        //simulator
96                        sim_profile_vec01t(Ww,vecW);
97                        pmsmsim_step ( Ww );
98                };
99                // collect data
100                ut ( 0 ) = 0.0;//KalmanObs[0];
101                ut ( 1 ) = 0.0;//KalmanObs[1];
102                dt ( 0 ) = KalmanObs[2];
103                dt ( 1 ) = KalmanObs[3];
104
105                //estimator
106                KFE.bayes ( concat ( dt,ut ) );
107                M.bayes ( concat ( dt,ut ) );
108
109                Xt.set_row ( tK,vec ( x,9 ) ); //vec from C-array
110                Dt.set_row ( tK, concat ( dt,ut,vec_1(sqrt(pow(ut(0),2)+pow(ut(1),2))), vec_1(sqrt(pow(dt(0),2)+pow(dt(1),2))) ) );
111                XtE.set_row ( tK,KFEep.mean() );
112                XtM.set_row ( tK,Mep.mean() );
113        }
114
115        it_file fou ( "pmsm_sim.it" );
116
117        fou << Name ( "xth" ) << Xt;
118        fou << Name ( "Dt" ) << Dt;
119        fou << Name ( "xthE" ) << XtE;
120        fou << Name ( "xthM" ) << XtM;
121        //Exit program:
122
123        return 0;
124}
Note: See TracBrowser for help on using the browser.