在各種偽距定位算法中，最小二乘法是一種比較簡單而廣泛的方法，該算法可以分為以下幾步：
1、准備數據與設置初始值
這里准備數據，主要是對於各顆可見衛星，收集到它們在同一時刻的偽距測量值，計算測量值的各項偏差、誤差成分的校正量，然后計算出誤差校正后的偽距測量值，這里假設偽距為理想距離加上隨機高斯誤差。設置初始值，假設大概知道位置坐標，則設定其為初始值，也可根據上一次定位結果設定；若什么都不了解，那么初值設置為0，只不過多幾次迭代過程罷了。
2、非線性方程組線性化（不詳細解釋，就是得到雅克比矩陣）。
3、求解線性方程組，此處運用最小二乘法。
4、更新非線性方程組的根
5、判斷牛頓迭代的收斂性，用定位結果的二范數是否小於定位精度為判定標准。
貼出偽距定位算法的MATLAB代碼：
1、主函數dingwei_main.m
1 %偽距定位算法（多星） 2 %2014.7.23 3 %距離單位為km 4 %----------------------------begin----------------------------------------- 5 clear;clc; 6 c=299792.458; %光速 7 8 %通過星歷參數解算到所在地可見衛星的坐標位置 9 sat13=[-7134.529244 16113.648836 23709.205570]; 10 sat22=[-22383.700040 18533.233168 5307.245613]; 11 sat23=[-5384.901317 28971.622323 2079.796362]; 12 sat14=[637.466571 28016.053841 9347.297933]; 13 sat12=[-11568.199533 -3328.511543 26977.312423]; 14 sat21=[-28908.916747 -577.061760 6051.375658]; 15 sat5=[-1205.651181 28296.890128 -8397.025036]; 16 sat4=[16456.527324 12347.282494 21199.173063]; 17 18 sat=[sat13;sat22;sat23; sat14; sat12; sat21; sat5; sat4];%多衛星位置矩陣 19 %所在地實際大地坐標，用來與定位結果作比較 20 nanjing=[-2604.298533 4743.297217 3364.978513]; 21 %理想偽距測量值 22 r0 = zeros(8,1); 23 for i = 1:8 24 r0(i,1)=norm(sat(i,:)-nanjing); 25 end 26 %加入零均值，方差為80的隨機高斯分布，模擬含有誤差的偽距r 27 r = r0 + sqrt(80)*randn(size(r0))*1e-3; 28 %-------------------------------------------------------------------------- 29 %Newton迭代法 30 %設定迭代初值，若無法估計則全部假設為0 31 xyzt=[0 0 0 0]; 32 33 for i = 1:100 % 最大迭代次數設為100次 34 f = dingwei_fun(xyzt,sat,r); 35 df = dingwei_dfun(xyzt,sat); 36 37 %左除，具有良好的數值穩定性，在MATLAB中此已經為最小二乘意義下的解 38 %delta(xyzt)=G^(-1)*b 39 delta=-df\f; 40 xyzt(1)=xyzt(1)+delta(1); 41 xyzt(2)=xyzt(2)+delta(2); 42 xyzt(3)=xyzt(3)+delta(3); 43 xyzt(4)=xyzt(4)+delta(4); 44 45 p=norm(delta); %定位精度 46 if (p<1e-10) 47 break; 48 end 49 end 50 51 disp('迭代次數為') 52 i 53 disp('用戶位置為') 54 xyzt(1:3) 55 disp('用戶鍾差為') 56 xyzt(4) 57 %--------------------------------end---------------------------------------
2、非線性方程組dingwei_fun.m
1 function f=dingwei_fun(xyzt,sat,r) 2 %多衛星定位方程函數 3 %xyzt(1:3)為用戶位置(km) 4 %xyzt(4)為用戶鍾差(s) 5 %r為測量得到的偽距(km) 6 %sat為衛星數據 7 c=299792.458; %光速 8 9 [m,n]=size(sat); 10 for i=1:m 11 f(i)=norm(sat(i,:)-xyzt(1:3))+c*xyzt(4)-r(i); 12 end 13 f=f';
3、方程組偏導數矩陣
1 %多顆衛星定位的偏導數矩陣 2 function df=dingwei_dfun(xyzt,sat) 3 %xyzt為用戶位置及鍾差 4 %sat為衛星數據 5 c=299792.458; %光速 6 7 [m,n]=size(sat); 8 xyz=xyzt(1:3); 9 for i=1:m 10 for j=1:3 11 df(i,j)=(xyz(j)-sat(i,j))/norm(sat(i,:)-xyz(j)); 12 end 13 end 14 df(:,4)=c; %線性函數ct，對t求偏導為c