root/applications/pmsm/pmsmDS.h @ 350

/*!
  \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
  -----------------------------------
*/

#include <stat/loggers.h>
#include <estim/libKF.h>
#include "simulator_zdenek/simulator.h"
#include "pmsm.h"

//! Simulator of PMSM machine with predefined profile on omega
class pmsmDS : public DS {

protected:
        //! indeces 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;
        //! options for logging, - log predictions of 'true' voltage
        bool opt_modu;
        //! options for logging, - 
public:
        //! Constructor with fixed sampling period
        pmsmDS ()   {Dt=125; Drv=RV ( "{o_ua o_ub o_ia o_ib t_ua t_ub o_om o_th Mz }" );}
        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 );
        }
        //! parse options: "modelu" => opt_modu=true;
        void set_options ( string &opt ) {
                opt_modu = ( opt.find ( "modelu" ) !=string::npos );
        }
        void getdata ( vec &dt ) {dt.set_subvector(0,vec ( KalmanObs,6 ));dt(6)=x[2];dt(7)=x[3];dt(8)=x[8];}
        void write ( vec &ut ) {}

        void step() {
                static int ind=0;
                static double dW; // increase of W
                static double Ww; // W
                static double Mz; // W
                if ( t>=dt_prof*ind ) {
                        ind++;
                        // check omega profile and set dW
                        if ( ind<profileWw.length() ) {
                                //linear increase
                                if ( profileWw.length() ==1 ) {
                                        Ww=profileWw ( 0 ); dW=0.0;
                                }
                                else {
                                        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_add ( logger &L ) {
                L_x = L.add ( rx, "x" );
                L_oy = L.add ( ry, "o" );
                L_ou = L.add ( ru, "o" );
                L_iu = L.add ( ru, "t" );
                // log differences
                if ( opt_modu ) {
                        L_optu = L.add ( ru, "model" );
                }
        }

        void logit ( logger &L ) {
                L.logit ( L_x, vec ( x,4 )      );
                L.logit ( L_oy, vec_2 ( KalmanObs[2],KalmanObs[3] ) );
                L.logit ( L_ou, vec_2 ( KalmanObs[0],KalmanObs[1] ) );
                L.logit ( L_iu, vec_2 ( KalmanObs[4],KalmanObs[5] ) );
                if ( opt_modu ) {
                        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;
                        L.logit ( L_optu , vec_2 ( ua,ub ) );
                }
                
        }

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

//! 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); 
                //assume we know state exactly:
                vec true_state=vec(x,4); // read from pmsm
                E.set_mu(true_state);
                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);
                if (pf) {secder=pf->eval2o(u-umin);}
                                
                umin =u;
                EKFfull::bayes(dt);
                old_true.set_subvector(0,true_state);
        }
        
        void log_add(logger &L, const string &name="" ){
                L_CRB=L.add(rx,"crb");
                L_err=L.add(rx,"err");
                L_sec=L.add(rx,"d2");
        }
        void logit(logger &L){
                L.logit(L_err, interr);
                L.logit(L_CRB,diag(_R()));
                L.logit(L_sec,secder);
        }
};

//! 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];
                        
                        E.set_mu(true_state);
                        mu = true_state;
                        //hack for ut
                        EKFfull::bayes(dt);
                }
        
                void log_add(logger &L, const string &name="" ){
                        L_CRB=L.add(concat(rx,RV("Mz",1,0)),"crbz");
                }
                void logit(logger &L){
                        L.logit(L_CRB,diag(_R()));
                }
};