projectiledes.asv

LQR源程序

function f=projectiledes(t,y,flag,DeltaCav,DeltaFinE,Theta)
%--------------------------------------------------------------------------------------------------
%STAT compute movement in latitude plane 垂直平面内的运动

global G M I2 S Lcav Lfin Sigma Cx0 Rho A
G=9.8;M=1860; I2=6770;S=0.05;Lcav=3.5;Lfin=4.5;Sigma=0.015;Cx0=0.815;A=0.15;Rho=1.030;
%--------------------------------------------------------------------------------------------------


%--------可以设定的参数----------------------------------------------------------------------------
T=50000;           %推力的设置
%global
%DeltaCav=0;  DeltaFinE=0;
%=-5*pi/180;


%=5*pi/180;
%--------------------------------------------------------------------------------------------------
%alfa=atan(w/u);%定义鱼雷的攻角
%V=w/sin(alfa); %由攻角导出鱼雷的速度

%-----------------------微分变量的定义-------------------------------------------------------------
u=y(1);%定义鱼雷在B1轴上的速度
w=y(2);%定义鱼雷在B3轴上的速度
q=y(3);%定义鱼雷在B2轴上的角速度
theta=y(4);%定义鱼雷俯仰角
%--------------------------------------------------------------------------------------------------
f(1)=(-q*w)+(-G*sin(theta))+T/M+(-1/2*Rho*S*(u.^2+w.^2)/M)*((cos(atan(w/u))*(1+Sigma)*Cx0*(cos(DeltaCav)).^2)+(-sin(atan(w./u))*(1+Sigma)*Cx0*(cos(DeltaCav))^2*sin(DeltaCav)))+(-1/2*Rho*2*A*(u.^2+w.^2)/M)*((cos(atan(w./u))*(1+Sigma)*Cx0*(cos(DeltaFinE))^2)+(-sin(atan(w./u))*(1+Sigma)*Cx0*(cos(DeltaFinE))));
f(2)=(q*u)+(1/2*G*cos(theta))+(-1/2*Rho*S*(u.^2+w.^2)/M)*((sin(atan(w/u))*(1+Sigma)*Cx0*(cos(DeltaCav)).^2)+(cos(atan(w./u)*(1+Sigma)*Cx0*(cos(DeltaCav))^2*sin(DeltaCav)))+(-1/2*Rho*2*A*(u.^2+w.^2)/M)*((sin(atan(w./u))*(1+Sigma)*Cx0*(cos(DeltaFinE).^2))+(cos(atan(w./u))*(1+Sigma)*Cx0*(cos(DeltaFinE)^2)*(sin(DeltaFinE)))));
f(3)=(-1/2*Rho*S*(u.^2+w.^2)/(M*I2))*((sin(atan(w./u))*(1+Sigma)*Cx0*(cos(DeltaCav))^2)+(cos(atan(w./u))*(1+Sigma)*Cx0*(cos(DeltaCav))^2*sin(DeltaCav)))*Lcav+(-1/2*Rho*2*A*(u.^2+w.^2)/(M*I2))*((sin(atan(w./u))*(1+Sigma)*Cx0*(cos(DeltaFinE))^2)+(cos(atan(w./u))*(1+Sigma)*Cx0*(cos(DeltaFinE))))*(-Lfin);
f(4)=q;
f(5)=u*cos(theta)+w*sin(theta);                  %雷体坐标系中的速度转换到地面坐标系
f(6)=u*(-sin(theta))+w*cos(theta);               %雷体坐标系中的速度转换到地面坐标系
f=f(:);
return
%--------------------------------------------------------------------------------------------------