calculateuserposition.m

GPS伪距定位代码

%程序8 利用可见卫星计算用户位置
function CalUserPosition=CalculateUserPosition(SatellitePosition)
%CalUserPosition=[X Y Z OK]
%Parameter OK express if the CalUserPosition is success.
%该程序用线性化方法求解4个或多个卫星的伪距,钟差方程
global Satellite Position    %This is for Debug purpose, we can debug the function seperately
%具体的算法可以参见第2章伪距定位
%假设我们接收到4个或多个伪距后,有如下方程:
%       /-------------------------
%      /      2        2        2
%Pi=/\/ (xi-x) + (yi-y) + (zi-z)   +ct,  i=1,2,3,4
%用线性化的方法求解
%用于4颗卫星就用最小二乘法求解
R1=6400;         %radii of the earth
C=300000;        %the speed of light
DeltaT=1e-3;     %钟差为e-4数量级秒,假设卫星间时钟一致,DeltaT=Tu-Ts;
%光速为3*e5公里,所以误差在1000KM以内
%钟差不能超过3*e-4;
SatellitePosNew=ones(1,3);
VisSatNum=0;
CalculateOK=1;

%SatellitePosition=[17746    17572    7365    1;     %第1颗卫星可见,且x,y,z坐标已知
                   %12127    -9774    21091   1;     %第2颗卫星可见,且x,y,z坐标已知
                   %13324    -18178   14392   1;     %第3颗卫星可见,且x,y,z坐标已知
                   %14000    -13073   19058   1;     %第4颗卫星可见,且x,y,z坐标已知
                   %19376    -15756   -7365   1;     %第5颗卫星可见,且x,y,z坐标已知
                   %0        0        0       0;     %第6颗卫星不可见,赋全0
                   %0        0        0       0;     %第7颗卫星不可见,赋全0
                   %0        0        0       0;     %第8颗卫星不可见,赋全0
                   %0        0        0       0;     %第9颗卫星不可见,赋全0
                   %0        0        0       0;     %第10颗卫星不可见,赋全0
                   %0        0        0       0;     %第11颗卫星不可见,赋全0
                   %0        0        0       0;     %第12颗卫星不可见,赋全0
                   %0        0        0       0;     %第13颗卫星不可见,赋全0
                   %0        0        0       0;     %第14颗卫星不可见,赋全0
                   %0        0        0       0;     %第15颗卫星不可见,赋全0
                   %0        0        0       0;     %第16颗卫星不可见,赋全0
                   %0        0        0       0;     %第17颗卫星不可见,赋全0
                   %0        0        0       0;     %第18颗卫星不可见,赋全0
                   %0        0        0       0;     %第19颗卫星不可见,赋全0
                   %0        0        0       0;     %第20颗卫星不可见,赋全0
                   %0        0        0       0;     %第21颗卫星不可见,赋全0
                   %0        0        0       0;     %第22颗卫星不可见,赋全0
                   %0        0        0       0;     %第23颗卫星不可见,赋全0
                   %0        0        0       0;     %第24颗卫星不可见,赋全0
                   %6400     0        0       0];    %25行前三列为用户坐标
%the following code find out the visible satelite position
%put it in a new matrix
for k=1:24
    if(SatellitePosition(k,4)==1)
        VisSatNum=VisSatNum+1;
        SatellitePosNew=[SatellitePosNew;SatellitePosition(k,1:3)];
    end   %if
end       %for
SatellitePosNew(1,:)=[];
if(VisSatNum<4)
    CalculateOK=0;
    CalUserPosition=[0 0 0 0];
    return
end
Prange=ones(1,VisSatNum);
UserPos=SatellitePosition(25,1:3);
%求解用户接收机收到的信息
for n=1:VisSatNum
    Prange(1,n)=sqrt((SatellitePosNew(n,:)-UserPos)*(SatellitePosNew(n,:)-UserPos)')+C*DeltaT;
end
%用户接收机测到了P1,P2,P3,P4,...,并已知卫星坐标
%用此计算自己的坐标,假设初始位置和钟差
%At first we guess the user is at center inside of the earth
%DeltaT=1e-3;
%钟差为e-4数量级秒,假设卫星间时钟一致,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(1,VisSatNum);
    for n=1:VisSatNum
        R(1,n)=sqrt((Wxyz(n,:)-XYZ0)*(Wxyz(n,:)-XYZ0)')+DeltaT0*C;
    end
    DeltaP=R-Prange;
    A=ones(VisSatNum,3);
    for n=1:VisSatNum
        A(n,:)=(Wxyz(n,:)-XYZ0)./R(1,n);
    end   %for
    H=[A ones(VisSatNum,1)];
    DeltaX=inv(H'*H)*H'*DeltaP';
    TempDeltaX=DeltaX(1:3);
    Error=max(abs(TempDeltaX));
    XYZ0=XYZ0+DeltaX(1:3,:)';
    if(ComputeTime<10)
        CalculateRecord=[CalculateRecord;XYZ0];
    end
    DeltaT0=DeltaX(4,1)/(-C);
end   %while
if(ComputeTime==1000)
    CalUserPosition=[0 0 0];
else
    CalUserPosition=[XYZ0;CalculateRecord];
end