matlab - 间接(误差状态)卡尔曼滤波器的结构是什么？误差方程是如何导出的？

Question

我一直在尝试为机器人实现导航系统，该系统使用惯性测量单元 (IMU) 和已知地标的相机观测值，以便在其环境中定位自身。我选择了间接反馈卡尔曼滤波器(又名错误状态卡尔曼滤波器，ESKF)来执行此操作。我在 Extended KF 方面也取得了一些成功。

我阅读了很多文本，我用来实现 ESKF 的两本是“Quaternion kinematics for the error-state KF”和“一种基于卡尔曼滤波器的 IMU 相机校准算法”(付费论文, 谷歌能力)。 我使用第一个文本是因为它更好地描述了 ESKF 的结构，第二个是因为它包含了有关视觉测量模型的详细信息。在我的问题中，我将使用第一篇文章中的术语:“标称状态”、“错误状态”和“真实状态”；指的是IMU积分器，卡尔曼滤波器，以及两者的组合(标称减去误差)。

下图显示了我在 Matlab/Simulink 中实现的 ESKF 的结构；如果您不熟悉 Simulink，我将简要解释该图。绿色部分是标称状态积分器，蓝色部分是 ESKF，红色部分是标称状态和误差状态之和。 “RT” block 是可以忽略的“速率转换”。

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

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

亲切的问候，

Answer 1

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

1. 你有一个外部 IMU 积分器(绿色，标记为“INS”，有时称为机械化，你将其描述为“标称状态”，但我也看到它称为“引用状态”)。这种方法可以自由地将 IMU 外部集成到 KF 中，通常会选择这种方法，因此您可以以与 KF 预测/更新步骤(间接形式)不同(高得多)的速率进行这种集成。从历史上看，我认为这很流行，因为 KF 通常是计算量大的部分。
2. 您还将您的 IMU 作为 u 馈入 KF block ，我假设它是 KF 的“命令”输入。这是外部集成器的替代品。在直接 KF 中，您会将 IMU 数据视为测量值。为了做到这一点，IMU 必须对(位置、速度和)加速度和(方向和)角速度进行建模:否则不可能有 H 这样 Hx 可以产生估计的 IMU 输出项)。如果您改为将 IMU 测量值作为命令输入，则您的预测步骤可以简单地充当积分器，因此您只需对速度和方向进行建模。

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

关于版本 (1) 的测量方程，在任何 KF 中，您只能预测从您的状态中可以知道的事情。这种情况下的 KF 状态是误差项的向量，因此您只能预测“位置误差”之类的东西。因此，您需要将 z 中的测量值预先设置为位置误差。因此，让您的测量值是您的“估计真实状态”与您从“嘈杂的相机观察”中获得的位置之间的差异。这个确切的想法可以用间接 KF 的 xHat 输入来表示。我对那里发生的 MATLAB/Simulink 事情一无所知。

关于求和 block (红色)的现实考虑，我建议您引用 another answer about indirect Kalman filters .