飞行器机动飞行质点弹道仿真-侧向和纵向-龙格库塔/欧拉法

%%飞行器机动飞行质点弹道仿真 tic; clc;clear all;close all;dbstop if error; format long %% 仿真时间和步长 t_simu=20; dt = 0.01; num =t_simu/dt; Time=linspace(0,t_simu,num); %% 系统初值 V = 1000; theta = 0; psi_v = 0; %速度 弹道倾角 弹道偏角 x = 10000; y = 15000; z = 0; % xyz初始位置 psi_v11 = 0; theta11 = 0; %% 主循环 for i = 1:1:num t = Time(i) g = 9.8; %重力加速度 dz = 0*sin(2*pi/t_simu*t); %预设z方向的加速度 这里直接设为0 psi_v1 = asin(-dz/(V*cos(theta))); %根据公式$\dot z = - V\cos \theta \sin {\psi _v}$反求psi_v dpsi_v1 = (psi_v1-psi_v11)/dt; %对psi_v 求导 psi_v11 = psi_v1; a_z = -V*cos(theta)*dpsi_v1; %根据公式$ - V\cos \theta {\dot \psi _v} = {a_z}$反求控制指令 dy = -0.5*sin(2*pi/t_simu*t); %预设y方向的加速度 theta1 = asin(dy/V); %根据公式 $\dot y = V\sin \theta $算出theta1 dtheta1 = (theta1-theta11)/dt; theta11 = theta1; a_y = V*dtheta1+g*cos(theta1); %% 系统模型 欧拉和龙格库塔 % [x,y,z,theta,psi_v] = plant_MT_oula(V,a_y,theta,a_z,psi_v,x,y,z,dt); %欧拉法解算模型 state = [x; y; z; theta; psi_v]; %龙哥库塔解算模型 k1 = plant_MT_long(t, state, V, a_y, a_z); k2 = plant_MT_long(t+dt/2, state+dt/2*k1, V, a_y, a_z); k3 = plant_MT_long(t+dt/2, state+dt/2*k2, V, a_y, a_z); k4 = plant_MT_long(t+dt, state+dt*k3, V, a_y, a_z); state = state+dt/6*(k1+2*k2+2*k3+k4); x = state(1); y = state(2); z = state(3); theta = state(4); psi_v = state(5); %% 保存速度 t_save(i,:) = t; x_save(i,:) = x; y_save(i,:) = y; z_save(i,:) = z; end figure(1)%实际轨迹 plot3(x_save, z_save,y_save,'LineWidth', 1.4); hold on; xlabel('$$x{\rm{[}}m{\rm{]}}$$','Interpreter','latex','fontsize',13); ylabel('$$z{\rm{[}}m{\rm{]}}$$','Interpreter','latex','fontsize',13); zlabel('$$y{\rm{[}}m{\rm{]}}$$','Interpreter','latex','fontsize',13); grid on % axis equal toc