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

资 源 简 介

该程序是一个专门针对捷联式惯性导航系统（SINS）在静止基座条件下进行初始对准研究的仿真平台。其核心功能是利用卡尔曼滤波（Kalman Filter）技术，在已知载体地理位置的前提下，通过对加速度计和陀螺仪的输出数据进行处理，精确估计出载体坐标系相对于地理坐标系（通常为北-东-地坐标系）的初始姿态阵。 实现方法上，程序首先建立了静基座下的线性化误差模型，将速度误差和失准角误差作为系统的状态向量。在静止状态下，载体的理论速度应为零，因此程序将加速度计解算出的速度误差作为滤波器的测量输入，通过卡尔曼滤波过程不

详 情 说 明

捷联惯导静基座初始对准卡尔曼滤波仿真项目说明文档

项目介绍

本项目是一个专门用于捷联式惯性导航系统（SINS）在静止基座条件下进行初始对准研究的仿真平台。通过模拟加速度计和陀螺仪在静止状态下的输出，利用卡尔曼滤波（Kalman Filter）技术实时估计载体的姿态误差以及传感器偏置。该仿真能够精确展示失准角从较大初始误差收敛到稳定值的全过程，是研究惯导系统初始对准算法、滤波器参数优化及系统误差特性的重要参考工具。

功能特性

1. 高精度传感器误差建模：模拟了陀螺仪和加速度计的复杂误差特性，包括常值偏置（Bias）和随机游走（Random Walk / White Noise），能够更真实地反映传感器在实际环境中的工作状态。
2. 十维状态空间卡尔曼滤波器：系统状态向量涵盖了二维速度误差、三维失准角、三维陀螺漂移以及二维加速度计偏置，实现了对导航参数和传感器零偏的同步估计。
3. 静基座导航动力学模拟：基于地球自转、当地重力加速度及地理纬度，精确生成机体系下的理想惯性数据，并实现了姿态阵的一阶更新与正交化修正。
4. 全方位性能分析可视化：仿真结束后自动生成姿态解算曲线、失准角收敛历程、协方差矩阵（P阵）收敛轨迹以及卡尔曼增益变化情况等多维度图表，便于定性与定量分析滤波性能。

实现逻辑说明

仿真程序按照标准的组合导航解算与滤波流程设计，具体分为以下五个核心步骤：

1. 物理环境与参数配置 程序首先定义了地球长半径、自转角速度和重力加速度等常数，设置了位于北纬34.246度的仿真环境。同时，将传感器误差参数（如deg/h和g）转换为程序内部使用的国际单位制（rad/s和m/s^2）。

2. 模拟IMU原始数据生成 根据设定的初始姿态阵，将地理坐标系（n系）下的地球自转分量和重力分量投影至机体系（b系）。随后，在理想输出的基础上叠加常值偏置和受采样频率影响的随机噪声，生成600秒长度的陀螺仪与加速度计原始采样序列。

3. 卡尔曼滤波器初始化 在进入循环前，初始化状态向量X和协方差矩阵P。初始相位误差设定为较大的值（如方位角5度，水平角1度），以模拟粗对准后的精对准状态。观测矩阵H被配置为仅观测北向和东向的速度误差，这是因为在静止基座下，载体的理想理论速度始终为零。

4. 对准循环递归计算 在每个采样周期内，程序执行以下操作：

• 姿态与速度更新：使用补偿后的传感器数据更新姿态阵（Cbn）和速度。姿态更新考虑了地球自转补偿，并利用SVD分解确保姿态阵的正交公正。
• 状态预测：根据离散化的系统状态转移矩阵Phi，预测当前时刻的状态量X和估计误差协方差P。
• 观测更新：将当前解算出的速度值作为观测余量，计算卡尔曼增益K，进而修正状态向量X和协方差P。

5. 结果处理与显示 程序将每一时刻的失准角估计值、姿态解算值以及滤波器的内部状态（P阵、K增益）记录下来，并在仿真结束后绘制趋势曲线，提供最终姿态矩阵的数值输出。

关键算法与技术细节

• 系统方程简化：程序采用了简化的静基座线性化误差运动学方程。其中，水平速度误差受重力与水平失准角耦合影响，而失准角的变化则受地球自转以及陀螺漂移的共同作用。
• 单位权值转换：在处理随机游走噪声时，严格遵循了连续系统噪声强度与离散系统协方差之间的转换关系（即乘以1/sqrt(dt)或dt），确保了Q、R阵设置的物理意义明确。
• 姿态修正模式：程序当前采用开环估计模式，即卡尔曼滤波器估计出的失准角作为状态量输出，而不实时反馈修改姿态更新循环中的Cbn矩阵。这种方式更便于观察滤波器本身的收敛特性和稳定性。
• 矩阵正交化：在姿态演进过程中，由于计算舍入误差和一阶近似，姿态阵可能失去正交性。程序通过常用的SVD（奇异值分解）方法对Cbn进行单位化和正交化处理。

系统要求

• 软件环境：MATLAB R2016b 或更高版本。
• 工具箱：基础MATLAB模块（无需额外安装符号计算或工程工具箱）。
• 硬件要求：建议内存4GB以上，仿真过程涉及大规模矩阵运算与绘图。

使用方法

1. 打开MATLAB软件，并将当前文件夹切换至仿真程序所在的路径。
2. 在控制台直接运行程序。
3. 等待仿真循环结束（进度可通过循环索引观察），程序将自动弹出四张性能分析分析图表。
4. 查看命令行输出的最后时刻姿态矩阵及失准角估计值，评估对准精度。