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

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

A=[A;2*(xi-x1),2*(yi-y1),2*(zi-z1];  %系数矩阵Ab=[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    clearxyz = 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(data),4);for t=1:length(data)    rtlength(t,1) =sqrt((data(t,1)-A(1))^2+(data(t,2)-A(2))^2+(data(t,3)-A(3))^2)+(rand(1)-0.5)*1;    rtlength(t,2) =sqrt((data(t,1)-B(1))^2+(data(t,2)-B(2))^2+(data(t,3)-B(3))^2)+(rand(1)-0.5)*1;     rtlength(t,3) =sqrt((data(t,1)-C(1))^2+(data(t,2)-C(2))^2+(data(t,3)-C(3))^2)+(rand(1)-0.5)*1;    rtlength(t,4) =sqrt((data(t,1)-D(1))^2+(data(t,2)-D(2))^2+(data(t,3)-D(3))^2)+(rand(1)-0.5)*1;end% rtlength 每一行为随机点到4个基站的距离plot3(data(:,1),data(:,2),data(:,3),'.');hold on plot3(A(1),A(2),A(3),'*r');plot3(B(1),B(2),B(3),'*r');plot3(C(1),C(2),C(3),'*r');plot3(D(1),D(2),D(3),'*r');axis ([0 6 0 8 0 3])grid on;xyz=zeros(3,length(rtlength));for h=1:length(rtlength)              AP =[0,0,0                  0,8,0                  6,0,0                  0,0,3] ;              num_ap = length(AP);              if num_ap>=4              Q =zeros(1,num_ap);              for j=1:num_ap                 Q(1,j) =0.5;              end               Q = diag(Q);              for i=1:num_ap                       Node(i)=AP(i,1)^2+AP(i,2)^2+AP(i,3)^2;    %固定参数便于位置估计              end              A=[];b=[];              L =rtlength(h,:);       %TOA测距              for i=1:num_ap             %三边定位公式逐一作差化成矩阵：A*x=b                  A=[A;2*(AP(i,1)-AP(1,1)),2*(AP(i,2)-AP(1,2)),2*(AP(i,3)-AP(1,3))];  %系数矩阵A                  b=[b;L(1)^2-L(i)^2+Node(i)-Node(1)];      %增广矩阵b              end               x=inv(A'*inv(Q)*A)*(A'*inv(Q)*b)      %利用最小二乘法求解目的点坐标位置               xyz(:,h)=x;%                sprintf('%2.2f%%', (line/bg)*100)    %%2.2f是保留2位小数了，也可以直接写%f              endendxyzzz = zeros(length(data),3);xyzzz = xyz';answerx = 0;answery = 0;for i=1:length(xyzzz)    answerx = answerx+abs(xyzzz(i,1)-data(i,1));    answery = answery+abs(xyzzz(i,2)-data(i,2));endanswerx = answerx/length(xyzzz);answery = answery/length(xyzzz); 

上述程序固定4个基站，在一个固定空间中生成1000个随机的移动台，在距离上添加一定的随机误差，用添加误差后的距离与4个基站坐标进行预测目标点的坐标，同时计算误差，同时注意的是所用的最小二乘法是加权后的最小二乘法
最小二乘法和加权后的最小二乘法的区别：

加权最小二乘（WLS）最一般的用法是克服异方差。比方说，现在有一个多元回归y = bX +
e（矩阵表示，【X’】代表矩阵X转置）。原来的一般最小二乘（OLS）公式是 b = (X’X)^(-1) * X’y

而在异方差情况下，由于不满足OLS的五大假定，因此OLS的结果不再有效（not efficient，不是not
valid）。因此相应的做法是将异方差矩阵分解，并左乘到回归模型中，得到的结果就是WLS回归。比如说，异方差阵为W，且W的逆可以分解为W^(-1)
= P’P，那么经过一系列推导，可以知道 b* = (X’P’PX)^(-1) * X’P’Py

换言之，正如题主所言，要用矩阵P去变换这个X和y，从而得到WLS回归，其中W矩阵里的元素，就是权重（weight）。
至于选择什么权重，就取决于W矩阵的设定形式。

举个简单的例子，设一个一元回归y = bx + e，**而扰动项e的协方差阵W是一个对角矩阵**ps。。。这句话博主还没有看懂如果干扰值时一个随机变量，一个随机变量如何求协方差矩阵，即W = diag(s1, s2,
…, sn)，其中si代表第i个对角元，si ≠ sj 那么W^(-1) = diag(1/s1, 1/s2, …, 1/sn)
如果用sqrt(a)表示a的开方，那么P矩阵就是P = diag(sqrt(1/s1), … sqrt(1/sn)) 从而说b* =
Σ(xi * yi/si) / Σ(xi * xi/si)
可以看到，权重在这里是1/si，而对数据的变换方法是每个数据都乘以sqrt(1/si)