www.pudn.com > GNSS_SDR.zip > postNavigation.m, change:2010-11-22,size:14116b


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. 
%计算导航结果(伪距、位置)。最后从WGS84坐标系转至用户坐标系,ECEF坐标系或者其他 
%任何坐标系 
%[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). 
%                      - 包括伪距、接收机钟差、坐标(ECEF和用户坐标系) 
%       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 theGNU 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. 
% 由于至少需要导航电文前3个子帧才能完成导航,而整个导航电文有5个子帧(30s),加上 
% 以防获取子帧正好在一个子帧的中间,我们至少需要36s数据。  
 
if (settings.msToProcess < 36000) || (sum([trackResults.status] ~= '-') < 4) 
    % Show the error message and exit 显示错误并退出 
    disp('由于数据信息太短或者卫星通道数不够,程序退出!'); 
    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 --------------- 
    %--- 复制5个子帧 ------------------------------------------------------- 
    navBitsSamples = trackResults(channelNr).I_P(subFrameStart(channelNr) - 20 : ... 
                               subFrameStart(channelNr) + (1500 * 20) -1)'; 
 
    %--- Group every 20 vales of bits into columns ------------------------ 
    %--- 20个一组排列 ------------------------------------------------------ 
    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. 
    % 转换为1和0 
    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 检测卫星数是否超过3颗 = 
if (isempty(activeChnList) || (size(activeChnList, 2) < 4)) 
    % Show error message and exit 显示错误并退出 
    disp('卫星导航电文数据不够,退出!'); 
    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 position (0,0) in the sky 
    % plot. 
    % 以防出现位置(0,0) 
    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); 
% if (currMeasNr>10 && currMeasNr<30) 
%     navSolutions.channel.rawP(3, currMeasNr)=navSolutions.channel.rawP(3, currMeasNr)+20*rand(1,1); 
% end 
%% 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   
    % 3颗卫星以上才可以得到3维位置 
    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 转换为ECEF坐标系=============== 
        [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(['可见星: ', num2str(currMeasNr), ',无法进行三维定位']); 
 
        %--- 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... 
 
save navResults.mat navSolutions