function?CalUserPosition?=?CalUserPosition（SatellitePosition）
%?CalUserPosition=[X?Y?Z?OK]返回參數(shù)定義
%?參數(shù)OK表示CalUserPosition是否成功
%?該程序用先行話方法求解4個(gè)或者多個(gè)衛(wèi)星的偽距，時(shí)鐘差
%具體方法可以看偽距定位原理
%假設(shè)我們接收到4個(gè)或者多個(gè)偽距后
%???????/-------------------------------------------------
%??????/??????2????????2????????2
%PR=/\/?（Xi-X）?+?（Yi-Y）?+?（Zi-Z）?+cti=1234
%4課以及少于四顆可以定位求解
RI=6400;?
C=300000;
DeltaT?=?1e-3;%時(shí)鐘誤差為e-4數(shù)量級(jí)秒
SatellitePosNew?=?ones（13）;%一行三列全為一
VisSatNum?=?0;
CalculateOK?=?1;
%首先找出可以接收到的衛(wèi)星，多于4個(gè)就計(jì)算，否則返回
for?k=1:24
????if（SatellitePosition（k4）==1）%StatellitePosition矩陣的第四列為1，表示接收到信號(hào)
????????VisSatNum?=?VisSatNum+1;
????????SatellitePosNew?=?[SatellitePosNew;SatellitePosition（k1:3）]
????end
end
SatellitePosNew（1:）=[];
if（VisSatNum?<4）
????CalculateOK?=?0;
????CalUserPosition=[0?0?0?0];
????return
end
Prange?=?ones（1VisSatNum）;
UserPos?=?SatellitePosition（251:3）;
%求解用戶接收機(jī)收到的偽距信息
for?n=1:VisSatNum
????Prange（1n）=sqrt（（SatellitePosNew（n:）-UserPos）...
????????*（SatellitePosNew（n:）-UserPos）‘）+C*DeltaT;
end
%用戶接收機(jī)測到P1?P2?P3?P4?....并已知衛(wèi)星坐標(biāo)
%用此計(jì)算自己的坐標(biāo)，假設(shè)初始位置和時(shí)鐘差

%給用戶位置賦初值
%DeltaT?=?1e-3;%假設(shè)衛(wèi)星時(shí)鐘一致，DeltaT?=?Tu-Ts
CalculateRecord?=?[1?1?1];
XYZ0?=?[0?0?0];
DeltaT0?=?0;
Wxyz?=?SatellitePosNew;
Error?=1000;?ComputeTime?=?0;
while?（Error>1）&（ComputeTime<1000）
????ComputeTime?=?ComputeTime?+?1;
????R?=?ones（1VisSatNum）;
for?n=1:VisSatNum
????R（1n）=sqrt（（Wxyz（n:）-XYZ0）*（Wxyz（n:）-XYZ0）‘）+DeltaT0*C;
end
DeltaP?=?R-Prange;
A=ones（VisSatNum3）;
for?n=1:VisSatNum
????A（n:）?=?（Wxyz（n:）-XYZ0）./R（1n）;
end
H=[A?ones（VisSatNum1）];
DeltaX?=?inv（H‘*H）*H‘*DeltaP‘;
????%多于4顆衛(wèi)星，上面隱含使用了最小二乘法
????TempDeltaX?=?DeltaX（1:3）;
????Error?=?max（abs（TempDeltaX））;
?????XYZ0=XYZ0?+?DeltaX（1:3:）‘;
?????if（ComputeTime<10）
?????CalculateRecord=[CalculateRecord;XYZ0];
end
????DeltaT0?=?DeltaX（41）/（-C）;
end
if?（ComputeTime==1000）
????CalUserPosition?=?[0?0?0];
else?
????CalUserPosition?=?[XYZ0;CalculateRecord];
end
%返回了迭代計(jì)算的中間值和最終值！
????

?????





????


??????????????

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----

?????文件???????2270??2017-09-08?17:20??02已知四個(gè)衛(wèi)星的位置來定位\CalUserPosition.m

?????文件????????673??2017-09-19?15:55??02已知四個(gè)衛(wèi)星的位置來定位\SatellitePosition.txt

?????文件????????673??2017-09-19?20:27??02已知四個(gè)衛(wèi)星的位置來定位\SatellitePosition0.txt

?????文件???????4544??2017-09-19?20:29??02已知四個(gè)衛(wèi)星的位置來定位\知道四個(gè)衛(wèi)星的位置來定位.jnt

?????目錄??????????0??2017-10-30?16:11??02已知四個(gè)衛(wèi)星的位置來定位

-----------?---------??----------?-----??----

?????????????????8160????????????????????5