www.pudn.com > GNSS_SDR.zip > postNavigation.asv, change:2008-09-24,size:12612b


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. 
 
%%%%%%%%% the below "if" section changed by Jiffer at 2008-09-24 %%%%%%%%%%%%% 
 
%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 
%%%%%%%%% the end of the change %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%% 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(blocked by Jffer at 2008-09-24) ===================== 
%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; 
%%Save subfreamstart by Jeffer 
 
navSolutions.subFrameStart=subFrameStart; 
 
%########################################################################## 
%#   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); 
 
%% Save the transmitTime                                     
navSolutions.transmitTime(currMeasNr)=transmitTime; 
 
%% 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...