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

我一直在尝试为一个机器人实现一个导航系统，该系统使用惯性测量单元（IMU）和已知地标的摄像机观测，以便在其环境中定位自身。我选择了间接反馈卡尔曼滤波器（也称为误差状态卡尔曼滤波器，ESKF）来实现这一点。我在扩展KF方面也取得了一些成功

我读过很多文章，我用来实现ESKF的两篇是“”和“基于卡尔曼滤波器的IMU摄像机校准算法”（pay walled paper，谷歌）。 我使用第一个文本是因为它更好地描述了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的东西一无所知

关于求和块（红色）的实际注意事项，请参考。

Q1）您的SIMULINK模型看起来很合适。让我来介绍一下我为导航应用所做的基于四元数机械化的KF

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

所有KF系统都从您想要消除噪音的模型的一些初步了解开始。反馈测量值以更好地演化状态（测量方程Y=CX）。在您的例子中，您所讨论的状态是四分位数中的错误，这将是4个值，dq1，dq2，dq3，dq4

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

你所说的误差方程是协方差，它有助于卡尔曼增益。协方差表示平均值的分布，它们有助于理解系统的中心/平均行为是如何随时间变化的。低协方差表示任何系统的平均行为偏差较小。随着KF周期的运行，协方差不断变小

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

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

问题2）EKF是一种很好的技术，只要你有一种非线性测量构造技术就可以使用。如果EKF在您的系统中有太多的变换，则在使用EKF时要非常小心，即不要尝试使用状态变换重建测量值，这会严重影响模型的神圣性，因为噪声协方差不会经历类似的变换