root/pmsm/sim.cpp @ 227

/*!
  \file
  \brief Simulation of disturbances in PMSM model, PWM and integration disturbances are distinguished
  \author Vaclav Smidl.

  -----------------------------------
  BDM++ - C++ library for Bayesian Decision Making under Uncertainty

  Using IT++ for numerical operations
  -----------------------------------
*/

#include <itpp/itbase.h>
#include <estim/libKF.h>
#include <estim/libPF.h>
#include <stat/libFN.h>

#include <stat/loggers.h>

#include "pmsm.h"
#include "simulator.h"
#include "sim_profiles.h"

using namespace itpp;
int main() {
        // Kalman filter
        int Ndat = 9000;
        double h = 1e-6;
        int Nsimstep = 125;
        
        dirfilelog L("exp/sim",Ndat);
        
        // internal model
        IMpmsm fxu;
        //                  Rs    Ls        dt           Fmag(Ypm) kp   p    J     Bf(Mz)
        fxu.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989,   1.5 ,4.0, 0.04, 0.0 );
        // observation model
        OMpmsm hxu;

        vec mu0= "0.0 0.0 0.0 0.0";
//      vec Qdiag ( "0.01 0.01 0.01 0.0001" ); //zdenek: 0.01 0.01 0.0001 0.0001
        vec Qdiag ( "0.07 0.056 0.0007 0.0007" ); //zdenek: 0.01 0.01 0.0001 0.0001
        vec Rdiag ( "0.005 0.005" ); //var(diff(xth)) = "0.034 0.034"
        EKFfull KFE ( rx,ry,ru );
        KFE.set_est ( mu0, diag ( vec("1 1 1 3.1415") )  );
        KFE.set_parameters ( &fxu,&hxu,diag(Qdiag),diag(Rdiag));
        
        
        int X_log = L.add(rx,"X");
        int Xp_log = L.add(rx,"Xp");
        int Xp2_log = L.add(rx,"Xp2");
        int E_log = L.add(rx,"E");
        int V_log = L.add(rx,"V");
        int U_log = L.add(ru,"U");
        int U2_log = L.add(ru,"U2");
        int R_log = L.add(RV("{_ }","16"),"R");
        int Ww_log = L.add(RV("{_ }","1"),"W");
        int R2_log = L.add(RV("{_ }","16"),"R2");
//      int O_log = L.add(RV("{_ }","16"),"O");
        L.init();

        // SET SIMULATOR
        //pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
        pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 6e-6, h );
        double Ww=0.0;
        vec dt ( 2 );

        vec xm = zeros(4);
        vec xt = zeros(4);
        vec xp = zeros(4);
        vec xp2 = zeros(4);
        vec xp3 = zeros(4);
        vec u=zeros(2);
        vec u2=zeros(2);
        ldmat R0(eye(4),1e-6*ones(4));
        ldmat R(R0);
        ldmat R2(R0);
        
        double frg=0.9;
//      vec vecW="0. 0. 0.2 0.4 0.4 0.2 0.0 -0.4 -0.6 -0.6 -0.4 0.0 0.0";
        vec vecW="1 2 4 9 4 2 0 -4 -9 -16 -4 0 0 0";
        vecW*=10.0;

        for ( int tK=1;tK<Ndat;tK++ ) {
                //Number of steps of a simulator for one step of Kalman
                for ( int ii=0; ii<Nsimstep;ii++ ) {
                        //simulator
                        //sim_profile_steps1(Ww);
                        sim_profile_vec01t(Ww,vecW);
                        pmsmsim_step ( Ww );
                };
                
                u(0)= KalmanObs[0];
                u(1)= KalmanObs[1];
                dt(0)= KalmanObs[2];
                dt(1)= KalmanObs[3];
                u2(0) = KalmanObs[4];
                u2(1) = KalmanObs[5];
                // Try what our model would predict!
                xp=fxu.eval(xm,u); 
                xp2=fxu.eval(xm,zeros(2)); //ZERO input!!!!!!!!
//              xp3=fxu.eval(xm,u2); 

//              KFE.bayes(concat(dt,u));
                // This is simulator prediction
                xt=vec(x,4); //vec from C-array
                //Covariance  
                R*=frg;
                R.add(R0,1-frg);
                R.opupdt(xt-xp,1.0);
                R2*=frg;
                R2.add(R0,1-frg);
                R2.opupdt(xt-xp2,1.0);
                xm = xt;
                L.logit(X_log, xt       ); 
                L.logit(Xp_log, xp      ); 
                L.logit(Xp2_log, xp2    ); 
                L.logit(U_log, u        ); 
                L.logit(U2_log, u2      ); 
                L.logit(Ww_log, vec_1(Ww)); 
                L.logit(R_log, vec(R.to_mat()._data(), 16 )); //3.33=1/(1-0.7)
                L.logit(R2_log, vec(R2.to_mat()._data(), 16 )); 
//              L.logit(E_log, KFE._e()->mean() ); 
//              L.logit(O_log, vec(iCh._data(),16)); //3.33=1/(1-0.7)
//              L.logit(Efix_log, KFEep.mean() ); 
//              L.logit(M_log,  Mep.mean() ); 
                
                L.step();
        }

        L.finalize(); 
        L.itsave("sim.it");
        return 0;
}