forked from ZhanLi/UWBIns
115 lines
2.6 KiB
Matlab
115 lines
2.6 KiB
Matlab
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);
|
||
|
||
|