Стабилизация нелинейной модели маятника с двумя нелинейностями управлениями минимальной интенсивности

(1.0-cos(ga*t2))/(ga*ga));

A[1][N]=-(pi/4.0-pi*cosh(ga*t3_t2)/2.0+sinh(ga*t3_t2)*sin(ga*t2)* x[0]-sinh(ga*t3_t2)*cos(ga*t2)*x[1]/ga-(3.0-pi/2.0*sinh(ga*t3_t2) *sin(ga*t2)/(ga*ga)-(pi/2.0-1.0)*(cosh(ga*t3_t2)-1.0)/(ga*ga));

A[2][N]=-(-pi*cosh(t_t3)/4.0-pi*ga*sinh(t_t3)*sinh(ga*t3_t2)/2.0+ga*sinh(t_t3)*

cosh(ga*t3_t2)*sin(ga*t2)*x[0]-sinh(t_t3)*cosh(ga* t3_t2)*cos(ga*t2)*x[1]-(3.0-pi/2.0)*sinh(t_t3)*cosh(ga*t3_t2)*

sin(ga*t2)/ga-(pi/2.0-1.0)*sinh(t_t3)*sinh(ga*t3_t2)/ga);

A[3][N]=-(-pi*sinh( t_t3)/4.0-pi*ga*cosh(t_t3)*

sinh(ga*t3_t2)/2.0+ga*cosh(t_t3)*cosh(ga*t3_t2)*sin(ga*t2)*x[0]-cosh(t_t3)*cosh(ga*t3_t2)*

cos(ga*t2)*x[1]-(3.0-pi/2.0)*cosh(t_t3)*

cosh(ga *t3_t2)*sin(ga*t2)/ga-(pi/2.0-1.0)*cosh(t_t3)*

sinh(ga*t3_t2)/ga;}

else

if ( StrZ==3 )

{t_t3 = N*h - n3*h;

t3 = n3*h;

for ( j=0; j<n3; j++ )

{t3_tn = n3*h - j*h;

t3_tw = n3*h - (j+1)*h;

A[0][j]=cosh(ga*t3_tw)-cosh(ga*t3_tn);

A[1][j]=ga*sinh(t_t3)*sinh(ga*t3_tw)-sinh(ga*t3_tn));

A[2][j]=ga*cosh(t_t3)*(sinh(ga*t3_tw)-sinh(ga*t3_tn));}

for ( j=n3; j<N; j++ )

{t_tn = N*h - j*h;

t_tw = N*h - (j+1)*h;

A[0][j]=0.0;

A[1][j]=cosh(t_tw)-cosh(t_tn);

A[2][j]=sinh(t_tw)-sinh(t_tn);}

// вектор b

A[0][N]=-(pi/4.0-cosh(ga*t3)*x[0]-sinh(ga*t3)*x[1]/ga- (pi/2.0-1.0)*(cosh(ga*t3)-1.0)/(ga*ga));

A[1][N]=-(-pi*cosh(t_t3)/4.0-ga*sinh(t_t3)*sinh(ga*t3)*x[0]-

sinh(t_t3)*cosh(ga*t3)*x[1]-(pi/2.0-1.0)*sinh(t_t3)*

sinh(ga*t3)/ ga);

A[2][N]=-(-pi*sinh(t_t3)/4.0-ga*cosh(t_t3)*sinh(ga*t3)*x[0]-

cosh(t_t3)*cosh(ga*t3)*x[1]-(pi/2.0-1.0)*cosh(t_t3)*

sinh(ga*t3)/ga ); }

else

if ( StrZ==4 )

{te = N*h;

for ( j=0; j<N; j++ )

{ t_tn = N*h - j*h;

t_tw = N*h - (j+1)*h;

A[0][j]=cosh(t_tw)-cosh(t_tn);

A[1][j]=sinh(t_tw)-sinh(t_tn);}

A[0][N]=-(-cosh(te)*x[0]-sinh(te)*x[1]);

A[1][N]=-(-sinh(te)*x[0]-cosh(te)*x[1]);}

for ( i=0; i<m; i++ )

b[i] = 0.0;

for ( j=0; j<N; j++ )

{ dn[j] = -1.0; dw[j] = 1.0;}

dn[N] = -100000000.0;

dw[N] = 100000000.0;

} // formParam()

void f_x ( double t, // Выч. f1(t) = x2(t)

double y[], // f2(t) = -sin(x1(t))+u(t)

double f[] )

{double ax, bx;

/*

if ( ( y[0] > -pi/4.0 ) && ( y[0] <= pi/4.0 ) )

{ ax = y[0];bx = 1.0; }

else

if ( ( y[0] > pi/4.0 ) && ( y[0] <= pi/2.0 ) )

{ ax = ga*ga * y[0] + pi/2.0 - 1.0; bx = ga*ga;}

Else

if ( ( y[0] > 3.0*pi/4.0 ) && ( y[0] <= 5.0*pi/4.0 ) )

{ ax=-y[0]+pi; bx=-1.0;}

f[0] = y[1];

f[1] = ax - bx * u[0];

*/

f[0] = y[1];

f[1] = sin( y[0] ) - cos( y[0] ) * u[0];

} // f_x()

# define True 1

# define False 0

void FEHL(int neqn, double t,

double y[], void (f_p)(double, double*, double*),

double *hc, double yp[],

double f1[], double f2[],

double f3[], double f4[],

double f5[], double s[])

{ double h;

double ch,rab;

int k;

void (*fun)(double, double*, double*);

h=*hc;

fun = f_p;

ch = h/4.0;

for (k=0; k<neqn; k++)

f5[k] = y[k] + ch*yp[k];

rab = t + ch;

fun(rab,f5,f1);

ch = 3.0 * h / 32.0;

for (k=0; k<neqn; k++)

f5[k] = y[k] + ch*(yp[k] + 3.0 * f1[k]);

rab = t + 3.0 * h / 8.0;

fun(rab,f5,f2);

ch = h / 2197.0;

for (k=0; k<neqn; k++)

f5[k] = y[k] + ch * (1932.0 * yp[k] + (7296.0 * f2[k] - 7200.0 * f1[k]));

rab = t + 12.0 * h / 13.0;

fun(rab,f5,f3);

ch = h / 4104.0;

for (k=0; k<neqn; k++)

f5[k] = y[k] + ch * ((8341.0 * yp[k] - 845.0 * f3[k]) + (29440.0 * f2[k] - 32832.0 * f1[k]));

rab = t + h;

fun(rab,f5,f4);

ch = h / 20520.0;

for (k=0; k<neqn; k++)

f1[k] = y[k] + ch * ((-6080.0 * yp[k] + (9295.0 * f3[k] - 5643.0 * f4[k])) + (41040.0 * f1[k] - 28352.0 * f2[k]));

rab = t + h / 2.0;

fun(rab,f1,f5);

// ВЫЧИСЛИТЬ ПРИБЛИЖЕННОЕ РЕШЕНИЕ В ТОЧКЕ t + h

ch = h / 7618050.0;

for (k=0; k<neqn; k++)

s[k] = y[k] + ch *((902880.0 * yp[k] + (3855735.0 * f3[k] - 1371249.0 * f4[k]))+(3953664.0 * f2[k] + 277020.0 * f5[k]));

*hc=h;

return;}

void RKF45 (int neqn, double t,

double tout, double y[],

void (f_p)(double, double*, double*), double *hc,

double yp[], double f1[],

double f2[], double f3[],

double f4[], double f5[],

int *iflagc, int *nfec,

int *kopc, int *initc,

int *jflagc, int *kflagc,

double *relerrc, double *abserrc,double *savrec, double *savaec)

{ int iflag,nfe,kop,init,jflag,

kflag;

double h,savre,savae,

relerr, abserr;

int maxnfe = 30000;

double remin = 1.0e-15;

int hfaild, output;

double a,ae,dt,ee,eeoet,

esttol,et,hmin,eps,u26,rer,s,scale,tol,toln,ypk,rab1;

int k, mflag;

void (*fun)(double, double*, double*);

fun = f_p;

iflag=*iflagc; nfe=*nfec; kop=*kopc; init=*initc; jflag=*jflagc;

kflag=*kflagc;

h=*hc; savre=*savrec; savae=*savaec;

relerr = *relerrc;

abserr = *abserrc;

// Проверить входные параметры

if (neqn < 1) goto m10;

if (relerr< 0.0||abserr<0.0)

goto m10;

mflag=abs(iflag);

if (mflag == 0 || mflag > 8)

goto m10;

if (mflag != 1) goto m20;

// Первый вызов, вычислить машинное эпсилон

eps = 1.0;

while (eps+1.0 > 1.0)

eps = eps/2.0;

u26 = 26.0*eps;

goto m50;

// Ошибки во входной информации

m10: iflag = 8;

goto mexit;

// Проверить возможность продолжения

m20: if (t==tout && kflag != 3)

goto m10;

if (mflag != 2) goto m25;

if (kflag == 3 || init==0)

goto m45;

if (kflag == 4) goto m40;

if (kflag == 5 && abserr == 0.0) goto m30;

if (kflag == 6 && relerr <= savre && abserr <= savae)

goto m30;

goto m50;

m25:if (iflag == 3)goto m45;

if (iflag == 4) goto m40;

if(iflag==5 && abserr > 0.0) goto m45;

m30: goto mexit;

m40: nfe = 0;

if (mflag == 2)

goto m50;

m45: iflag = jflag;

if (kflag == 3)

mflag = abs(iflag);

m50: jflag = iflag;

kflag = 0;

savre = relerr;

savae = abserr;

rer = 2.0*eps+remin;

if (relerr >= rer)

goto m55;

// Заданная граница относительной погрешности слишком мала

relerr = rer;

iflag = 3;kflag = 3;

goto mexit;

m55: dt = tout-t;

if (mflag==1) goto m60;

if (init==0) goto m65;

goto m80;

m60: init = 0;

kop = 0;

a = t;

fun(a,y,yp);

nfe = 1;

if (t != tout)

goto m65;

iflag = 2;

goto mexit;

m65: init = 1;

h = fabs(dt);

toln = 0.0;

for (k=0; k<neqn; k++)

{tol=relerr*fabs(y[k])+abserr;

if (tol > 0.0)

{ toln=tol;

ypk=fabs(yp[k]);

rab1=h*h*h*h*h;

if (ypk*rab1 > tol)

h=exp(0.2*log(tol/ypk));}}

if (toln <= 0.0) h=0.0;

rab1=u26*fabs(t);

if (fabs(t) < fabs(dt))

rab1=u26*fabs(dt);

if (h < rab1) h=rab1;

jflag=-2;

if (iflag > 0)

jflag=2;

m80: if (dt < 0.0) h=-h;

if (fabs(h) >= 2.0*fabs(dt))

kop=kop+1;

if (kop != 100 ) goto m85;

// Много выходов

kop=0;

iflag=7;

goto mexit;

m85: if (fabs(dt) > u26*fabs(t))

goto m95;

// Близко к выходной точке, проэкстраполировать и вернуться по месту вызова

for (k=0; k<neqn; k++)

y[k] = y[k] + dt * yp[k];

a=tout;

fun(a,y,yp);

nfe=nfe+1;

goto m300;

m95: output=False;

scale=2.0/relerr;

ae=scale*abserr;

// Пошаговое интегрирование

m100: hfaild=False;

hmin=u26*fabs(t);

dt=tout-t;

if(fabs(dt) >= 2.0*fabs(h))

goto m200;

if(fabs(dt) > fabs(h))

goto m150;

output=True;

h=dt;

goto m200;

m150: h=0.5*dt;

// Внутренний одношаговый интегратор

m200: if(nfe <= maxnfe)

goto m220;

iflag=4;

kflag=4;

goto mexit;

// Продолжить приближенное решение на один шаг длины h

m220: FEHL(neqn, t, y, f_p, &h, yp, f1, f2, f3, f4, f5, f1);

nfe=nfe+5;

eeoet=0.0;

for (k=0; k<neqn; k++)

{ et=fabs(y[k])+fabs(f1[k])+ae;

if (et > 0.0)

goto m240;

// Неправильная граница погрешности

iflag=5;

goto mexit;

m240: ee=fabs((-2090.0*yp[k]+(21970.0*f3[k]-15048.0*f4[k]))+

(22528.0*f2[k]-27360.0*f5[k]));

rab1=ee/et;

if (eeoet < rab1)

eeoet=rab1;}

esttol=fabs(h)*eeoet*scale/752400.0;

if (esttol <= 1.0)

goto m260;

// Неудачный шаг; уменьшить его величину

hfaild=True;

output=False;

s=0.1;

if (esttol < 59049.0)

s=0.9/(exp(0.2*log(esttol)));

h=s*h;

if (fabs(h) > hmin)

goto m200;

// Неудача даже при минимальном шаге

iflag=6;

kflag=6;

goto mexit;

// Успешный шаг

m260: t=t+h;

for (k=0; k<neqn; k++)

y[k]=f1[k];

a=t;

fun(a,y,yp);

nfe=nfe+1;

s=5.0;

if (esttol > 1.889568e-4)

s=0.9/(exp(0.2*log(esttol)));

if (hfaild)

{ if (s > 1.0)

s=1.0; }

rab1=s*fabs(h);

if (rab1 < hmin)

rab1=hmin;

if (h > 0.0)

h=fabs(rab1);

else

h=-fabs(rab1);

if (output)

goto m300;

if (iflag > 0)

goto m100;

// Интегрирование успешно завершено

iflag=-2;

goto mexit;

m300: iflag=2;

mexit: *iflagc=iflag; *nfec=nfe; *kopc=kop; *initc=init; *jflagc=jflag;

*kflagc=kflag;

*hc=h; *savrec=savre; *savaec=savae;

*relerrc = relerr;

*abserrc = abserr;

return;}

void prifl0 ( int *iflc,

double *rerrc,

double *aerrc )

{int ifl;

double rerr, aerr;

ifl = *iflc;

rerr = *rerrc;

aerr = *aerrc;

switch (ifl)

{case 3: /*printf ("ifl=%d\n rerr=%.12lf aerr=%.12lf\n",ifl,

rerr,aerr);*/

break;

case 4: printf ("много fp()\n ifl=%d\n", ifl);

break;

case 5: aerr = 1.0e-9;

/*printf("ifl=%d\n rerr=%.12lf aerr=%.12lf\n",ifl,rerr,aerr);

*/

break;

case 6: rerr *= 10.0;

printf ("ifl=%d\n rerr=%.12lf aerr=%.12lf\n", ifl, rerr, aerr)

ifl = 2;

break;

case 7: printf ("много вых. точек\n ifl=%d\n", ifl);

ifl = 2;

break;

case 8: printf ("ifl=%d\n", ifl);

break; }

*iflc = ifl;

*rerrc = rerr;

*aerrc = aerr;

//getchar();

}

Размещено на Allbest.ru