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

Implement atmosphere and dynamics data

Browse Source
This commit is contained in:
deamonkai
2026-03-28 16:33:32 -05:00
parent 563dbee7eb
commit c0f275caeb
9 changed files with 127 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
code/Interceptor_3DOF.m
View File

@@ -1,8 +1,8 @@
%% * Interceptor 3DOF*
%% Executive
clear all; clc;
t0 = 0; % Initial time. Target launch time.
tCommit = XX.; % Commit time (sec)
t0 = 0; % Initial time. Target launch time.
tCommit = 50.; % Commit time (sec)
sigEl = 0; % Missile elevation angle standard deviation
maxTime = 150; % estimated maximum engagement time in sec
dt = 0.01; % integration interval (sec)
@@ -21,9 +21,10 @@ 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
Mspeed0 = 1.e-06;% Missile initial speed in ENU frame (m/sec); must be non-zero for unit vector calculation.
Maz = 0; % Assumed coplanar engagement geometry until a source doc says otherwise.
Mel = 40; % Missile launch 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))];

33
code/M3dxdt.m
View File

@@ -1,22 +1,23 @@
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];
g = 9.8066; % acceleration due to gravity
gravity = [0; 0; -g]; % gravity vector
diameter = 0.120; % missile diameter in m.
S = pi*(diameter/2)^2; % reference area in m^2
[acousticSpeed,rho] = getRho(missileState);
Mspeed = norm(missileState(4:6));
mach = Mspeed/acousticSpeed;
% Transcribed from the provided missile-data plots.
timeCurve = [0 2.8 2.8001 11.6 11.6001 25]'; %seconds
thrustCurve = [6200 6200 1200 1200 0 0]'; % Newtons
massCurve = [31.8 25.1 25.1 20.9 20.9 20.9]'; % kg
machCurve = [0 0.5 0.7 0.9 1.0 1.05 1.1 1.2 1.5 2.0 2.5 3.0];
dragCurve = [0.185 0.180 0.220 0.280 0.330 0.360 0.355 0.330 0.250 0.165 0.110 0.090];
machCurveB = [0 0.5 0.7 0.8 0.9 1.0 1.05 1.1 1.2 1.5 2.0 2.5 3.0];
dragCurveB = [0.150 0.145 0.145 0.160 0.200 0.300 0.315 0.305 0.275 0.205 0.130 0.080 0.045];
if(Mt<=timeCurve(end-1))
Cd = interp1(machCurveB,dragCurveB,mach,'linear','extrap');

35
code/T3dxdt.m
View File

@@ -1,23 +1,24 @@
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];
g = 9.8066; % acceleration due to gravity
gravity = [0; 0; -g]; % gravity vector
diameter = 0.24; % missile diameter in m.
S = pi*(diameter/2)^2; % reference area in m^2
[acousticSpeed,rho] = getRho(x);
mach = norm(x(4:6))/acousticSpeed;
% Transcribed from the provided target-data plots.
timeCurve = [0 7.5 7.5001 45]'; %seconds
thrustCurve = [77000 77000 0 0]'; % Newtons
massCurve = [410 210 210 210]'; % kg
machCurve = [0 0.5 0.7 0.9 1.0 1.1 1.2 1.3 1.5 2.0 2.5 3.0 3.5 4.0];
dragCurve = [0.40 0.40 0.43 0.60 0.75 0.87 0.86 0.80 0.74 0.60 0.50 0.44 0.40 0.39];
machCurveB = [0 0.5 0.7 0.9 1.0 1.1 1.2 1.3 1.5 2.0 2.5 3.0 3.5 4.0];
dragCurveB = [0.31 0.31 0.34 0.47 0.62 0.78 0.77 0.72 0.64 0.50 0.40 0.34 0.30 0.27];
if(t<=timeCurve(end-1))
Cd = interp1(machCurveB,dragCurveB,mach,'linear','extrap');

71
code/getGuidance.m
View File

@@ -1,41 +1,44 @@
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
function [guideAccel] = getGuidance(Mt,missileState,targetState)
% This function computes the acceleration command to the missile required to
% intercept a target
missileState = missileState(:);
targetState = targetState(:);
% Compute proportional navigation (PN) guidance command
PNGain = 0; % PN guidance gain
terminalPNGain = 4;
KVP = 0.5;
blindRng = 2; %Seeker blind range (m)
acqRng = 5000; % Acquisition Range (m)
g = 9.8066; % Acceleration due to gravity (m/sec^2)
gLimit = 40.; % 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>0.5
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);
% LOSR is line-of-sight rate
LOSR = cross(relRng,relVel)/dot(relRng,relRng);
if norm(relRng)<acqRng
PNGain = terminalPNGain;
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
end
guideAccel = accelCmd + accelCmdVP;
if norm(relRng)<blindRng
guideAccel = [0;0;0];
end
end

24
code/getRho.m Normal file
View File

@@ -0,0 +1,24 @@
function [acousticSpeed,rho] = getRho(state)
% Compute atmospheric properties from mean sea level altitude.
g = 9.8066; % m/sec^2
lapserate = -0.0065; % K/m
R = 287.05; % J/kg-K
gamma = 1.4; % adiabatic constant for air
z0 = 0; % mean sea level reference altitude (m)
T0 = 288.15; % sea level temperature (K)
P0 = 1.01325e5; % sea level pressure (Pa)
z = state(3); % altitude above mean sea level (m)
if z <= 11000
T = T0 + lapserate*(z-z0);
P = P0*(T/T0)^(-g/(lapserate*R));
else
T = T0 + lapserate*(11000-z0);
P11k = P0*(T/T0)^(-g/(lapserate*R));
P = P11k*exp(-g*(z-11000)/(R*T));
end
rho = P/(R*T);
acousticSpeed = sqrt(gamma*R*T);
end