在 OpenCV/C++ 中通过（扩展）卡尔曼滤波器实现数据融合

Posted 2023-02-21

【中文标题】在 OpenCV/C++ 中通过（扩展）卡尔曼滤波器实现数据融合

【英文标题】：Implementation of Data Fusion through (Extended) Kalman-Filter in OpenCV/C++

【发布时间】：2015-07-07 08:29:42

【问题描述】：

我正在开展一个项目，通过数据融合跟踪安装在移动设备上的摄像机的位置。 我得到的数据是

1) 来自源 A 的相机在 x、y 和 z 方向的速度

2) 来自源 B 的当前帧和最后一帧中的位置之间的差异（在 2D 中，Z 不应该改变任何一种方式）

我已经做了一个类似的项目，但没有任何数据融合，并使用了在 OpenCV 中实现的卡尔曼滤波器。

目前，我一直对我在网上找到的所有不同的实现技术感到困惑。

我将如何将获得的数据插入/组合到 KF/EKF 的不同组件中？

我有这个示例用于将 OpenCV-KF 更改为 EKF。它看起来很像我需要的东西，除了我的对象实际上是相机本身并且还能够在 y 轴和 x 轴上移动，轮流，....而且我得到的不是对象的像素坐标上述数据。 https://sites.google.com/site/timecontroll/home/extended-kalman-filtering-with-opencv

不幸的是，我不确定如何定义我的转换和测量函数以及相应的矩阵。

令 x(k) 为当前状态，deltaT 为自上次更新以来经过的时间。

对于 1，它可能类似于：

x(k) = x(k-1) + deltaT 乘以速度(k-1)

对于 2：

x(k) = x(k-1) + differenceBetweenFrames

所以让我们假设两个数据源的权重相同，这让我认为结合起来会是这样的：

x(k) = x(k-1) + (deltaT 乘以速度(k-1) + differenceBetweenFrames)/2

对于测量：我通过源 A 获得 X、Y 和 Z 方向的速度，以及通过源 B 在 X 和 Y 坐标中当前帧与最后一帧之间的差异。

这是有道理的还是我已经错了？ 我将如何调整我的转换和测量矩阵以及相应地更新我的当前状态？

【问题讨论】：

相机运动是线性系统，线性卡尔曼滤波器应该不够吗？

抱歉，澄清一下：相机安装在移动设备上，加速度会发生变化，这就是为什么我担心它不像我想象的那样线性。仍然..是否线性我不确定如何调整我的矩阵以处理多个数据:(

只要只考虑相机位置、速度和加速度，即使加速度发生变化，系统也是线性的。但是，如果您还考虑相机方向、角速度等，那么它是非线性的。

是的，我猜它是非线性的。安装相机的设备可以轮流改变相机的方向（可能还有角速度，尽管我现在没有使用它）。即便如此，我认为我总体上了解如何将普通 KF 更改为 EKF，但我只是不确定如何用我得到的数据定义我的矩阵（无论是 KF 还是 EKF）

好的，你似乎明白了，但只是为了更清楚。如果您的状态向量仅包含位置、速度和/或加速度，那么它很可能是线性的。如果您的状态向量还包含方向和/或角速度，那么它可能是非线性的。线性或非线性来自您正在使用的模型（即状态向量、转换/测量函数），而不是您尝试建模的物理系统。

【参考方案1】：

您在正确的轨道上，但您似乎对转换函数和测量函数的概念感到困惑。

首先要做的是明确定义要考虑的状态向量。在您的情况下，位置 [px, py, pz] 和速度 [vx, vy, vz] 就足够了：

x_k = [px_k, py_k, pz_k , vx_k, vy_k, vz_k]

第二件事是选择一个转换函数来模拟系统的动态。请注意，这与传感器测量无关：它只是系统状态如何随时间演变的模型。在位置和速度的情况下，一个非常简单的模型如下（但如果需要，您可以选择使用更高级的模型）：

p_k = [px_k, py_k, pz_k ] = p_k-1 + dT 。 v_k-1

v_k = v_k-1

这可以写成矩阵形式如下：

       [ 1  0  0  dT 0  0  ]
       [ 0  1  0  0  dT 0  ]
       [ 0  0  1  0  0  dT ]
x(k) = [ 0  0  0  1  0  0  ] . x(k-1)
       [ 0  0  0  0  1  0  ]
       [ 0  0  0  0  0  1  ]

最后，您需要导出传感器的测量函数。该测量函数表示当我们知道状态向量时可以推断出传感器测量值的方式。

你的源 A 测量系统的速度，因此测量函数如下：

h_A( x_k ) = v_k

或以矩阵形式：

           [ 0 0 0 1 0 0 ]
hAx(k) = [ 0 0 0 0 1 0 ] . x(k)
           [ 0 0 0 0 0 1 ]

您的源B测量位置差异，因此测量功能如下：

h_B( x_k ) = p_k - p_k-1 = p_k-1 + dT 。 v_k-1 - p_k-1 = dT。 v_k-1 = dT。 v_k

或以矩阵形式：

             [ 0  0  0  dT 0  0  ]
hB x(k)  = [ 0  0  0  0  dT 0  ] . x(k)
             [ 0  0  0  0  0  dT ]

---

如果您想对源 B 有更准确的测量功能，您可能需要在状态向量中添加上一个时间步的位置，如下所示：

x_k = [px_k, py_k, pz_k , px_k-1, py_k-1, pz_k-1,vx_k, vy_k, vz_k]

然后转换函数将变为：

       [ 1  0  0  0  0  0  dT 0  0  ]
       [ 0  1  0  0  0  0  0  dT 0  ]
       [ 0  0  1  0  0  0  0  0  dT ]
x(k) = [ 0  0  0  1  0  0  0  0  0  ] . x(k-1)
       [ 0  0  0  0  1  0  0  0  0  ]
       [ 0  0  0  0  0  1  0  0  0  ]
       [ 0  0  0  0  0  0  1  0  0  ]
       [ 0  0  0  0  0  0  0  1  0  ]
       [ 0  0  0  0  0  0  0  0  1  ]

源 A 的测量函数将变为：

           [ 0 0 0 0 0 0 1 0 0 ]
hAx(k) = [ 0 0 0 0 0 0 0 1 0 ] . x(k)
           [ 0 0 0 0 0 0 0 0 1 ]

源 B 的测量函数将变为：

           [ 1  0  0 -1  0  0  0  0  0 ]
hBx(k) = [ 0  1  0  0 -1  0  0  0  0 ] . x(k)
           [ 0  0  1  0  0 -1  0  0  0 ]

---

话虽如此，测量系统的速度及其位置的差异几乎是相同的。由于您从不测量位置，因此您的状态可能无法观察到，这可能是个问题。请查看以下文档以了解更多详细信息：

“使用卡尔曼滤波器进行状态估计”，作者 F.Haugen (pdf) “可控性和可观察性：工具 卡尔曼滤波器设计”，Southall、Buxton 和 Marchant (pdf)

【讨论】：

太棒了，感谢您的详尽解释。我现在将通过这个并尝试理解所有内容。如有必要，我可能会再向您询问更多问题。非常感谢。

我还有一个关于数据融合方面和 2 个不同测量矩阵的问题。您能否根据我发布的链接中的示例解释如何合并这两种变体？

我也不是 100% 确定你的 hB 矩阵/功能。让我们忘记在 SourceB 中完成的计算等，我得到的结果只是两帧之间 X 和 Y 坐标的差异，因此如果只看这个 Source，就没有速度要考虑。如果您已经考虑过这一点，我想知道您为什么决定在那里进行该等式。感谢和抱歉给您带来的麻烦。

对于 hB 矩阵/函数，我简单地使用转换模型从 p(k) 和 v(k) 逼近 p(k-1)。正如我最后所说，如果您更喜欢使用 p(k)-p(k-1) 形式的 hB 函数，则需要修改状态向量和转换/测量函数。对于您的另一个问题，每次进行新测量时，都需要使用适当的测量功能进行预测+更新。

啊，好的，谢谢。关于你的最后一句话：我认为可以使用我从源 A 收到的加权值......假设 0.7 和来自源 B 的值加权 0.3 来获得一个新值，该值将这两个测量值合并为新预测.除此之外，我已经知道每次获得新测量值时都必须进行预测和更新。仍然非常感谢您的投入，您已经帮了我很多忙