UWBIns/Code/Matlab/UWBAOALocation/script.m

clc;
clear;
close all;
filename = 'combined_data14.xlsx';

% <EFBFBD><EFBFBD>ȡExcel<EFBFBD>ļ<EFBFBD>
data = readtable(filename);

VR_X = str2double(data.VR_X);
VR_Y = str2double(data.VR_Y);
VR_Z = str2double(data.VR_Z);

Dist = data.D;
raw_pdoa = data.P;
UWB_X = data.Xcm;
UWB_Y = data.Ycm;

pdoa_offset = 0.0;
Lambda = 3e8/6.5e9;    % UWB<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
D_M = 0.0208;          % <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߼<EFBFBD><EFBFBD><EFBFBD>

raw_pdoa = raw_pdoa - pdoa_offset;
% PDOA<EFBFBD>쳣ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pdoa = OutlierFilter(raw_pdoa);

% <EFBFBD><EFBFBD><EFBFBD><EFBFBD>PDOA<EFBFBD>˲<EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD>ͼ
figure
plot(raw_pdoa);hold on
plot(pdoa);
legend('ԭʼPDOA','<EFBFBD>쳣<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PDOA');
title('PDOA<EFBFBD>쳣<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD>');
xlabel('Time');  % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>X<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǩ
ylabel('<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD> ');  % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Y<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǩ

pdoa_filtered = firstOrderFilter(pdoa, 0.9);

% <EFBFBD><EFBFBD><EFBFBD><EFBFBD>PDOA<EFBFBD>˲<EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD>ͼ
figure
plot(pdoa);hold on
plot(pdoa_filtered);
legend('δ<EFBFBD>˲<EFBFBD>PDOA','<EFBFBD>˲<EFBFBD>PDOA');
title('PDOA<EFBFBD>˲<EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD>');
xlabel('Time');  % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>X<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǩ
ylabel('<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD> ');  % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Y<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǩ

dist_filtered = firstOrderFilter(Dist, 0.9);

figure
plot(Dist);hold on
plot(dist_filtered);
legend('ԭʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>','<EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>');
title('<EFBFBD>˲<EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD>');
xlabel('Time');  % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>X<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǩ
ylabel('<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD> ');  % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Y<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǩ


% <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭʼpdoa<EFBFBD>źż<EFBFBD><EFBFBD><EFBFBD>AOA
aoa = CalUWBAOA(pdoa);
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>źż<EFBFBD><EFBFBD><EFBFBD>AOA
aoa_filtered = CalUWBAOA(pdoa_filtered);

% <EFBFBD><EFBFBD><EFBFBD><EFBFBD>AOA<EFBFBD>˲<EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD>ͼ
figure
plot(aoa);hold on
plot(aoa_filtered);
legend('δ<EFBFBD>˲<EFBFBD>AOA ','<EFBFBD>˲<EFBFBD>AOA');
title('AOA<EFBFBD><EFBFBD><EFBFBD><EFBFBD>δ<EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PODA<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD>');
xlabel('Time');  % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>X<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǩ
ylabel('Angle (<EFBFBD>Ƕȡ<EFBFBD>)');  % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Y<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǩ

filter_x = dist_filtered .* sin(aoa_filtered / 180 * pi) / 100;
filter_y = dist_filtered .* cos(aoa_filtered / 180 * pi) / 100;

% % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>AOA<EFBFBD>˲<EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD>ͼ
% figure
% plot(UWB_X/100,UWB_Y/100);hold on
% plot(filter_x,filter_y);
% plot(-VR_X,-VR_Z);
% legend('ԭʼ<EFBFBD><EFBFBD>λ ','<EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㶨λ','lighthouse<EFBFBD><EFBFBD>ֵ');
% title('<EFBFBD><EFBFBD>λ<EFBFBD>˲<EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD>');
% xlabel('X(m)');  % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>X<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǩ
% ylabel('Y(m)');  % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Y<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǩ


% <EFBFBD><EFBFBD><EFBFBD><EFBFBD>lighthouse<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
angle_degrees = 90;  % <EFBFBD><EFBFBD><EFBFBD>磬90<EFBFBD><EFBFBD>
angle_radians = deg2rad(angle_degrees);  % ת<EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
R = [cos(angle_radians) -sin(angle_radians); sin(angle_radians) cos(angle_radians)];
x_l = -VR_X;
y_l = -VR_Z;
points = [x_l'; y_l'];
rotated_points = R * points;

% <EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD> x <EFBFBD><EFBFBD> y <EFBFBD><EFBFBD><EFBFBD><EFBFBD>
x_lr = rotated_points(1, :)' + 1.0;
y_lr = rotated_points(2, :)' + 0.8;

n = 500;
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD>AOA<EFBFBD>˲<EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD>ͼ
figure
plot(filter_x(1:n),filter_y(1:n),'b.');hold on
plot(x_lr(1:n),y_lr(1:n));
legend('<EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㶨λ','lighthouse<EFBFBD><EFBFBD>ֵ');
title('<EFBFBD><EFBFBD>λ<EFBFBD>˲<EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD>');
xlabel('X(m)');  % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>X<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǩ
ylabel('Y(m)');  % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Y<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǩ

err_x = mean(abs(filter_x -x_lr));
err_y = mean(abs(filter_y -y_lr));

fprintf('UWB<EFBFBD><EFBFBD>Lighthouse<EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD> Error X: %.2f m,Y: %.2f m\n',err_x,err_y);