www.gusucode.com > GPS仿真Matlab编程源码程序 > GPS仿真Matlab编程源码程序/postNavigation.m
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...