【滤波算法】基于拓展卡尔曼滤波实现惯性导航系统（INS）的误差状态估计附matlab代码

matlab科研社

于 2025-03-22 15:00:02 发布

阅读量1k

点赞数 10

文章标签：算法 matlab 数据结构

本文链接：https://blog.csdn.net/Matlab_dashi/article/details/146440332

版权

✅作者简介：热爱科研的Matlab仿真开发者，擅长数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。

🍎 往期回顾关注个人主页：Matlab科研工作室

🍊个人信条：格物致知,完整Matlab代码及仿真咨询内容私信。

🔥 内容介绍

惯性导航系统(Inertial Navigation System, INS)是一种自主式的导航定位技术，它利用安装在载体上的惯性测量单元(Inertial Measurement Unit, IMU)实时测量载体的角速度和加速度，并以此推算出载体的位置、速度和姿态等信息。由于INS的自主性、不受外界环境干扰等优点，其在航空航天、船舶航行、机器人导航等领域得到了广泛应用。然而，INS也存在一个固有缺陷，即其导航精度会随着时间推移而不断累积误差。这些误差来源于IMU传感器的固有误差（例如零偏、标度因数误差）、初始对准误差以及计算过程中的舍入误差等。因此，如何有效地抑制INS的误差累积，提高其导航精度，一直是INS领域研究的重要课题。

为了解决INS的误差累积问题，一种常用的方法是采用滤波算法，对INS的误差状态进行估计和补偿。卡尔曼滤波(Kalman Filter, KF)作为一种最优状态估计算法，在INS误差估计中被广泛应用。然而，由于INS中的运动方程通常是非线性的，传统的线性卡尔曼滤波无法直接应用于INS的误差估计。为此，人们提出了多种非线性卡尔曼滤波算法，其中拓展卡尔曼滤波(Extended Kalman Filter, EKF)是应用最为广泛的一种。

本文将重点探讨基于拓展卡尔曼滤波实现INS的误差状态估计。首先，我们将介绍INS的基本原理及其误差特性，分析误差产生的原因和传播规律。其次，我们将推导基于误差状态模型的拓展卡尔曼滤波方程，并详细阐述其实现过程。最后，我们将讨论拓展卡尔曼滤波在INS误差估计中的优缺点，并展望未来的研究方向。

一、惯性导航系统（INS）的基本原理与误差特性

INS的核心思想是利用惯性定律，通过测量载体的角速度和加速度来推算其运动状态。IMU是INS的核心组成部分，它通常由三个陀螺仪和三个加速度计组成。陀螺仪用于测量载体在三个轴向上的角速度，加速度计用于测量载体在三个轴向上的加速度。

INS的工作流程大致可以分为以下几个步骤：

数据采集：
IMU实时采集载体的角速度和加速度信息。
数据预处理：
对采集到的IMU数据进行滤波、校准等预处理操作，以降低传感器误差的影响。
姿态解算：
利用陀螺仪数据解算载体的姿态信息，通常采用四元数法或方向余弦矩阵法。
速度解算：
利用加速度计数据，并在姿态解算的辅助下，解算载体的速度信息。
位置解算：
利用速度信息解算载体的位置信息。

INS的误差主要来源于以下几个方面：

IMU传感器误差：
IMU传感器自身存在各种误差，例如零偏(Bias)、标度因数误差(Scale Factor Error)、随机噪声(Random Noise)等。这些误差会直接影响INS的导航精度。
初始对准误差：
INS需要进行初始对准，以确定其初始姿态和位置。初始对准误差会随着时间的推移而不断累积，导致导航精度下降。
计算误差：
在INS的计算过程中，由于舍入误差、截断误差等因素，也会产生一定的误差。
安装误差：
IMU在安装过程中可能存在一定的安装误差，例如安装角度误差等。

INS的误差传播具有复杂的特性。通常，位置误差会随着时间呈立方增长，速度误差会随着时间呈二次增长，姿态误差会随着时间呈线性增长。因此，仅仅依靠INS自身进行导航，其精度会随着时间的推移而迅速下降。

二、基于误差状态模型的拓展卡尔曼滤波方程推导

为了有效地抑制INS的误差累积，需要对INS的误差状态进行估计和补偿。拓展卡尔曼滤波是一种常用的非线性卡尔曼滤波算法，可以用于INS的误差状态估计。

首先，需要建立INS的误差状态模型。通常，INS的误差状态向量可以定义为：

ini

x = [δφ, δv, δp, ε, ∇]ᵀ

其中：

δφ：姿态误差向量，表示真实姿态与计算姿态之间的偏差。
δv：速度误差向量，表示真实速度与计算速度之间的偏差。
δp：位置误差向量，表示真实位置与计算位置之间的偏差。
ε：陀螺仪的零偏向量，表示陀螺仪的零偏。
∇：加速度计的零偏向量，表示加速度计的零偏。

接下来，需要建立误差状态模型的动态方程和观测方程。由于INS中的运动方程是非线性的，因此需要进行线性化处理。通常采用一阶泰勒展开法，将非线性方程线性化。

1. 动态方程：

误差状态的动态方程可以表示为：

δẋ = Fδx + Gω

其中：

F：状态转移矩阵，描述误差状态的传播规律。F矩阵的推导需要根据具体的INS运动方程进行线性化处理。例如，姿态误差的传播与陀螺仪的测量值有关，速度误差的传播与加速度计的测量值和姿态误差有关，位置误差的传播与速度误差有关。陀螺仪和加速度计的零偏通常被建模为随机游走过程。
G：系统噪声矩阵，描述系统噪声对误差状态的影响。
ω：系统噪声向量，通常假设其服从高斯分布。

2. 观测方程：

为了实现误差状态的估计，需要引入外部观测信息，例如GNSS (Global Navigation Satellite System)数据、高度计数据等。观测方程描述了观测值与误差状态之间的关系。观测方程可以表示为：

ini

z = Hδx + ν

其中：

z：观测向量，表示外部观测信息。例如，如果采用GNSS数据作为观测值，则z可以表示GNSS的位置信息与INS计算的位置信息之间的偏差。
H：观测矩阵，描述误差状态与观测值之间的关系。H矩阵的推导需要根据具体的观测方式进行。
ν：观测噪声向量，通常假设其服从高斯分布。

有了动态方程和观测方程，就可以应用拓展卡尔曼滤波进行误差状态估计。拓展卡尔曼滤波的迭代过程包括以下两个步骤：

1. 预测步骤：

状态预测： δxk|k-1 = Fk-1δxk-1|k-1
协方差预测： Pk|k-1 = Fk-1Pk-1|k-1Fk-1ᵀ + GQGᵀ

其中：

δxk|k-1：k时刻的状态预测值，基于k-1时刻的后验估计。
Pk|k-1：k时刻的协方差预测值，反映状态预测的不确定性。
Q：系统噪声的协方差矩阵。

2. 更新步骤：

卡尔曼增益： Kk = Pk|k-1Hkᵀ(HkPk|k-1Hkᵀ + Rk)-1
状态更新： δxk|k = δxk|k-1 + Kk(zk - Hkδxk|k-1)
协方差更新： Pk|k = (I - KkHk)Pk|k-1

其中：

Kk：卡尔曼增益，用于衡量观测信息对状态估计的影响程度。
δxk|k：k时刻的后验估计值，结合了预测值和观测值的信息。
Pk|k：k时刻的后验协方差矩阵，反映状态估计的不确定性。
R：观测噪声的协方差矩阵。
I：单位矩阵。

通过不断地进行预测和更新，拓展卡尔曼滤波可以对INS的误差状态进行实时估计。然后，可以利用估计得到的误差状态对INS的输出进行补偿，从而提高INS的导航精度。例如，可以用估计得到的姿态误差对姿态解算结果进行修正，用估计得到的速度误差对速度解算结果进行修正，用估计得到的位置误差对位置解算结果进行修正。

三、拓展卡尔曼滤波在INS误差估计中的优缺点及未来研究方向

拓展卡尔曼滤波作为一种常用的非线性卡尔曼滤波算法，在INS误差估计中具有以下优点：

简单易实现：
EKF的实现相对简单，只需要对非线性方程进行一阶泰勒展开即可。
计算效率较高：
EKF的计算量相对较小，可以满足实时性的要求。
应用广泛：
EKF已经在INS误差估计领域得到了广泛应用，并取得了较好的效果。

然而，拓展卡尔曼滤波也存在一些缺点：

线性化误差：
EKF采用一阶泰勒展开法进行线性化处理，忽略了高阶项，可能导致较大的线性化误差，尤其是在非线性程度较高的情况下。
对初始值敏感：
EKF对初始值的选择比较敏感，如果初始值选择不当，可能会导致滤波发散。
协方差矩阵计算复杂：
EKF需要计算协方差矩阵的逆矩阵，计算量较大，且容易出现数值不稳定问题。

为了克服拓展卡尔曼滤波的缺点，人们提出了多种改进的非线性卡尔曼滤波算法，例如无迹卡尔曼滤波(Unscented Kalman Filter, UKF)、粒子滤波(Particle Filter, PF)等。

无迹卡尔曼滤波：
UKF采用无迹变换(Unscented Transformation, UT)来逼近非线性函数，避免了线性化过程，能够更好地处理非线性问题。
粒子滤波：
PF采用蒙特卡洛方法，利用大量粒子来近似状态分布，能够处理任意形式的非线性问题，但计算量较大。

未来的研究方向包括：

研究更精确的非线性滤波算法：
例如，可以研究基于深度学习的滤波算法，利用神经网络来学习非线性动态模型和观测模型，从而提高滤波精度。
研究多传感器融合的滤波方法：
可以将INS与其他传感器（例如GNSS、激光雷达、视觉传感器）的数据进行融合，利用多传感器的互补性，提高导航精度和可靠性。
研究自适应滤波算法：
可以根据INS的运行状态和环境条件，自动调整滤波器的参数，从而提高滤波器的鲁棒性和适应性。
研究基于云计算的滤波算法：
可以将滤波算法部署在云计算平台上，利用云计算平台的强大计算能力和存储能力，实现大规模INS数据的处理和分析。