【目标定位】基于UWB实数据的事件触发扩散卡尔曼滤波定位附Matlab代码

最新推荐文章于 2025-09-05 14:14:03 发布

原创最新推荐文章于 2025-09-05 14:14:03 发布 · 363 阅读

4 ·

CC 4.0 BY-SA版权

文章标签：

#matlab #开发语言

✅作者简介：热爱科研的Matlab仿真开发者，擅长数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。

🍎 往期回顾关注个人主页：Matlab科研工作室

🍊个人信条：格物致知,完整Matlab代码及仿真咨询内容私信。

🔥 内容介绍

在无人机定位与导航领域，“高精度” 与 “低资源消耗” 往往是一对需要平衡的核心需求 —— 尤其是在室内实验室等封闭场景中，四旋翼的实时定位不仅需要依赖可靠的状态估计，还需考虑计算资源占用、数据传输带宽等实际限制。而事件触发扩散卡尔曼滤波（Event-Triggered Diffusion Kalman Filter, ET-DKF）算法，恰好为这一需求提供了优质解决方案：在 9×10m 的实验室环境中，它既能实现四旋翼的精准定位，又能通过 “事件触发” 机制减少数据交互与计算开销，显著节省系统资源。今天我们就来拆解这个定位方案的实验逻辑、核心组件与结果解读，同时说明其数据与代码结构的复用价值。

一、实验背景：为什么需要 “事件触发” 的四旋翼定位？

室内四旋翼定位的传统方案（如连续卡尔曼滤波）虽能保证精度，但存在一个关键问题：资源浪费。这类方案通常采用 “周期性更新” 策略 —— 无论四旋翼的状态变化是否显著，都会固定频率（如 10Hz、20Hz）进行数据采集、滤波计算与结果传输。但在实际场景中：

当四旋翼悬停或缓慢移动时，状态变化极小，频繁更新并无必要；

持续的计算与数据传输会占用处理器资源、消耗电池电量，还可能导致通信链路拥堵。

而事件触发机制的核心优势，就是打破 “固定周期” 的限制：只有当四旋翼的状态误差超过预设阈值（即 “事件发生”）时，才触发滤波更新与数据交互。这种 “按需更新” 的模式，能在保证定位精度的前提下，大幅降低资源消耗，完美适配实验室环境下四旋翼的低成本、低功耗定位需求。

二、实验系统构成：从 “真值获取” 到 “状态估计” 的全链路

要实现四旋翼的精准定位与算法验证，实验系统需包含 “真值采集”“状态估计”“结果可视化” 三大核心模块，各组件的功能与协作逻辑如下：

1. 地面真值（Ground-Truth）获取：动作捕捉系统（Motion Capture）

要判断定位算法的精度，必须先有 “绝对准确” 的参考标准 —— 这就是动作捕捉系统的作用。在 9×10m 的实验室中，我们通过部署多台高速红外摄像头（通常是 OptiTrack 或 Vicon 系统），实时追踪四旋翼上粘贴的反光标记点，从而计算出四旋翼的真实三维位置与姿态（位置精度可达毫米级，姿态精度可达 0.1°）。

在实验可视化中，这部分真值会以蓝色矩形框呈现 —— 它是判断 “估计位置是否准确” 的唯一基准，所有定位误差的计算都以它为参考。

2. 状态估计核心：事件触发扩散卡尔曼滤波（ET-DKF）

ET-DKF 是整个定位方案的 “大脑”，它的工作逻辑分为 “事件触发判断” 与 “扩散卡尔曼滤波” 两部分：

事件触发判断：预设一个 “误差阈值”（如位置误差 0.1m、速度误差 0.05m/s），持续监测四旋翼的 “当前估计状态” 与 “预测状态” 的偏差。只有当偏差超过阈值时，才触发 “数据上传”（将传感器数据发送给滤波模块）与 “滤波更新”（重新计算最优估计值）；若偏差未超阈值，则不进行更新，直接沿用之前的估计结果。

扩散卡尔曼滤波：不同于传统的单节点卡尔曼滤波，DKF 采用 “分布式计算” 思路 —— 将滤波任务拆解到多个局部节点（如四旋翼自身的处理器与实验室的边缘计算节点），各节点并行计算局部状态估计，再通过 “扩散融合” 策略整合各节点结果，最终得到全局最优估计。这种分布式架构不仅提升了计算效率，还增强了系统的鲁棒性（单个节点故障不影响整体定位）。

在实验可视化中，ET-DKF 输出的四旋翼估计位置以红色加号（+）呈现 —— 它与蓝色矩形框的重叠程度，直接反映了定位算法的精度。

3. 误差量化可视化：基于扩散误差协方差矩阵的椭圆

除了直观的位置对比，实验还通过椭圆来量化定位的 “不确定性”—— 这些椭圆的大小与形状，由 ET-DKF 的扩散误差协方差矩阵计算得到：

协方差矩阵中的对角线元素，代表四旋翼在 x、y、z 三个轴向上的位置估计方差（方差越小，精度越高）；

椭圆的长轴与短轴，分别对应方差最大与最小的方向；椭圆的面积越小，说明定位结果的不确定性越低，算法越可靠。

例如：当四旋翼悬停时，误差椭圆会逐渐缩小（不确定性降低）；当四旋翼快速移动触发 “事件更新” 时，椭圆可能短暂扩大后迅速收敛，这体现了算法对动态状态的快速响应能力。

⛳️ 运行结果

📣 部分代码

% This file to generate a snapshot (figure 6) in the paper

%% Clean up console and variables

clc; close all; clear all;

addpath('classes');

addpath('utilities');

%% Load saved SLATS data

%load('cache\ped01_20sec_skip46_Th_4.mat');

load('cache\temp.mat');

node_ids = nm.getNodeIds();

node_names = nm.getNodeNames();

%% Plot Position Convergence

%%{

cfigure(12,10);

handles = [];

p_errors = [];

rb = 1;

rb_id = 'ntb-mobile';

rb_idx = nm.getNodeIdx(rb_id);

xyzm_all = [];

xyze_all = [];

err3d = [];

mse3d =[];

tstart = t_history(1);

MeasFlagGood =[];

timeUpdateFlagGood = [];

pDKAL_Good=double.empty(0,2);

for i=1:length(t_history);

if i > length(p_history)

break;

end

t = t_history(i);

[xyz_mocap, lat] = nm.dataparser.getMocapPos( rb, t );

%if lat < 0.050

xyz_est = p_history{i}(rb_idx,:);

xyz_err = xyz_mocap - xyz_est;

xyzm_all = [xyzm_all; xyz_mocap];

xyze_all = [xyze_all; xyz_est];

p_errors = [p_errors; t xyz_err];

err3d = [err3d; norm(xyz_err)];

mse3d = [ mse3d; mse(xyz_err)];

pDKAL_Good = [pDKAL_Good ; [pDKAL_history(i,1) pDKAL_history(i,2) ]];

MeasFlagGood =[MeasFlagGood MeasFlag_history(i)];

timeUpdateFlagGood = [timeUpdateFlagGood timeUpdateFlag_history(i)];

% end

end

%%MSE = mean(xyz_err.^2)

%%RMSE = sqrt(mean(xyz_err.^2))

ax1=subplot(3,1,1);

tstart = p_errors(1,1);

plot(p_errors(:,1) - tstart, err3d,'--gs','MarkerSize',2,'LineWidth',1,...

'MarkerEdgeColor',[0,0.7,0.9],'MarkerFaceColor','red');

set(gca,'fontweight','bold','FontSize',14,'FontName','Arial');

xlabel('Time (sec)','fontweight','bold','FontSize',14,'FontName','Arial');

ylabel('3D Localization error (m)','fontweight','bold','FontSize',9,'FontName','Arial');

grid on;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

ax2=subplot(3,1,2);

%plot(pDKAL_Good(:,1) - pDKAL_Good(1,1), pDKAL_Good(:,2), 'LineWidth',2);

plot(pDKAL_Good(:,1) - pDKAL_Good(1,1), pDKAL_Good(:,2),'--gs','MarkerSize',2,'LineWidth',1,...

'MarkerEdgeColor',[0,0.7,0.9],'MarkerFaceColor','red');

hold on;

plot([0, pDKAL_Good(end,1)-pDKAL_Good(1,1)], [PDESIRED PDESIRED], '--k', 'LineWidth',2);

xlim([0 t_stop]);

ylim([0 6.8]);

grid on;

set(gca,'fontweight','bold','FontSize',14,'FontName','Arial');

xlabel('Time (sec)','fontweight','bold','FontSize',14,'FontName','Arial');

lgd=legend( {'Trace','$\pi_{\max}$'},'Interpreter','Latex');

lgd.FontSize = 10;

lgd.FontWeight = 'normal';

disp(sprintf('PDESIRED= %s',PDESIRED))

disp(sprintf('mean_err3d= %s', mean(err3d)))

disp(sprintf('var_err3d= %s',var(err3d)))

ax3=subplot(3,1,3);

tstart = p_errors(1,1);

plot(p_errors(:,1) - tstart,MeasFlagGood,'--gs','MarkerSize',4,'LineWidth',1,...

'MarkerEdgeColor','black','MarkerFaceColor','red');

ylim([-0.5 1.5]);

grid on;

set(gca,'fontweight','bold','FontSize',14,'FontName','Arial','ytick',[0 1]);

xlabel('Time (sec)','fontweight','bold','FontSize',14,'FontName','Arial');

ylabel('Meas. Flag','fontweight','bold','FontSize',14,'FontName','Arial');

time = p_errors(:,1) - tstart;

time_slots = time(find(MeasFlagGood==1))';

%ylim([-1 2]);

% legends

savemsgsM

sentmsgsM

sentmsgsD

reporterr = [ PDESIRED,mean(err3d),std(err3d),rms(err3d),mean(p_errors(:,2)), sqrt( mean( p_errors(:,2).^2 ) ),mean(p_errors(:,3)), sqrt( mean( p_errors(:,3).^2 ) ),mean(p_errors(:,4)), sqrt( mean( p_errors(:,4).^2 ) )]

reportmsg = [PDESIRED,savemsgsM,sentmsgsM,sentmsgsD]

%disp(sprintf('report= %s',report))

%saveplot('output/ped01_conditional');