mmolloy/Hayden-Project
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initialize Hayden project repository

Browse Source
This commit is contained in:
deamonkai
2026-03-28 11:22:16 -05:00
commit 595c63b934
11 changed files with 525 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

148
code/Interceptor_3DOF.m Normal file
View File

@@ -0,0 +1,148 @@
%% * Interceptor 3DOF*
%% Executive
clear all; clc;
t0 = 0; % Initial time. Target launch time.
tCommit = XX.; % Commit time (sec)
sigEl = 0; % Missile elevation angle standard deviation
maxTime = 150; % estimated maximum engagement time in sec
dt = 0.01; % integration interval (sec)
maxNum = maxTime/dt; % maximum number of steps in integration loop
simTime(1) = t0;
%% Target initial conditions
Tp0 = [75000; 0; 0 ];
Tspeed0 = 1.e-06;
QE = 63; LOF = -180; % Quadrant elevation and line-of-fire in degrees. LOF measured from x-axis.
Tv0 = Tspeed0*[cosd(QE)*cosd(LOF); cosd(QE)*sind(LOF); sind(QE)];%initial velocity vector in ENU frame
targetState = [Tp0; Tv0]; % initial target state vector (position & velocity) 6 states
Target_speed(1,:) = norm(targetState(4:6));
TxDot = T3dxdt(t0,targetState);
Toutput_vector(1,:)= [t0,targetState',TxDot(4:6)'];
%% Missile initial conditions
Mp0 = [0; 0; 0]; % Missile initial position in ENU frame (m)
Mspeed0 = 1.e-06;% Missile initial speed in ENU frame (m/sec); must be non-zero for unit vector calculation.
Maz = XX; Mel = XX; % Missile launch azimuth and elevation (deg).
Mv0 = Mspeed0*[cosd(Mel)*cosd(Maz); cosd(Mel)*sind(Maz); sind(Mel)];%initial velocity vector in ENU frame
missileState = [Mp0; Mv0]; % Missile state vector in ENU frame (m. and m/sec.)
Moutput_vector(1,:)= [t0,missileState', 0, 0, 0];
missileSpeed(1,:) = [t0,norm(missileState(4:6))];
Mmach(1) = 0.;
%% Trajectory Computation Loops
Tt = t0;
for ind = 2:maxNum
% Get Target State Vector
[Ttime,Tx]=ode45(@T3dxdt,[Tt Tt+dt],targetState);% x and time are values of xVector at t+dt
Tt = Ttime(end); % last element in the "time" vector
targetState = Tx(end,:); % last row in the "x" array. state at time(end).
TxDot = T3dxdt(Tt,targetState'); % find velocity and acceleration using dxdt; target xdot
Toutput_vector(ind, :)= [Tt,targetState,TxDot(4:6)'];% xDot(4:6) is acceleration
Target_speed(ind,:) = norm(targetState(4:6));
Trange = norm(targetState(1:3));
Mt = Tt-tCommit-dt; % Missile time. Clock starts at missile launch (tCommit).
Mmach(ind) = Mmach(1);
missileSpeed(ind,:) = missileSpeed(1,:);
Moutput_vector(ind, :)= Moutput_vector(1, :);
simTime(ind) = Tt; %Simulation time.
% Get Missile State Vector
if Mt >= 0
% [Mtime,Mx]=ode45(@(t,x)M3dxdt(t,x,targetState),[Mt Mt+dt], missileState);% x and time are values of xVector at t+dt
[Mtime,Mx]=ode45(@(Mt,x)M3dxdt(Mt,x,targetState),[Mt Mt+dt], missileState);% x and time are values of xVector at t+dt
Mt = Mtime(end); % last element in the "time" vector
missileState = Mx(end,:); % last row in the "x" array. state at time(end).
MxDot = M3dxdt(Mt,missileState',targetState); % find velocity and acceleration using dxdt; missile xdot
[acousticSpeed,rho] = getRho(missileState);
Mspeed = norm(missileState(4:6));
Mmach(ind) = Mspeed/acousticSpeed;
missileSpeed(ind,:) = [Tt,norm(missileState(4:6))];
missileAccel(ind,:) = [Mt,norm(MxDot(4:6))];
Moutput_vector(ind, :)= [Mt,missileState,MxDot(4:6)'];% xDot(4:6) is acceleration
% Compute Relative Data
relOutput(ind,:) = [Tt,targetState(1:3),missileState(1:3)];
Toutput(ind,:) = [Tt,targetState];
relState = targetState-missileState; % Relative state vector (target-missile).
relRange(ind) = norm(targetState(1:3)-missileState(1:3));
vClosing(ind) = dot(relState(4:6)',relState(1:3)/norm(relState(1:3))); % Closing speed
if relRange(ind)<10
dt = 0.0001;
end
% disp(relRange(ind));
if targetState(3)<=0 || vClosing(ind)>0
break
end
end
end
missDist = getMD(vClosing(1:ind),simTime(1:ind),relRange(1:ind));
disp(missDist);
%% Plot and write results
figure(6)
clf
axis equal;
grid on;
hold on;
title('Target and Missile Trajectories')
plot(Toutput_vector(:,2),Toutput_vector(:,4),'r','linewidth',2);
plot(Moutput_vector(:,2),Moutput_vector(:,4),'b','linewidth',2);
xlabel('Downrange (m)')
ylabel('Altitude (m)')
hold on;
grid on;
figure(2)
%axis equal;
grid on;
hold on;
xlim([0 90000]);
ylim([-5000 5000]);
zlim([0 12000]);
plot3(Toutput_vector(:,2),Toutput_vector(:,3),Toutput_vector(:,4),'r','linewidth',2);
plot3(Moutput_vector(:,2),Moutput_vector(:,3),Moutput_vector(:,4),'b','linewidth',2);
hold on;
grid on;
figure(3)
clf
grid on;
plot(missileAccel(:,1),missileAccel(:,2),'k','linewidth',2);
title('Missile Acceleration Magnitude')
xlabel('Time Since Missile Launch (sec)')
ylabel('Missile Acceleration (m/sec^2)')
grid on;
figure(4)
clf
grid on;
plot(missileSpeed(:,1),missileSpeed(:,2),'k','linewidth',2);
title('Missile Speed versus Time')
xlabel('Time Since Target Launch (sec)')
ylabel('Missile Speed (m/sec)')
grid on;
figure(5)
clf
grid on;
plot(simTime(:),vClosing(:),'k','linewidth',2);
title('Closing Speed versus Time')
xlabel('Time Since Target Launch (sec)')
ylabel('Closing Speed (m/sec)')
grid on;
figure(1)
clf
grid on;
plot(simTime(:),Target_speed(:),'k','linewidth',2);
title('Target Speed versus Time')
xlabel('Time (sec)')
ylabel('Target Speed (m/sec)')
grid on;
figure(7)
clf
grid on;
plot(simTime(:),Mmach(:),'k','linewidth',2);
title('Missile Mach Number versus Time')
xlabel('Time (sec)')
ylabel('Missile Mach Number')
grid on;
writematrix(Toutput_vector,'Ttrajectory.csv');
writematrix(Moutput_vector,'Mtrajectory.csv');