基于UKF算法的惯性导航系统定位与姿态估计程序

资 源 简 介

本项目利用MATLAB自主开发了一套完整的无迹卡尔曼滤波（UKF）算法框架，专门用于解决惯性导航系统（INS）在非线性环境下的状态估计问题。系统功能涵盖了复杂的非线性运动建模、无迹变换（UT）实现以及多维状态解算。核心流程包括根据当前状态分布生成Sigma点集，通过非线性动力学模型进行状态传播，并利用非线性观测方程提取观测预测值。该算法有效规避了传统扩展卡尔曼滤波（EKF）需要计算雅可比矩阵的复杂过程，并大幅提升了在强非线性场景下的滤波精度。项目实现了对载体位置、速度、姿态角以及惯性敏感器（加速度计和陀螺

详 情 说 明

基于无迹卡尔曼滤波算法的惯性导航系统定位与姿态估计

项目介绍

本项目提供了一套完整的无迹卡尔曼滤波（UKF）算法实现框架，专门用于惯性导航系统（INS）在三维空间中的状态估计。通过MATLAB自主构建的非线性动力学模型，系统能够实时处理来自加速度计和陀螺仪的仿真数据，并在存在传感器噪声和随机漂移的环境下，精确解算载体的位置、速度和姿态。相比于传统的扩展卡尔曼滤波（EKF），本项目通过无迹变换（UT）直接处理高阶非线性项，有效提升了在复杂动态轨迹下的滤波精度和稳定性。

功能特性

1. 多维状态估计：系统可同时对16维状态量进行实时估计，包括三维位置、三维速度、四维姿态四元数、三维陀螺仪零偏以及三维加速度计零偏。
2. 非线性运动建模：内置三维螺旋轨迹发生器，模拟载体在复杂空间中的运动，并生成包含重力矢量、传感器噪声及零偏不稳定性的模拟IMU数据。
3. 无迹变换核心实现：基于Cholesky分解生成Sigma点集，通过非线性传播完成状态预测与观测更新，规避了繁琐的雅可比矩阵计算。
4. 传感器误差补偿：支持对加速度计和陀螺仪的随机漂移进行实时在线估计与自动纠偏，确保长航时导航的可靠性。
5. 稳健的姿态解算：采用四元数保持姿态更新，有效避免了欧拉角表示法中的奇异性问题，并结合一阶泰勒展开进行高频传播。
6. 全方位可视化分析：提供三维轨迹对比、姿态角误差曲线、传感器零偏收敛过程以及综合定位RMS误差的仿真分析图表。

使用方法

1. 启动软件：在计算机上安装并运行MATLAB（建议R2016b及以上版本）。
2. 加载程序：将包含程序核心逻辑的脚本文件置于MATLAB当前工作路径下。
3. 执行仿真：在命令行窗口输入主函数名称并回车，系统将自动开始60秒的导航仿真。
4. 查看结果：仿真结束后，系统会自动弹出四个分析图表，并在命令行输出末端位置误差及平均姿态误差的具体数值。

系统要求

1. 软件环境：MATLAB R2016b 或更高版本。
2. 硬件要求：标准PC配置，内存4GB以上。
3. 依赖项：无需第三方工具箱，代码基于MATLAB基础函数实现。

实现逻辑与核心流程

1. 参数初始化：定义采样频率（100Hz）、重力加速度以及各传感器噪声方差。初始化16维状态向量，其中姿态以四元数形式表达。
2. 轨迹与数据生成：构造载体三维螺旋运动轨迹。模拟IMU输出时，将导航系下的加速度转换为机体系，并叠加载荷、重力、零偏和高斯白噪声。
3. 滤波循环：

- 观测更新步（10Hz）：当系统检测到外部位置或速度观测信号时，生成Sigma点，通过观测映射提取预测值，计算卡尔曼增益并修正当前状态，同时对四元数进行归一化处理。 - 预测更新步（100Hz）：利用Sigma点集通过非线性状态转移方程（INS运动学模型）进行时间传播，计算预测均值与协方差。

1. 状态转移方程：

- 补偿陀螺仪和加速度计的当前估计偏置。 - 利用四元数派生的旋转矩阵完成比力转换。 - 执行位置（位移公式）、速度（比力积分）和姿态（四元数微分方程）的更新。

关键算法与函数说明

1. Sigma点生成函数：利用Cholesky分解对协方差矩阵进行分解，生成分布在当前均值周围的2n+1个采样点，以捕获非线性分布特征。
2. 非线性状态转移函数：封装了INS的核心运动学逻辑。通过IMU测量值驱动状态传播，涵盖了从机体系到导航系的坐标变换以及四元数乘法更新逻辑。
3. 坐标与姿态工具：

- 欧拉角与旋转矩阵（DCM）转换。 - 欧拉角与四元数双向转换。 - 专用四元数乘法函数，用于处理姿态增量。 - 姿态矩阵生成函数，用于在传播步中处理旋转矢量。

1. 性能评价：实时计算估计轨迹与理想真实轨迹之间的均方根误差（RMS），并动态记录传感器偏置的收敛轨迹，用于评估滤波器的收敛性能。