最小二乘定位解算matlab代码

合集下载

相关主题

1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。

R1=6378137;

C=3e8;

DeltaT=1e-3;

SatellitePosition=[177461757273651;12127-9774210911;13324-181********;14000 -130********;19376-15756-73651;zeros(19,4);6400200-12000];

SatellitePosNew=ones(1,3);

VisSatNum=0;

%VisSatNum:可见的卫星个数;

CalculateOK=1;

for k=1:24

if(SatellitePosition(k,4)==1)

VisSatNum=VisSatNum+1;

SatellitePosNew=[SatellitePosNew;SatellitePosition(k,1:3)];

end

SatellitePosNew(1,:)=[];%把SatellitePosNew的第一行去掉

if(VisSatNum<4)

CalculateOK=0;

CalUserPosition=[000];

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

CalculateRecord=[111];

XYZ0=[6400100-1200];

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)')+DeltaT*C;%DeltaT0????

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'

TempDeltaX=DeltaX(1:3);

Error=max(abs(TempDeltaX));

XYZ0=XYZ0+DeltaX(1:3,:)';

if(ComputeTime<100)

CalculateRecord=[CalculateRecord;XYZ0];

end

DeltaT0=DeltaX(4,1)/(-C);

end%while

if(ComputeTime==1000)

CalUserPosition=[000];

else

for m=1:1:3

XYZ(m)=XYZ0(m)+DeltaX(m)

end

CalUserPosition=XYZ

end