基于鲁棒滤波的静止基座惯性导航初始对准系统

资 源 简 介

该项目旨在解决惯性导航系统（INS）在静止基座进行初始对准时，传统卡尔曼滤波在面对非高斯噪声、环境突发振动或传感器观测离群点时精度大幅下降的问题。系统通过建立高维误差状态方程，将姿态误差、速度误差以及惯性传感器零偏作为状态变量。核心算法采用自主设计的鲁棒滤波方案，利用M估计理论或Huber惩罚函数，根据实时观测残差的大小动态调整观测噪声协方差矩阵。当检测到外部干扰导致观测值异常时，算法能够自动降低异常数据的权重，从而确保初始对准过程在复杂的实验环境下仍具有极高的鲁棒性和稳定性。 功能模块涵盖了粗对准阶段与

详 情 说 明

基于鲁棒滤波算法的静止基座惯性导航初始对准系统

项目介绍

本项目致力于解决惯性导航系统（INS）在静止基座进行初始对准时，由于环境突发振动、人员走动或传感器随机离群点导致的观测精度下降问题。传统的卡尔曼滤波（KF）在面对非高斯噪声时极易发散或产生巨大误差，本系统通过引入基于M估计理论的鲁棒滤波算法，实现了在复杂干扰环境下的高精度姿态估计、速度修正及传感器偏置校准。

功能特性

1. 复杂环境模拟：系统能够模拟静止基座下的陀螺仪和加速度计输出，并特别针对实际工况中常见的瞬时干扰，在特定时间段人为构造高幅值的加速度计观测离群点，用于验证算法的抗干扰能力。
2. 解析法粗对准：利用重力矢量和地球自转角速度在载体系与地理坐标系下的投影关系，通过多矢量对准原理快速获取初始姿态矩阵，为后续精对准提供良好的初始状态。
3. 高维误差状态滤波：构建了包含3维姿态误差、3维速度误差、3维陀螺零偏和3维加速度计零偏在内的12维状态空间模型。
4. 鲁棒抗离群点策略：核心算法集成了基于Huber惩罚函数的自适应调整机制。通过实时监测预测残差（Innovation），动态调整观测噪声协方差矩阵，自动削弱异常观测值的权重。
5. 综合性能对比：系统同步运行标准卡尔曼滤波与鲁棒卡尔曼滤波，并提供欧拉角误差、残差范数等指标的实时动态曲线对比及统计分析。

使用方法

1. 环境配置：确保计算机已安装MATLAB（建议R2020a及以上版本）。
2. 参数自定义：用户可根据实际传感器规格修改程序开头的传感器误差参数（如陀螺零偏、随机游走、加速度计噪声密度等）。
3. 运行仿真：运行主程序脚本，系统将自动开始数据生成、粗对准、精对准循环及结果绘图。
4. 结果解读：通过生成的姿态收敛曲线和残差监测图，观察算法在离群点干扰时刻（如100秒和200秒附近）的表现，并参考命令行输出的精度统计报告。

系统要求

1. 软件环境：MATLAB R2016b 或更高版本。
2. 基础工具箱：无需特殊工具箱，代码基于基础数值运算和绘图函数实现。

功能实现逻辑说明

1. 参数初始化与IMU建模：

定义采样率（100Hz）和对准时长（300s）。基于特定纬度计算地球自转角速度分量和重力矢量分量。向理想IMU数据中添加常值零偏、白噪声。

1. 离群点干扰注入：

在模拟数据的特定索引区间（如100-101秒和200-201秒），向加速度计数据中叠加较大幅度的随机振动噪声，以此模拟地基震动等突发外部干扰。

1. 粗对准流程：

提取前10秒的数据进行平均化处理，消除高频噪声影响。采用三矢量/双矢量对准逻辑，通过交叉乘积构造基准坐标系，计算从载体系到地理系的初始变换矩阵。

1. 误差状态转移建模：

算法核心包含了一个3x3的反对称矩阵函数用于处理旋转矢量。状态转移矩阵Phi考虑了姿态角误差与传感器偏置之间的耦合关系，以及速度误差随重力和姿态变化的演变逻辑。

1. 滤波器更新机制：

标准KF分支：严格按照经典的预测与更新步骤执行，观测值直接采用零速约束。 鲁棒KF分支：计算残差向量及其归一化标准差。当检测到某个观测通道的归一化残差超过预设门限（gamma=2.5）时，判定为收到离群点干扰。此时，系统会根据残差偏离程度成比例地放大该通道对应的观测噪声方差R，从而在增益计算中抑制异常输入。

1. 状态反馈与姿态更新：

滤波器估计的是误差状态。在每一时间步，将估计出的姿态误差角通过旋转矢量补偿的方式作用于当前的姿态阵，并扣除估算的速度误差。反馈后的误差状态量清零，进入下一轮迭代。

关键算法与实现细节

1. M估计原理：

代码中通过判断 abs(v_k(i)) / sigma_v(i) > gamma 来识别异常。其背后的数学逻辑是：当新信息与旧有估计发生严重冲突时，系统不再信任当前的观测值，而是通过增大其对应的R值来降低卡尔曼增益，使权值向预测值倾斜。

1. 姿态矩阵更新：

系统采用了简化的旋转运动学更新方程，结合反对称矩阵实现姿态阵的实时推算。这种方式在初始对准的小角度误差假设下具有极高的计算效率。

1. 坐标转换与欧拉角提取：

自定义了从旋转矩阵提取欧拉角的子函数，遵循ZYX转动顺序（偏航、俯仰、横滚），确保输出结果符合导航工程中常用的物理定义。

1. 性能评价体系：

代码最后通过计算对准过程最后10%稳态阶段的标准差（STD），量化对比标准算法与鲁棒算法在面对干扰时的对准精度提升比例，直观展示了鲁棒算法对非高斯扰动的抑制效果。