三边定位的最小二乘法运用

已知：条件基站到移动台的距离D，基站的坐标(xi,yi,zi)
求：移动台的坐标(x,y,z)
运用最小二乘法的核心是找到最小二乘法的矩阵形式即
Ax=B
在本次实验中通过三边定位计算得到

A=[A;2*(xi-x1),2*(yi-y1),2*(zi-z1];  %系数矩阵A
b=[b;D1^2-Di^2+xi^2+yi^2+zi^2-x1^2-y1^2-z1^2];      %增广矩阵b

根据这两个矩阵就可以求出预测的x矩阵：
x=inv(A’A)*(A’b)

matlab 代码实现

 clc
    clear
xyz = zeros(2,1);
% 设定基站的值
A=[0,0,0];
B=[0,8,0];
C=[6,0,0];
D=[0,0,3];
data=rand(1000,3)*10;
data(:,1) = rand(1000,1)*6;
data(:,2) = rand(1000,1)*8;
data(:,3) = rand(1000,1)*3;
length(data);
rtlength =zeros(length(