diff --git a/Code/Matlab/UWBAOALocation/CalUWBAOA.m b/Code/Matlab/UWBAOALocation/CalUWBAOA.m new file mode 100644 index 0000000..c194124 --- /dev/null +++ b/Code/Matlab/UWBAOALocation/CalUWBAOA.m @@ -0,0 +1,24 @@ +function [ aoa ] = CalUWBAOA(pdoa) + Lambda = 3e8/6.5e9; % UWB波长 + D_M = 0.0208; % 久凌天线间距 + aoa = zeros(length(pdoa),1); + for ii=1:length(pdoa) + if pdoa(ii) < -180.0 + pdoa(ii)=pdoa(ii)+ 360.0; + elseif pdoa(ii) > 180.0 + pdoa(ii)=pdoa(ii)-360.0; + end + phase_m = pdoa(ii) * (Lambda/360.0); + % 天线非线性矫正 + % coef=[-14205, 419, 4.59, 0.8361,0]; + % phase_m = coef(1)+coef(2)*phase_m+coef(3)*phase_m^2+coef(4)*phase_m^3+coef(5)*phase_m^4; + alfa = phase_m / D_M; + if alfa < -1 + alfa = -1; + elseif alfa > 1 + alfa = 1; + end + aoa(ii)=asind(alfa); + end +end + diff --git a/Code/Matlab/UWBAOALocation/OutlierFilter.m b/Code/Matlab/UWBAOALocation/OutlierFilter.m new file mode 100644 index 0000000..56fa54b --- /dev/null +++ b/Code/Matlab/UWBAOALocation/OutlierFilter.m @@ -0,0 +1,38 @@ +function [ filtered ] = OutlierFilter( data ) + init_count = 0; + init_flag = 0; + filtered = zeros(1, length(data)); + filtered(1) = data(1); + for i = 2:length(data) + % 等待数据初始化、数据没有过大的波动才能正确追踪数据 + if(abs(data(i) - data(i-1)) < 30 && init_flag == 0) + filtered(i) = data(i); + init_count = init_count + 1; + if(init_count >= 5) + init_flag = 1; + end + continue; + end + % 如果没有初始化则不做任何处理 + if(init_flag == 0) + filtered(i) = data(i); + continue; + end + + % 默认情况下，滤波后数据等于原始值 + filtered(i) = data(i); + % 如果数据出现过大的波动 + if(abs(filtered(i) - filtered(i - 1)) > 40) + % 计算数据反向后的差距, 因为目前数据经常看到相位差刚好是反向的 + dif1 = abs(data(i) + filtered(i - 1)); + % 如果反向后差别很小，说明是反向的错误，直接用当前值取负处理填充 + if(dif1 < 20) + filtered(i) = -data(i); + else + % 如果数据不完全是反向的，那就用上一个滤波值 + filtered(i) = filtered(i-1); + end + end + end +end + diff --git a/Code/Matlab/UWBAOALocation/README.md b/Code/Matlab/UWBAOALocation/README.md new file mode 100644 index 0000000..0564220 --- /dev/null +++ b/Code/Matlab/UWBAOALocation/README.md @@ -0,0 +1 @@ +# UWB-AOA 浣嶇疆瑙ｇ畻妗嗘灦 \ No newline at end of file diff --git a/Code/Matlab/UWBAOALocation/combined_data14.xlsx b/Code/Matlab/UWBAOALocation/combined_data14.xlsx new file mode 100644 index 0000000..e2069c7 Binary files /dev/null and b/Code/Matlab/UWBAOALocation/combined_data14.xlsx differ diff --git a/Code/Matlab/UWBAOALocation/firstOrderFilter.m b/Code/Matlab/UWBAOALocation/firstOrderFilter.m new file mode 100644 index 0000000..50ab787 --- /dev/null +++ b/Code/Matlab/UWBAOALocation/firstOrderFilter.m @@ -0,0 +1,18 @@ +function filtered_data = firstOrderFilter(data, alpha) + % 数据校验 + if ~isvector(data) + error('Data input must be a vector.'); + end + if alpha < 0 || alpha > 1 + error('Alpha must be between 0 and 1.'); + end + + % 初始化过滤后的数据数组 + filtered_data = zeros(size(data)); + filtered_data(1) = data(1); % 初始化第一个元素，通常设为原始数据的第一个值 + + % 应用一阶滑动窗口滤波 + for i = 2:length(data) + filtered_data(i) = alpha * filtered_data(i - 1) + (1 - alpha) * data(i); + end +end diff --git a/Code/Matlab/UWBAOALocation/script.m b/Code/Matlab/UWBAOALocation/script.m new file mode 100644 index 0000000..57b8ee0 --- /dev/null +++ b/Code/Matlab/UWBAOALocation/script.m @@ -0,0 +1,114 @@ +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); + +