work/git/mixpp/pmsm/pmsm.h

#ifndef PMSM_H
#define PMSM_H

#include <stat/libFN.h>

//TODO hardcoded RVs!!!
RV rx ( "{ia ib om th }");
RV ru ( "{ua ub }");
RV ry ( "{oia oib }");
RV rU = concat(ru, RV("{dua dub }"));

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

vec x2o_dbg(4);

class IMpmsm : public diffbifn {
protected:
        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;

public:
        IMpmsm() :diffbifn (rx.count(), rx, ru ) {};
        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 + 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 ) {
                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 ) = 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" );};

};

class IMpmsm2o : public diffbifn {
        protected:
                double Rs, Ls, dt, Ypm, kp, p,  J, Mz;

        public:
                IMpmsm2o() :diffbifn (rx.count(), rx, rU ) {};
                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 );
                        double dua = u0 ( 2 )/dt;
                        double dub = u0 ( 3 )/dt;

                        double cth = cos(thm);
                        double sth = sin(thm);
                        double d2t = dt*dt/2;
                        
                        double dia = (- Rs/Ls*iam +  Ypm/Ls*omm * sth + uam/Ls);
                        double dib = (- Rs/Ls*ibm -  Ypm/Ls*omm * cth + ubm/Ls);
                        double dom = kp*p*p * Ypm/J *( ibm * cth-iam * sth ) - p/J*Mz;
                        double dth = omm;
                        
                        double d2ia = (- Rs/Ls*dia +  Ypm/Ls*(dom * sth + omm*cth) + dua/Ls);
                        double d2ib = (- Rs/Ls*dib -  Ypm/Ls*(dom * cth - omm*sth) + dub/Ls);
                        double d2om = kp*p*p * Ypm/J *( dib * cth-ibm*sth - (dia * sth + iam *cth));
                        double d2th = dom;
                        
                        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*d2th; // <0..2pi>
                        
                        x2o_dbg(0)=d2t*d2ia;
                        x2o_dbg(1)=d2t*d2ib;
                        x2o_dbg(2)=d2t*d2om;
                        x2o_dbg(3)=d2t*d2th;
                        
                        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 ) {
                        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 ) = 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" );};

};

class IMpmsmStat : public IMpmsm {
        IMpmsmStat() :IMpmsm() {};
        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;// + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
                //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" );};

};

class OMpmsm: public diffbifn {
public:
        OMpmsm() :diffbifn (2, rx,ru ) {};

        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;
        }
};

#endif //PMSM_H

---