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

Question

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

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

2) 来自源 B

的当前帧和最后一帧位置之间的差异（在 2D 中，Z 不应以任何方式改变）

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

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

我如何处理 inserting/combining 我进入 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

您的方向是正确的，但您似乎对转换函数和测量函数的概念感到困惑。

首先要做的是明确定义要考虑的状态向量。在你的情况下，位置 [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 ]
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 有一个更准确的测量函数，你可能想在你的状态向量中添加上一个时间步的位置，如下所示：

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 ]

也就是说，测量系统的速度和位置的差异几乎是一样的。由于您从不测量位置，因此您的状态可能无法观察到，这可能会成为一个问题。查看以下文档了解更多详情：

"State estimation with Kalman Filter" 由 F.Haugen (pdf)
"可控性和可观察性：工具卡尔曼滤波器设计”，作者：Southall、Buxton 和 Marchant (pdf)

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

Implementation of Data Fusion through (Extended) Kalman-Filter in OpenCV/C++

c++

location

opencv

kalman-filter