捷联惯性导航系统静基座精对准卡尔曼滤波仿真

资 源 简 介

该项目旨在实现捷联惯性导航系统（SINS）在静止基座条件下的高精度精对准过程模拟。项目核心在于利用卡尔曼滤波（Kalman Filter）技术，在粗对准提供的初始姿态基础上，进一步抑制传感器噪声并修正姿态误差角。 具体实现过程中，程序建立了包含姿态误差（phi）、速度误差、陀螺仪零漂及加速度计零偏等状态量的线性化误差模型。通过将惯导系统结算出的速度与静基座下的理论速度（零速度）之差作为观测值，实时更新滤波器状态。 该仿真涵盖了捷联更新算法、状态转移矩阵的构建、系统噪声协方差阵（Q）与测量噪声协方差阵（R）

详 情 说 明

捷联惯性导航系统（SINS）静基座精对准卡尔曼滤波仿真

项目介绍

本项目实现了一个完整的捷联惯性导航系统（SINS）在静止基座条件下的精对准仿真流程。通过模拟惯性测量单元（IMU）在受随机噪声、常值漂移和偏置影响下的输出，利用卡尔曼滤波技术对初始姿态误差及传感器偏差进行估计。项目模拟了从粗对准到精对准的完整过程，验证了在外部无高精度参考时，仅依靠静基座零速度观测来抑制导航姿态误差的可行性。

功能特性

本项目具备以下核心功能：

1. 惯导误差模型建模：构建了包含3维姿态误差角、2维水平速度误差、3维陀螺常值漂移以及2维加速度计常值偏置在内的10维状态空间模型。
2. 传感器数据仿真：基于静态基座地理位置，模拟生成包含地球自转角速度和重力加速度的IMU原始数据，并叠加了常值漂移和随机游走噪声。
3. 粗对准算法：实现了基于重力矢量法和地球自转矢量法的解析粗对准。
4. 精对准算法：采用扩展线性化模型的卡尔曼滤波算法，通过东、北向速度误差作为观测值进行状态更新。
5. 闭环补偿机制：在滤波更新周期内，利用估计出的误差量实时修正航向、姿态及速度，并将对应状态量归零。
6. 可视化分析：输出姿态误差收敛曲线、陀螺漂移估计曲线以及协方差（P阵）收敛过程图。

使用方法

1. 环境配置：确保计算机已安装 MATLAB 环境。
2. 运行仿真：在 MATLAB 指令窗口中直接运行主程序脚本。
3. 结果观察：程序将自动弹出三个图形窗口，分别展示：

• 东、北向水平姿态角及航向角误差随时间的收敛情况。
• 三轴陀螺仪常值漂移（deg/h）的估计过程。
• 姿态项和陀螺漂移项的协方差标准差收敛情况，用于验证滤波器的稳健性。

1. 数据查看：程序结束后，命令行窗口会打印出最终的姿态方向余弦矩阵、最终姿态角数值以及陀螺漂移的估计结果。

系统要求

• 软件支持：MATLAB R2016a 及以上版本。
• 硬件要求：标准 PC 配置，内存不低于 4GB。
• 基础知识：需具备捷联惯导基础理论、卡尔曼滤波原理以及矩阵论相关知识。

实现逻辑分析

系统初始化与IMU仿真 程序首先根据设置的地理位置（纬度、经度、高度）计算地球自转角速度及当地重力加速度在导航系（东-北-天）下的投影。通过初始姿态矩阵将导航系下的物理量映射至载体系，生成理想的陀螺仪和加速度计输出。随后向理想输出中加入设置好的常值漂移、随机游走（ARW）以及加速度计偏置和噪声。

粗对准过程 主程序利用前2秒的采样数据，通过对加速度计输出取平均值获取重力矢量在载体系的投影，结合对陀螺仪取平均获取的地球自转矢量。通过矢量交叉运算（重力矢量法）构建粗对准姿态矩阵。为了模拟实际精对准环境，程序在粗对准结果上人为注入了东、北、天三个方向的初始角度误差。

捷联机械编排 在每一个仿真步长（0.01s）内，系统执行简化的静基座惯导结算：

• 姿态更新：采用旋转向量四元数法。考虑了地球自转对姿态更新的影响，通过计算旋转向量并将其转化为增量四元数，维持姿态矩阵的实时更新。
• 速度更新：将载体系加速度计输出投影至导航系，补偿重力加速度后积分得到瞬时速度。

卡尔曼滤波精对准逻辑 程序在主循环内同步进行滤波器的预测与更新：

• 状态预测：基于10维状态量构建系统的连续时间状态转移矩阵 F。其中，姿态误差变化受地球自转及陀螺漂移驱动；速度误差受姿态误差及加速度计偏置驱动。程序利用一阶离散化方法计算离散转移阵，并更新系统协方差矩阵 P。
• 观测更新：每隔1秒（即100个采样周期）进行一次量测。以计算出的东、北向速度（真值为0，故观测值即为瞬时速度项）作为量测输入。计算卡尔曼增益 K，更正 10 维状态量 X 并更新 P。
• 闭环修正：滤波更新后，立即利用估计出的前3维姿态误差角通过反对称阵修正姿态阵，并修正速度值；随后将已补偿的状态量项（姿态和速度误差项）复位为零，防止误差重复补偿。

关键函数与算法细节说明

姿态表示与转换逻辑 系统内建了多套算法用于姿态处理。姿态角转换函数采用 Z-X-Y（航向-横滚-俯仰）旋转序列，其对应的方向余弦矩阵涵盖了三角函数的组合运算。为保证计算精度和避免奇异性，内部更新采用四元数代数（包含四元数乘法、四元数转矩阵、矩阵转四元数等逻辑）。

反对称阵生成逻辑 程序在多处使用了反对称矩阵（Skew-symmetric matrix）运算，这是由于姿态误差微分方程及速度误差方程中涉及大量的矢量叉乘运算。通过将矢量转化为矩阵乘法，简化了卡尔曼滤波状态转移矩阵的构建。

系统噪声协方差（Q）与测量噪声（R）设置 Q 矩阵的设置严格基于传感器物理参数。陀螺仪的角随机游走方差直接对应姿态误差项的预测噪声；加速度计的速度随机游走方差对应速度误差项的预测噪声。R 矩阵则代表了对于“基座静止”这一观测条件的置信度。

统计与收敛分析逻辑 程序实时记录了 P 阵对角线元素的平方根，这代表了状态估计的标准差。通过半对数坐标系绘图，可以清晰观测到水平姿态角（较快收敛）与航向角（收敛缓慢）以及敏感器偏差项在可观测性上的差异，这符合静基座精对准的物理规律。