使用卡尔曼滤波器进行三维物体位置预测，可变时间段

Question

我正在研究先进的视觉系统，它包括两个静态相机（用于获得准确的三维物体位置）和一些瞄准装置。物体检测和立体视觉模块已经完成。不幸的是，由于目标系统的延迟，必须开发一个合适的预测模块。

我使用卡尔曼滤波器进行了一些测试，但它的工作不够准确。

kalman = cv2.KalmanFilter(6,3,0)
...
            kalman.statePre[0,0]  = x
            kalman.statePre[1,0]  = y
            kalman.statePre[2,0]  = z
            kalman.statePre[3,0]  = 0
            kalman.statePre[4,0]  = 0
            kalman.statePre[5,0]  = 0

            kalman.measurementMatrix = np.array([[1,0,0,0,0,0],[0,1,0,0,0,0],[0,0,1,0,0,0]],np.float32)

            kalman.transitionMatrix = np.array([[1,0,0,1,0,0],[0,1,0,0,1,0],0,0,1,0,0,1],[0,0,0,1,0,0],[0,0,0,0,1,0],[0,0,0,0,0,1]],np.float32)

            kalman.processNoiseCov = np.array([[1,0,0,0,0,0],[0,1,0,0,0,0],0,0,1,0,0,0],[0,0,0,1,0,0],[0,0,0,0,1,0],[0,0,0,0,0,1]],np.float32) * 0.03


            kalman.measurementNoiseCov = np.array([[1,0,0],[0,1,0],0,0,1]],np.float32) * 0.003

我注意到两帧之间的时间段每次都不同（由于各种检测时间）。

1. 我如何使用上次时间戳差异作为输入？ （Transition matrices？，controlParam？）
2. 我想确定预测时间，例如想要在0,5秒或1.5秒
中预测对象的位置

我可以提供示例输入3d点。

提前致谢

Answer 1

1。我如何使用上次时间戳差异作为输入？ （Transition matrices？，controlParam？）

通过预测矩阵控制步长。您还需要调整过程噪声协方差矩阵以控制不确定性增长。

您正在使用恒速预测模型，因此p_x(t+dt) = p_x(t) + v_x(t)·dt将使用时间步长dt预测X中的位置（对于坐标，Y和Z，相同）。在这种情况下，您的预测矩阵应为：

kalman.transitionMatrix = np.array([[1,0,0,dt,0,0],[0,1,0,0,dt,0],0,0,1,0,0,dt],[0,0,0,1,0,0],[0,0,0,0,1,0],[0,0,0,0,0,1]],np.float32)

我离开了过程噪音cov。作为一项运动的配方。小心平方或不平整dt项。

2。我想确定预测时间，例如想要在0,5秒或1.5秒

内预测物体的位置

您可以采用两种不同的方法：

1. 使用小的固定dt（例如0.02秒，50Hz）并在循环中计算预测，直到达到目标（例如从相机获取新的观察结果）。
2. 在线调整预测和处理噪声矩阵到所需的dt（问题中为0.5 / 1.5秒）并执行单个预测步骤。

如果您询问如何预测相机的检测时间，那应该是一个不同的问题，我恐怕无法帮助您： - ）