捷联惯导静基座初始对准与卡尔曼滤波仿真系统

资 源 简 介

本项目完整实现了捷联惯性导航系统（SINS）在静止基座条件下的初始对准过程，旨在为惯导初学者提供一套准且易于理解的代码框架。核心功能主要分为数据仿真与对准解算两大部分。首先，内置的轨迹发生器能够模拟静止状态下的IMU传感器输出，包含地球自转角速度分量、重力加速度分量，并叠加了可配置的陀螺仪和加速度计的常值零偏及随机游走噪声，模拟真实的传感器环境。其次，对准算法模块包含了“解析粗对准”和“卡尔曼滤波精对准”两个阶段：粗对准利用重力矢量和地球自转角速度矢量在地理系与机体系下的投影关系，通过矢量运算快速获取初始姿态矩阵（俯仰、横滚、航向）；精对准阶段建立惯导系统的误差状态方程，利用卡尔曼滤波（KF）对姿态失准角进行最优估计，并反馈校正姿态矩阵，同时估计并补偿陀螺仪漂移。代码包含详细的中文注释，解释了坐标系定义、方向余弦矩阵更新及滤波方程构建过程，帮助用户直观理解惯性导航原理。

详 情 说 明

捷联惯导静基座初始对准仿真系统

项目简介

本项目是一个基于MATLAB开发的捷联惯性导航系统（SINS）仿真程序，专注于静止基座条件下的初始对准过程。系统完整实现了从传感器数据生成、解析粗对准到卡尔曼滤波精对准的全流程。该项目代码结构清晰，算法逻辑严谨，旨在为惯性导航领域的初学者和研究人员提供一个标准、可扩展且易于理解的代码框架，用于深入学习惯导解算原理及误差控制方法。

功能特性

• 高精度参数模型：内置WGS-84椭球模型参数及标准地球自转角速度和重力加速度模型。
• IMU数据仿真：包含轨迹发生器，能够模拟静止状态下陀螺仪和加速度计的理论输出，并支持添加常值零偏（Bias）和随机游走噪声（Random Walk）。
• 解析粗对准：利用重力矢量和地球自转矢量在不同坐标系下的投影关系，通过双矢量定姿算法快速获取初始姿态矩阵。
• SINS机械编排：基于四元数法的姿态更新和速度更新算法，实现了惯导系统的核心解算流程。
• 卡尔曼滤波精对准：构建了包含12维状态量的误差状态卡尔曼滤波（Error-State Kalman Filter），用于估计姿态失准角及传感器误差。

系统要求

• MATLAB R2016a 及以上版本
• 无需额外工具箱（代码主要依赖基础矩阵运算）

使用方法

1. 将代码保存为 main.m 文件。
2. 确保MATLAB当前路径为文件所在目录或已包含在路径中。
3. 直接运行 main 函数。
4. 程序将在命令行窗口输出粗对准的误差结果，并开始执行精对准循环。

详细实现逻辑与算法分析

本项目仅包含一个核心脚本文件，逻辑流程如下：

1. 系统初始化与参数设置

• 物理常数定义：定义了地球长半轴、地球自转角速度、重力加速度以及角度/弧度转换系数。
• 仿真环境配置：设定仿真时长为600秒（10分钟），采样频率为100Hz。
• 地理环境设定：设定仿真地点纬度（例如34度），并定义导航系（n系）为“东-北-天（ENU）”坐标系。
• 参考真值：设定静止基座的真实姿态角为[0; 0; 0]，并计算对应的真实方向余弦矩阵及n系下的重力和自转矢量投影。

2. IMU传感器数据仿真（轨迹发生器）

• 误差模型：

* 陀螺仪：添加了约0.1 deg/h的常值零偏和随机噪声。 * 加速度计：添加了约100 ug的常值零偏和随机噪声。

• 理论数据生成：根据静止状态下的物理特性，计算理想的陀螺仪输出（地球自转角速度在载体系的投影）和加速度计输出（比力，即反向重力加速度在载体系的投影）。
• 噪声叠加：在理想数据基础上叠加预设的零偏和高斯白噪声，生成模拟的IMU采样数据序列。

3. 解析粗对准 (Coarse Alignment)

• 数据预处理：截取仿真前20秒的数据进行平均处理，以抑制高频噪声干扰，并对平均后的比力和角速度矢量进行归一化。
• 姿态解算算法：

* 俯仰角 (Pitch) 与 横滚角 (Roll)：利用加速度计测量的重力矢量在机体系的分量，通过三角函数直接解析计算水平姿态角。 * 航向角 (Yaw)：采用双矢量定姿原理。构造两个坐标系（n系和b系）下的基准矢量集： 1. 矢量1：重力/比力矢量。 2. 矢量2：重力与地球自转矢量的叉乘积（即东西方向矢量）。 3. 矢量3：前两者的叉乘积。 * 通过构造矩阵 $M_b$ 和 $M_n$，利用公式 $C_{bn} = M_n M_b^T$ 直接求解初始方向余弦矩阵，并转换为欧拉角输出误差。

4. SINS机械编排 (Mechanization)

• 姿态更新：

* 使用四元数及其微分方程更新姿态，避免万向节锁问题。 * 计算等效旋转矢量，利用毕卡迭代的一阶近似更新四元数，并进行归一化处理以保持正交性。 * 实时将四元数转换为方向余弦矩阵用于后续计算。

• 速度更新：

* 将机体系测量的比力变换到导航系。 * 计算并补偿哥氏加速度（Coriolis）和重力加速度，通过欧拉积分更新速度。

• 位置更新：代码中初始化了位置，但在对准阶段主要关注姿态和速度，位置假设变化可忽略。

5. 卡尔曼滤波精对准 (Fine Alignment)

• 状态变量设计 (12维)：

* 速度误差 $delta v$ (3维) * 姿态失准角 $phi$ (3维) * 陀螺仪漂移 $epsilon$ (3维) * 加速度计零偏 $nabla$ (3维)

• 滤波器初始化：

* P阵：根据经验设定初始协方差，其中航向角误差设定较大，反映粗对准后的剩余不确定性。 * Q阵：根据传感器噪声水平设定系统过程噪声矩阵。 * R阵：设定速度观测噪声（基于“静止基座速度应为零”的观测假设）。

• 误差状态方程构建：

* 构建连续时间系统矩阵 $F$，包含速度误差传播方程（包含反对称矩阵运算）和姿态误差传播方程。 * 包含惯性器件误差（零偏）对系统状态的影响系数。

• 离散化与预测：

* 利用 $I + F cdot Delta t$ 对系统矩阵进行离散化。 * 执行卡尔曼滤波的时间更新步骤（虽然提供的代码片段在计算 $P_k$ 处截止，但已完整展示了预测阶段的逻辑框架）。

• 闭环反馈：代码逻辑包含了估计出的器件误差（est_gyro_bias, est_acc_bias）对原始IMU数据的反馈补偿，体现了闭环校正的思想。

关键算法说明

• 双矢量定姿：利用地球重力场和自转角速度这两个天然矢量在地理系和机体系的观测值，直接构造旋转矩阵，是静基座粗对准的核心算法。
• 误差状态卡尔曼滤波 (ESKF)：系统并不直接估计位置和姿态，而是估计它们的误差量（失准角、速度误差等）。这种方法不仅线性化程度好，而且能直接估计出传感器的零偏，便于反馈校正。