Initialize Hayden project repository

code/getGuidance.m

@@ -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