-
Notifications
You must be signed in to change notification settings - Fork 0
/
navigation.py
66 lines (54 loc) · 2.57 KB
/
navigation.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
from objects import MovingObject, PursuitObject
import numpy as np
from numpy.linalg import norm
# All these functions return the rotation degree(or rads) per tick for the pursuit object
def pure_pursuit(pursuit_obj: PursuitObject, target_obj: MovingObject):
"""
Pure-Pursuit Homing
A_cmd = LOS
Missile velocity vector points at the target alongside LOS
"""
los_vector = (target_obj.location_arr - pursuit_obj.location_arr)
los_angle = pursuit_obj.speed_vector.anglebetween(los_vector) # los angle
if np.dot(pursuit_obj.speed_vector.rotate(deg=-90, inplace=False), los_vector) > 0:
los_angle = -los_angle
return los_angle
def simple_proportional_navigation(pursuit_obj: PursuitObject, target_obj: MovingObject, N=3):
"""
Simple Proportional Navigation
A_cmd = N * theta_LOS
N: Navigation Gain (proportional constant normally 3~5)
theta_LOS: LOS Rotation Rate
Missile velocity vector rotation matches LOS rate
Acceleration is commanded normal to missile velocity vector
"""
a_cmd_rad = N * pursuit_obj.calculate_los_rate(target_obj)
return np.rad2deg(a_cmd_rad)
def true_proportional_navigation(pursuit_obj: PursuitObject, target_obj: MovingObject, N=3):
"""
True Proportional Navigation
A_cmd = N * V_c * theta_LOS
N: Navigation Game
V_c: Range Closing Rate
theta_LOS: LOS Rotation Rate
"""
a_cmd_rad = N * pursuit_obj.calculate_closing_speed(target_obj) * pursuit_obj.calculate_los_rate(target_obj)
return np.rad2deg(a_cmd_rad)
def augumented_proportional_navigation(pursuit_obj: PursuitObject, target_obj: MovingObject, N=3):
"""
Augumented Proportional Navigation
A_cmd = N * V_c * theta_LOS + (N * n_T)/2
N: Navigation Game
V_c: Range Closing Rate
theta_LOS: LOS Rotation Rate
n_T: Target Acceleration rate normal to LOS
"""
dt = 1 # los difference time (1 tick)
old_los_vector = pursuit_obj.previous_target_location_arr - pursuit_obj.previous_pursuit_location_arr
old_normal_velocity = np.dot(pursuit_obj.previous_target_velocity_arr, old_los_vector) / norm(old_los_vector)
new_los_vector = target_obj.location_arr - pursuit_obj.location_arr
new_normal_velocity = np.dot(target_obj.speed_vector.array, new_los_vector) / norm(new_los_vector)
normal_acceleration_rate = (new_normal_velocity - old_normal_velocity) / dt
print(normal_acceleration_rate)
a_cmd_rad = N * pursuit_obj.calculate_closing_speed(target_obj) * pursuit_obj.calculate_los_rate(target_obj) + (N * normal_acceleration_rate) / 2
return np.rad2deg(a_cmd_rad)