root/pmsm/pmsm_mix.cpp @ 111

/*!
  \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 <stat/emix.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;
}

//!Extended Kalman filter with unknown \c Q
class EKFful_unQR : public EKFfull , public BMcond {
public:
        //! Default constructor
        EKFful_unQR ( RV rx, RV ry,RV ru,RV rQR ) :EKFfull ( rx,ry,ru ),BMcond ( rQR ) {};
        void condition ( const vec &Q0 ) {
                Q=diag(Q0(0,3));
                R=diag(Q0(4,5));
        };
/*      void bayes(const vec dt){
        EKFfull::bayes(dt);
                
                vec xtrue(4);
                //UGLY HACK!!! reliance on a predictor!!
                xtrue(0)=x[0];
                xtrue(1)=x[1];
                xtrue(2)=x[2];
                xtrue(3)=x[3];
                
                BM::ll = -0.5* ( 4 * 1.83787706640935 +log(det(P)) +xtrue* ( inv(P)*xtrue ) );
        }*/
};

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

        dirfilelog L("exp/pmsm_mix",1000);
        //memlog L(Ndat);
        
        // 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);

        RV rQR("10 11", "{Q R }", "4 2 ","0 0");
        EKFful_unQR EKU (rx,ry,ru,rQR);
        EKU.set_est ( mu0,  1*ones ( 4 ) );
        EKU.set_parameters ( &fxu,&hxu,diag(Qdiag),diag(Rdiag) );

        //QU model
        egamma Gcom(rQR);Gcom.set_parameters(ones(6),vec("1 1 1e4 1e10 1 1"));
        cout << Gcom.mean() <<endl;
        cout << Gcom.sample() <<endl;
        euni Ucom(rQR); Ucom.set_parameters(zeros(6),vec("60 60 0.03 1e-8 100 100"));
        cout << Ucom.mean() <<endl;
        cout << Ucom.sample() <<endl;
        Array<epdf*> Coms(2);
        Coms(0) = &Gcom;
        Coms(1) = &Ucom;
        emix Eevol(rQR);        Eevol.set_parameters("0.5 0.5", Coms);
        cout << Eevol.sample() <<endl;

        mmix_triv evolQR(rQR,rQR,&Eevol);
        MPF<EKFful_unQR> M ( rx,rQR, evolQR, evolQR, Npar, EKU );

        epdf& Efix_ep = Efix._epdf();
        epdf& M_ep = M._epdf();
        
        //LOG
        RV rUD("12 13 14 15", "{u_isa u_isb i_isa i_isb }", ones_i(4),zeros_i(4));
        int X_log = L.add(rx,"X");
        int Efix_log = L.add(rx,"XF");
        int M_log = L.add(concat(rx,rQR),"M");
        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];

                //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(Efix_log, Efix_ep.mean() ); 
                L.logit(M_log,  M_ep.mean() ); 
                
                L.step(false);
        }

        L.step(true);
        //L.itsave("sim_var.it");       
        

        return 0;
}