root/applications/pmsm/pmsm.h @ 666

#ifndef PMSM_H
#define PMSM_H

#include <math/functions.h>
#include "base/user_info.h"

/*! \defgroup PMSM 
@{
*/

using namespace bdm;

//TODO hardcoded RVs!!!
RV rx ( "{ia ib om th }");
RV ru ( "{o_ua o_ub }");
RV ry ( "{oia oib }");

// class uipmsm : public uibase{
//      double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
// };

//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
class IMpmsm : public diffbifn {
protected:
        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;

        bool compensate;
public:
        IMpmsm() :diffbifn ( ) {dimy=4; dimx = 4; dimu=2; dimc=6;compensate=true;};
        //! Set mechanical and electrical variables
        virtual void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;}

        void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){
/*              ua=u0[0];
                ub=u0[1];
                return;*/
                double sq3=sqrt ( 3.0 );
                double i1=x0(0);
                double i2=0.5* ( -i1+sq3*x0[1] );
                double i3=0.5* ( -i1-sq3*x0[1] );
                double u1=u0(0);
                double u2=0.5* ( -u1+sq3*u0(1) );
                double u3=0.5* ( -u1-sq3*u0(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;
        }

        vec eval ( const vec &x0, const vec &u0 ) {
                // last state
                const double &iam = x0 ( 0 );
                const double &ibm = x0 ( 1 );
                const double &omm = x0 ( 2 );
                const double &thm = x0 ( 3 );
                double uam;
                double ubm;

                if (compensate){
                        modelpwm(x0,u0,uam,ubm);
                } else {
                        uam = u0(0);
                        ubm = u0(1);
                }
                
                
                vec xk( 4 );
                //ia
                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
                //ib
                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
                //om
                xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
                //th
                xk ( 3 ) = thm + omm*dt; // <0..2pi>
                if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
                if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
                return xk;
        }

        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
                const double &iam = x0 ( 0 );
                const double &ibm = x0 ( 1 );
                const double &omm = x0 ( 2 );
                const double &thm = x0 ( 3 );
                // d ia
                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
                // d ib
                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
                A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
                // d om
                A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
                A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
                A ( 2,2 ) = 1.0;
                A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
                // d th
                A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
        }

        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};

        void from_setting( const Setting &root )
        {       

                const SettingResolver& params_b=root["params"];
                const Setting& params=params_b.result;
                
                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
                         params["kp"], params["p"], params["J"], 0.0);
                int comp;
                if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
        };

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

UIREGISTER ( IMpmsm );

//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
class IMpmsm2o : public IMpmsm {
        protected:
//              double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
                //! store first derivatives for the use in second derivatives
                double dia, dib, dom, dth;
                //! d2t = dt^2/2, cth = cos(th), sth=sin(th)
                double d2t, cth, sth;
                double iam, ibm, omm, thm, uam, ubm;
        public:
                IMpmsm2o() :IMpmsm () {};
        //! Set mechanical and electrical variables
                void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0; d2t=dt*dt/2;}

                vec eval ( const vec &x0, const vec &u0 ) {
                // last state
                        iam = x0 ( 0 );
                        ibm = x0 ( 1 );
                        omm = x0 ( 2 );
                        thm = x0 ( 3 );
                        uam = u0 ( 0 );
                        ubm = u0 ( 1 );

                        cth = cos(thm);
                        sth = sin(thm);
                        
                        dia = (- Rs/Ls*iam +  Ypm/Ls*omm * sth + uam/Ls);
                        dib = (- Rs/Ls*ibm -  Ypm/Ls*omm * cth + ubm/Ls);
                        dom = kp*p*p * Ypm/J *( ibm * cth-iam * sth ) - p/J*Mz;
                        dth = omm;
                                                
                        vec xk=zeros ( 4 );
                        xk ( 0 ) =  iam + dt*dia;// +d2t*d2ia;
                        xk ( 1 ) = ibm + dt*dib;// +d2t*d2ib;
                        xk ( 2 ) = omm +dt*dom;// +d2t*d2om;
                        xk ( 3 ) = thm + dt*dth;// +d2t*dom; // <0..2pi>
                        
                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
                        return xk;
                }

                //! eval 2nd order Taylor expansion, MUST be used only as a follow up AFTER eval()!!
                vec eval2o(const vec &du){
                        double dua = du ( 0 )/dt;
                        double dub = du ( 1 )/dt;
                        
                        vec xth2o(4);
                        xth2o(0) = (- Rs/Ls*dia +  Ypm/Ls*(dom * sth + omm*cth) + dua/Ls);
                        xth2o(1) = (- Rs/Ls*dib -  Ypm/Ls*(dom * cth - omm*sth) + dub/Ls);
                        xth2o(2) = kp*p*p * Ypm/J *( dib * cth-ibm*sth - (dia * sth + iam *cth));
                        xth2o(3) = dom;
                        // multiply by dt^2/2
                        xth2o*=d2t/2;
                        return xth2o;
                }
                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
                         iam = x0 ( 0 );
                         ibm = x0 ( 1 );
                         omm = x0 ( 2 );
                         thm = x0 ( 3 );
                // d ia
                        A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
                        A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
                // d ib
                        A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
                        A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
                // d om
                        A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
                        A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
                        A ( 2,2 ) = 1.0;
                        A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
                // d th
                        A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
                        // FOR d2t*dom!!!!!!!!!
/*                      A ( 3,0 ) = dt* kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
                        A ( 3,1 ) = dt* kp*p*p * Ypm/J*dt* ( cos ( thm ) );
                        A ( 3,2 ) = dt;
                        A ( 3,3 ) = 1.0 + dt* kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );*/
                }

                void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};

};


UIREGISTER ( IMpmsm2o );

//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$, equation for \f$\omega\f$ is omitted.$
class IMpmsmStat : public IMpmsm {
        public:
        IMpmsmStat() :IMpmsm() {};
        //! Set mechanical and electrical variables
        void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;}

        vec eval ( const vec &x0, const vec &u0 ) {
                // last state
                double iam = x0 ( 0 );
                double ibm = x0 ( 1 );
                double omm = x0 ( 2 );
                double thm = x0 ( 3 );
                double uam = u0 ( 0 );
                double ubm = u0 ( 1 );

                vec xk=zeros ( 4 );
                //ia
                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
                //ib
                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
                //om
                xk ( 2 ) = omm - p/J*dt*Mz;// + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) );
                //th
                xk ( 3 ) = rem(thm + omm*dt,2*pi); // <0..2pi>
                return xk;
        }

        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
//              double iam = x0 ( 0 );
//              double ibm = x0 ( 1 );
                double omm = x0 ( 2 );
                double thm = x0 ( 3 );
                // d ia
                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
                // d ib
                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
                A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
                // d om
                A ( 2,0 ) = 0.0;//kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
                A ( 2,1 ) = 0.0;//kp*p*p * Ypm/J*dt* ( cos ( thm ) );
                A ( 2,2 ) = 1.0;
                A ( 2,3 ) = 0.0;//kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
                // d th
                A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
        }

        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};

};

UIREGISTER ( IMpmsmStat );


//! State for PMSM with unknown Mz
class IMpmsmMz: public IMpmsm{
        public:
                IMpmsmMz()  {dimy=5; dimx = 5; dimu=2;};
        //! extend eval by Mz
                vec eval ( const vec &x0, const vec &u0 ) {
                        vec x(4);
                        Mz = x0(4); //last of the state is Mz
                
                //teh first 4 states are same as before (given that Mz is set)
                        x=IMpmsm::eval(x0,u0); // including model of drops!
                        return concat(x,Mz);
                }
                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
                //call initial
                        if (full) A.clear();
                        IMpmsm::dfdx_cond(x0,u0,A,full);
                        A(2,4)=- p/J*dt;
                        A(4,4)=1.0;
                }       
};

UIREGISTER ( IMpmsmMz );

//! State for PMSM with unknown Mz
class IMpmsmStatMz: public IMpmsmStat{
        public:
                IMpmsmStatMz()  {dimy=5; dimx = 5; dimu=2;};
        //! extend eval by Mz
                vec eval ( const vec &x0, const vec &u0 ) {
                        vec x(4);
                        Mz = x0(4); //last of the state is Mz
                
                //teh first 4 states are same as before (given that Mz is set)
                        x=IMpmsmStat::eval(x0,u0); // including model of drops!
                        return concat(x,Mz);
                }
                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
                //call initial
                        if (full) A.clear();
                        IMpmsmStat::dfdx_cond(x0,u0,A,full);
                        A(2,4)=- p/J*dt;
                        A(4,4)=1.0;
                }       
};

UIREGISTER ( IMpmsmStatMz );


//! Observation model for PMSM drive and its derivative with respect to \f$x\f$
class OMpmsm: public diffbifn {
public:
        OMpmsm() :diffbifn () {dimy=2;dimx=4;dimu=0;};

        vec eval ( const vec &x0, const vec &u0 ) {
                vec y ( 2 );
                y ( 0 ) = x0 ( 0 );
                y ( 1 ) = x0 ( 1 );
                return y;
        }

        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
                A.clear();
                A ( 0,0 ) = 1.0;
                A ( 1,1 ) = 1.0;
        }
};

UIREGISTER ( OMpmsm );

//! Observation model for PMSM drive and its derivative with respect to \f$x\f$ for full vector of observations
class OMpmsm4: public diffbifn {
        public:
                OMpmsm4() :diffbifn () {dimy=4;dimx=4;dimu=2;};

                vec eval ( const vec &x0, const vec &u0 ) {
                        vec y ( 4 );
                        y  = x0 ;
                        return y;
                }

                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
                        if (full) A=eye(4);
                }
};

UIREGISTER ( OMpmsm4 );





/*!@}*/
#endif //PMSM_H