原创程序，请勿翻卖
如需帮助，或有导航、定位滤波相关的代码定制需求，请通过文末卡片联系作者

程序简介

本程序构建了一套完整的 UWB测距 → EKF协同定位 → 一致性编队控制 闭环仿真框架。

UWB测距模型

测距原理： 每架无人机利用 UWB 信号与 $N_{anc}=6$ 个固定锚点进行飞行时差（TDOA）测距，同时进行无人机间互测距。

代码说明：

range_meas(i,j,k) = d_true + sigma_uwb * randn();

通过 randn() 叠加白噪声，模拟真实硬件测量误差。

分布式编队控制

拓扑结构： 采用环形+对角连接的无向图，邻接矩阵 Adj 保证通信图连通，是编队一致性收敛的充分条件。

控制量有幅值限制 $|{\mathbf{u}}_i|\le 3.0{\text{ m/s}}^2$，避免执行器饱和。

关键参数调节指南

参数	变量名	默认值	影响
UWB噪声标准差	sigma_uwb	0.10 m	增大则定位RMSE上升
EKF加速度扰动	sigma_acc	0.05 m/s²	影响滤波器响应速度
编队位置增益	K_formation	2.0	增大则收敛更快，但可能超调
速度阻尼增益	K_damping	1.5	增大则振荡减小
无人机数量	N_uav	4	可扩展，需同步修改邻接矩阵
锚点数量	N_anc	6	≥4时可保证三维可观性

运行结果

本仿真程序运行后将输出以下图形结果：

UWB测距误差分析
展示各无人机节点之间的UWB测距值与真实距离的对比，以及高斯噪声分布直方图，验证UWB测距模型的准确性（典型测距标准差设定为0.1 m）。

多无人机三维轨迹图

以三维坐标系展示4架无人机在仿真全程中的飞行轨迹，真实轨迹（虚线）与EKF估计轨迹（实线）叠加显示，同时标注UWB锚点位置，直观呈现协同定位精度。

各无人机定位误差曲线
分别绘制每架无人机在X、Y、Z三轴方向的定位误差随时间变化曲线，EKF滤波后误差收敛范围一般在0.05～0.15 m以内。

编队控制误差曲线
绘制每架无人机相对于期望编队位置的误差收敛曲线，验证分布式编队控制律（一致性算法）的稳定性，误差一般在15～20秒内收敛至0.1 m以下。

综合性能指标统计
以柱状图形式汇总各无人机的平均定位误差（RMSE）、编队误差均值及控制输入能耗，提供量化的性能评估。

MATLAB源代码

部分代码如下：

%% 基于UWB的多无人机协同定位与编队控制仿真
% 功能：
%   1. 建立UWB测距模型（含高斯测量噪声）
%   2. 基于扩展卡尔曼滤波（EKF）实现多无人机协同定位
%   3. 基于一致性算法实现多无人机分布式编队控制
%   4. 可视化输出定位误差、轨迹及编队效果
% 作者:matlabfilter（V同号，除前期达成一致外，付费咨询）
% 2026-05-06 / Ver1
clc; clear; close all;
rng(0);

%% 仿真参数配置
dt      = 0.05;           % 仿真步长 (s)
T_total = 30;             % 仿真总时长 (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;
            20,   0,   0;
            20,  20,   0;
             0,  20,   0;
            10,  10,   5;
            10,   0,   3 ];  % N_anc × 3


%% 期望编队构型（相对于编队中心，单位：m）

完整代码下载：
https://download.csdn.net/download/callmeup/92853094