-
Notifications
You must be signed in to change notification settings - Fork 1
/
lambertsolver.m
137 lines (109 loc) · 3.04 KB
/
lambertsolver.m
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
function [ vo, vf ] = lambertsolver( ro, rf, dto, mu )
%lambertsolver.m Solves Lambert's Problem through Universal Variables
%Formulation. This is solver is intended for our solar system only
% Inputs:
% ro = initial position vector w.r.t the Sun
% rf = final position vector w.r.t the Sun
% dto = desired transfer time
% m = gravitational parameter [km^3/s^2]
% Outputs:
% vo = Transfer ellipse initial velocity vector w.r.t the sun
% vf = Transfer ellipse final velocity vector w.r.t the sun
%% Initialize
% If TOF = 0, stop computing
if dto == 0
vo = Inf;
vf = Inf;
return
end
% Calculate delta nu
nu1 = atan2(ro(2), ro(1));
nu2 = atan2(rf(2), rf(1));
dnu = nu2 - nu1;
if dnu < 0
dnu = dnu + 2*pi;
end
% Determine trajectory type
if dnu < pi
DM = 1;
else
DM = -1;
end
% Calculate variable A
A = DM*sqrt(norm(ro)*norm(rf)*(1 + cos(dnu)));
% Break statement for trajectoresi parallel to Earth
if dnu == 0 && A == 0
display('Trajectory can not be computed')
end
% Pick initial psi values
psi = 0;
c2 = 1/2;
c3 = 1/6;
% Define upper and lower range
psi_hi = 4*pi^2;
psi_lo = -4*pi;
% Define starting time
dt = 0;
%% Compute
timer = 1;
while abs(dt-dto) > 10^-6
y = norm(ro) + norm(rf) + A*(psi*c3 - 1)/sqrt(c2);
if (A > 0)&&(y < 0)
count = 1;
while (count < 100)&&(y < 0)
psi = 0.8*(1.0/c3)*( 1.0 - (norm(ro) + norm(rf))*sqrt(c2)/A);
if (psi > 1e-6)
c2 = (1-cos(sqrt(psi)))/psi;
c3 = (sqrt(psi)-sin(sqrt(psi)))/sqrt(psi^3);
elseif (psi < -1e-6)
c2 = (1 - cosh(sqrt(-psi)))/psi;
c3 = (sinh(sqrt(-psi)) - sqrt(-psi))/sqrt((-psi)^3);
else
c2 = 0.5;
c3 = 1/6;
end
if abs(c2) > 1e-7
y = norm(ro)+norm(rf)+A*(psi*c3-1)/sqrt(c2);
else
y = ro + rf;
end
count = count + 1;
end
end
chi = sqrt(y/c2);
dt = (chi^3*c3 + A*sqrt(y))/sqrt(mu);
if dt <= dto
psi_lo = psi;
else
psi_hi = psi;
end
psi = (psi_hi + psi_lo)/2;
if psi > 10^-6
c2 = (1 - cos(sqrt(psi)))/psi;
c3 = (sqrt(psi) - sin(sqrt(psi)))/sqrt(psi^3);
else if (psi < -10^-6)
c2 = (1 - cosh(sqrt(-psi)))/psi;
c3 = (sinh(sqrt(-psi)) - sqrt(-psi))/(sqrt(-psi^3));
else
c2 = 1/2;
c3 = 1/6;
end
end
if timer > 1000
disp('WARNING: Too many iterations - While loop terminated')
break
end
timer = timer + 1;
end
if timer > 1000
vo = Inf;
vf = Inf;
return
end
f = 1 - y/norm(ro);
gdot = 1 - y/norm(rf);
g = A*sqrt(y/mu);
%% Output
vo = (rf - f*ro)/g;
vf = (gdot*rf - ro)/g;
end