最小二乘定位解算matlab代码

合集下载

相关主题

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

R1 =6378137;

C = 3e8;

DeltaT = 1e-3;

SatellitePosition = [17746 17572 7365 1;12127 -9774 21091 1;13324 -18178 14392 1;14000 -13073 19058 1; 19376 -15756 -7365 1;zeros(19,4);6400 200 -1200 0];

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 = [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

CalculateRecord = [1 1 1];

XYZ0 = [6400 100 -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 = [0 0 0];

else

for m = 1:1:3

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

end

CalUserPosition = XYZ

end