caluserposition.m

GPS 的地球卫星运转情况仿真 还可以生成GPS信号

function CalUserPosition=CalUserPosition(SatellitePosition)
%CalUserPosition=[X Y Z OK]返回参数定义
%参数OK表示CalUserPosition是否成功
%该程序用显性化方法求解4个或多个卫星的伪距、钟差方程
%具体的算法可以参见本书前文所述的伪距定位原理
%假设我们接收到4个或多个伪距后，有如下的方程
%        /ˉˉˉˉˉˉˉˉˉˉˉˉˉˉˉˉˉˉˉˉˉˉ
%      /
%PR=/\/
%用线性化的方法求解
%4颗及少于4颗卫星可以定位求解，多于4颗卫星的用最小二乘法求解
R1=6400; %地球的半径
C=30000;%光速
DeltaT=1e-3; %钟差为e-4数量级秒，假设卫星间时钟一致，DeltaT=Tu-Ts;
%光速为3*e5公里，所以误差在1000公里以内；
%钟差不易超过3*e-4,否则不收敛；

SatellitePosNew=ones(1,3);
VisSatNum=0;
CalculateOK=1;
%首先可以接收到的卫星，多于4颗继续运算，否则返回；
for k=1:24
    if (SatellitePosition(k,4) ==1)
       VisSatNum=VisSatNum+1;
       SatellitePosNew=[SatellitePosNew;SatellitePosition(k,1:3)]
    end
end
SatellitePosNew(1,:)=[];
if (VisSatNum<4) %不足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....并以知卫星坐标，
%用此计算自己的坐标，假设初始位置和钟差
%给用户位置赋初值；
%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';
     %多于4颗卫星，以上隐含了使用最小二乘法
     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
 %返回了迭代计算的中间值和最终值！