-
Notifications
You must be signed in to change notification settings - Fork 46
/
postNavigation.m
executable file
·253 lines (207 loc) · 11.9 KB
/
postNavigation.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
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
function [navSolutions, eph] = postNavigation(trackResults, settings)
%Function calculates navigation solutions for the receiver (pseudoranges,
%positions). At the end it converts coordinates from the WGS84 system to
%the UTM, geocentric or any additional coordinate system.
%
%[navSolutions, eph] = postNavigation(trackResults, settings)
%
% Inputs:
% trackResults - results from the tracking function (structure
% array).
% settings - receiver settings.
% Outputs:
% navSolutions - contains measured pseudoranges, receiver
% clock error, receiver coordinates in several
% coordinate systems (at least ECEF and UTM).
% eph - received ephemerides of all SV (structure array).
%--------------------------------------------------------------------------
% SoftGNSS v3.0
%
% Copyright (C) Darius Plausinaitis
% Written by Darius Plausinaitis with help from Kristin Larson
%--------------------------------------------------------------------------
%This program is free software; you can redistribute it and/or
%modify it under the terms of the GNU General Public License
%as published by the Free Software Foundation; either version 2
%of the License, or (at your option) any later version.
%
%This program is distributed in the hope that it will be useful,
%but WITHOUT ANY WARRANTY; without even the implied warranty of
%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
%GNU General Public License for more details.
%
%You should have received a copy of the GNU General Public License
%along with this program; if not, write to the Free Software
%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301,
%USA.
%--------------------------------------------------------------------------
%CVS record:
%$Id: postNavigation.m,v 1.1.2.22 2006/08/09 17:20:11 dpl Exp $
%% Check is there enough data to obtain any navigation solution ===========
% It is necessary to have at least three subframes (number 1, 2 and 3) to
% find satellite coordinates. Then receiver position can be found too.
% The function requires all 5 subframes, because the tracking starts at
% arbitrary point. Therefore the first received subframes can be any three
% from the 5.
% One subframe length is 6 seconds, therefore we need at least 30 sec long
% record (5 * 6 = 30 sec = 30000ms). We add extra seconds for the cases,
% when tracking has started in a middle of a subframe.
if (settings.msToProcess < 36000) || (sum([trackResults.status] ~= '-') < 4)
% Show the error message and exit
disp('Record is to short or too few satellites tracked. Exiting!');
navSolutions = [];
eph = [];
return
end
%% Find preamble start positions ==========================================
[subFrameStart, activeChnList] = findPreambles(trackResults, settings);
%% Decode ephemerides =====================================================
for channelNr = activeChnList
%=== Convert tracking output to navigation bits =======================
%--- Copy 5 sub-frames long record from tracking output ---------------
navBitsSamples = trackResults(channelNr).I_P(subFrameStart(channelNr) - 20 : ...
subFrameStart(channelNr) + (1500 * 20) -1)';
%--- Group every 20 vales of bits into columns ------------------------
navBitsSamples = reshape(navBitsSamples, ...
20, (size(navBitsSamples, 1) / 20));
%--- Sum all samples in the bits to get the best estimate -------------
navBits = sum(navBitsSamples);
%--- Now threshold and make 1 and 0 -----------------------------------
% The expression (navBits > 0) returns an array with elements set to 1
% if the condition is met and set to 0 if it is not met.
navBits = (navBits > 0);
%--- Convert from decimal to binary -----------------------------------
% The function ephemeris expects input in binary form. In Matlab it is
% a string array containing only "0" and "1" characters.
navBitsBin = dec2bin(navBits);
%=== Decode ephemerides and TOW of the first sub-frame ================
[eph(trackResults(channelNr).PRN), TOW] = ...
ephemeris(navBitsBin(2:1501)', navBitsBin(1));
%--- Exclude satellite if it does not have the necessary nav data -----
if (isempty(eph(trackResults(channelNr).PRN).IODC) || ...
isempty(eph(trackResults(channelNr).PRN).IODE_sf2) || ...
isempty(eph(trackResults(channelNr).PRN).IODE_sf3))
%--- Exclude channel from the list (from further processing) ------
activeChnList = setdiff(activeChnList, channelNr);
end
end
%% Check if the number of satellites is still above 3 =====================
if (isempty(activeChnList) || (size(activeChnList, 2) < 4))
% Show error message and exit
disp('Too few satellites with ephemeris data for postion calculations. Exiting!');
navSolutions = [];
eph = [];
return
end
%% Initialization =========================================================
% Set the satellite elevations array to INF to include all satellites for
% the first calculation of receiver position. There is no reference point
% to find the elevation angle as there is no receiver position estimate at
% this point.
satElev = inf(1, settings.numberOfChannels);
% Save the active channel list. The list contains satellites that are
% tracked and have the required ephemeris data. In the next step the list
% will depend on each satellite's elevation angle, which will change over
% time.
readyChnList = activeChnList;
transmitTime = TOW;
%##########################################################################
%# Do the satellite and receiver position calculations #
%##########################################################################
%% Initialization of current measurement ==================================
for currMeasNr = 1:fix((settings.msToProcess - max(subFrameStart)) / ...
settings.navSolPeriod)
% Exclude satellites, that are belove elevation mask
activeChnList = intersect(find(satElev >= settings.elevationMask), ...
readyChnList);
% Save list of satellites used for position calculation
navSolutions.channel.PRN(activeChnList, currMeasNr) = ...
[trackResults(activeChnList).PRN];
% These two lines help the skyPlot function. The satellites excluded
% do to elevation mask will not "jump" to possition (0,0) in the sky
% plot.
navSolutions.channel.el(:, currMeasNr) = ...
NaN(settings.numberOfChannels, 1);
navSolutions.channel.az(:, currMeasNr) = ...
NaN(settings.numberOfChannels, 1);
%% Find pseudoranges ======================================================
navSolutions.channel.rawP(:, currMeasNr) = calculatePseudoranges(...
trackResults, ...
subFrameStart + settings.navSolPeriod * (currMeasNr-1), ...
activeChnList, settings);
%% Find satellites positions and clocks corrections =======================
[satPositions, satClkCorr] = satpos(transmitTime, ...
[trackResults(activeChnList).PRN], ...
eph, settings);
%% Find receiver position =================================================
% 3D receiver position can be found only if signals from more than 3
% satellites are available
if size(activeChnList, 2) > 3
%=== Calculate receiver position ==================================
[xyzdt, ...
navSolutions.channel.el(activeChnList, currMeasNr), ...
navSolutions.channel.az(activeChnList, currMeasNr), ...
navSolutions.DOP(:, currMeasNr)] = ...
leastSquarePos(satPositions, ...
navSolutions.channel.rawP(activeChnList, currMeasNr)' + satClkCorr * settings.c, ...
settings);
%--- Save results -------------------------------------------------
navSolutions.X(currMeasNr) = xyzdt(1);
navSolutions.Y(currMeasNr) = xyzdt(2);
navSolutions.Z(currMeasNr) = xyzdt(3);
navSolutions.dt(currMeasNr) = xyzdt(4);
% Update the satellites elevations vector
satElev = navSolutions.channel.el(:, currMeasNr);
%=== Correct pseudorange measurements for clocks errors ===========
navSolutions.channel.correctedP(activeChnList, currMeasNr) = ...
navSolutions.channel.rawP(activeChnList, currMeasNr) + ...
satClkCorr' * settings.c + navSolutions.dt(currMeasNr);
%% Coordinate conversion ==================================================
%=== Convert to geodetic coordinates ==============================
[navSolutions.latitude(currMeasNr), ...
navSolutions.longitude(currMeasNr), ...
navSolutions.height(currMeasNr)] = cart2geo(...
navSolutions.X(currMeasNr), ...
navSolutions.Y(currMeasNr), ...
navSolutions.Z(currMeasNr), ...
5);
%=== Convert to UTM coordinate system =============================
navSolutions.utmZone = findUtmZone(navSolutions.latitude(currMeasNr), ...
navSolutions.longitude(currMeasNr));
[navSolutions.E(currMeasNr), ...
navSolutions.N(currMeasNr), ...
navSolutions.U(currMeasNr)] = cart2utm(xyzdt(1), xyzdt(2), ...
xyzdt(3), ...
navSolutions.utmZone);
else % if size(activeChnList, 2) > 3
%--- There are not enough satellites to find 3D position ----------
disp([' Measurement No. ', num2str(currMeasNr), ...
': Not enough information for position solution.']);
%--- Set the missing solutions to NaN. These results will be
%excluded automatically in all plots. For DOP it is easier to use
%zeros. NaN values might need to be excluded from results in some
%of further processing to obtain correct results.
navSolutions.X(currMeasNr) = NaN;
navSolutions.Y(currMeasNr) = NaN;
navSolutions.Z(currMeasNr) = NaN;
navSolutions.dt(currMeasNr) = NaN;
navSolutions.DOP(:, currMeasNr) = zeros(5, 1);
navSolutions.latitude(currMeasNr) = NaN;
navSolutions.longitude(currMeasNr) = NaN;
navSolutions.height(currMeasNr) = NaN;
navSolutions.E(currMeasNr) = NaN;
navSolutions.N(currMeasNr) = NaN;
navSolutions.U(currMeasNr) = NaN;
navSolutions.channel.az(activeChnList, currMeasNr) = ...
NaN(1, length(activeChnList));
navSolutions.channel.el(activeChnList, currMeasNr) = ...
NaN(1, length(activeChnList));
% TODO: Know issue. Satellite positions are not updated if the
% satellites are excluded do to elevation mask. Therefore rasing
% satellites will be not included even if they will be above
% elevation mask at some point. This would be a good place to
% update positions of the excluded satellites.
end % if size(activeChnList, 2) > 3
%=== Update the transmit time ("measurement time") ====================
transmitTime = transmitTime + settings.navSolPeriod / 1000;
end %for currMeasNr...