root/pmsm/sim_var.cpp @ 99

/*
  \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 <stat/libFN.h>
#include <estim/libKF.h>
#include <estim/libPF.h>

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

#include <stat/loggers.h>

using namespace itpp;
//!Extended Kalman filter with unknown \c Q

void set_simulator_t ( double &Ww ) {

        if ( t>0.0002 ) x[8]=1.2;  // 1A //0.2ZP
        if ( t>0.4 ) x[8]=10.8; // 9A
        if ( t>0.6 ) x[8]=25.2;  // 21A

        if ( t>0.7 ) Ww=2.*M_PI*10.;
        if ( t>1.0 ) x[8]=1.2;  // 1A
        if ( t>1.2 ) x[8]=10.8; // 9A
        if ( t>1.4 ) x[8]=25.2;  // 21A

        if ( t>1.6 ) Ww=2.*M_PI*50.;
        if ( t>1.9 ) x[8]=1.2;  // 1A
        if ( t>2.1 ) x[8]=10.8; // 9A
        if ( t>2.3 ) x[8]=25.2;  // 21A

        if ( t>2.5 ) Ww=2.*M_PI*100;
        if ( t>2.8 ) x[8]=1.2;  // 1A
        if ( t>3.0 ) x[8]=10.8; // 9A
        if ( t>3.2 ) x[8]=25.2;  // 21A

        if ( t>3.4 ) Ww=2.*M_PI*150;
        if ( t>3.7 ) x[8]=1.2;  // 1A
        if ( t>3.9 ) x[8]=10.8; // 9A
        if ( t>4.1 ) x[8]=25.2;  // 21A

        if ( t>4.3 ) Ww=2.*M_PI*0;
        if ( t>4.8 ) x[8]=-1.2;  // 1A
        if ( t>5.0 ) x[8]=-10.8; // 9A
        if ( t>5.2 ) x[8]=-25.2;  // 21A

        if ( t>5.4 ) Ww=2.*M_PI* ( -10. );
        if ( t>5.7 ) x[8]=-1.2;  // 1A
        if ( t>5.9 ) x[8]=-10.8; // 9A
        if ( t>6.1 ) x[8]=-25.2;  // 21A

        if ( t>6.3 ) Ww=2.*M_PI* ( -50. );
        if ( t>6.7 ) x[8]=-1.2;  // 1A
        if ( t>6.9 ) x[8]=-10.8; // 9A
        if ( t>7.1 ) x[8]=-25.2;  // 21A

        if ( t>7.3 ) Ww=2.*M_PI* ( -100. );
        if ( t>7.7 ) x[8]=-1.2;  // 1A
        if ( t>7.9 ) x[8]=-10.8; // 9A
        if ( t>8.1 ) x[8]=-25.2;  // 21A
        if ( t>8.3 ) x[8]=10.8; // 9A
        if ( t>8.5 ) x[8]=25.2;  // 21A

        x[8]=0.0;
}

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

        dirfilelog L("exp/sim_var",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 );
        vec xtm=zeros ( 4 );
        vec xdif=zeros ( 4 );
        vec xt ( 4 );
        vec ddif=zeros(2);
        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 );
        OMpmsm hxu;
        mat Qt=zeros ( 4,4 );
        mat Rt=zeros ( 2,2 );

        // ESTIMATORS
        vec mu0= "0.0 0.0 0.0 0.0";
        vec Qdiag ( "62 66 454 0.03" ); //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 ( rx,ry,ru );
        Efix.set_est ( mu0, 1*eye ( 4 )  );
        Efix.set_parameters ( &fxu,&hxu,Q,R);

        EKFfull Eop ( rx,ry,ru );
        Eop.set_est ( mu0, 1*eye ( 4 ) );
        Eop.set_parameters ( &fxu,&hxu,Q,R);

        EKFfull Edi ( rx,ry,ru );
        Edi.set_est ( mu0, 1*eye ( 4 ) );
        Edi.set_parameters ( &fxu,&hxu,Q,R);

        epdf& Efix_ep = Efix._epdf();
        epdf& Eop_ep = Eop._epdf();
        epdf& Edi_ep = Edi._epdf();

        //LOG
        RV rQ("10", "{Q }", "16","0");
        RV rR("11", "{R }", "4","0");
        int X_log = L.add(rx,"X");
        int Efix_log = L.add(rx,"XF");
        int Eop_log = L.add(rx,"XO");
        int Edi_log = L.add(rx,"XD");
        int Q_log = L.add(rQ,"");
        int R_log = L.add(rR,"");
        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++ ) {
                        set_simulator_t ( Ww );
                        pmsmsim_step ( Ww );
                };
                // simulation via deterministic model
                ut ( 0 ) = KalmanObs[0];
                ut ( 1 ) = KalmanObs[1];
                dt ( 0 ) = KalmanObs[2];
                dt ( 1 ) = KalmanObs[3];

                xt = fxu.eval ( xtm,ut );
                //Results:
                // in xt we have simulaton according to the model
                // in x we have "reality"
                xtm ( 0 ) =x[0];xtm ( 1 ) =x[1];xtm ( 2 ) =x[2];xtm ( 3 ) =x[3];
                xdif = xtm-xt;
                if ( xdif ( 3 ) >pi ) xdif ( 3 )-=2*pi;
                if ( xdif ( 3 ) <-pi ) xdif ( 3 ) +=2*pi;
                
                ddif = hxu.eval(xtm,ut) - dt;

                //Rt = 0.9*Rt + xdif^2
                Qt*=0.01;
                Qt += outer_product ( xdif,xdif ); //(x-xt)^2
                Rt*=0.01;
                Rt += outer_product ( ddif,ddif ); //(x-xt)^2

                //ESTIMATE
                Efix.bayes(concat(dt,ut));
                //
                Eop.set_parameters ( &fxu,&hxu,(Qt+1e-16*eye(4)),(Rt+1e-3*eye(2)));
                Eop.bayes(concat(dt,ut));
                //
                Edi.set_parameters ( &fxu,&hxu,(diag(diag(Qt))+1e-16*eye(4)),(diag(diag(Rt))+1e-3*eye(2)));
                Edi.bayes(concat(dt,ut));
                
                //LOG
                L.logit(X_log, vec(x,4)); //vec from C-array
                L.logit(Efix_log, Efix_ep.mean() ); 
                L.logit(Eop_log, Eop_ep.mean() ); 
                L.logit(Edi_log, Edi_ep.mean() ); 
                L.logit(Q_log, vec(Qt._data(),16) ); 
                L.logit(R_log, vec(Rt._data(),4) ); 
                
                L.step(false);
        }

        L.step(true);

        return 0;
}