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

% 读取Excel文件
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波长
D_M = 0.0208;          % 久凌天线间距

raw_pdoa = raw_pdoa - pdoa_offset;
% PDOA异常值处理
pdoa = OutlierFilter(raw_pdoa);

% 绘制PDOA滤波前后对比图
figure
plot(raw_pdoa);hold on
plot(pdoa);
legend('原始PDOA','异常处理后的PDOA');
title('PDOA异常处理前后对比');
xlabel('Time');  % 设置X轴标签
ylabel('相位差 ');  % 设置Y轴标签

pdoa_filtered = firstOrderFilter(pdoa, 0.9);

% 绘制PDOA滤波前后对比图
figure
plot(pdoa);hold on
plot(pdoa_filtered);
legend('未滤波PDOA','滤波PDOA');
title('PDOA滤波前后对比');
xlabel('Time');  % 设置X轴标签
ylabel('相位差 ');  % 设置Y轴标签

dist_filtered = firstOrderFilter(Dist, 0.9);

figure
plot(Dist);hold on
plot(dist_filtered);
legend('原始测距','滤波后测距');
title('滤波前后对比');
xlabel('Time');  % 设置X轴标签
ylabel('相位差 ');  % 设置Y轴标签


% 利用原始pdoa信号计算AOA
aoa = CalUWBAOA(pdoa);
% 利用滤波后的信号计算AOA
aoa_filtered = CalUWBAOA(pdoa_filtered);

% 绘制AOA滤波前后对比图
figure
plot(aoa);hold on
plot(aoa_filtered);
legend('未滤波AOA ','滤波AOA');
title('AOA采用未滤波和滤波后的PODA计算对比');
xlabel('Time');  % 设置X轴标签
ylabel('Angle (角度°)');  % 设置Y轴标签

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

% % 绘制AOA滤波前后对比图
% figure
% plot(UWB_X/100,UWB_Y/100);hold on
% plot(filter_x,filter_y);
% plot(-VR_X,-VR_Z);
% legend('原始定位 ','滤波后解算定位','lighthouse真值');
% title('定位滤波前后对比');
% xlabel('X(m)');  % 设置X轴标签
% ylabel('Y(m)');  % 设置Y轴标签


% 创建lighthouse的旋转矩阵
angle_degrees = 90;  % 例如，90度
angle_radians = deg2rad(angle_degrees);  % 转换为弧度
% 创建旋转矩阵
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;

% 提取旋转后的 x 和 y 向量
x_lr = rotated_points(1, :)' + 1.0;
y_lr = rotated_points(2, :)' + 0.8;

n = 500;
% 绘制AOA滤波前后对比图
figure
plot(filter_x(1:n),filter_y(1:n),'b.');hold on
plot(x_lr(1:n),y_lr(1:n));
legend('滤波后解算定位','lighthouse真值');
title('定位滤波前后对比');
xlabel('X(m)');  % 设置X轴标签
ylabel('Y(m)');  % 设置Y轴标签

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

fprintf('UWB和Lighthouse真值误差 Error X: %.2f m,Y: %.2f m\n',err_x,err_y);