【MATLAB例程】UKF（无迹卡尔曼滤波）的组合导航，状态量为平面8维，观测量为XY坐标。附中文注释-优快云博客

在这里插入图片描述

代码实现了一个基于无迹卡尔曼滤波器（Unscented Kalman Filter, UKF）的二维组合导航系统，融合了惯性测量单元（IMU）和全球导航卫星系统（GNSS）的数据。该系统采用8维误差状态模型，能够实时估计载体的位置、速度、姿态以及传感器偏差。

文章目录

程序简介
运行结果
MATLAB源代码

程序简介

系统概述

IMU（惯性测量单元）：就像人的内耳平衡器官，能感知加速度和旋转
GNSS（全球导航卫星系统）：就像GPS，能提供精确的位置信息

核心技术：无迹卡尔曼滤波：UKF

传统的扩展卡尔曼滤波器需要对复杂的非线性方程进行线性化近似，就像用直线段拼接曲线一样，会产生误差。而UKF采用了一种更聪明的方法：

精确采样：选择一些关键的"采样点"来代表整个系统状态的分布
直接传播：让这些采样点直接通过真实的非线性方程
统计重构：从传播后的采样点重新构造出系统状态的统计特性

这就像用多个探路者走不同路线，然后汇总信息得出最佳路径，比简单的线性近似更准确。

系统状态设计

8维状态向量

系统跟踪8个关键状态量：

位置信息（2维）

X方向位置：载体在东西方向的坐标
Y方向位置：载体在南北方向的坐标

速度信息（2维）

X方向速度：载体在东西方向的运动速度
Y方向速度：载体在南北方向的运动速度

姿态信息（1维）

航向角：载体的朝向，相当于指南针读数

传感器偏差（3维）

陀螺仪偏差：陀螺仪的系统性误差
X方向加速度计偏差：X轴加速度计的系统性误差
Y方向加速度计偏差：Y轴加速度计的系统性误差

低频GNSS数据融合

GNSS以较低的频率（1Hz，每秒一次）提供精确的位置信息：

GNSS数据特点：

精度高：通常能达到米级精度
频率低：更新较慢，不能捕捉快速运动
受环境影响：在隧道、高楼间可能失效

融合策略：
当有GNSS数据时，系统会：

比较GNSS观测位置与IMU推算位置
计算两者差异（新息）
根据各传感器的可信度分配权重
智能地修正整个状态估计

算法工作流程

预测阶段

每个时间步，系统都会基于当前状态和IMU数据进行预测：

生成采样点：围绕当前状态生成多个代表性采样点
状态传播：让每个采样点通过运动模型传播到下一时刻
统计汇总：从传播后的采样点计算预测状态和不确定度

更新阶段

当收到GNSS观测数据时：

观测预测：预测每个采样点对应的观测值
计算增益：根据预测不确定度和观测噪声计算卡尔曼增益
状态修正：使用增益矩阵融合观测信息，修正状态估计
协方差更新：更新状态估计的不确定度

仿真验证场景

运动模型

代码模拟了一个典型的测试场景：载体沿半径50米的圆形轨迹匀速运动。这种轨迹具有：

连续的加速度变化（向心加速度）
恒定的角速度
可预测的真实轨迹用于性能评估

传感器模拟

IMU传感器模拟：

在真实的加速度和角速度基础上添加随机噪声
模拟传感器偏差的随机游走特性
考虑实际传感器的噪声水平

GNSS传感器模拟：

在真实位置基础上添加高斯白噪声
模拟较低的更新频率
间歇性提供观测数据

性能评估与对比

对比基准

代码提供了三种导航方案的对比：

纯IMU积分：

优点：高频率，短期精度高，不依赖外部信号
缺点：误差累积严重，长期漂移明显

纯GNSS定位：

优点：无累积误差，长期稳定
缺点：更新频率低，动态响应差，信号容易中断

UKF组合导航：

综合两者优势：高频率 + 长期稳定
智能误差修正：自动校准传感器偏差
鲁棒性强：单一传感器失效时仍能工作

性能指标

系统使用均方根误差（RMSE）评估性能：

位置RMSE：反映定位精度
速度RMSE：反映速度估计精度

典型结果显示，UKF组合导航的精度显著优于单一传感器方案。

运行结果

二维轨迹图像对比：
在这里插入图片描述
两个维度的位置曲线对比：

两个维度的速度曲线对比：

误差曲线：

MATLAB源代码

部分代码如下：

% 二维状态量的UKF例程（有严格的组合导航推导）
% 基于8维误差状态模型：位置、速度、航向、陀螺偏差、加速度计偏差
% 4维观测量：XY位置（2）
% 作者：matlabfilter（原创程序，请勿翻卖）
% 2025-08-28/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)

%% 过程噪声协方差矩阵Q (8×8)
% 状态顺序：[位置(2), 速度(2), 姿态(1), 陀螺偏差(1), 加速度计偏差(2)]
Q = zeros(8, 8);
% 位置噪声（通过速度积分产生）
Q(1:2, 1:2) = eye(2) * (accel_noise_std * dt^2)^2;
% 速度噪声
Q(3:4, 3:4) = eye(2) * (accel_noise_std * dt)^2;
% 姿态噪声
Q(5, 5) = eye(1) * (gyro_noise_std * dt)^2;
% 陀螺偏差噪声
Q(6, 6) = eye(1) * (gyro_bias_std * dt)^2;
% 加速度计偏差噪声
Q(7:8, 7:8) = eye(2) * (accel_bias_std * dt)^2;

%% 观测噪声协方差矩阵R (2×2)
% 观测量：GNSS位置(2) 
R = eye(2) * gnss_pos_noise_std^2;

%% 初始化
% 真实轨迹参数（圆形运动）
radius = 50;        % 圆形轨迹半径 (m)