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

资 源 简 介

本项目是一个基于MATLAB开发的微型滤波仿真程序，专门针对捷联惯性导航系统（SINS）在静止基座条件下的初始对准过程进行建模与分析。该程序主要通过卡尔曼滤波（Kalman Filter）算法来实现精细对准，旨在帮助初学者深入理解惯性导航误差方程及最优估计理论。核心功能包括：构建基于速度误差和失准角的线性化误差状态方程作为系统模型；建立以惯导计算速度与零速（静止条件）之差为观测量的观测方程；模拟生成包含高斯白噪声和常值零偏的陀螺仪及加速度计原始数据；利用卡尔曼滤波器迭代估计系统的失准角（俯仰、横滚、航向误差）以及惯性器件的误差参数（如陀螺仪漂移）。程序详细展示了滤波器增益的变化、状态协方差矩阵的更新以及误差状态的收敛过程，最终通过反馈校正实现对初始姿态矩阵的精确修正，为后续的纯惯导解算提供可靠的初始基准。

详 情 说 明

惯导系统静基座卡尔曼滤波初始对准程序

项目介绍

本项目是一个用于模拟捷联惯性导航系统（SINS）在静止基座条件下进行初始对准的仿真程序。它基于MATLAB环境开发，完整展现了从传感器数据生成、惯性解算到卡尔曼滤波误差估计与反馈校正的全过程。

该仿真旨在帮助研究人员和学生理解惯性导航系统的误差传播机理，特别是如何利用零速修正（ZUPT）技术结合卡尔曼滤波器，在静止条件下估计并修正系统的姿态失准角以及惯性器件（陀螺仪和加速度计）的零偏误差。

功能特性

• 高精度环境模拟：内置WGS-84地球椭球模型，能够根据设定的经纬高计算当地重力加速度和地球自转角速度分量，生成精确的真值参考。
• 传感器误差建模：模拟真实的惯性测量单元（IMU）输出，包含了常值零偏（Bias）和角度/速度随机游走（Random Walk）噪声，支持对陀螺仪和加速度计精度的自定义配置。
• 12维卡尔曼滤波设计：构建了包含姿态误差（3维）、速度误差（3维）、陀螺仪零偏（3维）和加速度计零偏（3维）的系统状态方程，实现多源误差的同时估计。
• 闭环反馈校正：实现了反馈校正机制，滤波器的估计结果直接用于修正系统的姿态四元数和速度，模拟实际工程中的精对准过程。
• 可视化分析：程序结束后自动生成收敛曲线图，展示俯仰、横滚、航向失准角的收敛过程以及陀螺仪零偏的估计效果。

使用方法

1. 环境准备：确保安装有MATLAB软件（建议R2016b及以上版本）。
2. 文件准备：确保主脚本与所需的惯导工具箱函数（如 a2mat, m2quat, sins_update, askew, earth 等）在同一路径下。
3. 运行仿真：直接运行主脚本。程序将执行300秒（默认设置）的仿真过程。
4. 结果查看：仿真完成后，控制台将输出状态信息，并弹出图形窗口展示失准角收敛精度和器件零偏估计结果。

系统要求

• MATLAB 所有版本兼容（需包含基础数学库）
• 需要配合基础惯导解算函数库使用（如秦永元老师书中的配套代码库或PSINS工具箱基础函数）

核心算法与实现逻辑

1. 参数初始化与真值生成

程序首先定义了地球物理常量（如地球半径、自转角速率、偏心率等）。基于设定的初始经纬高，计算当地的重力矢量和地球自转矢量在导航系（NED：北东地）下的投影。由于是静基座对准，通过真值姿态矩阵（默认为水平且指北）反推惯性系下的比力和角速度，作为无噪声的传感器真值。

2. IMU 误差模拟

为了接近真实物理环境，程序在真值数据上叠加了误差模型：

• 白噪声：基于设定的随机游走系数，生成高斯白噪声。
• 常值零偏：叠加固定的陀螺仪漂移（如 0.1 deg/h）和加速度计偏置（如 100 ug）。
• 初始不对准：人为引入较大的初始姿态误差（俯仰/横滚1度，航向5度），用于测试滤波器的收敛能力。

3. 卡尔曼滤波器设计 (Kalman Filter)

程序采用间接法卡尔曼滤波（Error-State Kalman Filter），状态向量 X 为12维：

• Phi (1-3): 东、北、天三个方向的失准角误差。
• dV (4-6): 东、北、天三个方向的速度误差。
• Epsilon (7-9): 陀螺仪三个轴向的常值零偏。
• Delta (10-12): 加速度计三个轴向的常值零偏。

状态方程 (F矩阵)： 基于SINS误差微分方程构建。姿态误差方程考虑了地球自转与姿态矩阵的耦合；速度误差方程考虑了比力积分与科里奥利力的影响。将连续时间系统矩阵 F 进行离散化处理得到状态转移矩阵 Phikk。

观测方程 (H矩阵)： 利用静止基座条件，系统的真实速度应为零。将SINS解算输出的速度与零速之差作为观测量（Z = Vn_sins - 0）。观测矩阵 H 主要提取状态向量中的速度误差项。

4. 仿真主循环 (Main Loop)

在每个采样周期（Ts = 0.01s）内执行以下步骤：

1. 数据生成：合成包含噪声和零偏的IMU量测数据。
2. 捷联解算 (Mechanization)：利用含有误差的传感器数据进行姿态、速度和位置更新（调用 sins_update）。
3. 时间更新：计算系统状态转移矩阵，预测下一时刻的状态 X 和协方差矩阵 P。
4. 量测更新：计算卡尔曼增益 K，利用速度误差观测值更新状态估计 X 和协方差 P。
5. 反馈校正 (Feedback)：

* 姿态修正：利用估计出的失准角构建修正四元数，对当前姿态进行左乘修正，并归一化。 * 速度修正：直接从当前速度中减去估计出的速度误差。 * 状态重置：完成修正后，将滤波器状态向量中的姿态和速度误差项清零（闭环模式），而传感器零偏状态保留以便持续收敛。

5. 结果分析与记录

在循环中，程序实时记录了仿真时间、惯导解算姿态、估计的失准角误差以及器件零偏估计值。特别地，程序计算了SINS姿态与真值姿态之间的差异（真实误差），以便与卡尔曼滤波器的估计误差进行对比验证，最终通过绘图直观展示对准精度从初始的大误差逐渐收敛至零附近的动态过程。