【MATLAB例程】UKF组合导航例程，15维状态量、3维观测量。附代码，有中文注释、误差输出图像。附下载链接

在这里插入图片描述

三维状态量的UKF例程（严格的组合导航推导）15维误差状态模型：位置(3)、速度(3)、姿态(3)、陀螺偏差(3)、加速度计偏差(3)。3维的观测量：XYZ位置

文章目录

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

代码简介

概述

这是一个基于无迹卡尔曼滤波（Unscented Kalman Filter, UKF）的三维组合导航系统仿真程序。该系统融合了惯性测量单元（IMU）和全球导航卫星系统（GNSS）的数据，实现了高精度的三维导航定位。

系统特点

1. 完整的15维误差状态模型

位置状态（3维）：X、Y、Z三轴位置信息
速度状态（3维）：X、Y、Z三轴速度信息
姿态状态（3维）：横滚角、俯仰角、偏航角
陀螺仪偏差（3维）：三轴陀螺仪零偏估计
加速度计偏差（3维）：三轴加速度计零偏估计

2. 多传感器融合

IMU传感器：提供高频率的加速度和角速度测量
GNSS接收机：提供低频率但高精度的位置观测（XYZ三维位置数据）
传感器同步：IMU以100Hz工作，GNSS以1Hz工作

3. 先进的滤波算法

采用无迹卡尔曼滤波（UKF）算法，相比扩展卡尔曼滤波（EKF）具有更好的非线性处理能力
通过Sigma点采样捕捉系统的非线性特性
自适应的噪声建模和协方差更新

仿真场景

轨迹设计

运动模式：螺旋上升轨迹
水平运动：半径50米的圆形轨迹
垂直运动：以0.5m/s的速度匀速上升
角速度：0.1 rad/s的水平旋转

噪声参数

陀螺仪噪声：0.01°/s 标准差
加速度计噪声：0.001 m/s² 标准差
GNSS位置噪声：0.3m 标准差
传感器偏差：考虑时变特性

性能评估

对比分析

程序提供三种导航解算结果的对比：

真实轨迹：理论参考轨迹
纯IMU积分：仅使用IMU数据的导航解
UKF融合结果：IMU/GNSS融合后的最优估计

可视化输出

三维轨迹图：直观显示各种方法的轨迹对比
时间序列图：位置、速度、姿态的时域变化
误差分析图：定量评估各方法的精度
统计结果：RMSE等性能指标

运行结果

三维轨迹的对比：
在这里插入图片描述
位置曲线对比：

速度曲线对比：

误差曲线对比：

在这里插入图片描述
命令行窗口输出的图像：

MATLAB源代码

程序结构：
在这里插入图片描述

部分代码如下：

% 三维状态量的UKF例程（严格的组合导航推导）
% 基于15维误差状态模型：位置(3)、速度(3)、姿态(3)、陀螺偏差(3)、加速度计偏差(3)
% 基于3维观测：XYZ位置（3）
% 作者：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.01 * pi/180;      % 陀螺噪声标准差 (rad/s)
accel_noise_std = 0.001;             % 加速度计噪声标准差 (m/s^2)
gyro_bias_std = 0.001 * pi/180;      % 陀螺偏差标准差 (rad/s)
accel_bias_std = 0.0001;             % 加速度计偏差标准差 (m/s^2)

% GNSS观测噪声
gnss_pos_noise_std = 0.3;             % GNSS位置噪声标准差 (m)

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

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

%% 初始化
% 真实轨迹参数（螺旋上升运动）
radius = 50;        % 水平圆形轨迹半径 (m)
angular_vel = 0.1;  % 角速度 (rad/s)
climb_rate = 0.5;   % 上升速率 (m/s)