背景
无人机追踪无人车应用场景
在现代科技发展背景下，无人机对无人车的追踪在多个领域具有重要应用。在智能交通系统中，无人机可追踪无人车，用于实时监测交通流量、路况，辅助无人车规划最优路径，提高整体交通效率。在物流配送场景里，无人机能追踪运输货物的无人车，实时掌握运输状态，及时发现潜在问题，如车辆故障、偏离路线等，保障货物按时、准确送达。在安防监控领域，无人机追踪无人车可用于边境巡逻、重要区域安保等任务，增强安全防控能力。
追踪面临的挑战
然而，实现无人机对无人车的精确追踪面临诸多挑战。一方面，无人车的运动具有不确定性，其行驶速度、方向可能因路况、任务需求等因素频繁变化，这使得准确预测其位置变得困难。另一方面，环境因素会对追踪造成干扰，如恶劣天气（暴雨、沙尘等）会影响传感器性能，导致获取的无人车位置信息不准确；复杂地形（山区、城市高楼林立区域）可能遮挡信号，使无人机与无人车之间的通信中断或数据传输不完整。此外，无人机自身的运动也存在噪声和误差，如飞行姿态的微小变动会影响其对无人车位置的测量精度。
自适应卡尔曼滤波器的优势
自适应卡尔曼滤波器为解决这些挑战提供了有效途径。与传统卡尔曼滤波器相比，它能够根据系统状态的变化自动调整滤波器参数，更好地适应无人车运动的不确定性以及环境干扰。自适应卡尔曼滤波器可以实时估计系统噪声和测量噪声的统计特性，从而在不同的情况下都能保持较高的追踪精度，增强追踪系统的鲁棒性和可靠性，满足无人机对无人车追踪在复杂多变环境下的需求。
原理
卡尔曼滤波器基础原理
自适应卡尔曼滤波器原理
- 噪声估计与调整：自适应卡尔曼滤波器的核心在于能够实时估计过程噪声协方差 Qk 和观测噪声协方差 Rk。常见的自适应方法有基于极大似然估计、自适应遗忘因子等。以基于极大似然估计为例，通过对观测数据的统计分析，计算出使得观测数据出现概率最大的噪声协方差值。具体来说，假设在一段时间内收集了一系列观测值 {zi}i=1k，根据卡尔曼滤波器的预测和更新公式，建立关于 Qk 和 Rk 的似然函数 L(Qk,Rk)，通过优化算法（如梯度下降法）求解使 L(Qk,Rk) 最大的 Qk 和 Rk 值。这样，当无人车运动状态发生变化（如突然加速、转弯）或环境干扰改变（如进入不同天气区域）时，自适应卡尔曼滤波器能够自动调整噪声协方差，使滤波器更好地适应实际情况，提高追踪精度。
- 模型自适应调整：除了噪声协方差的自适应估计，自适应卡尔曼滤波器还可以对状态转移模型和观测模型进行自适应调整。例如，当发现无人车的运动模式与原状态转移模型假设不符时（如原本假设无人车做匀速直线运动，实际开始做曲线运动），可以通过对观测数据的分析和机器学习算法，调整状态转移矩阵 Fk 的参数，使其更符合无人车的实际运动。同样，对于观测模型，如果发现传感器在某些情况下测量误差较大，可以调整观测矩阵 Hk 或增加观测方程的补偿项，以提高观测数据与实际状态之间的映射准确性。这种模型的自适应调整进一步增强了自适应卡尔曼滤波器在复杂多变环境下追踪无人车的能力。
通过基于自适应卡尔曼滤波器，无人机能够更准确地追踪无人车，克服无人车运动不确定性和环境干扰带来的挑战，为相关应用场景提供可靠的追踪解决方案。
运行结果
核心代码
cclc; clear;
%% Load Data
load('uav_ekf_output.mat'); % Should contain: v_uav, omega_uav
load('ugv_ekf_output.mat'); % Should contain: v_ugv, omega_ugv
tags = readtable('noisy_apriltag_data.csv');
v_ugv = [vx_ugv, vy_ugv,zeros(1000,1)];
v_uav = [vx_uav',vy_uav',vz_uav'];
% Extract measurements
z_meas = [tags.rel_x, tags.rel_y, tags.rel_z];
% Time step (assumed constant)
dt = 0.025; % 40 Hz sampling
% Data length
N = size(z_meas, 1);
%% EKF Initialization
x_est = zeros(6, N); % [x, y, z, vx, vy, vz]
P = eye(6); % Initial state covariance
% Process noise covariance (Q)
Q = diag([0.01, 0.01, 0.01, 0.1, 0.1, 0.1]);
% Measurement noise covariance (R)
R = diag([0.05, 0.05, 0.05]); % Tuned for AprilTag noise
% State transition matrix (linearized)
F = [eye(3), dt * eye(3);
zeros(3), eye(3)];
% Observation model (H)
H = [eye(3), zeros(3)];
tic
%% EKF Loop
for k = 2:N-1
% Relative velocity = ugv - uav
rel_v = v_ugv(k, :) - v_uav(k, :);
%% Prediction Step
x_pred = F * x_est(:, k-1);
x_pred(4:6) = rel_v'; % Update velocity directly from inputs
P_pred = F * P * F' + Q;
%% Measurement Update
z = z_meas(k, :)';
z_pred = H * x_pred;
y = z - z_pred; % Innovation
S = H * P_pred * H' + R; % Innovation covariance
K = P_pred * H' / S; % Kalman gain
x_est(:, k) = x_pred + K * y; % State update
P = (eye(6) - K * H) * P_pred; % Covariance update
end
ex_time = toc
%% Plot Results
time = (0:N-1) * dt;
time = (0:N-1);
figure;
plot(time, x_est(1,:), 'r-', time, z_meas(:,1), 'b--');grid on,
ylabel('Relative X [m]'); legend('EKF', 'AprilTag');
xlabel('Time');
ylim([-30 25]);
figure,
plot(time, x_est(2,:), 'r-', time, z_meas(:,2), 'b--');grid on,
ylabel('Relative Y [m]'); legend('EKF', 'AprilTag');
xlabel('Time');
ylim([-25 20]);
figure,
plot(time, x_est(3,:), 'r-', time, z_meas(:,3), 'b--');grid on,
ylabel('Relative Z [m]'); legend('EKF', 'AprilTag');
xlabel('Time');
ylim([-10 10]);
参考文献
[1] 朱自谦。一种用于机动目标跟踪的新自适应卡尔曼滤波算法 [C]//1990 年控制理论及其年会应用论文集（3）.1990.DOI:10.1007/BF02943552.
[2] 唐思嘉，王其，马云鹏，等。基于阈值过滤的自适应无迹卡尔曼滤波在农业无人机组合导航中的应用 [J].安徽科技学院学报，2024, 38(6):94-103.
