伪距定位原理是啥?

Posted 2023-04-11

参考技术A 伪距定位原理
每个GPS卫星均发送它所在的准确位置和发送信号
起始时间.GPS接收机接送到这些信号后,根据发送信号与收到信号的时间间隔来计算接收机到卫星之间的距离.当接收机计算出它与三颗星以上卫星之间的距离后,它与地球表面所处位置就被确定下来.这类似于接收机在以三颗卫星为底的三棱锥的顶点上.如图9-33,由于测距受大气延迟和接收机时钟与卫星时钟不同步的影响,这个距离不是几何距离,故称它为"伪距".伪距观测量是卫星发射信号的时刻和信号到达接收机时刻之差,即=
-
=(1/c)P,由于两个时间尺度都是误差,引入GPS时作为理想的时间尺度必须加以修正.设卫星钟的改正数为V,接收机钟的改正数为.
则有+
V=
+=
(,为标准时间)
(1/c)=
-
=(
-)+
V-
顾及电离层延时影响和对流延时影响,因此,完整的伪距公式为=+
+-CV+C.设=(x,y,z)T为卫星在地心坐标系中的位置矢量,可由卫星星历计算得到.=(x,y,z)T为接收机的地心坐标,是已知量,则上式即可改写为=|-|+
+
-CV-C.

伪距定位算法

伪距定位算法：
1.准备数据与设置初始解：准备卫星位置（经地球自转校正后）和伪距值
设置接收机初始位置、接收机钟差
2.非线性方程线性化
3.求解非线性方程
4.更新非线性方程的根
5.迭代完毕

function [pos, el, az, dop] = leastSquarePos(satpos, obs, settings)
%satpos卫星坐标xyz;obs观测伪距
%Function calculates the Least Square Solution.
%
%[pos, el, az, dop] = leastSquarePos(satpos, obs, settings);
%
%   Inputs:
%       satpos      - Satellites positions (in ECEF system: [X; Y; Z;] -
%                   one column per satellite)
%       obs         - Observations - the pseudorange measurements to each
%                   satellite:
%                   (e.g. [20000000 21000000 .... .... .... .... ....])
%       settings    - receiver settings
%
%   Outputs:
%       pos         - receiver position and receiver clock error 
%                   (in ECEF system: [X, Y, Z, dt]) 
%       el          - Satellites elevation angles (degrees)
%       az          - Satellites azimuth angles (degrees)
%       dop         - Dilutions Of Precision ([GDOP PDOP HDOP VDOP TDOP])

%--------------------------------------------------------------------------
%                           SoftGNSS v3.0
%--------------------------------------------------------------------------
%Based on Kai Borre
%Copyright (c) by Kai Borre
%Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen
%
% CVS record:
% $Id: leastSquarePos.m,v 1.1.2.12 2006/08/22 13:45:59 dpl Exp $
%==========================================================================

%=== Initialization =======================================================
nmbOfIterations = 7;

dtr     = pi/180;
pos     = zeros(4, 1);
X       = satpos;
nmbOfSatellites = size(satpos, 2);

A       = zeros(nmbOfSatellites, 4);
omc     = zeros(nmbOfSatellites, 1);
az      = zeros(1, nmbOfSatellites);
el      = az;

%=== Iteratively find receiver position ===================================
for iter = 1:nmbOfIterations

    for i = 1:nmbOfSatellites
        if iter == 1
            %--- Initialize variables at the first iteration --------------
            Rot_X = X(:, i);
            trop = 2; % 对流层矫正值
        else
            %--- Update equations -----------------------------------------
            rho2 = (X(1, i) - pos(1))^2 + (X(2, i) - pos(2))^2 + ...
                   (X(3, i) - pos(3))^2;%伪距预测值，即r
            traveltime = sqrt(rho2) / settings.c ;

            %--- Correct satellite position (do to earth rotation) --------
            Rot_X = e_r_corr(traveltime, X(:, i));
            %X是(t-tao)(卫星发射信号)时刻的在WGS-84坐标系下的卫星位置；
            %Rot_X是t(接收机接收到信号)时刻在WGS-84坐标下的卫星位置
            %两者差别很大倒不是因为卫星绝对位置移动了多少，而是地球自转导致坐标系移动了
            %--- Find the elevation angel of the satellite ----------------
            [az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :));
            %根据接收机位置和伪距矢量计算方位角、高度角、距离
            if (settings.useTropCorr == 1)
                %--- Calculate tropospheric correction --------------------
                trop = tropo(sin(el(i) * dtr), ...
                             0.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
            else
                % Do not calculate or apply the tropospheric corrections
                trop = 0;
            end
        end % if iter == 1 ... ... else 

        %--- Apply the corrections ----------------------------------------
        omc(i) = (obs(i) - norm(Rot_X - pos(1:3), 'fro') - pos(4) - trop);
% n = norm(v) 返回向量 v 的欧几里德范数。此范数也称为 2-范数、向量模或欧几里德长度。
%omc就是b
        %--- Construct the A matrix ---------------------------------------
        A(i, :) =  [ (-(Rot_X(1) - pos(1))) / obs(i) ... %Rot_X是卫星的位置，pos是接收机大体位置
                     (-(Rot_X(2) - pos(2))) / obs(i) ...
                     (-(Rot_X(3) - pos(3))) / obs(i) ...
                     1 ];
    end % for i = 1:nmbOfSatellites

    % These lines allow the code to exit gracefully in case of any errors
    if rank(A) ~= 4
        pos     = zeros(1, 4);
        return
    end

    %--- Find position update ---------------------------------------------
    x   = A \\ omc;   %只用了4颗卫星，所以没有用最小二乘；
%     \\
%     对线性方程组 Ax = B 求解 x
% 
%     此 MATLAB 函数 对线性方程组 A*x = B 求解。矩阵 A 和 B 必须具有相同的行数。如果 A 未正确缩放或接近奇异值，MATLAB
%     将会显示警告信息，但还是会执行计算。
% 
%     x = A\\B
%     x = mldivide(A,B)
    %--- Apply position update --------------------------------------------
    pos = pos + x;
    
end % for iter = 1:nmbOfIterations

pos = pos';

%=== Calculate Dilution Of Precision ======================================
if nargout  == 4
    %--- Initialize output ------------------------------------------------
    dop     = zeros(1, 5);
    
    %--- Calculate DOP ----------------------------------------------------
    Q       = inv(A'*A);
    
    dop(1)  = sqrt(trace(Q));                       % GDOP    
    dop(2)  = sqrt(Q(1,1) + Q(2,2) + Q(3,3));       % PDOP
    dop(3)  = sqrt(Q(1,1) + Q(2,2));                % HDOP
    dop(4)  = sqrt(Q(3,3));                         % VDOP
    dop(5)  = sqrt(Q(4,4));                         % TDOP
end