c++ - 在 OpenCV/C++ 中通过(扩展)卡尔曼滤波器实现数据融合

Question

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

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 坐标中当前帧与上一帧之间的差异。

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

Answer 1

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

首先要做的是明确定义要考虑的状态 vector 。在你的情况下，位置 [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  ]

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

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

h_A( x_k ) = v_k

或者矩阵形式:

           [ 0 0 0 1 0 0 ]
hA{x(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 有一个更准确的测量函数，你可能想在你的状态 vector 中添加上一个时间步的位置，如下所示:

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 ]
hA{x(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 ]
hB{x(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)