【MATLAB例程】三维组合导航仿真（使用UKF），误差状态模型：位置+速度+姿态+陀螺偏差+加速度计偏差-CSDN博客

在这里插入图片描述

三维状态量的 $U K F$ 例程（严格的组合导航推导）使用基于15维误差状态模型：位置(3)、速度(3)、姿态(3)、陀螺偏差(3)、加速度计偏差(3)；6维的观测量：位置(3)、速度(3)

文章目录

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

程序简介

程序实现了三维状态量的无迹卡尔曼滤波（UKF）组合导航仿真，其核心思想是通过 IMU（加速度计 + 陀螺仪）的高频积分来推算运动轨迹，同时借助 GNSS 的低频观测进行修正，从而获得更加稳定和准确的定位与姿态解算结果。

主要特点和流程如下：

状态建模

状态维度为 15维误差状态模型，分别表示三维位置、速度、姿态（欧拉角）、陀螺偏差和加速度计偏差。
系统过程噪声协方差矩阵 Q 和观测噪声协方差矩阵 R 由 IMU 与 GNSS 噪声特性推导而来。

真实轨迹与传感器数据生成

模拟对象运动为 三维螺旋上升轨迹（圆周运动 + 匀速上升）。
IMU数据：由真实轨迹计算比力与角速度，再加入噪声与偏差。
GNSS观测：每秒提供一次三维位置与速度观测，其余时刻缺省。更新频率可以更改

纯IMU积分解算

作为对比基线，仅利用IMU数据进行姿态、速度和位置更新。由于误差累计，积分结果会逐渐漂移。

UKF滤波流程

UKF 使用无迹变换传播非线性系统：

生成 Sigma 点：
预测步骤：将 Sigma 点输入到非线性状态转移函数中，得到预测状态与协方差。
更新步骤：若有 GNSS 观测，则通过观测模型修正预测值，计算卡尔曼增益更新状态；否则保持预测值。

结果与对比

程序绘制了 三维轨迹、位置曲线、速度曲线、姿态角变化 等对比图。
给出了 位置、速度和姿态误差的 RMSE 统计，验证 UKF 相比纯IMU积分显著降低漂移误差。

运行结果

三轴轨迹对比：
在这里插入图片描述
各轴位置和速度变化曲线：

在这里插入图片描述
三维空间中的位置误差、速度误差变化曲线：

MATLAB源代码

完整代码如下：

% 三维状态量的UKF例程（严格的组合导航推导）
% 基于15维误差状态模型：位置(3)、速度(3)、姿态(3)、陀螺偏差(3)、加速度计偏差(3)
% 作者：matlabfilter
% 2025-08-25/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)
gnss_vel_noise_std = 0.1;           % GNSS速度噪声标准差 (m/s)

%% 过程噪声协方差矩阵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 (6×6)
% 观测量：GNSS位置(3) + GNSS速度(3)
R = zeros(6, 6);
R(1:3, 1:3) = eye(3) * gnss_pos_noise_std^2;
R(4:6, 4:6) = eye(3) * gnss_vel_noise_std^2;

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