GPS单点定位

x0=0;%初始化
y0=0;
z0=0;
for i=1:5%迭代次数，5次就差不多了
x=[4791839.793   24513555.750 14424694.880 2438267.619   24699645.220 20750469.480];%得到的xyz数据
y=[-16027953.710 2290238.988 -12687500.250 -27845730.100 -2345295.901 -18429010.410 ];
z=[23259013.420  14685609.220 20602453.000 3484060.793   14750395.800 -7962146.345];
d=[24115231.586  23852698.710 22389920.802 24577326.825  23384345.177 24479089.841];
R=((x-x0).^2+(y-y0).^2+(z-z0).^2).^0.5;
L=d-R;
A=[(x'-x0)./R' (y'-y0)./R' (z'-z0)./R' -1*ones(6,1)];
dx=-inv(A'*A)*A'*L';
x0=x0+dx(1);%改正
y0=y0+dx(2);
z0=z0+dx(3);
disp(['第',num2str(i),'次迭代','x=',num2str(x0),'  y=',num2str(y0),' z=',num2str(z0)]);
end

这是结果