高精度航天级惯导卡尔曼滤波仿真工具

资 源 简 介

本项目针对航天领域中捷联惯导系统（SINS）在静基座条件下的高精度对准需求，开发了一套基于卡尔曼滤波（Kalman Filter）的最优估计仿真平台。其核心功能是实现从粗对准阶段到精对准阶段的平滑过渡，通过建立精细化的10维系统误差状态方程，实时估计包括三轴姿态角误差、三轴速度误差及惯性器件（陀螺与加速度计）随机漂移在内的关键状态变量。项目实现了基于零速修正（ZUPT）原理的观测模型，将系统计算速度与零参考速度的偏差作为量测输入，利用循环迭代预测与更新逻辑，实现在复杂噪声环境下对航向、俯仰、横滚姿态的快速解算与修正。此外，该平台支持用户自定义传感器噪声统计特性（Q阵与R阵）以及初始协方差矩阵（P阵），可动态评估在不同陀螺精度与环境干扰下系统的对准精度与收敛时间。该系统能够真实模拟航天器地面测试与发射准备期的状态演变，为航天工程师提供算法验证依据、误差来源分析及系统参数配置方案，保障导航系统在正式启动前具备高精度的姿态评价基准。

详 情 说 明

高精度航天级惯导初始对准卡尔曼滤波仿真工具

项目介绍

本项目是一款专门针对航天级捷联惯导系统（SINS）设计的静基座初始对准仿真平台。在航天器发射或地面测试阶段，高精度的初始姿态基准是保障后续导航任务成功的核心。本工具通过模拟真实的物理环境与传感器特性，实现了从粗对准到精对准的完整逻辑，利用卡尔曼滤波技术在存在噪声和漂移的环境下提取最优姿态估计。

功能特性

1. 航天级精度模拟：系统预设了陀螺仪 0.01 deg/h 量级的常值漂移和高精度加速度计偏置，能够模拟极高性能硬件的运行状态。
2. 10维状态估计：除了基础的姿态误差和速度误差，系统还额外建立了四维传感器误差模型（三轴陀螺漂移及Z轴加速度计偏置），显著提升了长周期对准的稳定性。
3. 闭环修正机制：算法采用反馈校准模式，实时将卡尔曼滤波估计出的误差量补偿到捷联惯导的姿态矩阵与速度变量中，防止误差累积。
4. 零速修正（ZUPT）：利用静基座条件下载体名义速度为零的特性，通过观测计算速度与参考零速的偏差来抑制系统发散。
5. 全方位评估维度：支持输出姿态角误差收敛曲线、速度残差、陀螺漂移估计曲线以及协方差矩阵（P阵）的开方评价（RMS），提供算法收敛性的量化指标。

使用方法

1. 环境配置：确保计算机已安装 MATLAB 运行环境。
2. 参数自定义：根据需求修改主逻辑开头的传感器误差参数（如 gyro_bias_std）或地理位置参数（lat）。
3. 执行计算：运行仿真主程序，系统将依次执行数据生成、10秒粗对准以及后续的精对准滤波计算。
4. 结果分析：计算完成后，程序会自动弹出性能评估图谱，并在命令行界面打印最终的姿态误差报告与 RMSE 评估结果。

系统要求

1. 软件环境：MATLAB R2016b 或更高版本。
2. 硬件要求：标准 PC 即可，内存建议 8GB 以上以保证大量循环运算的顺畅。

实现逻辑详述

#### 1. 地球物理建模与传感器仿真 程序首先根据 WGS-84 坐标系参数建立地球物理模型，计算仿真所在位置的重力矢量与地球自转角速度矢量。通过设定的真值姿态矩阵，将地理系下的物理量投影至载体系，并叠加常值偏置、角随机游走（ARW）及速度随机游走（VRW）噪声，生成高频（100Hz）的原始 IMU 数据。

#### 2. 解析法粗对准 在进精对准之前，程序利用前10秒的 IMU 采样数据进行重力矢量和地球自转矢量的均值提取。通过解析法建立载体系（b系）与地理系（n系）之间的初始映射，解决精对准滤波要求的“小角度误差”前提条件。

#### 3. 捷联惯导机械编排 在滤波循环中，算法实现了简化的静基座机械编排：

• 姿态更新：使用旋转矢量法处理陀螺仪输出，通过反对称矩阵构造旋转矩阵增量，确保姿态更新的数学严谨性。
• 速度更新：将加表输出转换至地理系，扣除重力项后积分获取瞬时速度。

#### 4. 10维卡尔曼滤波核心 这是项目的核心算法，包含以下步骤：

• 状态向量定义：包含三轴姿态误差 ($phi$)、三轴速度误差 ($delta v$)、三轴陀螺常值飘移 ($epsilon$) 和 Z 轴加速度计偏置 ($nabla_z$)。
• 系统传播：基于 SINS 误差方程构建 F 矩阵。姿态误差受地球自转约束，速度误差受比力交叉耦合影响。
• 时间预测：进行状态预测与协方差矩阵 P 的离散更新。
• 量测更新：以计算速度为观测量，通过 H 矩阵提取速度误差状态，计算卡尔曼增益 K，实现对所有10个维度的观测修正。

#### 5. 误差反馈与数据后处理 每一采样周期完成更新后，立即将姿态误差和速度误差反馈至当前的导航状态中，并对已修正的状态量进行归零处理（闭环反馈）。同时，记录每一步的估计值与真值的偏差。

关键函数与算法分析

• get_euler 函数：提取姿态矩阵中的欧拉角。采用航天标准的 Z-Y-X (3-2-1) 转动序列，能够从方向余弦矩阵中准确解算出横滚、俯仰和航心角。
• psi_psi_corr 函数：专门处理航向角的周期性跳变问题。由于角度处于 [-pi, pi] 区间，该函数通过差值修正确保在跨越正负180度时误差计算不会出现异常尖峰。
• 矩阵指数映射：在姿态更新部分，利用 sin/cos 级数展开实现从旋转矢量到方向余弦矩阵的直接转换，保证了数值计算的精度。
• 系统噪声矩阵 Q 与量测噪声矩阵 R：代码中严格按照角随机游走和速度随机游走的物理定义设置 Q 阵，根据零点漂移水平设置 R 阵，体现了对卡尔曼滤波物理含义的深度理解。