root/applications/pmsm/mpf_load.cpp @ 930

/*!
  \file
  \brief Simulation of disturbances in PMSM model, EKF runs with simulated covariances
  \author Vaclav Smidl.

  \ingroup PMSM
  -----------------------------------
  BDM++ - C++ library for Bayesian Decision Making under Uncertainty

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


#include <stat/functions.h>
#include <estim/kalman.h>
#include <estim/particles.h>
#include <estim/ekf_template.h>
#include <math/chmat.h>

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

#include <stat/loggers.h>

using namespace bdm;

class IMpmsm_load :  public IMpmsm {
        public:
                IMpmsm_load() :IMpmsm() {};
                void condition ( const vec &val ) {Mz = val(0);}
};

int main() {
        // Kalman filter
        int Ndat = 90000;
        double h = 1e-6;
        int Nsimstep = 125;
        int Npart = 200;

        dirfilelog L("exp/mpf_load",1000);
        
        // SET SIMULATOR
        pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
        double Ww = 0.0;
        vec dt ( 2 );
        vec ut ( 2 );
        
        IMpmsm_load fxu;
        IMpmsm fxu0;
        //                  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 );
        fxu0.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989,   1.5 ,4.0, 0.04, 0.0 );
        OMpmsm hxu;

        // ESTIMATORS
        vec mu0= "0.0 0.0 0.0 0.0";
        vec Qdiag0 ( "62 66 454 0.03" ); //zdenek: 0.01 0.01 0.0001 0.0001
        vec Qdiag ( "6 6 1 0.003" ); //zdenek: 0.01 0.01 0.0001 0.0001
        vec Rdiag ( "100 100" ); //var(diff(xth)) = "0.034 0.034"
        mat Q =diag( Qdiag );
        mat R =diag ( Rdiag );
        EKFfull Efix;
        Efix.set_est ( mu0, 1*eye ( 4 )  );
        Efix.set_parameters ( &fxu0,&hxu,diag(Qdiag0),R);

        mlnorm<ldmat> evolMz;
        evolMz.set_parameters(mat("1"),vec("0"),ldmat(1.0*vec("1")));
        evolMz.condition(" 0.0");
        
        EKFCh_cond Ep;
        Ep.set_est ( mu0, 1*eye ( 4 ) );
        Ep.set_parameters ( &fxu,&hxu,Q,R);
        
        MPF<EKFCh_cond> M ( &evolMz, &evolMz, Npart, Ep  );
        M.set_est(evolMz._epdf());

        //LOG
        int X_log = L.add(rx,"X");
        int E_log = L.add(rx,"EX");
        int M_log = L.add(concat(RV("Mz",1),rx),"MX");
        L.init();

        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++ ) {
                        sim_profile_steps1 ( Ww , true);
                        pmsmsim_step ( Ww );
                };
                // simulation via deterministic model
                ut ( 0 ) = KalmanObs[4];
                ut ( 1 ) = KalmanObs[5];
                
                dt ( 0 ) = KalmanObs[2];
                dt ( 1 ) = KalmanObs[3];
        
                //ESTIMATE
                Efix.bayes(concat(dt,ut));
                //
                M.bayes(concat(dt,ut));
                
                //LOG
                L.logit(X_log,  vec(x,4)); //vec from C-array
                L.logit(E_log, Efix.posterior().mean()); 
                L.logit(M_log, M.posterior().mean()); 
                
                L.step();
        }

        L.finalize();
        //L.itsave("sim_var.it");       
        

        return 0;
}