root/pmsm/sim.cpp @ 217

/*
  \file
  \brief Models for synchronous electric drive using IT++ and BDM
  \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;
        int Npart = 50;
        
        dirfilelog L("exp/pmsm_sim2",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("{_ }","4"),"R");
//      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., 0.0*3e-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 R(eye(4),0.001*ones(4));
        mat Ch=zeros(4,4);
        fsqmat eCh(4);
        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_2slowrevs(Ww);
                        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,u2); 
                xp3=fxu.eval(xm,u2); 

//              KFE.bayes(concat(dt,u));
                // This is simulator prediction
                xt=vec(x,4); //vec from C-array
                //Covariance  
                R*=0.7;
                R.opupdt(xt-xp2,1.0);
                Ch = diag(sqrt(R._D()))*R._L();
                //eCh = KFE._e()->_R();
                eCh = KFE._R();
                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(R_log, diag(Ch.T()*Ch) ); //3.33=1/(1-0.7)
                L.logit(V_log, diag(eCh.to_mat()) ); //3.33=1/(1-0.7)
//              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("xxx.it");
        return 0;
}