更新UWB-AOA的解算框架

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
+

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
+

Code/Matlab/UWBAOALocation/README.md

@@ -0,0 +1 @@
+# UWB-AOA 位置解算框架

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

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);
+
+