root/tests/pmsm_sim.cpp @ 62

Revision 54, 4.2 kB (checked in by smidl, 17 years ago)

funkcni verze testu + pmsm

  • 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 "pmsm.h"
19#include "simulator.h"
20
21#include <netcdfcpp.h>
22void write_to_nc ( NcFile &nc, mat &X, std::string Xn, Array<std::string> A ) {
23        char tmpstr[200];
24        int Len = X.rows();
25
26        sprintf ( tmpstr,"%s.length",Xn.c_str() );
27        NcDim* lengt = nc.add_dim ( tmpstr, ( long ) Len );
28        for ( int j=0; j<X.cols(); j++ ) {
29                if ( j<A.length() )
30                        sprintf ( tmpstr,"%s_%s",Xn.c_str(), ( A ( j ) ).c_str() );
31                else
32                        sprintf ( tmpstr,"%s_%d",Xn.c_str(),j );
33                // Create variables and their attributes
34                NcVar* P = nc.add_var ( tmpstr, ncDouble, lengt );
35                const double* Dp = X._data();
36                P->put ( &Dp[j*Len],Len );
37        }
38}
39
40
41using namespace itpp;
42//!Extended Kalman filter with unknown \c Q
43class EKF_unQ : public EKFCh , public BMcond {
44public:
45        //! Default constructor
46        EKF_unQ ( RV rx, RV ry,RV ru,RV rQ ) :EKFCh ( rx,ry,ru ),BMcond ( rQ ) {};
47        void condition ( const vec &Q0 ) {
48                Q.setD ( Q0,0 );
49                //from EKF
50                preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );
51        };
52};
53
54
55int main() {
56        // Kalman filter
57        int Ndat = 30000;
58        double h = 1e-6;
59        int Nsimstep = 125;
60        int Npart = 1000;
61
62        // internal model
63        IMpmsm fxu;
64        //                  Rs    Ls        dt       Fmag(Ypm) kp   p    J     Bf(Mz)
65        fxu.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989,   1.5 ,4.0, 0.04, 0.0 );
66        // observation model
67        OMpmsm hxu;
68
69        vec mu0= "0.0 0.0 0.0 0.0";
70        vec Qdiag ( "0.01 0.01 0.00001 0.00001" ); //zdenek: 0.01 0.01 0.0001 0.0001
71        vec Rdiag ( "0.0005 0.0005" ); //var(diff(xth)) = "0.034 0.034"
72        chmat Q ( Qdiag );
73        chmat R ( Rdiag );
74        EKFCh KFE ( rx,ry,ru );
75        KFE.set_parameters ( &fxu,&hxu,Q,R );
76        KFE.set_est ( mu0, chmat ( 1*ones ( 4 ) ) );
77
78        RV rQ ( "100","{Q}","2","0" );
79        EKF_unQ KFEp ( rx,ry,ru,rQ );
80        KFEp.set_parameters ( &fxu,&hxu,Q,R );
81        KFEp.set_est ( mu0, chmat ( 1*ones ( 4 ) ) );
82
83        mgamma evolQ ( rQ,rQ );
84        MPF<EKF_unQ> M ( rx,rQ,evolQ,evolQ,Npart,KFEp );
85        // initialize
86        evolQ.set_parameters ( 100.0 ); //sigma = 1/10 mu
87        evolQ.condition ( "0.01 0.01" ); //Zdenek default
88        epdf& pfinit=evolQ._epdf();
89        M.set_est ( pfinit );
90        evolQ.set_parameters ( 1000.0 );
91
92        //
93
94        epdf& KFEep = KFE._epdf();
95        epdf& Mep = M._epdf();
96
97        mat Xt=zeros ( Ndat ,9 ); //true state from simulator
98        mat Dt=zeros ( Ndat,4+2 ); //observation
99        mat XtE=zeros ( Ndat, 4 );
100        mat XtM=zeros ( Ndat,6 ); //Q + x
101
102        // SET SIMULATOR
103        pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
104        double Ww=0.0;
105        static int k_rampa=1;
106        static long k_rampa_tmp=0;
107        vec dt ( 2 );
108        vec ut ( 2 );
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                        Ww+=k_rampa*2.*M_PI*2e-4;    //1000Hz/s
115                        if ( Ww>2.*M_PI*150. ) {
116                                Ww=2.*M_PI*150.;
117                                if ( k_rampa_tmp<500000 ) k_rampa_tmp++;
118                                else {k_rampa=-1;k_rampa_tmp=0;}
119                        };
120                        if ( Ww<-2.*M_PI*150. ) Ww=-2.*M_PI*150.; /* */
121
122                        pmsmsim_step ( Ww );
123                };
124                // collect data
125                ut ( 0 ) = KalmanObs[0];
126                ut ( 1 ) = KalmanObs[1];
127                dt ( 0 ) = KalmanObs[2];
128                dt ( 1 ) = KalmanObs[3];
129
130                //estimator
131                KFE.bayes ( concat ( dt,ut ) );
132                M.bayes ( concat ( dt,ut ) );
133
134                Xt.set_row ( tK,vec ( x,9 ) ); //vec from C-array
135                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))) ) );
136                XtE.set_row ( tK,KFEep.mean() );
137                XtM.set_row ( tK,Mep.mean() );
138        }
139
140        it_file fou ( "pmsm_sim.it" );
141
142        fou << Name ( "xth" ) << Xt;
143        fou << Name ( "Dt" ) << Dt;
144        fou << Name ( "xthE" ) << XtE;
145        fou << Name ( "xthM" ) << XtM;
146        //Exit program:
147
148        ////////////////
149        // Just Testing
150        NcFile nc ( "pmsm_sim.nc", NcFile::Replace ); // Create and leave in define mode
151        if ( ! nc.is_valid() ) {        std::cerr << "creation of NCFile failed."<<endl;}
152
153        write_to_nc ( nc,Xt,"X","{isa isb om th }" );
154        write_to_nc ( nc,XtM,"XtM","{q1 q2 isa isb om th }" );
155        write_to_nc ( nc,XtE,"XE","{isa isb om th }" );
156        write_to_nc ( nc,Dt,"Dt","{isa isb ua ub }" );
157        return 0;
158}
Note: See TracBrowser for help on using the browser.