| 225 | |
| 226 | //! Evolution model of \f$ \omega, \vartheta\f$ for a PMSM drive and its derivative with respect to \f$x\f$ |
| 227 | class IMpmsmOTM : public diffbifn { |
| 228 | protected: |
| 229 | double Rs, Ls, dt, Ypm, kp, p, J, Mz; |
| 230 | |
| 231 | bool compensate; |
| 232 | bool cutoff; |
| 233 | public: |
| 234 | IMpmsmOTM() :diffbifn ( ) {dimy=2; dimx = 3; dimu=4; dimc=6;compensate=false;cutoff=true;}; |
| 235 | //! Set mechanical and electrical variables |
| 236 | 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;} |
| 237 | |
| 238 | void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){ |
| 239 | /* ua=u0[0]; |
| 240 | * ub=u0[1]; |
| 241 | * return;*/ |
| 242 | double sq3=sqrt ( 3.0 ); |
| 243 | double i1=x0(0); |
| 244 | double i2=0.5* ( -i1+sq3*x0[1] ); |
| 245 | double i3=0.5* ( -i1-sq3*x0[1] ); |
| 246 | double u1=u0(0); |
| 247 | double u2=0.5* ( -u1+sq3*u0(1) ); |
| 248 | double u3=0.5* ( -u1-sq3*u0(1) ); |
| 249 | |
| 250 | double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1; |
| 251 | double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2; |
| 252 | double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3; |
| 253 | ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0; |
| 254 | ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3; |
| 255 | } |
| 256 | |
| 257 | vec eval ( const vec &x0, const vec &u0 ) { |
| 258 | // last state |
| 259 | const double &omm = x0 ( 0 ); |
| 260 | const double &thm = x0 ( 1 ); |
| 261 | const double &Mm = x0 ( 2 ); |
| 262 | double uam; |
| 263 | double ubm; |
| 264 | |
| 265 | if (compensate){ |
| 266 | modelpwm(x0,u0,uam,ubm); |
| 267 | } else { |
| 268 | uam = u0(0); |
| 269 | ubm = u0(1); |
| 270 | } |
| 271 | |
| 272 | const double &iam = u0 ( 2 ); |
| 273 | const double &ibm = u0 ( 3 ); |
| 274 | |
| 275 | vec xk( 3 ); |
| 276 | //ia |
| 277 | //om |
| 278 | xk ( 0 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mm; |
| 279 | //th |
| 280 | xk ( 1 ) = thm + omm*dt; // <0..2pi> |
| 281 | xk (2) = Mm; |
| 282 | |
| 283 | if (cutoff) { |
| 284 | if ( xk ( 1 ) >pi ) xk ( 1 )-=2*pi; |
| 285 | if ( xk ( 1 ) <-pi ) xk ( 1 ) +=2*pi; |
| 286 | } |
| 287 | return xk; |
| 288 | } |
| 289 | |
| 290 | void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) { |
| 291 | // const double &omm = x0 ( 0 ); |
| 292 | const double &thm = x0 ( 1 ); |
| 293 | // const double &Mm = x0 ( 2 ); |
| 294 | |
| 295 | const double &iam = u0 ( 2 ); |
| 296 | const double &ibm = u0 ( 3 ); |
| 297 | |
| 298 | // d ia |
| 299 | // d om |
| 300 | A ( 0,0 ) = 1.0; |
| 301 | A ( 0,1 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) ); |
| 302 | A ( 0,2 ) = -p/J*dt; |
| 303 | // d th |
| 304 | A ( 1,0 ) = dt; |
| 305 | A ( 1,1 ) = 1.0; |
| 306 | A ( 1,2 ) = 0.0; |
| 307 | // d M |
| 308 | A ( 2,0 ) = 0.0; |
| 309 | A ( 2,1 ) = 0.0; |
| 310 | A ( 2,2 ) = 1.0; |
| 311 | } |
| 312 | |
| 313 | void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );}; |
| 314 | |
| 315 | void from_setting( const Setting &root ) |
| 316 | { |
| 317 | |
| 318 | const SettingResolver& params_b=root["params"]; |
| 319 | const Setting& params=params_b.result; |
| 320 | |
| 321 | set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \ |
| 322 | params["kp"], params["p"], params["J"], 0.0); |
| 323 | int comp=0; |
| 324 | if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);} |
| 325 | int cuto=0; |
| 326 | if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);} |
| 327 | }; |
| 328 | |
| 329 | // TODO dodelat void to_setting( Setting &root ) const; |
| 330 | }; |
| 331 | |
| 332 | UIREGISTER ( IMpmsmOTM ); |
| 876 | //! Observation model for PMSM drive in reduced form coordinates |
| 877 | class OMpmsmOTM: public diffbifn { |
| 878 | public: |
| 879 | double Rs, Ls, dt, Ypm, kp, p, J, Mz; |
| 880 | |
| 881 | 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;} |
| 882 | |
| 883 | OMpmsmOTM() :diffbifn () {dimy=2;dimx=2;dimu=4;}; |
| 884 | |
| 885 | vec eval ( const vec &x0, const vec &u0 ) { |
| 886 | vec y ( 2 ); |
| 887 | const double &omm = x0(0); |
| 888 | const double &thm = x0(1); |
| 889 | |
| 890 | const double &uam = u0 ( 0 ); |
| 891 | const double &ubm = u0 ( 1 ); |
| 892 | const double &iam = u0 ( 2 ); |
| 893 | const double &ibm = u0 ( 3 ); |
| 894 | |
| 895 | y ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls; |
| 896 | //ib |
| 897 | y ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls; |
| 898 | |
| 899 | return y; |
| 900 | } |
| 901 | |
| 902 | void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) { |
| 903 | const double &omm = x0(0); |
| 904 | const double &thm = x0(1); |
| 905 | |
| 906 | A ( 0,0 ) = Ypm/Ls*dt* sin ( thm ); |
| 907 | A ( 0,1 ) = Ypm/Ls*dt*omm * ( cos ( thm ) ); |
| 908 | A( 0,2) = 0.0; |
| 909 | // d ib |
| 910 | A ( 1,0 ) = -Ypm/Ls*dt* cos ( thm ); |
| 911 | A ( 1,1 ) = Ypm/Ls*dt*omm * ( sin ( thm ) ); |
| 912 | A(1,2) = 0.0; |
| 913 | |
| 914 | } |
| 915 | |
| 916 | void from_setting( const Setting &root ) |
| 917 | { |
| 918 | |
| 919 | const SettingResolver& params_b=root["params"]; |
| 920 | const Setting& params=params_b.result; |
| 921 | |
| 922 | set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \ |
| 923 | params["kp"], params["p"], params["J"], 0.0); |
| 924 | }; |
| 925 | |
| 926 | }; |
| 927 | |
| 928 | UIREGISTER ( OMpmsmOTM ); |
| 929 | |
| 930 | |
| 931 | |