? ? ? 在各種偽距定位算法中，最小二乘法是一種比較簡單而廣泛的方法，該算法可以分為以下幾步： ? ? ?

1、準(zhǔn)備數(shù)據(jù)與設(shè)置初始值

這里準(zhǔn)備數(shù)據(jù)，主要是對于各顆可見衛(wèi)星，收集到它們在同一時刻的偽距測量值，計(jì)算測量值的各項(xiàng)偏差、誤差成分的校正量，然后計(jì)算出誤差校正后的偽距測量值，這里假設(shè)偽距為理想距離加上隨機(jī)高斯誤差。設(shè)置初始值，假設(shè)大概知道位置坐標(biāo)，則設(shè)定其為初始值，也可根據(jù)上一次定位結(jié)果設(shè)定；若什么都不了解，那么初值設(shè)置為0，只不過多幾次迭代過程罷了。

2、非線性方程組線性化（不詳細(xì)解釋，就是得到雅克比矩陣）。

3、求解線性方程組，此處運(yùn)用最小二乘法。

4、更新非線性方程組的根

5、判斷牛頓迭代的收斂性，用定位結(jié)果的二范數(shù)是否小于定位精度為判定標(biāo)準(zhǔn)。

貼出偽距定位算法的MATLAB代碼：

1、主函數(shù)dingwei_main.m

1 %偽距定位算法（多星） 2 %2014.7.23 3 %距離單位為km 4 %----------------------------begin----------------------------------------- 5 clear;clc; 6 c=299792.458; %光速 7 8 %通過星歷參數(shù)解算到所在地可見衛(wèi)星的坐標(biāo)位置 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];%多衛(wèi)星位置矩陣 19 %所在地實(shí)際大地坐標(biāo)，用來與定位結(jié)果作比較 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的隨機(jī)高斯分布，模擬含有誤差的偽距r 27 r = r0 + sqrt(80)*randn(size(r0))*1e-3; 28 %-------------------------------------------------------------------------- 29 %Newton迭代法 30 %設(shè)定迭代初值，若無法估計(jì)則全部假設(shè)為0 31 xyzt=[0 0 0 0]; 32 33 for i = 1:100 % 最大迭代次數(shù)設(shè)為100次 34 f = dingwei_fun(xyzt,sat,r); 35 df = dingwei_dfun(xyzt,sat); 36 37 %左除，具有良好的數(shù)值穩(wěn)定性，在MATLAB中此已經(jīng)為最小二乘意義下的解 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('迭代次數(shù)為') 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 %多衛(wèi)星定位方程函數(shù) 3 %xyzt(1:3)為用戶位置(km) 4 %xyzt(4)為用戶鐘差(s) 5 %r為測量得到的偽距(km) 6 %sat為衛(wèi)星數(shù)據(jù) 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、方程組偏導(dǎo)數(shù)矩陣

1 %多顆衛(wèi)星定位的偏導(dǎo)數(shù)矩陣 2 function df=dingwei_dfun(xyzt,sat) 3 %xyzt為用戶位置及鐘差 4 %sat為衛(wèi)星數(shù)據(jù) 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; %線性函數(shù)ct，對t求偏導(dǎo)為c

?

?

?

?

轉(zhuǎn)載于:https://www.cnblogs.com/iamlsj/p/3870023.html

《新程序員》：云原生和全面數(shù)字化實(shí)踐50位技術(shù)專家共同創(chuàng)作，文字、視頻、音頻交互閱讀

總結(jié)

以上是生活随笔為你收集整理的伪距定位算法(matlab版)的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。