-
Notifications
You must be signed in to change notification settings - Fork 112
/
lamberts_method.py
149 lines (105 loc) · 3.7 KB
/
lamberts_method.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
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
""" Implements lambert's method for two topocentric radius vectors at different times. Supports both Earth-centered."""
import numpy as np
from poliastro.core.stumpff import c2, c3
from astropy import units as uts
from astropy import constants as cts
# declare astronomical constants in appropriate units
mu_Earth = cts.GM_earth.to(uts.Unit('km3 / s2')).value
def F_z_i(z, t, r1, r2, A):
""" Function F for Newton's method
:param z:
:param t:
:param r1:
:param r2:
:param A:
:return:
F: function
"""
mu = mu_Earth
C_z_i = c2(z)
S_z_i = c3(z)
y_z = r1 + r2 + A * (z * S_z_i - 1.0) / np.sqrt(C_z_i)
F = (y_z / C_z_i) ** 1.5 * S_z_i + A * np.sqrt(np.abs(y_z)) - np.sqrt(mu) * t
return F
def dFdz(z, r1, r2, A):
""" Derivative of Function F for Netwon's Method
:param z:
:param r1:
:param r2:
:param A:
:return:
df: derivative for Netwon's method.
"""
mu = mu_Earth
if z == 0:
C_z_i = c2(0)
S_z_i = c3(0)
y_0 = r1 + r2 + A * (0 * S_z_i - 1.0) / np.sqrt(C_z_i)
dF = np.sqrt(2) / 40.0 * y_0**1.5 + A / 8.0 * (np.sqrt(y_0) + A * np.sqrt(1 / 2 / y_0))
else:
C_z_i = c2(z)
S_z_i = c3(z)
y_z = r1 + r2 + A * (z * S_z_i - 1.0) / np.sqrt(C_z_i)
dF = (y_z / C_z_i)**1.5 * \
(1.0 / 2.0 / z * (C_z_i - 3.0 * S_z_i/ 2.0 / C_z_i) + 3.0 * S_z_i + 2.0 / 4.0 / C_z_i) +\
A / 8.0 * (3.0 * S_z_i / C_z_i * np.sqrt(y_z) + A * np.sqrt(C_z_i / y_z))
return dF
def lamberts_method(R1, R2, delta_time, trajectory_cw=False):
""" lamberts method that generates velocity vectors for each radius vectors that are put in here.
Similar to https://esa.github.io/pykep/documentation/core.html#pykep.lambert_problem
:param R1: radius vector of measuements #1
:param R2: radius vector of measuements #2
:param delta_time: delta time between R1 and R2 measurements, in seconds. Only works for same pass
:param trajectory_cw: bool. True for retrograde motion (clockwise), False if counter-clock wise
:return:
V1: velocity vector of R1
V2: velocity vector of R2
ratio: absolute tolerance result
"""
r1 = np.linalg.norm(R1)
r2 = np.linalg.norm(R2)
c12 = np.cross(R1, R2)
theta = np.arccos(np.dot(R1, R2) / r1 / r2)
# todo: automatic check for pro or retrograde orbits is needed. currently set to prograde
if trajectory_cw == False:
trajectory = "prograde"
if trajectory_cw == True:
trajectory = "retrograde"
#inclination = 0.0
#if inclination >= 0.0 and inclination < 90.0:
if trajectory == "prograde":
if c12[2] >= 0:
theta = theta
else:
theta = np.pi*2 - theta
#if inclination >= 90.0 and inclination < 180.0:
if trajectory == "retrograde":
if c12[2] < 0:
theta = theta
else:
theta = np.pi*2 - theta
A = np.sin(theta) * np.sqrt(r1 * r2 / (1.0 - np.cos(theta)))
z = 0.0
while F_z_i(z, delta_time, r1, r2, A) < 0.0:
z = z + 0.1
tol = 1.e-10 # tolerance
nmax = 6000 # iterations
ratio = 1
n = 0
while (np.abs(ratio) > tol) and (n <= nmax):
n = n + 1
ratio = F_z_i(z, delta_time, r1, r2, A)
if np.isnan(ratio) == True:
break
ratio = ratio / dFdz(z, r1, r2, A)
z = np.abs(z - ratio)
C_z_i = c2(z)
S_z_i = c3(z)
y_z = r1 + r2 + A * (z * S_z_i - 1.0) / np.sqrt(C_z_i)
f = 1.0 - y_z / r1
mu = mu_Earth
g = A * np.sqrt(y_z / mu)
gdot = 1.0 - y_z / r2
V1 = 1.0 / g * (np.add(R2, -np.multiply(f, R1)))
V2 = 1.0 / g * (np.multiply(gdot, R2) - R1)
return V1, V2, np.abs(ratio)