-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathCOE15_estimator_wrapper.m
189 lines (164 loc) · 6.37 KB
/
COE15_estimator_wrapper.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
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
function [a, ecc, inc, RAAN, omega, M0,...
Cus, Cuc, Crs, Crc, Cis, Cic, ...
IDOT, OMEGA_DOT, delta_n, flag, NumIter, fit_type] = ...
COE15_estimator_wrapper(time, pos, vel,...
initial_guess, Wmat, ConvCrit, ...
fit_parameters, theta_g, coeff_R2, coeff_A2, coeff_C2)
%% DESCRIPTION
%
% Written by: Tyler Reid ([email protected])
% PI: Todd Walter, Per Enge
% Lab: Stanford University GPS Lab
% Date: May , 2017
% Updated: May 24, 2017
%
% -------------------------------------------------------------------------
% FUNCTION DESCRIPTION
%
% This function fits the GPS / L5 SBAS MOPS ephemeris message to
% precision orbit data.
%
% -------------------------------------------------------------------------
% FUNCTION DESCRIPTION
%
% This function fits the GPS / L5 SBAS MOPS ephemeris message to
% precision orbit data. It attempts combinations of removals of certain
% orbital elements in the event of convergence failure and chooses the best
% option.
%
% -------------------------------------------------------------------------
% INPUT:
% time = Input vector of times starting from 0 [sec] which
% are consistent with the pos / vel (assumes a fixed
% time step).
% pos = Position of the spacecraft in an ECEF
% coordinate frame.
% vel = Velocity of the spacecraft in an ECEF
% coordinate frame.
% initial_guess = Initial guess of orbital elements in the form:
% initial_guess = [a; e; inc; RAAN; omega; M0;
% Cus; Cuc; Crs; Crc; Cis; Cic;
% IDOT; OMEGA_DOT; delta_n];
% Wmat = Values to form the weighting matrix for least
% squares, there should be one value for each point.
% ConvCrit = Value of the convergence criteria used in
% evaluating the newton decrement.
% fit_parameters= Parameters to be fit of the 15 orbital elements.
% theta_g = Greenwich mean sidereal time [rad], see: utc2gmst.m
% coeff_R2, coeff_A2, coeff_C2 =
% Analytical coefficients for the URE equations.
%
% -------------------------------------------------------------------------
% OUTPUT:
% a = Best fit semi-major axis
% e = Best fit eccentricity
% inc = Best fit inclination
% RAAN = Best fit right ascension of the ascending node
% omega = Best fit argument of perigee
% M0 = Best fit mean anomaly at epoch
% Cus,Cuc = Best fit along-track harmonic correction terms
% Crs,Crc = Best fit radial harmonic correction terms
% Cis,Cic = Best fit cross-track harmonic correction terms
% IDOT = Best fit inclination correction rate
% OMEGA_DOT = Best fit rate in right ascention
% delta_n = Best fit orbital rate offset
% flag = 0 if convergence fails, 1 if successfull
% NumIter = Number of Iterations
%
%% MAIN
% Define the fit parameter scenarios to be tested.
fit_scenarios = repmat(fit_parameters, 10, 1);
% No RAAN.
fit_scenarios(2, 4) = 0;
% No omega.
fit_scenarios(3, 5) = 0;
% No inclination.
fit_scenarios(4, 3) = 0;
% No RAAN / inc.
fit_scenarios(5, 3) = 0;
fit_scenarios(5, 4) = 0;
% No omega / inc.
fit_scenarios(6, 3) = 0;
fit_scenarios(6, 5) = 0;
% No RAAN / ecc.
fit_scenarios(7, 2) = 0;
fit_scenarios(7, 4) = 0;
% No omega / ecc.
fit_scenarios(8, 2) = 0;
fit_scenarios(8, 4) = 0;
% No RAAN / omega.
fit_scenarios(9, 4) = 0;
fit_scenarios(9, 5) = 0;
% No RAAN / omega / inc.
fit_scenarios(10, 3) = 0;
fit_scenarios(10, 4) = 0;
fit_scenarios(10, 5) = 0;
% No RAAN / omega / ecc.
fit_scenarios(11, 2) = 0;
fit_scenarios(11, 4) = 0;
fit_scenarios(11, 5) = 0;
% No RAAN / omega / ecc / inc.
fit_scenarios(12, 2) = 0;
fit_scenarios(12, 3) = 0;
fit_scenarios(12, 4) = 0;
fit_scenarios(12, 5) = 0;
% Get the number of scenarios.
[num_scenarios,~] = size(fit_scenarios);
% Initialize the rms ure (this is a dummy value).
rms_ure_best = 999;
for i = 1:num_scenarios
% Try fitting without removing elements.
[a_test, ecc_test, inc_test, RAAN_test, omega_test, M0_test,...
Cus_test, Cuc_test, Crs_test, Crc_test, Cis_test, Cic_test, ...
IDOT_test, OMEGA_DOT_test, delta_n_test, ...
flag_test, NumIter_test] = ...
COE15_estimator(time, pos, initial_guess, Wmat, ConvCrit, ...
fit_scenarios(i,:));
% If the inclination is less than zero, we have failed.
if inc_test < 0
flag_test = 1;
end
% Evaluate the message performance.
if flag_test == 0
[~, ~, rms_ure, ~, ~, ~, ~] = ...
eph_error_analysis(sqrt(a_test), ecc_test, inc_test,...
RAAN_test, omega_test, M0_test, ...
Cus_test, Cuc_test, Crc_test, Crs_test, ...
Cic_test, Cis_test, ...
IDOT_test, OMEGA_DOT_test, delta_n_test, ...
time, pos, vel, theta_g, ...
coeff_R2, coeff_A2, coeff_C2);
end
if flag_test == 0
if rms_ure < rms_ure_best
% Assign the best rms_ure seen so far.
rms_ure_best = rms_ure;
% Assign the fit type.
fit_type = i;
% Assign the orbital elements as the output.
a = a_test; % [m]
ecc = ecc_test; % [-]
inc = inc_test; % [rad]
RAAN = RAAN_test; % [rad]
omega = omega_test; % [rad]
M0 = M0_test; % [rad]
Cus = Cus_test; % [rad]
Cuc = Cuc_test; % [rad]
Crs = Crs_test; % [m]
Crc = Crc_test; % [m]
Cis = Cis_test; % [rad]
Cic = Cic_test; % [rad]
IDOT = IDOT_test; % [rad/s]
OMEGA_DOT = OMEGA_DOT_test; % [rad/s]
delta_n = delta_n_test; % [rad/s]
% Number of iterations.
NumIter = NumIter_test;
% Set the flag.
flag = flag_test;
% If we succeed without having to remove any elements, stop.
if i == 0
break;
end
end
end
end