伪距定位算法

A=[15524471.175   -16649826.222   13512272.387 
22274133.8630

-2304058.534   23287906.465    11917038.105  19920232.728

16680243.357   -3069625.561    23378551.047  22566287.8520

-14799931.395  -21425358.240   6069947.224  
21491226.260];

B=[-730000 -5440000 3230000];%用户位置近似

B0=[0,0,0,0];                %定位初始值

xyzt=[1 1 1 1];

i=0;

while(sqrt(xyzt(1,1)^2+xyzt(1,2)2+xyzt(1,3)^2+xyzt(1,4)2)>1.0e-5)   %判断收敛性

for i=1:4

R(1,i)=sqrt((A(i,1)-B0(1,1))^{2+(A(i,2)-B0(1,2))}2+(A(i,3)-B0(1,3))^2);           %观测矢量长度

end

for i=1:4

for j=1:3

dr(i,j)=-(A(i,j)-B0(1,j))/R(1,i);     
%单位观测矢量xyz分量

end

end

G=ones(4);

for i=1:4     
G(i,1:3)=dr(i,:);              %几何矩阵

BB(i,:)=A(i,4)-R(1,i)-B0(1,4);

end

XYZT=((G’*G)^(-1))*G’*BB;

%最小二乘法求解线性方程组

xyzt=XYZT’;

B0=B0+xyzt;              %接收机位置坐标

i=i+1;                    %迭代次数

end

disp(‘接收机位置及钟差º’)

B0

disp(‘迭代次数’)

i