GPS卫星定位MATLAB代码

function [CalUserPosition, CalculateOK] = CalculateUserPosition(SatellitePosition) SatellitePosition=[17746 17572 7365 1; 12127 -9774 21091 1; 13324 -18178 14392 1; 14000 -13073 19058 1; 19376 -15756 -7365 1; zeros(19, 4); %卫星不可见，坐标赋0值 2013 4933 5467 0]; %第25行前三列为用户真实坐标 %输入参数： %卫星位置坐标SatellitePosition %输出参数： %用户位置坐标：CalUserPosition，是一个矩阵，第一行表示最终定位结果，后面 几行显示定位计算的中间过程结果； %参数CalculateOK表示用户位置计算是否成功，1为成功，0为失败； %该程序用线性化方法求解四个或多个卫星的伪距、钟差方程 %假设我们接收到4个或者更多伪距后，有如下方程 %PR = sqrt( (xj-x)^2 + (yj-y)^2 + (zj-z)^2 ) + ct , j=1,2,3,4 %使用最小二乘法求解 c=3e5; %光速，单位：km/s； DeltaT=1e-3; %钟差为1e-4数量级秒，假设卫星钟间时钟一致， DeltaT=Tu-Ts；钟差不宜超过3e-4，否则不收敛； VisSatNum=0; CalculateOK=1; %首先找出可以接收到的卫星，多于4颗继续运算，否则返回 SatellitePosNew=[]; for k=1:24 if SatellitePosition(k,4)==1 VisSatNum=VisSatNum+1; SatellitePosNew=[SatellitePosNew; SatellitePosition(k,1:3)]; end %if end %for if VisSatNum<4 %不足4颗可见卫星 CalculateOK=0; CalUserPosition=[0 0 0]; return end %if 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 XYZ0=[0 0 0]; %给用户位置赋初值 CalculateRecord=XYZ0; %此变量用于保存每一步迭代计算的中间结果 DeltaT0=0; %时钟差初始值 Wxyz=SatellitePosNew; %卫星位置坐标 Error=1000; ComputeTime=0; while (Error>0.01) && (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 %for 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'; %最小二乘法求卫星位置 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]; CalculateOK=0; else CalUserPosition=[XYZ0; CalculateRecord]; end