function [x,theta] = WLS(XN,mean_aoa,mean_d,Q)
%PLE 此处显示有关此函数的摘要
%   此处显示详细说明
nn = size(XN,2);
A = zeros(2*nn, 6);
b = zeros(2*nn, 1);
for j=1:nn
    XN_Tag(1,j)=mean_d(j)*sind(mean_aoa(j));
    XN_Tag(2,j)=mean_d(j)*cosd(mean_aoa(j));
    A(2*j-1,:)  = [XN_Tag(1,j) 0 XN_Tag(2,j) 0 1 0];
    A(2*j,:)    = [0 XN_Tag(1,j) 0 XN_Tag(2,j) 0 1];
    b(2*j-1:2*j)=[XN(1,j) XN(2,j)];     
end 
f=(A'*A)^(-1)*A'*b;
for i=1:2
    for j=1:nn
        B(2*j-1,2*j-1) = XN(2,j)-f(6);
        B(2*j,2*j) = XN(1,j)-f(5);
    end
  W = inv(B*Q*B');
  f=(A'*W*A)\A'*W*b;
end
x=(f(5:6));
theta1(1)=atan2d(-f(2),f(1));
theta1(2)=atan2d(f(3),f(4));
% theta1(1)=acosd(-f(1));
% theta1(2)=asind(-f(2));
% theta1(3)=asind(f(3));
% theta1(4)=acosd(-f(4));

theta = mean(theta1);
end