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

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=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); ``

e（矩阵表示，【X’】代表矩阵X转置）。原来的一般最小二乘（OLS）公式是 b = (X’X)^(-1) * X’y

valid）。因此相应的做法是将异方差矩阵分解，并左乘到回归模型中，得到的结果就是WLS回归。比如说，异方差阵为W，且W的逆可以分解为W^(-1)
= P’P，那么经过一系列推导，可以知道 b* = (X’P’PX)^(-1) * X’P’Py

…, sn)，其中si代表第i个对角元，si ≠ sj 那么W^(-1) = diag(1/s1, 1/s2, …, 1/sn)

Σ(xi * yi/si) / Σ(xi * xi/si)

1 0