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');

39
code/M3dxdt.m Normal file
View File

@@ -0,0 +1,39 @@
function MxDot = M3dxdt(Mt,missileState,targetState)
%% Missile is a 120 mm Rocket with an IR Seeker
% Compute Missile xDot = [velocity; acceleration]'
g = 9.8066; % acceleration due to gravity
gravity = [0; 0; -g]; % gravity vector
diameter = 0.120; % missile diameter in m.
S = XX; % reference area in m^2
[acousticSpeed,rho] = getRho(missileState);
Mspeed = norm(missileState(4:6));
mach = Mspeed/acousticSpeed;
timeCurve = [XX]'; %seconds
thrustCurve = [XX]'; % Newtons
massCurve = [XX]'; % kg
machCurve = [XX];
dragCurve = [XX];
machCurveB = [XX];
dragCurveB = [XX];
if(Mt<=timeCurve(end-1))
Cd = interp1(machCurveB,dragCurveB,mach,'linear','extrap');
else
Cd = interp1(machCurve,dragCurve,mach,'linear','extrap');
end
mass = interp1(timeCurve,massCurve,Mt,'linear','extrap');
thrust = interp1(timeCurve,thrustCurve,Mt,'linear','extrap');
%Compute acceleration --------------------------------------------
if Mspeed<=20
step = 0;
else
step = 1;
end
dragAccel = -0.5*rho*Cd*S*missileState(4:6)*norm(missileState(4:6))/mass;% drag acceleration in m/sec^2
thrustAccel = thrust*missileState(4:6)/(mass*(norm(missileState(4:6))));
guideAccel = getGuidance(Mt,missileState,targetState);
accel = dragAccel + thrustAccel + gravity*step + guideAccel; % total acceleration
MxDot = [missileState(4:6);accel];
end

34
code/T3dxdt.m Normal file
View File

@@ -0,0 +1,34 @@
function TxDot = T3dxdt(t,x)
%% Threat is a 240 mm Rocket
% Compute Rocket xDot = [velocity; acceleration]'
g = 9.8066; % acceleration due to gravity
gravity = [0; 0; -g]; % gravity vector
diameter = 0.24; % missile diameter in m.
S = XX; % reference area in m^2
[acousticSpeed,rho] = getRho(x);
mach = norm(x(4:6))/acousticSpeed;
timeCurve = [XX]'; %seconds
thrustCurve = [XX]'; % Newtons
massCurve = [XX]'; % kg
machCurve = [XX];
dragCurve = [XX];
machCurveB = [XX];
dragCurveB = [XX];
if(t<=timeCurve(end-1))
Cd = interp1(machCurveB,dragCurveB,mach,'linear','extrap');
else
Cd = interp1(machCurve,dragCurve,mach,'linear','extrap');
end
mass = interp1(timeCurve,massCurve,t,'linear','extrap');
thrust = interp1(timeCurve,thrustCurve,t,'linear','extrap');
%Compute acceleration --------------------------------------------
dragAccel = -0.5*rho*Cd*S*x(4:6)*norm(x(4:6))/mass;% drag acceleration in m/sec^2
thrustAccel = thrust*x(4:6)/(mass*(norm(x(4:6))));
accel = dragAccel + thrustAccel + gravity; % total acceleration
TxDot = [x(4:6);accel];
end

41
code/getGuidance.m Normal file
View File

@@ -0,0 +1,41 @@
function [guideAccel] = getGuidance(Mt,missileState,targetState)
% This function computes the acceleration command to the missile required to
% intercept a target
% Compute proportional navigation (PN) guidance command
PNGain = 0; % PN guidance gain
KVP = XX;
blindRng = XX; %Seeker blind range (m)
acqRng = XX; % Acquisition Range (m)
g = 9.8066; % Acceleration due to gravity (m/sec^2)
gLimit = XX.; % Lateral acceleration limit in gees
relState = targetState' - missileState;
relRng = relState(1:3);
relVel = relState(4:6);
mslVel = missileState(4:6);
% Velocity pursuit guidance
if Mt>XX
nHat = cross(mslVel,relRng);
accelCmdVP = KVP*cross(nHat,mslVel/norm(mslVel))/norm(relRng);
else
accelCmdVP = [0; 0; 0];
end
if norm(accelCmdVP)>gLimit*g
accelCmdVP = gLimit*g*accelCmdVP/norm(accelCmdVP);
end
% LOSR is line-of-sight rate
LOSR = cross(relRng,relVel)/dot(relRng,relRng);
if norm(relRng)<acqRng
PNGain = XX;
accelCmdVP = [0; 0; 0];
end
%accelCmd = -PNGain*norm(relVel)*cross(relRng/norm(relRng),LOSR);
accelCmd = -PNGain*norm(relVel)*cross(mslVel/norm(mslVel),LOSR);
if norm(accelCmd)>gLimit*g
accelCmd = gLimit*g*accelCmd/norm(accelCmd);
end
guideAccel = accelCmd + accelCmdVP;
disp(norm(accelCmdVP));
if norm(relRng)<blindRng
guideAccel = [0;0;0];
end
end

7
code/getMD.m Normal file
View File

@@ -0,0 +1,7 @@
function missDist = getMD(vClosing,simTime,relRange)
%Algorithm to compute PCA (point of closest approach).
delVc = vClosing(end)-vClosing(end-1);
delT = simTime(end)-simTime(end-1);
tMiss = -vClosing(end-1)*(delT/delVc);
missDist = abs(relRange(end-1)+vClosing(end-1)*tMiss);
end