【MATLAB例程】严格推导的二维平面下的组合导航，模拟UKF（无迹卡尔曼滤波）融合IMU和GNSS，附代码下载链接

程序完整展示了 二维平面下、IMU+GNSS组合导航中UKF的建模与实现，并清晰体现了滤波对抗传感器噪声与漂移的效果。
8维状态量、4维观测量

程序简介

程序介绍

本程序实现了基于 误差状态模型 的 组合导航无迹卡尔曼滤波（UKF） 仿真，状态量包括位置、速度、航向、陀螺偏差和加速度计偏差。程序主要对比了 纯IMU积分结果 与 UKF估计结果 在位置、速度上的误差表现，并计算均方根误差（$RMSE$）。

仿真轨迹采用 二维圆形运动，通过$IMU$（加速度计+陀螺仪）和GNSS（位置+速度观测）实现数据融合。

核心步骤

1. 状态建模

2. 过程模型（预测）

3. UKF预测与更新

程序功能

• 生成真实轨迹、IMU与GNSS观测数据；
• 实现纯IMU积分导航，作为对比基准；
• 实现UKF融合导航，显著抑制误差漂移；
• 绘制轨迹对比、位置误差、速度误差曲线；
• 计算RMSE，量化滤波效果。

运行结果

各方法得到的二维轨迹示意图：

$XY$轴位置曲线对比：

$XY$轴速度曲线对比：

位置误差对比曲线：

能看出来UKF估计的误差明显比纯IMU（滤波前）的小。

MATLAB源代码

完整代码如下：

% 二维状态量的UKF例程（有严格的组合导航推导）
% 基于8维误差状态模型：位置、速度、航向、陀螺偏差、加速度计偏差
% 作者：matlabfilter
% 2025-08-24/Ver1

clear; clc; close all;
rng(0); % 固定随机种子

%% 系统参数设置
dt = 0.1;           % 采样时间间隔 (s)
total_time = 100;   % 总仿真时间 (s)
N = total_time / dt; % 采样点数

%% 噪声参数设置
% IMU噪声参数
gyro_noise_std = 0.1 * pi/180;      % 陀螺噪声标准差 (rad/s)
accel_noise_std = 0.01;             % 加速度计噪声标准差 (m/s^2)
gyro_bias_std = 0.01 * pi/180;      % 陀螺偏差标准差 (rad/s)
accel_bias_std = 0.001;             % 加速度计偏差标准差 (m/s^2)

% GNSS观测噪声
gnss_pos_noise_std = 3;             % GNSS位置噪声标准差 (m)
gnss_vel_noise_std = 0.1;           % GNSS速度噪声标准差 (m/s)

%% 过程噪声协方差矩阵Q 
...

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

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