【MATLAB例程】联邦卡尔曼滤波，主滤波与子滤波融合GPS、IMU、里程计多传感器数据。提供源代码下载链接

本程序实现标准结构的联邦卡尔曼滤波（$FKF$），融合$IMU+GNSS$与$IMU+odom$两个子滤波器，通过主滤波器统一估计二维位置、速度与加速度。系统考虑公共信息管理、信息加权融合及反馈机制，具备高精度和鲁棒性，适用于多传感器导航仿真。

程序介绍

本联邦卡尔曼滤波(Federated Kalman Filter)的MATLAB例程适用于多传感器融合系统。介绍如下：

系统架构

• 主滤波器: 负责全局状态融合
• $GNSS$子滤波器: 处理位置观测数据
• IMU子滤波器: 处理加速度观测数据
• 里程计子滤波器: 处理速度观测数据

代码结构

主程序部分:
├── 参数设置
├── 轨迹生成
├── 传感器数据生成  
├── 滤波器初始化
├── 主滤波循环
├── 结果分析
└── 扩展功能演示函数定义部分:
├── generate_true_trajectory()
├── generate_sensor_data()
├── initialize_federated_filter()
├── run_federated_filter()
├── time_update()
├── measurement_update()
├── federated_fusion()
├── information_feedback()
├── analyze_results()
└── demo_adaptive_federated_filter()

运行结果

各方法得到的轨迹对比：

各方法得到的误差对比：

位置误差的分布柱状图：

命令行窗口输出的误差特性：

MATLAB源代码

代码结构：

部分代码如下：

%% 联邦卡尔曼滤波(Federated Kalman Filter)仿真例程，标准联邦架构,两个子滤波器 + 一个主滤波器
% 子滤波器1：IMU+GNSS
% 子滤波器2：IMU+odom
% 主滤波器：融合两个子滤波器的信息
% author:Evand（V：matlabfilter）
% 2025-08-05/Ver1clear; clc; close all;
rng(0);%% 系统参数设置
dt = 0.01;           % 采样时间 (s)
T_sim = 100;         % 仿真时间 (s)
N = T_sim / dt;     % 仿真步数% 状态向量: [x, y, vx, vy, ax, ay]' (位置、速度、加速度)
n_states = 6;% 系统噪声参数
process_noise_std = 0.1;  % 过程噪声标准差% 传感器噪声参数
GNSS_noise_std = 5;       % GNSS位置噪声标准差 (m)
imu_noise_std = 0.1;     % IMU加速度噪声标准差 (m/s²)
odom_noise_std = 4;    % 里程计速度噪声标准差 (m/s)fprintf('开始联邦卡尔曼滤波仿真...\n');%% 生成真实轨迹
t = (0:N-1) * dt;
...

完整代码下载链接：https://download.csdn.net/download/callmeup/91597532

或：

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