How to implement proportional guidance in 2D

Question

I am trying to implement proportional navigation in order to create a missile class that receives the target location every frame update and intercepts it accordingly.

I would like the missile to accelerate in the direction it is facing with the idea that it has a rocket motor pushing in forward. The guidance algorithm then has to rotate it like the stabilization fins on an actual missile.

I managed to get simpler algorithms working but was unhappy with the results which is why I am trying this algorithm (so please do not suggest other ones unless they achieve similar results to this; I am choosing this purely off of aesthetic reasons).

The issue is the missile does turn towards the target but far too slowly to intercept it.

I looked at a few resources but if it helps I followed this example to the best of my abilities. I am working in the Processing Development Environment.

Main loop:

Missile missile;
Target target;

void setup()
{
  fullScreen();
  
  missile = new Missile();
  target = new Target();
}

void draw()
{
  background(0);
  
  target.Update();
  missile.Update(target);
}

Target class:

class Target
{
  PVector Location = new PVector();
  PVector Velocity = new PVector();
  PVector Acceleration = new PVector();
  float Speed = 100;
  float DeltaT = 0.0166;
  
  Target()
  {
    Location.set(width - 15,height - 15);
    Velocity.set(-Speed * DeltaT,0);
    Acceleration.set(0,0);
  }
  
  void Update()
  {
    Location.add(Velocity);
    
    Draw();
  }
  
  void Draw()
  {
    stroke(255,0,0);
    fill(255,0,0);
    ellipse(Location.x,Location.y,10,10);
  }
}

Missile class:

class Missile
{
  PVector LocationC = new PVector();    //***---Current Location
  PVector LocationP = new PVector();    //***---Previous Location
  PVector Velocity = new PVector();
  PVector Acceleration = new PVector();
  
  PVector TargetLocC = new PVector(0,0);  //***---Current Target Location
  PVector TargetLocP = new PVector(0,0);  //***---Previous Target Location
  PVector TargetVelC = new PVector(0,0);  //***---Current Target Velocity
  PVector TargetVelP = new PVector(0,0);  //***---Previous Target Velocity

  PVector RTM_C = new PVector();    //***---Current Vector to target
  PVector RTM_P = new PVector();    //***---Previous Vector to target
  PVector LOS_Delta = new PVector();
  float LOS_Rate = 0;
  float VC = 0;     //***---CLosing Velocity
  float N = 5;     //***--- Navigation Gain
  float DeltaT = 0.0166;
  
  PVector Commanded_Accel = new PVector(0,0.1);
  
  Missile()
  {
    LocationC.set(width/2,10);
    Velocity.set(0,0.1);
    Acceleration.set(0,0.1);
  }
  
  void Update(Target target)
  {
    UpdateCurrentValues(target);
    ProportionalGuidence();
    UpdatePreviousValues();
    Draw();
    //println(degrees(Velocity.heading()));
  }
   
  void ProportionalGuidence()
  {    
    //***---Get missile target distances of previous frame and new frame
      RTM_P = TargetLocP.copy().sub(LocationP.copy());
      RTM_C = TargetLocC.copy().sub(LocationC.copy());
      
      //***---Normalise Target vectors
      RTM_P.normalize();
      RTM_C.normalize();      
      
      //***--Calculate LOS Rate
      LOS_Delta = RTM_C.copy().sub(RTM_P.copy());
      if(frameCount > 1)
      {
        PVector crossProduct = RTM_P.cross(RTM_C);
        LOS_Rate = crossProduct.z/(RTM_P.mag() * RTM_C.mag());
      }
      else
      {
        LOS_Rate = 0;
      }
      
      //***---Calculate closure rate
      VC = RTM_C.mag()-RTM_P.mag();
      
      //***---Calculate acceleration
      Commanded_Accel = RTM_C.mult(N * VC * LOS_Rate + (0.5*N));
      Acceleration = Commanded_Accel.mult(DeltaT);
  }
  
  void UpdateCurrentValues(Target target)
  {
    Velocity.add(Acceleration);
    LocationC.add(Velocity);
    
    TargetLocC = target.Location;
    TargetVelC = target.Velocity;
  }
  
  void UpdatePreviousValues()
  {
    LocationP = LocationC.copy();
    TargetLocP = TargetLocC.copy();
    TargetVelP = TargetVelC.copy();
  }
  
  void Draw()
  {    
    stroke(255);
    fill(255);
    ellipse(LocationC.x, LocationC.y,5,5);
  }
}

Answer 1

I managed to figure out why my code wasn't working. I had another look at the math in the example I mentioned in my question. Because my initial attempt at the example did not work I consulted another source from which I gained the insight to add this snippet of code in my missile class:

        PVector crossProduct = RTM_P.cross(RTM_C);
        LOS_Rate = crossProduct.z/(RTM_P.mag() * RTM_C.mag());

While looking at the example I saw I made a mistake in the calculation of Commanded_Accel. I fixed my function and the result was a missile which was able to successfully intercept a target. The updated class code is below just in case it might be a benefit to someone in the future.

class Missile 
{
  PVector MissileLocC = new PVector();    //***---Missile current location
  PVector MissileLocP = new PVector();    //***---Missile previous location
  PVector MissileVel = new PVector();    //***---Missile velocity
  PVector MissileAcc = new PVector();    //***---Missile acceleration
  float Accel = 2;
  
  PVector TargetLocC = new PVector();    //***---Target current location
  PVector TargetLocP = new PVector();    //***---Target previous location
  
  PVector VectorTargetC = new PVector();  //***---Current vector to target
  PVector VectorTargetP = new PVector();  //***---Previous vector to target
  PVector LOS_Delta = new PVector(); 
  PVector Guided_Accel = new PVector(); 
  float LOS_Rate = 0;
  float N = 6;    //***---Proportional algorithm gain
  float Nt = 9.8;    //***---Arbitrary Nt value
  float Vc = 0;    //***--- Closure rate
  
  float DeltaT = 0.0166;   //***---1/framerate (60)
  
  boolean FirstFrame = true;  //***---Set false after first frame when previous values have been set.

  Missile() 
  {
    MissileLocC = new PVector(width-5,height-5);
    MissileVel = new PVector(0,-5);
    MissileLocP = MissileLocC.copy().sub(MissileVel.copy());
    MissileAcc = new PVector(0,Accel * DeltaT);
    
    TargetLocC = new PVector(0,0);
    TargetLocP = new PVector(0,0);
    
    LOS_Delta = new PVector(0,0);
  }

  void Update(Target target) 
  {
    UpdateCurrentValues(target); 
    ProportionalGuidence(target);
    UpdatePreviousValues();
    
    
    Draw();
  }

  void ProportionalGuidence(Target target) 
  {
    if(!FirstFrame)
    {
      VectorTargetC = TargetLocC.copy().sub(MissileLocC.copy());
      VectorTargetP = TargetLocP.copy().sub(MissileLocP.copy());
      
      VectorTargetC.normalize();
      VectorTargetP.normalize();
      
      if(VectorTargetP.mag() == 0)
      {
        LOS_Delta.set(0,0);
        LOS_Rate = 0;
      }
      else
      {
        LOS_Delta = VectorTargetC.copy().sub(VectorTargetP.copy());
        LOS_Rate = LOS_Delta.mag();
      }
      
      Vc = -LOS_Rate;
      
      Guided_Accel = VectorTargetC.copy().mult(N * Vc * LOS_Rate).add(LOS_Delta.mult(Nt*N*0.5)).add(MissileVel.copy().setMag(Accel*DeltaT));
    }
  }

  void UpdateCurrentValues(Target target) 
  {
    MissileAcc = Guided_Accel;
    MissileVel.add(MissileAcc);
    MissileLocC.add(MissileVel);
    
    TargetLocC = target.Location.copy();
  }

  void UpdatePreviousValues() 
  {    
    MissileLocP = MissileLocC.copy();
    TargetLocP = TargetLocC.copy();
    
    if(FirstFrame)
    {
      FirstFrame = false;
    }
  }

  void Draw() 
  {
    push();
    translate(MissileLocC.x, MissileLocC.y);
    rotate(MissileVel.heading());
    stroke(255);
    fill(255);
    ellipse(0, 0, 5, 5);
    pop();
  }
}