41 lines
1.3 KiB
Matlab
41 lines
1.3 KiB
Matlab
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 |