root/pmsm/mpf_u_weight.cpp @ 262

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

cleanup of include files

RevLine 
[228]1/*!
2  \file
3  \brief Voltage U is multiplied by an unknown weight w which is estimated by MPF
4  \author Vaclav Smidl.
5
6  \ingroup PMSM
7
8  -----------------------------------
9  BDM++ - C++ library for Bayesian Decision Making under Uncertainty
10
11  Using IT++ for numerical operations
12  -----------------------------------
13*/
14
15
[262]16
[228]17#include <estim/libKF.h>
18#include <estim/libPF.h>
19#include <stat/libFN.h>
20
21#include "pmsm.h"
22#include "simulator.h"
23#include "sim_profiles.h"
24
[254]25using namespace bdm;
[228]26
27//!Extended Kalman filter with unknown \c Q
28class EKFCh_cond : public EKFCh , public BMcond {
29public:
30        //! Default constructor
31        EKFCh_cond ( RV rx, RV ry,RV ru,RV rC ) :EKFCh ( rx,ry,ru ),BMcond ( rC ) {};
32        void condition ( const vec &val ) {
33                pfxu->condition ( val );
34        };
35};
36
37class IMpmsm_w :  public IMpmsm {
38protected:
39        double w;
40public:
41        IMpmsm_w() :IMpmsm(),w ( 1.0 ) {};
42        //! Set mechanical and electrical variables
43
44        void condition ( const vec &val ) {w = val ( 0 );}
45        vec eval ( const vec &x0, const vec &u0 ) {
46                // last state
47                double iam = x0 ( 0 );
48                double ibm = x0 ( 1 );
49                double omm = x0 ( 2 );
50                double thm = x0 ( 3 );
51                double uam = u0 ( 0 );
52                double ubm = u0 ( 1 );
53
54                vec xk=zeros ( 4 );
55                //ia
56                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + w* uam*dt/Ls;
57                //ib
58                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + w* ubm*dt/Ls;
59                //om
60                xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
61                //th
62                xk ( 3 ) = thm + omm*dt; // <0..2pi>
63                if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
64                if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
65                return xk;
66        }
67
68};
69
70
71int main() {
72        // Kalman filter
73        int Ndat = 9000;
74        double h = 1e-6;
75        int Nsimstep = 125;
76        int Npart = 200;
77
78        mat Rnoise = randn ( 2,Ndat );
79
80        // internal model
81        IMpmsm fxu0;
82        IMpmsm_w fxu;
83        //                  Rs    Ls        dt       Fmag(Ypm)    kp   p    J     Bf(Mz)
84        fxu.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989, 1.5 ,4.0, 0.04, 0.0 );
85        fxu0.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989, 1.5 ,4.0, 0.04, 0.0 );
86        // observation model
87        OMpmsm hxu;
88
89        vec mu0= "0.0 0.0 0.0 0.0";
90        vec Qdiag ( "0.1 0.1 0.001 0.000001" ); //zdenek: 0.01 0.01 0.0001 0.0001
91        vec Rdiag ( "0.1 0.1" ); //var(diff(xth)) = "0.034 0.034"
92        chmat Q ( Qdiag );
93        chmat R ( Rdiag );
94        EKFCh KFE ( rx,ry,ru );
95        KFE.set_parameters ( &fxu0,&hxu,Q,R );
96        KFE.set_est ( mu0, chmat ( ones ( 4 ) ) );
97
98        RV rW ( "{w }" );
99        EKFCh_cond KFEp ( rx,ry,ru,rW );
100        KFEp.set_parameters ( &fxu,&hxu,Q,R );
101        KFEp.set_est ( mu0, chmat ( ones ( 4 ) ) );
102
103        mgamma_fix evolW ( rW,rW );
104        MPF<EKFCh_cond> M ( rx,rW,evolW,evolW,Npart,KFEp );
105        // initialize
106        vec W0="0.5";
107        evolW.set_parameters ( 10.0, W0, 1.0 ); //sigma = 1/10 mu
108        evolW.condition ( W0 );
109        epdf& pfinit=evolW._epdf();
110        M.set_est ( pfinit );
111        evolW.set_parameters ( 100.0, W0, 0.99 ); //sigma = 1/10 mu
112
113        mat Xt=zeros ( Ndat ,4 ); //true state from simulator
114        mat Dt=zeros ( Ndat,2+2 ); //observation
115        mat XtE=zeros ( Ndat, 4 );
116        mat Qtr=zeros ( Ndat, 4 );
117        mat XtM=zeros ( Ndat,1+4 ); //W + x
118        mat XtMTh=zeros ( Ndat,1 ); //W + x
119
120        // SET SIMULATOR
121        pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
122        vec dt ( 2 );
123        vec ut ( 2 );
124        vec xt ( 4 );
125        vec xtm=zeros ( 4 );
126        double Ww=0.0;
127        vec vecW="1 2 4 8 4 2 0 -4 -9 -16 -4 0 0 0";
128        vecW*=10.0;
129
130        for ( int tK=1;tK<Ndat;tK++ ) {
131                //Number of steps of a simulator for one step of Kalman
132                for ( int ii=0; ii<Nsimstep;ii++ ) {
133                        //simulator
134                        sim_profile_vec01t ( Ww,vecW );
135                        pmsmsim_step ( Ww );
136                };
137                ut ( 0 ) = KalmanObs[0];
138                ut ( 1 ) = KalmanObs[1];
139                dt ( 0 ) = KalmanObs[2]+0.3*Rnoise ( 0,t );
140                dt ( 1 ) = KalmanObs[3]+0.3*Rnoise ( 1,t );
141                xt = vec ( x,4 );
142
143                //estimator
144                KFE.bayes ( concat ( dt,ut ) );
145                M.bayes ( concat ( dt,ut ) );
146
147                Xt.set_row ( tK, xt ); //vec from C-array
148                Dt.set_row ( tK, concat ( dt,ut ) );
149                Qtr.set_row ( tK, Qdiag );
150                XtE.set_row ( tK,KFE._e()->mean() );
151                XtM.set_row ( tK,M._e()->mean() );
152                {
153                        double sumSin=0.0;
154                        double sumCos=0.0;
155                        vec mea ( 4 );
156                        vec* _w;
157
158                        for ( int p=0; p<Npart;p++ ) {
159                                mea = M._BM ( p )->_e()->mean();
160                                _w = M.__w();
161                                sumSin += ( *_w ) ( p ) *sin ( mea ( 3 ) );
162                                sumCos += ( *_w ) ( p ) *cos ( mea ( 3 ) );
163                        }
164                        double Th = asin ( sumSin );
165                        if ( sumCos<0 ) {
166                                if ( sumSin>0 ) {
167                                        Th = M_PI-Th;
168                                }
169                                else {
170                                        Th = -M_PI-Th;
171                                }
172                        }
173
174                        XtMTh.set_row ( tK,vec_1 ( Th ) );
175                }
176        }
177
178        it_file fou ( "mpf_u_weight.it" );
179
180        fou << Name ( "xth" ) << Xt;
181        fou << Name ( "Dt" ) << Dt;
182        fou << Name ( "Qtr" ) << Qtr;
183        fou << Name ( "xthE" ) << XtE;
184        fou << Name ( "xthM" ) << XtM;
185        fou << Name ( "xthMTh" ) << XtMTh;
186        //Exit program:
187
188        return 0;
189}
Note: See TracBrowser for help on using the browser.