forked from ZhanLi/UWBIns
34 lines
781 B
Mathematica
34 lines
781 B
Mathematica
|
function [x,theta] = WLS(XN,mean_aoa,mean_d,Q)
|
|||
|
|
%PLE <EFBFBD>˴<EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD>йش˺<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ժҪ
|
|||
|
|
% <EFBFBD>˴<EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD>ϸ˵<EFBFBD><EFBFBD>
|
|||
|
|
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
|
|||
|
|
|