间接（错误状态）卡尔曼滤波器的结构是什么？误差方程是如何导出的？

Question

我一直在尝试为使用惯性测量单元（IMU）和相机观察已知地标的机器人实施导航系统，以便在其环境中进行本地化。我选择了间接反馈卡尔曼滤波器（也就是误差状态卡尔曼滤波器，ESKF）来做到这一点。我在扩展KF方面取得了一些成功。

我已经阅读了很多文本，我用来实现ESKF的两个文本是＆＃34; Quaternion kinematics for the error-state KF＆＃34;基于卡尔曼滤波器的IMU相机校准算法＆＃34; （付费壁纸，google-able）。 我使用的是第一个文本，因为它更好地描述了ESKF的结构，第二个是因为它包含了有关视觉测量模型的详细信息。在我的问题中，我将使用第一个文本中的术语：＆＃39;名义状态＆＃39;错误状态＆＃39;并且＆＃39;真实状态＆＃39 ;;它指的是IMU积分器，卡尔曼滤波器，以及两者的组成（名义减去误差）。

下图显示了我在Matlab / Simulink中实现的ESKF的结构;如果您不熟悉Simulink，我将简要介绍该图表。绿色部分是标称状态积分器，蓝色部分是ESKF，红色部分是标称状态和错误状态的总和。 ＆＃39; RT＆＃39;块是＆＃39;速率转换＆＃39;这可以忽略。

我的第一个问题：这种结构是否正确？

我的第二个问题：测量模型的误差状态方程是如何导出的？ 在我的情况下，我尝试使用第二个文本的测量模型，但它没有用。

亲切的问候，

Answer 1

您的框图结合了两种间接方法，用于将IMU数据导入KF：

1. 您有一个外部IMU集成商（绿色，标记为＆＃34; INS＆＃34;有时称为机械化，并被您描述为＆＃34;名义状态＆＃34; ，但我也看到它称为＆＃34;参考状态＆＃34;）。这种方法可以自由地将IMU外部集成到KF中，并且通常选择这样，您可以以与KF预测/更新步骤（间接形式）不同（更高）的速率进行此集成。从历史上看，我认为这很受欢迎，因为KF通常是计算上昂贵的部分。
2. 您还将您的IMU作为u提供给KF块，我假设它是＆＃34;命令＆＃34;输入KF。这是外部集成商的替代。在直接KF中，您会将IMU数据视为测量值。为了做到这一点，IMU必须建模（位置，速度和）加速度和（方向和）角速度：否则不可能H使得Hx可以产生估计的IMU输出计算）。如果您将IMU测量值作为命令输入，则预测步骤可以简单地充当积分器，因此您只需要建模速度和方向。

您应该只选择其中一个选项。我认为第二个更容易理解，但它更接近直接卡尔曼滤波器，并且需要您预测/更新每个IMU样本，而不是（我假设）较慢的相机帧速率。

关于版本（1）的测量方程，在任何KF中，您只能预测您可以从您的州知道的事情。在这种情况下，KF状态是错误术语的向量，因此您只能预测像＃34;位置错误＆＃34;这样的事情。因此，您需要在z中预先调整测量结果，使其成为位置误差。因此，让您的测量结果与估计的真实状态和＃34;之间存在差异。和＃34;嘈杂的相机观察＆＃34;。这个确切的想法可以通过间接KF的xHat输入来表示。我对那里发生的MATLAB / Simulink事情一无所知。

关于求和块的实际考虑因素（红色），我建议您another answer about indirect Kalman filters。

Answer 2

Q1）您的SIMULINK模型看起来合适。让我阐述一下基于KF的四元数机械化，我曾为导航应用做过工作。

由于卡尔曼滤波器是一种优雅的数学技术，它借鉴了随机和测量的科学，它可以帮助您降低系统噪声，而无需对噪声进行精细建模。

所有KF系统都首先要对您想要消除噪音的模型有一些初步的了解。反馈测量值以更好地演变状态（测量方程式Y = CX）。在你的情况下，你所谈论的状态是四分之一的误差，即4个值，dq1，dq2，dq3，dq4。

KF在您的应用程序中运行良好，可以通过控制四元数周围的误差来准确地确定设备的姿态/方向。四元数是任何物体的空间方向，使用标量和矢量，更具体地说是角度和轴来理解。

您所谈论的误差方程是有助于卡尔曼增益的协方差。协方差表示均值周围的扩散，它们有助于理解系统的中心/平均行为如何随时间变化。低协方差表示对任何系统的平均行为的偏差较小。随着KF循环运行，协方差不断变小。

卡尔曼增益最终用于补偿测量估计值与摄像机实际测量值之间的误差。

同样，这种优雅的技术首先确保四元数值中的误差收敛于零。

Q2）只要您拥有非线性测量构造技术，EKF就是一种很好的技术。如果系统中的转换过多，请小心使用EKF，即不要尝试使用状态转换来重建测量，这会严重影响模型的神圣性，并且由于噪声协方差不会经历类似的转换，因此会有一旦矩阵不可逆，就有机会击中奇点。

您可以查看恒定增益KF方案，这将使您免于协方差传播并节省大量的计算工作量和时间。这些技术非常新颖，看起来非常有前景。它们主动吸收P（误差协方差），Q（模型噪声协方差）和R（测量噪声协方差），并与EKF方案很好地协同工作。