caluserposition.m

上一个的继续,连续有九个压缩文件,大家可以打开解压后自己慢慢享用,这个都是本人调试通过了的,希望共同进步!

SatellitePosition=[17746   17572   7356    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行前三列为用户坐标

%%%%%%%%%%%%%%%%%%%%%%%%%%function CalUserPosition=CalculateUserPosition(SatellitePosition)
%CalUserPosition=[X Y Z OK]返回参数定义
%参数OK表示CalUserPosition是否成功
%该程序用线性化方法求解4个或者多余4个卫星的伪距，钟差方程
% 具体算法采用伪距定位原理
%假如我们收到4个或者多余四个伪距后，有如下方程：
%       /---------------------------------     
%      /          
%PR=/\/     (xi-x)^2+(yi-y)^2+(zi-z)^2+ct ,i=1,2,3,4
%用线性化方法求解
%4颗或少于4颗卫星可以定位求解，多余4颗卫星用LS法求解
R1=6400;        %地球半径
c=300000;       %光速
DeltaT=1e-3;    %钟差为1e-4数量级秒，假设卫星间时钟一致，DelaT=Tu-Ts
%光速为3*e5公里，所以误差在1000KM以内
%钟差不宜超过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颗卫星
    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
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
    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
        DeltaaT0=DeltaX(4,1)/(-c);
    end
    if(ComputeTime==1000)
        CalUserPosition=[0 0 0];
    else
        CalUserPosition=[XYZ0;CalculateRecord];
    end
    
    plot(CalUserPosition);
    hold on;
    
   %返回迭代计算中间值和最终值 ！