matlab - 间接(错误状态)卡尔曼滤波器的结构是什么，以及如何得出错误方程式？

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

我已经阅读了许多文本，而我用来实现ESKF的两个文本是“Quaternion kinematics for the error-state KF”和“用于IMU摄像机校准的基于卡尔曼滤波器的算法”(付费墙纸，可在谷歌搜索)。
我使用第一个文本是因为它更好地描述了ESKF的结构，第二个文本是因为它包含了有关视觉测量模型的详细信息。在我的问题中，我将使用第一个文本中的术语:“名义状态”，“错误状态”和“真实状态”；指的是IMU积分器，卡尔曼滤波器以及两者的组成(标称负误差)。

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

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

我的第二个问题:如何得出测量模型的误差状态方程式？
就我而言，我尝试使用第二个文本的度量模型，但是它没有用。

亲切的问候，

最佳答案

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

您有一个外部IMU集成器(绿色，标记为“INS”，有时称为机械化，并被您称为“标称状态”，但我也看到它称为“引用状态”)。该方法可以自由地将IMU外部集成到KF，因此通常选择该方法，因此您可以以与KF预测/更新步骤(间接形式)不同(更高)的速率进行此集成。从历史上看，我认为这很流行，因为KF通常是计算上昂贵的部分。

您还已将IMU作为u馈入KF块，我假设这是输入到KF的“命令”。这是外部积分器的替代方案。在直接KF中，您会将IMU数据视为度量。为此，IMU必须建模(位置，速度和)加速度和(方向和)角速度:否则，就没有H，因此Hx可以产生估计的IMU输出项。如果改为将IMU测量值作为命令输入，则您的预测步骤可以简单地充当积分器，因此您只需建模速度和方向即可。

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

关于版本(1)的测量方程，在任何KF中，您只能预测从状态中可以了解的内容。在这种情况下，KF状态是误差项的向量，因此您只能预测“位置误差”之类的东西。结果，您需要对z中的测量进行预处理，以防止出现位置错误。因此，请根据“估计的真实状态”与“嘈杂的摄像头观察”中的位置之间的差异来进行测量。确切的想法可以通过输入到间接KF的xHat来表示。我对那里发生的MATLAB/Simulink东西一无所知。

关于求和块(以红色显示)的现实世界考虑因素，请引用another answer about indirect Kalman filters。