用于已知卫星坐标的定位

用于已知卫星坐标的定位,已知四颗卫星坐标及伪距，确定接收机所处三维坐标系的位置

function CalUserPosition = CalUserPosition（SatellitePosition）
% CalUserPosition=[X Y Z OK]返回参数定义
% 参数OK表示CalUserPosition是否成功
% 该程序用先行话方法求解4个或者多个卫星的伪距，时钟差
%具体方法可以看伪距定位原理
%假设我们接收到4个或者多个伪距后
%       /-------------------------------------------------
%      /      2        2        2
%PR=/\/ （Xi-X） + （Yi-Y） + （Zi-Z） +cti=1234
%4课以及少于四颗可以定位求解
RI=6400; 
C=300000;
DeltaT = 1e-3;%时钟误差为e-4数量级秒
SatellitePosNew = ones（13）;%一行三列全为一
VisSatNum = 0;
CalculateOK = 1;
%首先找出可以接收到的卫星，多于4个就计算，否则返回
for k=1:24
    if（SatellitePosition（k4）==1）%StatellitePosition矩阵的第四列为1，表示接收到信号
        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）;
%求解用户接收机收到的伪距信息
for n=1:VisSatNum
    Prange（1n）=sqrt（（SatellitePosNew（n:）-UserPos）...
        *（SatellitePosNew（n:）-UserPos）‘）+C*DeltaT;
end
%用户接收机测到P1 P2 P3 P4 ....并已知卫星坐标
%用此计算自己的坐标，假设初始位置和时钟差

%给用户位置赋初值
%DeltaT = 1e-3;%假设卫星时钟一致，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颗卫星，上面隐含使用了最小二乘法
    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
%返回了迭代计算的中间值和最终值！
    

     





    


              

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

     文件       2270  2017-09-08 17:20  02已知四个卫星的位置来定位\CalUserPosition.m

     文件        673  2017-09-19 15:55  02已知四个卫星的位置来定位\SatellitePosition.txt

     文件        673  2017-09-19 20:27  02已知四个卫星的位置来定位\SatellitePosition0.txt

     文件       4544  2017-09-19 20:29  02已知四个卫星的位置来定位\知道四个卫星的位置来定位.jnt

     目录          0  2017-10-30 16:11  02已知四个卫星的位置来定位

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

                 8160                    5