捷联惯导静基座初始对准卡尔曼滤波仿真程序

资 源 简 介

该项目是一个专门为初学者设计的MATLAB仿真环境，用于演示捷联惯导系统（SINS）在静止基座情况下的初始对准过程。程序详细实现了从粗对准到基于卡尔曼滤波的精对准全流程。在精对准阶段，程序建立了包含速度误差、姿态误差角以及传感器零偏的系统状态向量，通常采用10维或12维状态方程。利用静止状态下系统的外部参考速度（恒为0）作为观测输入，构建线性离散化卡尔曼滤波器。通过连续处理加速度计和陀螺仪的采样数据，利用重力向量和地球自转角速度信息，滤波器能够实时修正姿态矩阵并抑制传感器偏置带来的漂移。该小程序提供了完整的动态方程推导逻辑，不仅能够展示方位角、俯仰角和横滚角的收敛过程，还能帮助用户理解噪声矩阵Q和R对对准速度与精度的影响。它特别适合于研究惯性技术基础、误差补偿及状态估计，是进行惯导算法交流和二次开发的理想参考模板。

详 情 说 明

捷联惯导静基座初始对准卡尔曼滤波仿真程序

项目介绍

本项目是一个专为惯性导航初学者设计的MATLAB仿真环境，旨在演示捷联惯导系统（SINS）在静止基座条件下的初始对准原理。程序通过数值仿真模拟IMU（惯性测量单元）在静止状态下的输出，完整地呈现了从初步姿态确定（粗对准）到高精度误差估计（精对准）的技术路径。该项目不仅提供了核心算法的数学实现，还包含了详细的可视化工具，用于分析对准精度和收敛特性。

功能特性

1. 完整的对准流程：覆盖了静基座对准的两个关键阶段，包括基于矢量观测的解析法粗对准，以及基于12维卡尔曼滤波的精对准。
2. 高保真IMU数据模拟：在理想比力和陀螺角速度基础上，精确叠加了陀螺仪常值漂移、加速度计常值偏置、角度随机游走及速度随机游走噪声。
3. 12维状态估计算法：精对准部分采用了包含3维姿态误差角、3维速度误差、3维陀螺零偏及3维加速度计零偏的状态方程。
4. 闭环误差反馈：实现了卡尔曼滤波状态量对姿态矩阵和物理速度的实时修正，并包含状态矢量重置逻辑。
5. 综合性能评估：自动生成姿态误差收敛曲线、速度误差演变图、协方差1-sigma收敛曲线以及传感器零偏估计曲线。

使用方法

1. 确保您的计算机中已安装MATLAB（推荐版本R2016b及以上）。
2. 将仿真程序的脚本文件放置在MATLAB的工作路径下。
3. 在命令行窗口直接运行主仿真程序。
4. 运行结束后，程序将自动弹出四个子图窗口，展示对准性能分析结果，并在控制台打印最终的姿态误差报告。
5. 您可以通过修改代码中的传感器误差参数（如gyro_bias或accel_noise）来观察不同等级IMU对对准精度的影响。

系统要求

• 运行环境：MATLAB R2016b 或更高版本。
• 硬件配置：无需特殊计算资源，标准个人电脑均可流畅运行。
• 依赖库：无需额外安装任何工具箱，核心代码基于MATLAB标准矩阵运算。

实现逻辑说明

仿真逻辑严格遵循惯性导航标准算法流程：

1. 参数初始化：定义地球参数（半径、自转角速度、重力加速度）、地理位置（经纬度）以及仿真步长。
2. 真值生成：设定初始真实姿态角，并根据地理位置计算静基座下的理想陀螺输出（地球自转角速度分量）和理想加速度计输出（重力分量）。
3. 解析法粗对准：利用前30秒的IMU数据均值进行矢量匹配。通过重力矢量归一化和地球自转矢量归一化构建两个非共线矢量，利用辅助矩阵运算求解初始坐标转换矩阵。
4. 卡尔曼滤波精对准：

- 姿态更新：使用等效旋转矢量法（Rodrigues公式）更新姿态矩阵。 - 速度更新：在地理坐标系下进行比力投影，补偿重力加速度后积分得到瞬时速度。 - 预测步：构建12×12的系统状态转移矩阵，描述姿态角误差、速度误差与传感器偏置之间的耦合关系。 - 更新步：以地理系导航速度作为观测值（准则为静基座下参考速度恒为0），构建观测矩阵H，提取速度误差进行滤波更新。 - 反馈逻辑：每一采样周期后，将滤波估计出的姿态误差角通过反对称阵修正姿态矩阵，将速度误差反馈修正导航速度，并重置相关状态项。

关键函数与算法细节

1. 坐标转换逻辑：

- 使用 a2mat 函数将俯仰、横滚、航向角转换为地理系到机体系的旋转矩阵。 - 使用 mat2a 函数从旋转矩阵中逆向提取姿态角，用于误差比对。

1. 误差补偿算法：

- askew 函数用于生成矢量的反对称矩阵，这是处理姿态误差角反馈和系统状态空间描述的核心逻辑。 - rv2mat 函数实现了从等效旋转矢量到旋转矩阵的转换，保证了姿态更新过程中的正交性。

1. 状态方程设计：

- 姿态误差方程考虑了地球自转引起的导航系转动影响。 - 速度误差方程考虑了比力矢量在姿态误差作用下的投影误差。

1. 噪声处理：

- 滤波器通过 Qc 和 R 矩阵分别刻画了系统建模噪声（基于传感器游走特性）和观测噪声（基于参考速度精度）。