41 lines
1.7 KiB
Matlab
41 lines
1.7 KiB
Matlab
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 = 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');
|
|
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
|