分布式EKF协同导航算法（含协方差交互融合）代码，附下载链接

程序详解

算法总体框架

多节点集群的分布式EKF协同定额。系统融合三类信息源：

• 惯性测量单元（$IMU$）：用于状态预测
• 全球导航卫星系统（$GNSS$）：提供绝对位置观测
• 超宽带（$UWB$）：提供节点间相对距离约束

同时，为解决分布式系统中估计误差相关性未知问题，引入协方差交集（Covariance Intersection, CI）融合方法，提升系统一致性与稳定性。

分布式EKF算法流程

每个时间步，每个节点独立执行以下步骤：

1. 状态预测（IMU驱动）

2. GNSS更新

3. UWB相对观测更新

4. CI融合（关键步骤）

仿真设计

仿真设置如下：

• 节点数量：3
• 状态维度：6（位置 + 速度）
• 仿真步长：0.1 s
• 仿真时长：200步
• 噪声：
  • GNSS：高斯噪声
  • UWB：测距噪声

节点运动为匀速模型，用于验证算法性能。
如需其他参数，可在代码中修改：

运行结果

三维轨迹图：

误差曲线：

命令行窗口输出：

MATLAB源代码

部分代码如下：

%% 分布式EKF协同导航算法（含CI融合）代码
% 作者:matlabfilter（V同号，除前期达成一致外，付费咨询）
% 2026-03-28/Ver1

clc; clear; close all;
rng(0);
%% 参数
N = 3;              % 节点数
dt = 0.1;
T = 2000;
nx = 6;             % [p v]

Q = 0.001*eye(nx);
R_gnss = 1*eye(3);
R_uwb = 5;

omega = 0.9999;        % CI权重（越大表示越相信自身协方差）

%% 初始化
X = zeros(nx,N);
P = repmat(eye(nx),[1,1,N]);

X_true = zeros(nx,N);

traj_est = zeros(3,N,T);
traj_true = zeros(3,N,T);

完整代码：
https://download.csdn.net/download/callmeup/92768053

或：如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者