Introduction to Navigation and Guidance

(AE641, AE410)

Group 11 - Project Report

Sarthak Mehrotra - 2000100068

Kidron M. - 24M0019
Punyamurthy Jashwanth - 24M0018
Rakesh Kumar Gaula - 24M0060

Under the guidance of

Prof. Arnab Maity
1 Cubature Kalman Filter (Question 1)
This report describes the implementation of an Extended Kalman Filter (EKF) for tracking
an aircraft in 3D space. The system models the aircraft’s dynamics, simulates measurements,
and predicts the aircraft’s state (position, velocity, and turn rate) over time using noisy
measurements. The EKF corrects its predictions iteratively using the state and measurement
Jacobians derived from the system dynamics.

1.1 State Transition and Measurement Models

1.1.1 State Vector

The state vector x is defined as:

x = [x1 , x2 , x3 , x4 , x5 , x6 , x7 ]T

where:

• x1 , x2 , x3 : Position in Cartesian coordinates (m),

• x4 , x5 , x6 : Velocity components (m/s),

• x7 : Turn rate (rad/s).

1.1.2 State Transition Model

The state transition function models the evolution of the aircraft’s state:
 
x2
−x4 x7 
 
 
 x4 
 
f (x) =  x2 x7 
 
 
 x6 
 
 0 
 

1.1.3 Measurement Model

The measurement model provides observations of the range and elevation:

#  p 2 2 2

x + x + x
"
range 1 2 3
h(x) = =
 
x
elevation arctan √ 2 2 3
x1 +x2

1
1.1.4 Jacobians

The EKF requires the linearization of the state transition and measurement models:
∂f
A=
∂x
∂h
H=
∂x
These Jacobians were calculated symbolically and implemented as MATLAB functions
for efficient computation.

1.2 Simulation Parameters

1.3 True State and Measurement Calculation

The true state and measurements are calculated at each time step as follows:

1.3.1 True State Calculation

The true state xtrue is updated using the state transition model and process noise:
xtrue,k = xtrue,k−1 + ∆t · f (xtrue,k−1 ) + wk
where:
• f (x) is the state transition function,

• wk ∼ N (0, Q) is the process noise.

After applying the noise, constraints are enforced to ensure the velocity magnitude and
the turn rate remains constant as mentioned in the problem:
x4:6
q
v = x24 + x25 + x26 , x4:6 = · vdesired , x7 = turn rate.
v

1.3.2 Measurement Calculation

The true measurement ztrue is derived from the measurement model:

 p 
x21 + x
 2
2
+ x 2
3
ztrue =  
arctan √ x23 2
x1 +x2

Noisy measurements z are obtained by adding measurement noise:

zk = ztrue,k + vk , vk ∼ N (0, R),
where:
• vk is the measurement noise.

2
1.3.3 Process Summary

The complete process for the true state and measurement generation is:

1. Propagate the true state using the state transition model:

xtrue,k = xtrue,k−1 + ∆t · f (xtrue,k−1 ).

2. Add process noise:

xtrue,k ← xtrue,k + wk .

3. Enforce velocity and turn rate constraints:

x4:6
x4:6 ← · vdesired , x7 = turn rate.
∥x4:6 ∥
4. Compute the true measurement:

ztrue,k = h(xtrue,k ).

5. Add measurement noise:

zk = ztrue,k + vk .

1.3.4 Noise Covariances

• Process noise covariance matrix:

Q = diag([0, σ12 , 0, σ12 , 0, σ12 , σ22 ])

• Measurement noise covariance matrix:

R = diag([σr2 , σϕ2 ])

Values used:
σ1 = 0.2, σ2 = 7 × 10−3 , σr = 50, σϕ = 0.1

1.3.5 Initialization

• Initial true state:

xtrue (0) = [1000, 0, 2650, 150, 200, 0, 3]T

• Initial estimate:
xest (0) = xtrue (0) + Gaussian noise

• Initial covariance matrix P:

P = eye(7)

3
1.4 Extended Kalman Filter Algorithm
The EKF iteratively updates the state and covariance estimates using the following steps:

1.4.1 Time Update (Prediction)

• Predicted state:
xpred = xest + ∆t · f (xest )

• Predicted covariance:
Ppred = FPFT + Q

where F = I + ∆t · A.

1.4.2 Measurement Update (Correction)

• Measurement residual:
y = z − h(xpred )

• Kalman gain:
K = Ppred HT (HPpred HT + R)−1

• Updated state:
xest = xpred + Ky

• Updated covariance:
P = (I − KH)Ppred

1.5 Results
1.5.1 Root Mean Square Errors (RMSE)

• Position RMSE:
v
u
u1 X N
RMSEpos = t (xtrue, pos (k) − xest, pos (k))2
N k=1

• Velocity RMSE:
v
u
u1 X N
RMSEvel = t (xtrue, vel (k) − xest, vel (k))2
N k=1

4
 • Turn rate RMSE:
v
u
u1 X N
RMSEturn =t (xtrue, turn (k) − xest, turn (k))2
N k=1

Table 1: Root Mean Square Errors (RMSE) for Position, Velocity, and Turn Rate

Metric Axis/Component RMSE Value

X1 75.87
Position (m) X2 48.14
X3 16.86

X4 53.2955
Velocity (m/s) X5 33.8173
X6 6.7151

Turn Rate (rad/s) X7 0.0050

1.6 Conclusion
The EKF effectively tracked the aircraft’s position, velocity, and turn rate, with errors
mitigated over time despite noise in measurements and process dynamics. The simulation
demonstrates the robustness of the EKF for nonlinear systems like aircraft tracking.

2 Navigation laws (Question 2)

2.1 Pure Pursuit Guidance

2.1.1 Engagement Equations

Ṙ = VR = VT cos(αT − θ) − VM cos(αM − θ) (1)

Rθ̇ = Vθ = VT sin(αT − θ) − VM sin(αM − θ) (2)

For Pure Pursuit, where the missile points towards the target:

αM = θ

5
 Figure 1: Comparison of Position States

The engagement equations simplify to:

Ṙ = VR = VT cos(αT − θ) − VM (3)
Rθ̇ = Vθ = VT sin(αT − θ) (4)

2.1.2 Trajectory in the (Vθ , VR )-Space

The equations can be rearranged as:

VR + VM = VT cos(αT − θ) (5)
Vθ = VT sin(αT − θ) (6)

Squaring both equations yields:

(VR + VM )2 + Vθ2 = VT2

This represents a circle with radius VT and centered at (0, −VM ).

6
 Figure 2: Comparison of Velocity States and Turn Rate

2.1.3 Observations

• Intersubsection points on the VR -axis are stationary points where:

Vθ = 0 and V̇ R = V̇ θ = 0

• In the tail-chase mode, collision occurs if VM > VT .

• In the head-on mode, collision is possible for all VT and VM .

2.1.4 Time of Interception

R(Ṙ + 2VM ) = (VT2 − VM2 )t + b, b = R0 (Ṙ0 + 2VM ) (7)

7
 Figure 3: Trajectory of the aircraft in 3D

Figure 4: Speed comparison

At interception, R = 0, so the time of interception is:

b R0 (Ṙ0 + 2VM )
tf = =
VM2 − VT2
VM2 − VT2

8
2.1.5 Simulation

1. Initialization of Parameters

• Initial Conditions:
– Initial range between missile and target: R0 = 20, 000 m
π
– Initial Line-of-Sight (LOS) angle: θ0 = 30° = 6
radians
2π
– Target flight path angle: θT = 120° = 3
radians
• Speeds:
– Missile speed: VM = 800 m/s
– Target speed: VT = 400 m/s
• Simulation Parameters:
– Time increment: ∆t = 0.0001 s
– Total simulation duration: T = 100 s
– Time array: t ∈ [0, T ] with step size ∆t

2. Initializing Variables:

• Current range: R = R0 = 20, 000 m

• Current LOS angle: θ = θ0 = π
6
radians
• Missile flight angle: αM = θ0 = π
6
radians
• Missile initial position: (xM , yM ) = (0, 0)
• Target initial position:

(xT , yT ) = (R0 cos(θ0 ), R0 sin(θ0 )) = (17320.5, 10000) m

3. Simulation Loop:

(a) At each time step:

• Calculate relative velocity components:

VR = VT cos(θT − θ) − VM cos(αM − θ)

Vθ = VT sin(θT − θ) − VM sin(αM − θ)
• Update range and LOS angle:

R ← R + VR · ∆t
Vθ
θ←θ+ · ∆t
R
9
 • Update missile and target positions:

xM ← xM + VM cos(αM ) · ∆t, yM ← yM + VM sin(αM ) · ∆t

xT ← xT + VT cos(θT ) · ∆t, yT ← yT + VT sin(θT ) · ∆t

• Update missile flight path angle: αM ← θ

(b) Terminate the loop if the missile intercepts the target (R ≤ 1 m).

Figure 5: Interception paths for Pure Pursuit

2.1.6 Results

Fig 5 depicts a pure pursuit scenario where a missile (red path) intercepts a target (blue
path). The missile starts at the origin and continuously adjusts its trajectory to aim directly
at the moving target, which travels along a straight line. The curved missile path reflects
its efforts to pursue and intercept the target’s motion.
Fig 6 illustrates the missile’s flight path angle (αM ) over time in a pure pursuit scenario.
The steadily increasing trend represents the missile’s continuous adjustment of its trajectory
to align with the moving target, leading to a nonlinear escalation of the angle as it approaches
the target.

10
 Figure 6: Flight Path Angle for Pure Pursuit

Figure 7: Relative velocity for Pure Pursuit

Fig 7 shows the relative velocity components (Vθ vs VR ) during pure pursuit guidance.
The missile continuously points toward the target, causing the tangential velocity (Vθ ) to
increase and the radial velocity (VR ) to decrease (more negative) as the missile closes in.
The graph in Fig 8 demonstrates successful intercept with range decreasing linearly from
20km to 0km in 33.33 seconds.

11
 Figure 8: Range variation with time for Pure Pursuit

2.2 Proportional Navigation Guidance Law

Definition: Proportional Navigation keeps the line-of-sight (LOS) Proportional to the ini-
tial LOS, ensuring the angular rate of LOS ω = 0.

2.2.1 Key Characteristics

• Proportional navigation is optimal for non-maneuvering targets.

• Acceleration is perpendicular to the missile velocity vector and hence the missile ve-
locity remains constant.

• Ineffective for time-varying speed targets.

2.2.2 Implementation of Proportional Navigation Using PN Guidance

• PN guidance generates a guidance command to ensure that the rate of rotation of the
missile velocity vector is proportional to the LOS rate:

α̇M = N θ̇

where N is the navigation constant.

12
2.2.3 Simulation

1. Initialization of Parameters

• Initial Conditions:
– Initial range between missile and target: R0 = 20, 000 m
π
– Initial Line-of-Sight (LOS) angle: θ0 = 30° = 6
radians
2π
– Target flight path angle: θT = 120° = 3
radians
• Speeds:
– Missile speed: VM = 800 m/s
– Target speed: VT = 400 m/s
• Simulation Parameters:
– Navigation Constant: N = 1
– Time increment: ∆t = 0.01 s
– Total simulation duration: T = 100 s
– Time array: t ∈ [0, T ] with step size ∆t

2. Initializing Variables:

• Current range: R = R0 = 20, 000 m

• Current LOS angle: θ = θ0 = π
6
radians
• Missile flight angle: αM = θ0 = π
6
radians
• Missile initial position: (xM , yM ) = (0, 0)
• Target initial position:

(xT , yT ) = (R0 cos(θ0 ), R0 sin(θ0 )) = (17320.5, 10000) m

3. Simulation Loop: For each time step:

(a) Compute relative velocity components:

VR = vT cos(γT − θ) − vM cos(αM − θ),

Vθ = vT sin(γT − θ) − vM sin(αM − θ).

(b) Update the range and LOS angle:

R = R + VR · ∆t,
Vθ
θ̇ = ,
R
θ = θ + θ̇ · ∆t.

13
 (c) Update the missile angle based on PN guidance:

α̇M = N · θ̇, αM = αM + α̇M · ∆t.

(d) Compute new positions for the missile and target:

(xM , yM ) = (xM + vM cos(αM ) · ∆t, yM + vM sin(αM ) · ∆t),

(xT , yT ) = (xT + vT cos(γT ) · ∆t, yT + vT sin(γT ) · ∆t).

(e) Stop the simulation if R ≤ 1 m.

Figure 9: Interception Path for Proportional Navigation

2.2.4 Results

Fig 9 depicts a proportional navigation scenario where a missile (red path) intercepts a target
(blue path). The missile starts at the origin and continuously adjusts its trajectory to aim
directly at the moving target, which travels along a straight line. In this type of navigation,
the acceleration is perpendicular to the velocity vector.
In Fig 10, the decreasing lateral acceleration demonstrates that the missile makes sig-
nificant initial course corrections, which taper off as the missile nears the target and the
trajectory stabilizes.

14
 Figure 10: Lateral Acceleration for Proportional Navigation

Figure 11: Flight path angle for Proportional Navigation

In Fig 11, the increasing flight path angle indicates that the missile gradually aligns its
heading with the target, reflecting the adaptive corrections prescribed by the PN law.

15
 Figure 12: Range variation with time for Proportional Navigation

Figure 13: Relative Velocity trajectory for Proportional Navigation

In Fig 12, the consistent reduction in the missile-target range reflects the effectiveness of
PN, where the missile adjusts its trajectory to minimize the relative distance to the target.
The missile intercepts the target in 29.8 seconds.

16
 In Fig 13, this trajectory shows how PN maintains proportional corrections to ensure the
missile’s velocity vector converges toward the target’s position, minimizing miss distance.

2.3 True Proportional Navigation

Definition: TPN guidance is characterized by the commanded missile lateral acceleration
being proportional to the line-of-sight (LOS) angular rate and applied normal to the LOS:

aM = cθ̇, c>0

where aM is the missile lateral acceleration, θ̇ is the LOS angular rate, and c is a propor-
tionality constant.
For the target to be non-maneuvering with constant speed, the equations of motion are:

2.3.1 Relative Kinematics

Ṙ = Vr = VT cos(αT − θ) − VM cos(αM − θ)

Rθ̇ = Vθ = VT sin(αT − θ) − VM sin(αM − θ)

aM cos(αM − θ)
V̇M = aM sin(αM − θ), α̇M =
VM
Missile velocity VM is a time-varying quantity because aM is not normal to VM .

2.3.2 Differentiating the Velocity Components

For Vr :

V̇r = −VT sin(αT − θ)(−θ̇) + VM sin(αM − θ)(α̇M − θ̇) − V̇M cos(αM − θ)

= θ̇Vθ + aM sin(αM − θ) cos(αM − θ) − aM sin(αM − θ) cos(αM − θ)

= θ̇Vθ

For Vθ :

V̇θ = VT cos(αT − θ)(−θ̇) − VM cos(αM − θ)(α̇M − θ̇) − V̇M sin(αM − θ)

= −θ̇Vr − aM cos2 (αM − θ) − aM sin2 (αM − θ)

= −θ̇Vr − aM

17
2.3.3 Key Relations

V̇ r = θ̇V θ

V̇θ = −θ̇Vr − aM

V̇θ = −θ̇(Vr + c)

2.3.4 Combined Equation

Using these equations:

V̇ θV θ + V̇r Vr + cV̇r = 0

On integration:
Vθ2 + Vr2 + 2cVr = k

Rearranging:
Vθ2 + (Vr + c)2 = k + c2

2.3.5 Capture Equation and Conditions

The capture condition is given by:

Vθ2 + Vr2 + 2cVr < 0

Alternatively:
√
|Vθ | < 2N − 1|Vr |

For the capture circle to exist, the navigation constant must satisfy:
1
N>
2

2.3.6 Time of Interception

The time of interception is determined by:

RVr + 2cR = kt + b, b = R0 (Vr0 + 2c)

At interception, R = 0:

b R0 (Vr0 + 2c)
tf = − =− 2
k Vθ + Vr2 + 2cVr

18
2.3.7 Characteristics of TPN Guidance

• TPN assumes the target is non-maneuvering and has a constant speed.

• Missile lateral acceleration is proportional to the LOS rate.

• TPN is effective for non-maneuvering targets but struggles with maneuvering targets
or time-varying speeds.

2.3.8 Capture Regions

The capture regions are defined by the following:

Vθ2 + Vr2 + 2cVr < 0

√
|Vθ | < 2N − 1|Vr |

The capture region shrinks with smaller N values and expands as N increases beyond 1.

2.3.9 Simulation

1. Initialization of Parameters

• Initial Conditions:
– Initial range between missile and target: R0 = 20, 000 m
π
– Initial Line-of-Sight (LOS) angle: θ0 = 30° = 6
radians
2π
– Target flight path angle: θT = 120° = 3
radians
• Speeds:
– Missile speed: VM = 800 m/s
– Target speed: VT = 400 m/s
• Simulation Parameters:
– Navigation Constant: N = 3
– Time increment: ∆t = 0.001 s
– Total simulation duration: T = 100 s
– Time array: t ∈ [0, T ] with step size ∆t

2. Initializing Variables:

• Current range: R = R0 = 20, 000 m

19
 • Current LOS angle: θ = θ0 = π
6
radians
• Missile flight angle: αM = θ0 = π
6
radians
• Missile initial position: (xM , yM ) = (0, 0)
• Target initial position:

(xT , yT ) = (R0 cos(θ0 ), R0 sin(θ0 )) = (17320.5, 10000) m

3. Simulation Loop: The simulation loop iterates over the time vector. At each step:

(a) Range and Line-of-Sight (LOS) Angle:

p
R= (xt − xm )2 + (yt − ym )2 , θ = arctan 2(yt − ym , xt − xm ).

(b) LOS Rate:

Vt sin(γt − θ) − Vm sin(γm − θ)
θ̇ = .
R
(c) TPN Guidance Law:
am = N · Ṙ0 · θ̇.

This acceleration is decomposed into:

atangential = am sin(γm − θ), alateral = am cos(γm − θ).

(d) Missile Dynamics: Update the missile’s speed, heading, and position using:

Vm ← Vm + atangential · ∆t,
alateral
γ̇m = , γm ← γm + γ̇m · ∆t,
Vm
xm ← xm + Vm cos(γm ) · ∆t,
ym ← ym + Vm sin(γm ) · ∆t.

(e) Target Dynamics: Update target position using:

xt ← xt + Vt cos(γt ) · ∆t, yt ← yt + Vt sin(γt ) · ∆t.

(f) Intercept Condition: Terminate if R ≤ 1 m.

20
 Figure 14: Interception Path for True Proportional Navigation

Figure 15: Lateral Acceleration for True Proportional Navigation

2.3.10 Results

Fig 14 compares the trajectories of the missile (red) and the target (blue). Both start at
distinct positions, with the missile initially behind the target. Under the TPN guidance law:
21
 Figure 16: Flight path angle for True Proportional Navigation

Figure 17: Range variation with time for True Proportional Navigation

• The missile adjusts its path to intercept the target, leading to a curved trajectory.

• The intercept occurs as the missile accurately predicts and compensates for the target’s

22
 Figure 18: Relative Velocity trajectory for True Proportional Navigation

motion.

Fig 15 shows the lateral acceleration of the missile over time. The lateral acceleration
starts at a high value of 50 m/s2 and gradually decreases to 0 m/s2 . This behavior is typical of
the True Proportional Navigation (TPN) guidance law, where the missile adjusts its lateral
acceleration to align with the target’s motion, reducing the acceleration as it converges
towards the target.
Fig 16 depicts the missile’s flight path angle (αM ) gradually increasing from 30° to
70°, showing how the missile adjusts its heading to achieve intercept.
Fig 17 shows the range between the missile and target decreasing linearly from 20
km to 0 km over 26.32 seconds, indicating effective closure rate control.
Fig 18 represents the relative velocity trajectory in Vθ (transverse) vs VR (radial)
space, with the diagonal line indicating coordinated changes in both velocity components to
maintain an optimal interception course.

23