基于UWB测距的多无人机协同定位与编队控制仿真系统。程序运行后，将依次输出UWB测距误差分析、三维飞行轨迹、UKF定位误差、编队控制误差以及综合性能统计结果。
原创代码，请勿翻卖。包运行成功、不包讲解，可通过上方卡片联系作者，获取代码讲解、定制
文章目录
程序简介
程序围绕"UWB协同定位"和"多无人机编队控制"两个核心任务展开。整体流程可以分为仿真参数配置、轨迹生成、UWB测距建模、UKF状态估计、分布式编队控制和结果可视化六个部分。
首先，程序设置了仿真步长、仿真总时长、无人机数量、UWB锚点数量、测距噪声标准差、过程噪声参数以及编队控制增益等基础参数。其中，无人机数量设置为4，UWB固定锚点数量设置为6，用于构建一个典型的多无人机协同定位场景。
在UWB锚点布置部分，程序将多个锚点部署在飞行区域周围，并设置了不同高度的锚点位置。这样的空间布置方式能够为无人机提供较好的三维测距几何结构，有利于提高三维定位精度。
在轨迹生成部分，程序首先构造编队中心的三维运动轨迹。
程序将UWB测距建模、UKF非线性滤波、多无人机编队控制和性能评价集成在同一个MATLAB脚本中，结构清晰、运行方便，适合作为多无人机协同定位、集群导航、UWB三维定位和编队控制等相关研究的基础仿真例程。
运行结果
无人机三维飞行轨迹图
该图展示了多架无人机在三维空间中的真实轨迹和UKF估计轨迹，同时绘制了UWB固定锚点的位置。无人机按照预设编队构型围绕编队中心进行螺旋上升运动，UKF估计轨迹与真实轨迹基本保持一致，说明滤波器能够有效利用UWB测距信息完成三维定位估计。
UKF定位误差曲线 :
分别展示四架无人机在 X X X轴、 Y Y Y轴和 Z Z Z轴方向上的定位误差变化情况。通过三个方向的误差曲线，可以观察不同无人机在整个运动过程中的定位精度、误差波动范围以及滤波稳定性。
编队控制误差收敛曲线
图片展示了各无人机在分布式编队控制作用下的编队误差变化情况。随着控制过程推进，各无人机逐渐收敛到期望的相对位置，编队误差整体呈下降趋势，说明一致性控制律能够有效维持多无人机的空间编队结构。
综合性能统计图
以柱状图形式对各无人机的定位RMSE、编队平均误差和控制能耗进行综合对比，便于从定位精度、编队保持能力和控制代价三个角度评价系统性能。
程序运行结束后，MATLAB命令行还会输出如下形式的统计结果：
命令行窗口截图：
MATLAB源代码
部分代码如下：
matlab
%% 基于UWB的多无人机协同定位与编队控制仿真
% 功能：
% 1. 建立UWB测距模型（含高斯测量噪声）
% 2. 基于无迹卡尔曼滤波（UKF）实现多无人机协同定位
% 3. 基于一致性算法实现多无人机分布式编队控制
% 4. 可视化输出定位误差、轨迹及编队效果
% 作者:matlabfilter（V同号，除前期达成一致外，付费咨询）
% 2026-05-06 / Ver1
clc; clear; close all;
rng(0);
%% 仿真参数配置
dt = 0.05; % 仿真步长 (s)
T_total = 20; % 仿真总时长 (s)
N_step = T_total / dt; % 总步数
t = (0:N_step-1) * dt;
N_uav = 4; % 无人机数量
N_anc = 6; % UWB固定锚点数量
% UWB测距噪声标准差 (m)
sigma_uwb = 0.10;
% 过程噪声标准差
sigma_acc = 0.05; % 加速度扰动 (m/s²)
% 编队控制增益
K_formation = 2.0; % 位置误差增益
K_damping = 1.5; % 速度阻尼增益
% 通信拓扑邻接矩阵（无向连通图）
% UAV编号: 1-2-3-4，环形+对角连接
Adj = [0 1 1 0;
1 0 0 1;
1 0 0 1;
0 1 1 0];
%% UWB锚点布置（静态，单位：m）
% 锚点均匀分布于飞行区域周围，高度混合布置
anc_pos = [ 0, 0, 0;
10, 0, 0;
20, 20, 0;
0, 20, 0;
10, 10, 5;
10, 0, 3 ]; % N_anc × 3
%% 期望编队构型（相对于编队中心，单位：m）
% 编队
formation_offset = [ 3, 0, 0; % UAV 1
0, 3, 2; % UAV 2
-3, 0, 0; % UAV 3
0, -3, 2 ]; % UAV 4 (N_uav × 3)
%% 真实轨迹生成（螺旋上升 + 匀速平移）
% 编队中心轨迹完整代码下载：
https://download.csdn.net/download/callmeup/92853194