-
Notifications
You must be signed in to change notification settings - Fork 6
/
Copy pathcompute_LLA.m
130 lines (116 loc) · 6 KB
/
compute_LLA.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
% This function obtains approximated Sub-Satellite Points based upon
% orbital elements and the Greenwich hour angle.
% Parameters and units:
% time = Greenwich Hour Angle [degrees]
% a = Semi-Major Axis [Km]
% e = Eccentricity
% i = Inclination [degrees]
% W = RAAN [degrees]
% w = Argument of perigee [degrees]
% v0 = True Anomaly [degrees]
%
% This snippet is based upon Ennio Condoleo's 2013 orbit tracker.
function [lat, lon, alt] = compute_LLA(time,a,e,i,RAAN,w,v0)
% -------------------------------- CONST ---------------------------------%
RE = 6378; % Earth's radius [km]
muE = 398600.44; % Earth gravitational parameter [km^3/sec^2]
wE = (2*pi/86164); % Earth rotation velocity aorund z-axis [rad/sec]
% --------------------------------- CONV ---------------------------------%
RAAN = RAAN*pi/180; % RAAN [rad]
w = w*pi/180; % Argument of perigee [rad]
v0 = v0*pi/180; % True anomaly at the departure [rad]
i = i*pi/180; % inclination [rad]
% ------------------------------------------------------------------------%
%% ORBIT COMPUTATION
rp = a*(1-e); % radius of perigee [km]
ra = a*(1+e); % radius of apogee [km]
Vp = sqrt(muE*(2/rp-1/a)); % velocity at the perigee [km/s]
Va = sqrt(muE*(2/ra-1/a)); % velocity at the apogee [km/s]
n = sqrt(muE./a^3); % mean motion [rad/s]
p = a*(1-e^2); % semi-latus rectus [km]
T = 2*pi/n; % period [s]
h = sqrt(p*muE); % moment of the momentum [km^2/s]
h1 = sin(i)*sin(RAAN); % x-component of unit vector h
h2 = -sin(i)*cos(RAAN); % y-component of unit vector h
h3 = cos(i); % z-component of unit vector h
n1 = -h2/(sqrt(h1^2+h2^2)); % x-component of nodes' line
n2 = h1/(sqrt(h1^2+h2^2)); % y-component of nodes' line
n3 = 0; % z-component of nodes' line
N = [n1,n2,n3]; % nodes' line (unit vector)
%% TIME
norb = 0; % number of orbits
t0 = 0; % initial time [s]
tf = norb*T; % final time [s]
step = 10; % time step [s]
t = t0:step:tf+step; % vector of time [s]
%% DETERMINATION OF THE DYNAMICS
cosE0 = (e+cos(v0))./(1+e.*cos(v0)); % cosine of initial eccentric anomaly
sinE0 = (sqrt(1-e^2).*sin(v0))./(1+e.*cos(v0)); % sine of initial eccentric anomaly
E0 = atan2(sinE0,cosE0); % initial eccentric anomaly [rad]
if (E0<0) % E0�[0,2pi]
E0=E0+2*pi;
end
tp = (-E0+e.*sin(E0))./n+t0; % pass time at the perigee [s]
M = n.*(t-tp); % mean anomaly [rad]
%% Mk = Ek - e*sin(Ek);
% Eccentric anomaly (must be solved iteratively for Ek)
E = zeros(size(t,2),1);
for j=1:size(t,2)
E(j) = anom_ecc(M(j),e); % eccentric anomaly [rad]
end
%% True anomaly, Argument of latitude, Radius
sin_v = (sqrt(1-e.^2).*sin(E))./(1-e.*cos(E)); % sine of true anomaly
cos_v = (cos(E)-e)./(1-e.*cos(E)); % cosine of true anomaly
v = atan2(sin_v,cos_v); % true anomaly [rad]
theta = v + w; % argument of latitude [rad]
r = (a.*(1-e.^2))./(1+e.*cos(v)); % radius [km]
%% Satellite coordinates
% "Inertial" reference system ECI (Earth Centered Inertial)
xp = r.*cos(theta); % In-plane x position (node direction) [km]
yp = r.*sin(theta); % In-plane y position (perpendicular node direct.) [km]
xs = xp.*cos(RAAN)-yp.*cos(i).*sin(RAAN); % ECI x-coordinate SAT [km]
ys = xp.*sin(RAAN)+yp.*cos(i).*cos(RAAN); % ECI y-coordinate SAT [km]
zs = yp.*sin(i); % ECI z-coordinate SAT [km]
rs = p./(1+e.*cos(theta-w)); % norm of radius SAT [km]
%% GREENWICH HOUR ANGLE
greenwich0 = time;
%% SUB-SATELLITE-POINT
greenwich0 = greenwich0*pi/180; % Greenwich hour angle at the initial time [rad]
rot_earth = wE.*(t-t0)+greenwich0; % Greenwich hour angle at the time t [rad]
for j=1:size(t,2)
if rot_earth(j) < (-pi)
nn = ceil(rot_earth(j)/(-2*pi));
rot_earth(j) = rot_earth(j) + nn*2*pi;
elseif rot_earth(j) > (pi)
nn = fix(rot_earth(j)/(2*pi));
rot_earth(j) = rot_earth(j) - nn*2*pi;
end
end
LatSSP = asin(sin(i).*sin(theta)); % Latitude of Sub-Satellite-Point [rad]
LongSSP = atan2(ys./rs,xs./rs)-rot_earth'; % Longitude of Sub-Satellite-Point [rad]
% xSSP = RE.*cos(LatSSP).*cos(LongSSP)+1; % x-component of Sub-Satellite-Point [km]
% ySSP = RE.*cos(LatSSP).*sin(LongSSP)+1; % y-component of Sub-Satellite-Point [km]
% zSSP = RE.*sin(LatSSP)+0.1; % z-component of Sub-Satellite-Point [km]
lat = rad2deg(LatSSP(1));
lon = rad2deg(LongSSP(1));
alt = a - RE;
end
% This function computes the eccentric anomaly
% function [E] = anom_ecc(M,e)
% Resolución numerica para la ecuación: E-e*sin(E)=M
% E = anomalia eccentrica [rad]
% e = excentricidad
% M = anomalia media [rad]
% El programa calcula E con un error de orden -10
% Para mejorar la certeza modificar el valor de err
function [E] = anom_ecc(M,e)
E = M;
k = 1;
err = 1e-10;
% set search interval (sx, dx) and max derived value dymax
while (k>err)
y = e*sin(E)+M;
k = abs(abs(E)-abs(y));
E = y;
end
end