root/applications/pmsm/pmsmDS.h @ 1380

/*!
  \file
  \brief DataSource for experiments with realistic simulator of the PMSM model
  \author Vaclav Smidl.

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

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

#ifndef PMSM_DS
#define PMSM_DS

#include <base/loggers.h>
#include <estim/kalman.h>
#include "simulator_zdenek/simulator.h"
#include "pmsm.h"

using namespace bdm;

//! Simulator of PMSM machine with predefined profile on omega
class pmsmDS : public DS
{
        LOG_LEVEL(pmsmDS,logvoltage);
                
protected:
    //! indices of logged variables
    int L_x, L_ou, L_oy, L_iu, L_optu;
    //! Setpoints of omega in timespans given by dt_prof
    vec profileWw;
    //! Setpoints of Mz in timespans given by dt_prof
    vec profileMz;
    //! time-step for profiles
    double dt_prof;
    //! Number of miliseconds per discrete time step
    int Dt;    
public:
        double x[8];
        double KalmanObs[8];
    //! Constructor with fixed sampling period
    pmsmDS () : DS()
    {
        Dt=125;
        Drv=RV ( "{o_ua o_ub o_ia o_ib t_ua t_ub o_om o_th Mz }" );
                dtsize = Drv._dsize();
                pmsmsim_fill_xy(x,KalmanObs);
    }
    void set_parameters ( double Rs0, double Ls0, double Fmag0, double Bf0, double p0, double kp0, double J0, double Uc0, double DT0, double dt0 )
    {
        pmsmsim_set_parameters ( Rs0, Ls0, Fmag0, Bf0, p0, kp0, J0, Uc0, DT0, dt0 );
    }

        void getdata ( vec &dt ) const
    {
        dt.set_subvector(0,vec ( KalmanObs,6 ));
                // add measurement noise
                dt(2) += (0.02*randun()-0.01);
                dt(3) += (0.02*randun()-0.01);
                
                // true values
        dt(6)=x[2];
        dt(7)=x[3];
        dt(8)=x[8];
    }
    void write ( vec &ut ) {}

    void step()
    {
                pmsmsim_fill_xy(x,KalmanObs);
        static int ind=0;
        static double dW; // increase of W
        static double Ww; // W
        static double Mz; // W
        double t; 
                t=pmsmsim_get_t();
        if ( t>=dt_prof*ind )
        {
            ind++;
            // check omega profile and set dW
                        if ( ind <2 && profileWw.length() ==1 )
                        {
                                Ww=profileWw ( 0 );
                                dW=0.0;
                        }
                        if ( ind<profileWw.length() )
            {
                    dW = profileWw ( ind )-profileWw ( ind-1 );
                    dW *=125e-6/dt_prof;
            }
            else
            {
                dW = 0;
            }
            // Check Mz profile and set Mz
            if ( ind<profileMz.length() )
            {
                //sudden increase
                Mz = profileMz(ind);
            }
            else
            {
                Mz = 0;
            }
        }
        Ww += dW;
        //Simulate Dt seconds!
        for ( int i=0; i<Dt; i++ )
        {
            pmsmsim_step ( Ww , Mz);
        }
//              for ( int i=0;i<Dt;i++ ) {      pmsmsim_noreg_step ( Ww , Mz);}

        //discretization
/*        double ustep=1.2;
        KalmanObs [ 0 ] = ustep*itpp::round( KalmanObs [ 0 ]/ ustep) ;
        KalmanObs [ 1 ] = ustep*itpp::round(KalmanObs [ 1 ]/ ustep);*/
//         double istep=0.085;
//         KalmanObs [ 2 ] = istep*itpp::round( KalmanObs [ 2 ]/ istep) ;
//         KalmanObs [ 3 ] = istep*itpp::round(KalmanObs [ 3 ]/ istep);

    };

    void log_register ( logger &L , const string &prefix)
    {
                DS::log_register ( L, prefix );
                
       // log differences
        if ( log_level[logvoltage] )
        {
            L.add_vector ( log_level, logvoltage, RV("{ua, ub }"), "model" );
        }
    }

    void log_write ( logger &L )
    {
        if ( log_level[logvoltage] )
        {
            double sq3=sqrt ( 3.0 );
            double ua,ub;
            double i1=x[0];
            double i2=0.5* ( -i1+sq3*x[1] );
            double i3=0.5* ( -i1-sq3*x[1] );
            double u1=KalmanObs[0];
            double u2=0.5* ( -u1+sq3*KalmanObs[1] );
            double u3=0.5* ( -u1-sq3*KalmanObs[1] );

            double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
            double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
            double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
            ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
            ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
            log_level.store( logvoltage , vec_2 ( ua,ub ) );
        }

    }

    void set_profile ( double dt, const vec &Ww, const vec &Mz )
    {
        dt_prof=dt;
        profileWw=Ww;
        profileMz=Mz;
    }

    void from_setting( const Setting &root )
    {
                const SettingResolver& params_l(root["params"]);
                const Setting &params = params_l.result;
        set_parameters ( params["Rs"], params["Ls"], params["Fmag"], \
                         params["Bf"], params["p"], params["kp"], \
                         params["J"], params["Uc"], params["DT"], 1.0e-6 );

        // Default values of profiles for omega and Mz
        vec profW=vec("1.0");
        vec profM=vec("0.0");
        double tstep=1.0;
        UI::get( tstep, root,  "tstep");
        UI::get( profW, root, "profileW" );
        UI::get( profM, root, "profileM" );
        set_profile (tstep , profW, profM);

    }

    // TODO dodelat void to_setting( Setting &root ) const;
};

UIREGISTER ( pmsmDS );

//! Simulator of PMSM machine with predefined profile on omega
class pmsmDSctrl : public pmsmDS
{
        double Ww;
        double ua;
        double ub;
public:
        //! Constructor with fixed sampling period
    pmsmDSctrl() : pmsmDS()
        {
                Drv.add(RV ( "{Ww }" ));
                Urv=RV ( "{ua ub }" );
                dtsize = Drv._dsize();
                utsize = Urv._dsize();
        }
                
                void getdata ( vec &dt ) const
                {
                        pmsmDS::getdata(dt);
                        dt(9)=Ww;
                }
                void write (const vec &ut ) {
                        ua = ut(0);
                        ub = ut(1);
                }
                
                void step()
                {
                        double t=pmsmsim_get_t();
                        
                        static int ind=0;
                        static double dW; // increase of W
                        static double Mz; // W
                        t+=125e-6;
                        if ( t>=dt_prof*ind )
                        {
                                ind++;
                                // check omega profile and set dW
                                if ( ind <2 && profileWw.length() ==1 )
                                {
                                        Ww=profileWw ( 0 );
                                        dW=0.0;
                                }
                                if ( ind<profileWw.length() )
                                {
                                        dW = profileWw ( ind )-profileWw ( ind-1 );
                                        dW *=125e-6/dt_prof;
                                }
                                else
                                {
                                        dW = 0;
                                }
                                // Check Mz profile and set Mz
                                if ( ind<profileMz.length() )
                                {
                                        //sudden increase
                                        Mz = profileMz(ind);
                                }
                                else
                                {
                                        Mz = 0;
                                }
                        }
                        Ww += dW;
                        //Simulate Dt seconds!
                        for ( int i=0; i<Dt; i++ )
                        {
                                pmsmsim_noreg_step (ua , ub);
                        }
        };
        
        };
        
        UIREGISTER ( pmsmDSctrl );
        

#ifdef XXX
//! This class behaves like BM but it is evaluating EKF
class pmsmCRB : public EKFfull
{
protected:
    vec interr;
    vec old_true;
    vec secder;
    int L_CRB;
    int L_err;
    int L_sec;
public:
    //! constructor
    pmsmCRB():EKFfull()
    {
        old_true=zeros(6);
    }

    void bayes(const vec &dt)
    {
        static vec umin(2);
                vec u(2);
                vec &mu = est._mu();
                //assume we know state exactly:
        vec true_state=0;//TODO vec(x,4); // read from pmsm
        mu=true_state;

        //integration error
        old_true(4)=KalmanObs[4];
        old_true(5)=KalmanObs[5];// add U
        u(0) = KalmanObs[0]; // use the required value for derivatives
        u(1) = KalmanObs[1];
        interr = (true_state - pfxu->eval(old_true));

        //second derivative
        IMpmsm2o* pf = dynamic_cast<IMpmsm2o*>(pfxu.get());
        if (pf)
        {
            secder=pf->eval2o(u-umin);
        }

        umin =u;
        EKFfull::bayes(dt);
        old_true.set_subvector(0,true_state);
    }

    void from_setting( const Setting &root )
    {
        shared_ptr<diffbifn> IM = UI::build<diffbifn>(root, "IM");
        shared_ptr<diffbifn> OM = UI::build<diffbifn>(root, "OM");

        //parameters

        //statistics
        int dim=IM->dimension();

        vec mu0;
        if(root.exists("mu0"))
                        UI::get( mu0, root, "mu0");
                else
            mu0=zeros(dim);

        mat P0;
        if(root.exists( "dP0" ))
                {
                        vec dP0;        
                        UI::get(dP0,root, "dP0");
            P0=diag(dP0);
                }
                else if (root.exists("P0"))
                        UI::get(P0,root, "P0");
                else
            P0=eye(dim);

        set_statistics(mu0,P0);

        vec dQ;
        UI::get( dQ, root, "dQ");
        vec dR;
        UI::get( dR, root, "dR");
        set_parameters(IM, OM, diag(dQ) , diag(dR));

        //connect
        shared_ptr<RV> drv = UI::build<RV>(root, "drv");
        set_yrv(*drv);
        shared_ptr<RV> rv = UI::build<RV>(root, "rv");
        set_rv(*rv);
    }

    // TODO dodelat void to_setting( Setting &root ) const;
};

UIREGISTER ( pmsmCRB );


//! This class behaves like BM but it is evaluating EKF
class pmsmCRBMz : public EKFfull
{
protected:
    int L_CRB;
public:
    //! constructor
    pmsmCRBMz():EKFfull() {}

    void bayes(const vec &dt)
    {
//assume we know state exactly:
        vec true_state(5);
        true_state.set_subvector(0,vec(x,4)); // read from pmsm
        true_state(4)=x[8];

        vec &mu = est._mu();
        mu = true_state;
        //hack for ut
        EKFfull::bayes(dt);
    }


    void from_setting( const Setting &root )
    {
        shared_ptr<diffbifn> IM = UI::build<diffbifn>(root,"IM");
                shared_ptr<diffbifn> OM = UI::build<diffbifn>(root,"OM");

        //statistics
        int dim=IM->dimension();
        vec mu0;
        if( root.exists( "mu0"))
                        UI::get(mu0, root, "mu0");
                else
            mu0=zeros(dim);

        mat P0;

                if(root.exists("dP0"))
                {
                        vec dP0;                                
                        UI::get(dP0, root, "dP0");
                        P0=diag(dP0);
                }
                else if(root.exists("P0"))
                        UI::get( P0, root, "P0" );
                else
                        P0=eye(dim);

        set_statistics(mu0,P0);

        vec dQ;
        UI::get(dQ, root, "dQ");
        vec dR;
        UI::get(dR, root, "dR");
        set_parameters(IM, OM, diag(dQ), diag(dR));

        //connect
                shared_ptr<RV> drv = UI::build<RV>(root, "drv");
        set_yrv(*drv);
                shared_ptr<RV> rv = UI::build<RV>(root, "rv");
        set_rv(*rv);
    }

    // TODO dodelat void to_setting( Setting &root ) const;
};

UIREGISTER ( pmsmCRBMz );
#endif 

#endif