Программа FCT – сумма квадратов правых частей уравнений для балансировки в критерии выбора угла атаки, тяги и руля высоты из условия стационарного движения ЛА




(/курсовая/Optimal Control C++ Orginal/BALANCING/строка 156 «FCT»)

double FCT(double Bal[])

{ // объявление вспомогательных переменных

double sinG,cosG;// G - Gamma

double sinP,cosP;// P - Psi

double sinT,cosT,tanT;// T - Teta

double Delta;

double A;

double V,Vwx1,Vwy1,Vwz1;

double Alpha,Beta,AL,BE;

double sinAL,cosAL;

double OMGx,OMGy,OMGz;

double Cx,Cy,Cz;

double F,P1,P2,P3,P4,P5,P6;

double Px1,Py1,Pz1;

double dVx1,dVy1,dVz1;

double dVwx1,dVwy1,dVwz1;

double Wx,Wy,Wz;

double dAL,dBE;

double AMx,AMy,AMz;

double PMx1,PMy1,PMz1;

double Ix,Iy,Iz,Ixy,I2;

 

double DRGR=0.0,ETA=0.0,DNAPRV=0.0,DELERON=0.0;

double SIGF[6]={0.0};

double RST=0.0;

int i=0;

 

DRGR=Bal[0];

ETA=Bal[1];

Alpha=Bal[2];

DNAPRV=Bal[3];

DELERON=Bal[4];

 

Beta=BETATR;

double TETTR=Alpha+THETATR;

 

AL=Alpha*GR+Q[53];

BE=Beta*GR;

 

V=VTR;

Vwx1=V*cos(Beta)*cos(Alpha);

Vwy1=-V*cos(Beta)*sin(Alpha);

Vwz1=V*sin(Beta);

 

// вычисление

sinG=sin(GAMTR);

cosG=cos(GAMTR);

 

sinT=sin(TETTR);

cosT=cos(TETTR);

 

Delta=pow((288.16-0.0066*HTR)/288.16,4.255);

 

A=340.0-0.004*HTR;

if (HTR>=1.1E4)

A=295.0;

 

sinAL=sin(Alpha);

cosAL=cos(Alpha);

 

OMGx=W1xTR*cosAL-W1yTR*sinAL;

OMGy=W1xTR*sinAL+W1yTR*cosAL;

OMGz=W1zTR;

 

Cy=Q[7]+Q[8]*AL+Q[9]*ETA+Q[10]*Q[52];

Cx=Q[11]+Q[12]*Cy+Q[13]*sqr(Cy)+(Q[14]+Q[15]*AL+Q[16]*sqr(AL))*ETA

+(Q[17]+Q[18]*AL+Q[19]*sqr(AL))*Q[52];

Cz=Q[27]*BE+Q[28]*DNAPRV;

 

F=0.5*R00*Delta*sqr(V)*Q[49]*G;

 

P1=Cx*F;

P2=Cy*F;

P3=Cz*F;

Px1=P1*cosAL-P2*sinAL;

Py1=P1*sinAL+P2*cosAL;

Pz1=P3;

dVx1=(1.0/Q[1])*(DRGR*cos(Q[2])-Px1)+W1zTR*Vwy1-W1yTR*Vwz1-G*sinT;

dVy1=(1.0/Q[1])*(DRGR*sin(Q[2])+Py1)+W1xTR*Vwz1-W1zTR*Vwx1-G*cosT*cosG;

dVz1=Pz1/Q[1]-W1xTR*Vwy1+W1yTR*Vwx1+G*cosT*sinG;

 

SIGF[0]=dVx1*Q[1];

SIGF[1]=dVy1*Q[1];

SIGF[2]=dVz1*Q[1];

 

dVwx1=dVx1;

dVwy1=dVy1;

dVwz1=dVz1;

 

dAL=-(dVwy1*Vwx1-dVwx1*Vwy1)/(sqr(Vwx1)+sqr(Vwy1));

dBE=(dVwz1*(sqr(Vwx1)+sqr(Vwy1)+sqr(Vwz1))-Vwz1*(Vwx1*dVwx1

+Vwy1*dVwy1+Vwz1*dVwz1))/(sqrt(sqr(Vwx1)+sqr(Vwy1))

*(sqr(Vwx1)+sqr(Vwy1)+sqr(Vwz1)));

 

AMx=(Q[29]+Q[30]*AL)*DNAPRV+(Q[31]+Q[32]*AL)*BE

+Q[33]*DELERON+(Q[34]+Q[35]*AL+Q[36]*sqr(AL))*OMGx

*Q[50]/(2.0*V)+(Q[37]+Q[38]*AL)*OMGy*Q[50]/(2.0*V);

AMy=Q[39]*BE+Q[40]*DNAPRV+(Q[41]+Q[42]*AL+Q[43]*sqr(AL))*OMGx

*Q[50]/(2.0*V)+(Q[44]+Q[45]*AL+Q[46]*sqr(AL))*OMGy*Q[50]

/(2.0*V)+Q[47]*dBE*Q[50]/(2.0*V);

AMz=Q[20]+Q[21]*AL+Q[22]*sqr(AL)+Q[23]*ETA+Q[24]*Q[52]

+Q[25]*OMGz*Q[51]/V+Q[26]*dAL*Q[51]/V+(Q[48]-25.0)*Cy*0.01;

 

P4=AMx*F*Q[50];

P5=AMy*F*Q[50];

P6=AMz*F*Q[51];

PMx1=P4*cosAL+P5*sinAL;

PMy1=-P4*sinAL+P5*cosAL;

PMz1 = P6 + DRGR * 2.13;

 

SIGF[3]=(Q[4]*PMx1+Q[6]*PMy1-Q[6]*(Q[3]+Q[4]-Q[5])*W1xTR*W1zTR+(sqr(Q[4])-Q[4]*Q[5]+sqr(Q[6]))*W1yTR*W1zTR);

SIGF[4]=(Q[6]*PMx1+Q[3]*PMy1

-(sqr(Q[3])-Q[3]*Q[5]+sqr(Q[6]))*W1xTR*W1zTR+Q[6]

*(Q[3]+Q[4]-Q[5])*W1yTR*W1zTR);

SIGF[5]=(PMz1-(Q[4]-Q[3])*W1xTR*W1yTR-Q[6]*(sqr(W1yTR)

-sqr(W1xTR)));

 

 

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

{

RST+=sqr(SIGF[i]);

}

return RST;

 

}

Программа FLIGHTDYNAMIC (уравнения движения ЛА)

(/курсовая/Optimal Control C++ Orginal/ FLIGHTDYNAMIC.С++ Source)

// уравнение движения

void FX(double* dX, double* X, double* U, double* Q, double T)

{

/***************************************************************************

* X(12) - вектор состояния ЛА *

* ->X[1],X[2],X[3] <-> Vx1,Vy1,Vz1 - скорости ЛА *

* ->X[4],X[5],X[6] <-> wx1.wy1,wz1 - угловые скорости ЛА *

* ->X[7],X[8],X[9] <-> Gamma,Psi,Teta - углы крена, курса, тангажа *

* ->X[10],X[11],X[12] <-> Xg,Yg,Z,g - координаты центра масс ЛА в земной СК *

* U(4) - вектор управления *

* ->U[1] <-> P - тяга двигателя *

* ->U[2] <-> DELTAv - отклонение руля высоты *

* ->U[3] <-> DELTAn - отклонение руля направления *

* ->U[4] <-> DELTAe - отклонение элерона *

* Q(53) - вектор параметров ЛА *

***************************************************************************/

// объявление вспомогательных переменных

double sinG,cosG;// G - Gamma

double sinP,cosP;// P - Psi

double sinT,cosT,tanT;// T - Teta

double Delta;

double A;

double Vw,Vwx1,Vwy1,Vwz1;

double Alpha,Beta,AL,BE;

double sinAL,cosAL;

double OMGx,OMGy,OMGz;

double Cx,Cy,Cz;

double F,P1,P2,P3,P4,P5,P6;

double Px1,Py1,Pz1;

double dVx1,dVy1,dVz1;

double dVwx1,dVwy1,dVwz1;

double dAL,dBE;

double AMx,AMy,AMz;

double PMx1,PMy1,PMz1;

// вычисление

sinG=sin(X[7]);

cosG=cos(X[7]);

sinP=sin(X[8]);

cosP=cos(X[8]);

sinT=sin(X[9]);

cosT=cos(X[9]);

tanT=tan(X[9]);

Delta=pow((288.16-0.0066*X[11])/288.16,4.255);

A=340.0-0.004*X[11];

if (X[11]>=1.1e04)

A=295.0;

Vwx1=X[1];

Vwy1=X[2];

Vwz1=X[3];

Vw=sqrt(sqr(Vwx1)+sqr(Vwy1)+sqr(Vwz1));

Alpha=-atan(Vwy1/Vwx1);

Beta=asin(Vwz1/Vw);

AL=Alpha*GR+Q[53];

BE=Beta*GR;

sinAL=sin(Alpha);

cosAL=cos(Alpha);

OMGx=X[4]*cosAL-X[5]*sinAL;

OMGy=X[4]*sinAL+X[5]*cosAL;

OMGz=X[6];

Cy=Q[7]+Q[8]*AL+Q[9]*U[2]+Q[10]*Q[52];

Cx=Q[11]+Q[12]*Cy+Q[13]*sqr(Cy)+(Q[14]+Q[15]*AL+Q[16]*sqr(AL))*U[2]

+(Q[17]+Q[18]*AL+Q[19]*sqr(AL))*Q[52];

Cz=Q[27]*BE+Q[28]*U[3];

F=0.5*R00*Delta*sqr(Vw)*Q[49]*G;

P1=Cx*F;

P2=Cy*F;

P3=Cz*F;

Px1=P1*cosAL-P2*sinAL;

Py1=P1*sinAL+P2*cosAL;

Pz1=P3;

dVx1=(1.0/Q[1])*(U[1]*cos(Q[2])-Px1)+X[6]*X[2]-X[5]*X[3]-G*sinT;

dVy1=(1.0/Q[1])*(U[1]*sin(Q[2])+Py1)+X[4]*X[3]-X[6]*X[1]-G*cosT*cosG;

dVz1=Pz1/Q[1]-X[4]*X[2]+X[5]*X[1]+G*cosT*sinG;

dVwx1=dVx1;

dVwy1=dVy1;

dVwz1=dVz1;

dAL=-(dVwy1*Vwx1-dVwx1*Vwy1)/(sqr(Vwx1)+sqr(Vwy1));

dBE=(dVwz1*(sqr(Vwx1)+sqr(Vwy1)+sqr(Vwz1))-Vwz1*(Vwx1*dVwx1

+Vwy1*dVwy1+Vwz1*dVwz1))/(sqrt(sqr(Vwx1)+sqr(Vwy1))

*(sqr(Vwx1)+sqr(Vwy1)+sqr(Vwz1)));

AMx=(Q[29]+Q[30]*AL)*U[3]+(Q[31]+Q[32]*AL)*BE

+Q[33]*U[4]+(Q[34]+Q[35]*AL+Q[36]*sqr(AL))*OMGx

*Q[50]/(2.0*Vw)+(Q[37]+Q[38]*AL)*OMGy*Q[50]/(2.0*Vw);

AMy=Q[39]*BE+Q[40]*U[3]+(Q[41]+Q[42]*AL+Q[43]*sqr(AL))*OMGx

*Q[50]/(2.0*Vw)+(Q[44]+Q[45]*AL+Q[46]*sqr(AL))*OMGy*Q[50]

/(2.0*Vw)+Q[47]*dBE*Q[50]/(2.0*Vw);

AMz=Q[20]+Q[21]*AL+Q[22]*sqr(AL)+Q[23]*U[2]+Q[24]*Q[52]

+Q[25]*OMGz*Q[51]/Vw+Q[26]*dAL*Q[51]/Vw+(Q[48]-25.0)*Cy*0.01;

P4=AMx*F*Q[50];

P5=AMy*F*Q[50];

P6=AMz*F*Q[51];

PMx1=P4*cosAL+P5*sinAL;

PMy1=-P4*sinAL+P5*cosAL;

PMz1=P6;

//diff. equation

dX[1]=dVx1;

dX[2]=dVy1;

dX[3]=dVz1;

dX[4]=(1.0/(Q[3]*Q[4]-sqr(Q[6])))*(Q[4]*PMx1+Q[6]*PMy1-Q[6]

*(Q[3]+Q[4]-Q[5])*X[4]*X[6]+(sqr(Q[4])-Q[4]*Q[5]

+sqr(Q[6]))*X[5]*X[6]);

dX[5]=(1.0/(Q[3]*Q[4]-sqr(Q[6])))*(Q[6]*PMx1+Q[3]*PMy1

-(sqr(Q[3])-Q[3]*Q[5]+sqr(Q[6]))*X[4]*X[6]+Q[6]

*(Q[3]+Q[4]-Q[5])*X[5]*X[6]);

dX[6]=(1.0/Q[5])*(PMz1-(Q[4]-Q[3])*X[4]*X[5]-Q[6]*(sqr(X[5])

-sqr(X[4])));

dX[7]=X[4]-(X[5]*cosG-X[6]*sinG)*tanT;

dX[8]=(1.0/cosT)*(X[5]*cosG-X[6]*sinG);

dX[9]=X[6]*cosG+X[5]*sinG;

dX[10]=X[1]*cosP*cosT-X[2]*(cosP*sinT*cosG-sinP*sinG)+X[3]

*(sinP*cosG+cosP*sinT*sinG);

dX[11]=X[1]*sinT+X[2]*cosT*cosG-X[3]*cosT*sinG;

dX[12]=-X[1]*sinP*cosT+X[2]*(cosP*sinG+sinP*sinT*cosG)

+X[3]*(cosP*cosG-sinP*sinT*sinG);

}

 

 



Поделиться:




Поиск по сайту

©2015-2024 poisk-ru.ru
Все права принадлежать их авторам. Данный сайт не претендует на авторства, а предоставляет бесплатное использование.
Дата создания страницы: 2019-06-03 Нарушение авторских прав и Нарушение персональных данных
 

Поиск по сайту:

Читайте также:

Деталирование сборочного чертежа

Когда производственнику особенно важно наличие гибких производственных мощностей?

Собственные движения и пространственные скорости звезд

Тема 11. Банковские риски и способы их оценки

Опросник «Активность повседневной жизни»

Интересно:

Что такое ортодоксальный марксизм?

Роль геодезии в осуществлении землеустройства и ландшафтном строительстве

Что такое современная опера

Что такое сказка. Цели и задачи сказкотерапии

Обратная связь