gps最小二乘定位

GPS定位中应用较多的最小二乘定位，程序简单易懂，是大家学习和参考的好资料，

function [pos el az dop] = leastSquarePos（satpos obs settings）
%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

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;
            traveltime = sqrt（rho2） / settings.c ;

            %--- Correct satellite position （do to earth rotation） --------
            Rot_X = e_r_corr（traveltime X（: i））;

            %--- 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）;

        %--- Construct the A matrix ---------------------------------------
        A（i :） =  [ （-（Rot_X（1） - pos（1））） / obs（i） ...
                     （-（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;
    
    %--- Apply position update --------------------------------------------
    pos = pos + x;
    
end % for iter = 1:nmbOfIterations

pos = pos‘;

%=== Calculate Dilution Of Precision ===============

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       3572  2012-07-24 17:18  leastSquarePos.m

----------- ---------  ---------- -----  ----

                 3572                    1