✅作者简介热爱科研的Matlab仿真开发者擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。 往期回顾关注个人主页Matlab科研工作室 关注我领取海量matlab电子书和数学建模资料个人信条格物致知,完整Matlab代码获取及仿真咨询内容私信。 内容介绍一、背景一无人机追踪无人车应用场景在现代科技发展背景下无人机对无人车的追踪在多个领域具有重要应用。在智能交通系统中无人机可追踪无人车用于实时监测交通流量、路况辅助无人车规划最优路径提高整体交通效率。在物流配送场景里无人机能追踪运输货物的无人车实时掌握运输状态及时发现潜在问题如车辆故障、偏离路线等保障货物按时、准确送达。在安防监控领域无人机追踪无人车可用于边境巡逻、重要区域安保等任务增强安全防控能力。二追踪面临的挑战然而实现无人机对无人车的精确追踪面临诸多挑战。一方面无人车的运动具有不确定性其行驶速度、方向可能因路况、任务需求等因素频繁变化这使得准确预测其位置变得困难。另一方面环境因素会对追踪造成干扰如恶劣天气暴雨、沙尘等会影响传感器性能导致获取的无人车位置信息不准确复杂地形山区、城市高楼林立区域可能遮挡信号使无人机与无人车之间的通信中断或数据传输不完整。此外无人机自身的运动也存在噪声和误差如飞行姿态的微小变动会影响其对无人车位置的测量精度。三自适应卡尔曼滤波器的优势自适应卡尔曼滤波器为解决这些挑战提供了有效途径。与传统卡尔曼滤波器相比它能够根据系统状态的变化自动调整滤波器参数更好地适应无人车运动的不确定性以及环境干扰。自适应卡尔曼滤波器可以实时估计系统噪声和测量噪声的统计特性从而在不同的情况下都能保持较高的追踪精度增强追踪系统的鲁棒性和可靠性满足无人机对无人车追踪在复杂多变环境下的需求。二、原理一卡尔曼滤波器基础原理二自适应卡尔曼滤波器原理噪声估计与调整自适应卡尔曼滤波器的核心在于能够实时估计过程噪声协方差 Qk 和观测噪声协方差 Rk。常见的自适应方法有基于极大似然估计、自适应遗忘因子等。以基于极大似然估计为例通过对观测数据的统计分析计算出使得观测数据出现概率最大的噪声协方差值。具体来说假设在一段时间内收集了一系列观测值 {zi}i1k根据卡尔曼滤波器的预测和更新公式建立关于 Qk 和 Rk 的似然函数 L(Qk,Rk)通过优化算法如梯度下降法求解使 L(Qk,Rk) 最大的 Qk 和 Rk 值。这样当无人车运动状态发生变化如突然加速、转弯或环境干扰改变如进入不同天气区域时自适应卡尔曼滤波器能够自动调整噪声协方差使滤波器更好地适应实际情况提高追踪精度。模型自适应调整除了噪声协方差的自适应估计自适应卡尔曼滤波器还可以对状态转移模型和观测模型进行自适应调整。例如当发现无人车的运动模式与原状态转移模型假设不符时如原本假设无人车做匀速直线运动实际开始做曲线运动可以通过对观测数据的分析和机器学习算法调整状态转移矩阵 Fk 的参数使其更符合无人车的实际运动。同样对于观测模型如果发现传感器在某些情况下测量误差较大可以调整观测矩阵 Hk 或增加观测方程的补偿项以提高观测数据与实际状态之间的映射准确性。这种模型的自适应调整进一步增强了自适应卡尔曼滤波器在复杂多变环境下追踪无人车的能力。通过基于自适应卡尔曼滤波器无人机能够更准确地追踪无人车克服无人车运动不确定性和环境干扰带来的挑战为相关应用场景提供可靠的追踪解决方案。⛳️ 运行结果 部分代码clc; clear;%% Load Dataload(uav_ekf_output.mat); % Should contain: v_uav, omega_uavload(ugv_ekf_output.mat); % Should contain: v_ugv, omega_ugvtags readtable(noisy_apriltag_data.csv);v_ugv [vx_ugv, vy_ugv,zeros(1000,1)];v_uav [vx_uav,vy_uav,vz_uav];% Extract measurementsz_meas [tags.rel_x, tags.rel_y, tags.rel_z];% Time step (assumed constant)dt 0.025; % 40 Hz sampling% Data lengthN size(z_meas, 1);%% EKF Initializationx_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 Loopfor k 2:N-1% Relative velocity ugv - uavrel_v v_ugv(k, :) - v_uav(k, :);%% Prediction Stepx_pred F * x_est(:, k-1);x_pred(4:6) rel_v; % Update velocity directly from inputsP_pred F * P * F Q;%% Measurement Updatez z_meas(k, :);z_pred H * x_pred;y z - z_pred; % InnovationS H * P_pred * H R; % Innovation covarianceK P_pred * H / S; % Kalman gainx_est(:, k) x_pred K * y; % State updateP (eye(6) - K * H) * P_pred; % Covariance updateendex_time toc%% Plot Resultstime (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.往期回顾扫扫下方二维码