root/applications/pmsm/mpf_load.cpp @ 930

Revision 384, 2.7 kB (checked in by mido, 15 years ago)

possibly broken?

  • Property svn:eol-style set to native
Line 
1/*!
2  \file
3  \brief Simulation of disturbances in PMSM model, EKF runs with simulated covariances
4  \author Vaclav Smidl.
5
6  \ingroup PMSM
7  -----------------------------------
8  BDM++ - C++ library for Bayesian Decision Making under Uncertainty
9
10  Using IT++ for numerical operations
11  -----------------------------------
12*/
13
14
15#include <stat/functions.h>
16#include <estim/kalman.h>
17#include <estim/particles.h>
18#include <estim/ekf_template.h>
19#include <math/chmat.h>
20
21#include "pmsm.h"
22#include "simulator.h"
23#include "sim_profiles.h"
24
25#include <stat/loggers.h>
26
27using namespace bdm;
28
29class IMpmsm_load :  public IMpmsm {
30        public:
31                IMpmsm_load() :IMpmsm() {};
32                void condition ( const vec &val ) {Mz = val(0);}
33};
34
35int main() {
36        // Kalman filter
37        int Ndat = 90000;
38        double h = 1e-6;
39        int Nsimstep = 125;
40        int Npart = 200;
41
42        dirfilelog L("exp/mpf_load",1000);
43       
44        // SET SIMULATOR
45        pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
46        double Ww = 0.0;
47        vec dt ( 2 );
48        vec ut ( 2 );
49       
50        IMpmsm_load fxu;
51        IMpmsm fxu0;
52        //                  Rs    Ls        dt           Fmag(Ypm) kp   p    J     Bf(Mz)
53        fxu.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989,   1.5 ,4.0, 0.04, 0.0 );
54        fxu0.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989,   1.5 ,4.0, 0.04, 0.0 );
55        OMpmsm hxu;
56
57        // ESTIMATORS
58        vec mu0= "0.0 0.0 0.0 0.0";
59        vec Qdiag0 ( "62 66 454 0.03" ); //zdenek: 0.01 0.01 0.0001 0.0001
60        vec Qdiag ( "6 6 1 0.003" ); //zdenek: 0.01 0.01 0.0001 0.0001
61        vec Rdiag ( "100 100" ); //var(diff(xth)) = "0.034 0.034"
62        mat Q =diag( Qdiag );
63        mat R =diag ( Rdiag );
64        EKFfull Efix;
65        Efix.set_est ( mu0, 1*eye ( 4 )  );
66        Efix.set_parameters ( &fxu0,&hxu,diag(Qdiag0),R);
67
68        mlnorm<ldmat> evolMz;
69        evolMz.set_parameters(mat("1"),vec("0"),ldmat(1.0*vec("1")));
70        evolMz.condition(" 0.0");
71       
72        EKFCh_cond Ep;
73        Ep.set_est ( mu0, 1*eye ( 4 ) );
74        Ep.set_parameters ( &fxu,&hxu,Q,R);
75       
76        MPF<EKFCh_cond> M ( &evolMz, &evolMz, Npart, Ep  );
77        M.set_est(evolMz._epdf());
78
79        //LOG
80        int X_log = L.add(rx,"X");
81        int E_log = L.add(rx,"EX");
82        int M_log = L.add(concat(RV("Mz",1),rx),"MX");
83        L.init();
84
85        for ( int tK=1;tK<Ndat;tK++ ) {
86                //Number of steps of a simulator for one step of Kalman
87                for ( int ii=0; ii<Nsimstep;ii++ ) {
88                        sim_profile_steps1 ( Ww , true);
89                        pmsmsim_step ( Ww );
90                };
91                // simulation via deterministic model
92                ut ( 0 ) = KalmanObs[4];
93                ut ( 1 ) = KalmanObs[5];
94               
95                dt ( 0 ) = KalmanObs[2];
96                dt ( 1 ) = KalmanObs[3];
97       
98                //ESTIMATE
99                Efix.bayes(concat(dt,ut));
100                //
101                M.bayes(concat(dt,ut));
102               
103                //LOG
104                L.logit(X_log,  vec(x,4)); //vec from C-array
105                L.logit(E_log, Efix.posterior().mean()); 
106                L.logit(M_log, M.posterior().mean()); 
107               
108                L.step();
109        }
110
111        L.finalize();
112        //L.itsave("sim_var.it");       
113       
114
115        return 0;
116}
Note: See TracBrowser for help on using the browser.