导弹制导控制全仿真模型，滑模制导律，MATLAB代码

while abs(r) < abs(miss) || abs(r) == abs(miss) for m_kk = 1:1:T/h t = t + h; miss = r; if ym <= 11000 sound_v = 20.055*sqrt(288.16-0.0065*ym); else sound_v = 295.19; end Ma = Vm/sound_v; % A=[0 1 0 0 0 0; % 0 0 1 c1 0 c2; % 0 0 -1/tau_t 0 0 0; % 0 0 0 a1 1 a2; % 0 0 0 M1 M2 M3; % 0 0 0 0 0 -1/tau_s]; switch aero_data_index case 1 %此部分内容为已知气动数据% %IEEE-2008-Integrated Guidance and Control for Homing Missiles via Neural Network的数据 L_delta_z = 0.068*Vm; % Cy_deltaz*Q*S/mass L_alpha = 0.3487*Vm;% m/s/s Cy_alpha*Q*S/mass M_omega_z = -0.2741; % 1/s M_delta_z = -31.267; % 1/s^2 M_alpha = -17.801;% 1/s^2 L_beta = -L_alpha; L_delta_y = -L_delta_z; M_beta = M_alpha; M_omega_y = M_omega_z; M_delta_y = M_delta_z; a1 = -L_alpha/Vm; a2 = -L_delta_z/Vm; a_1 = L_beta/Vm;%注意此处的符号 a_2 = L_delta_y/Vm; M1 = M_alpha; M2 = M_omega_z; M3 = M_delta_z; case 2 % %外文文献Sliding-Mode Control for Integrated Missile Autopilot Guidance数据 L_Ba = 1190; % L_delta_z = 80; % Cy_deltaz*Q*S/mass L_alpha = L_Ba+L_delta_z;% m/s/s Cy_alpha*Q*S/mass M_Ba = -234.0; % 1/s^2 M_omega_z = -5; % 1/s M_delta_z = 160; % 1/s^2 M_alpha = M_Ba+M_delta_z;% 1/s^2 L_beta = -L_alpha; L_delta_y = -L_delta_z; M_beta = M_alpha; M_omega_y = M_omega_z; M_delta_y = M_delta_z; a1 = -L_alpha/Vm; a2 = -L_delta_z/Vm; a_1 = L_beta/Vm;%注意此处的符号 a_2 = L_delta_y/Vm; M1 = M_alpha; M2 = M_omega_z; M3 = M_delta_z; case 3 %数据来源：AIAA-1999-Software Tools for Nonlinear Missile Autopilot Design(含有纵向通道模型参数).pdf Ma_temp = Ma/2; a1 = 0.4008*Ma_temp*alpha^2-0.6419*Ma_temp*abs(alpha)-0.201*Ma_temp*(2-Ma_temp/3); a2 = -0.0403*Ma_temp; M_alpha = (49.82*Ma_temp^2*alpha^2-78.86*Ma_temp^2*abs(alpha)+3.6*Ma_temp^2*(-7-8/3*Ma_temp))*cos(alpha); M_omega_z = -2.12*Ma_temp^2; M_delta_z = -40.54*Ma_temp^2; L_alpha = -a1*Vm; L_delta_z = -a2*Vm; M1 = M_alpha; M2 = M_omega_z; M3 = M_delta_z; a_1 = 0.4008*Ma_temp*beta^2-0.6419*Ma_temp*abs(beta)-0.201*Ma_temp*(2-Ma_temp/3); a_2 = -0.0403*Ma_temp; M_beta = (49.82*Ma_temp^2*beta^2-78.86*Ma_temp^2*abs(beta)+3.6*Ma_temp^2*(-7-8/3*Ma_temp))*cos(beta); M_omega_y = -2.12*Ma_temp^2; M_delta_y = -40.54*Ma_temp^2; L_beta = a_1*Vm; L_delta_y = a_2*Vm; end at_xc = 0.0; at_yc = 15*g*(sin(2*pi/2*t));%目标加速度指令 at_zc = 15*g*(sin(2*pi/2*t));%目标加速度指令 %%%%%%%%%%%%%%%%%%%%%%%%%% [at_x,at_y,at_z] = at_fun(h,at_x,at_y,at_z,at_xc,at_yc,at_zc,tau_t); %%%%%%%%%%%%%%%%%%%%%%%%%% [Vt, theta_vt, fai_vt, xt, yt, zt] = VehicleEquation3dof_fun(h,at_x,at_y,at_z,Vt, theta_vt, fai_vt, xt, yt, zt); %%%%%%%%%%%%%%%%%%%%%%%%%% mass = 100; % kg Jx = 1.0; % kg*m^2 Jy = 100; % kg*m^2 Jz = Jy; % kg*m^2 Thrust = 0.0; % N Drag = 0.0; % N Lift = (L_alpha*alpha+L_delta_z*delta_z)*mass; % N SideForce = (L_beta*beta+L_delta_y*delta_y)*mass; % N Mx = 0.0; % N*m My = (M_beta*beta + M_omega_y*omega_y + M_delta_y*delta_y)*Jy; % N*m Mz = (M_alpha*alpha + M_omega_z*omega_z + M_delta_y*delta_z)*Jz; % N*m [Vm, alpha, beta, omega_x, omega_y, omega_z,xm,ym,zm,theta,fai,gama,theta_vm,fai_vm,gama_v,Vxm,Vym,Vzm] =... VehicleEquation6dof_fun(h, Thrust, Lift,SideForce, Drag, Mx, My, Mz, mass, g, Jx, Jy, Jz,... Vm, alpha, beta, omega_x, omega_y, omega_z,xm,ym,zm,theta,fai,gama,... theta_vm,fai_vm,gama_v); omega_x = 0.0; gama = 0.0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%% [delta_z,delta_y] = delta_fun(h,delta_zc,delta_yc,delta_z,delta_y,tau_s); %%%%计算导弹和目标的前置角%%%%%%%%%%%%%% DCM_LI = Euler2DCM_fun(fai_L,theta_L,0); % 惯性到视线 DCM_MI = Euler2DCM_fun(fai_vm,theta_vm,0);% 惯性到导弹弹道 DCM_ML = DCM_MI/DCM_LI; % 视线到导弹弹道 theta_m = DCM2Euler_fun(DCM_ML)*[0 1 0]'; % rad 导弹弹道前置偏角 fai_m = DCM2Euler_fun(DCM_ML)*[1 0 0]'; % rad 导弹弹道前置偏角 gama_m = DCM2Euler_fun(DCM_ML)*[0 0 1]'; % rad 导弹弹道前置滚转角 DCM_TI = Euler2DCM_fun(fai_vt,theta_vt,0);% 惯性到目标弹道 DCM_TL=DCM_TI/DCM_LI; % 视线到目标弹道 theta_t = DCM2Euler_fun(DCM_TL)*[0 1 0]'; % rad 目标弹道前置倾角 fai_t = DCM2Euler_fun(DCM_TL)*[1 0 0]'; % rad 目标弹道前置偏角 gama_t = DCM2Euler_fun(DCM_TL)*[0 0 1]'; % rad 目标弹道前置滚转角 Vr_x = (-[Vm 0 0]*DCM_ML+[Vt 0 0]*DCM_TL)*[1 0 0]'; % m/s 沿x方向的弹目相对速度 Vr_y = (-[Vm 0 0]*DCM_ML+[Vt 0 0]*DCM_TL)*[0 1 0]'; % m/s 垂直于视线沿y方向的弹目相对速度 Vr_z = (-[Vm 0 0]*DCM_ML+[Vt 0 0]*DCM_TL)*[0 0 1]'; % m/s 垂直于视线沿z方向的弹目相对速度 demta_z = Vr_y/r; % rad/s % LOS角速度矢量绕视线z轴 demta_y = -Vr_z/r; % rad/s % LOS角速度矢量绕视线y轴 [r,theta_L,fai_L] = TRM_fun(h,r,theta_L,fai_L,Vr_x,demta_z,demta_y); tgo = -r/Vr_x; if abs(r) > abs(miss) break; end if Sim_index == 1 xmarray = [xmarray;xm]; %#ok<AGROW> ymarray = [ymarray;ym]; %#ok<AGROW> zmarray = [zmarray;zm]; %#ok<AGROW> xtarray = [xtarray;xt]; %#ok<AGROW> ytarray = [ytarray;yt]; %#ok<AGROW> ztarray = [ztarray;zt]; %#ok<AGROW> end end if abs(r)>300.0 switch GC_index case 0 %PNG-NDI分开设计 ZEM_y=-Vr_x*tgo^2*demta_z+atn_y*tau_t^2*ff(tgo/tau_t)-amn_y*tau_m^2*ff(tgo/tau_m); amn_c=5.5*abs(Vr_x)*demta_z; am_c=amn_c/cos(theta_vm-theta_L); alpha_c=am_c/L_alpha; q_c=4.0*(alpha_c-alpha)+(L_alpha*alpha+L_delta_z*delta_z)/Vm; delta_zc=1/M_delta_z*(12.0*(q_c-omega_z)-M_alpha*alpha+M_omega_z*omega_z); am_carray=[am_carray;am_c];%#ok<AGROW> case 1 %SMCG-NDI分开设计 mu=500;%abs(tau_t*ff(tgo_filter/tau_t)*at_yc/cos(theta_vt+theta_L)); % 滑模控制中的常数 ZEM_y=-Vr_x*tgo^2*demta_z+atn_y*tau_t^2*ff(tgo/tau_t)-amn_y*tau_m^2*ff(tgo/tau_m); amn_eq=(Vemta+atn_y*tau_t*(1-exp(-tgo/tau_t))-amn_y*tau_m*(1-exp(-tgo/tau_m)))*d_Vr_x*r/(Vr_x^2*tau_m*ff(tgo/tau_m)); amn_c=amn_eq+mu*sat(ZEM_y)/(tau_m*ff(tgo/tau_m)); am_c=