三维目标位置预测使用卡尔曼滤波，可变时间段

Question

我正在研究先进的视觉系统，它由两个静态相机(用于获得准确的3d物体位置)和一些瞄准设备组成。目标检测和立体视觉模块已经完成。遗憾的是，由于目标系统的延迟，必须开发适当的预测模块。

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

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. 我怎么能使用最后的时间戳差异作为输入?(过渡矩阵?, controlParam ?)
2. 我想确定预测时间，例如想在0.5秒或1.5秒内预测物体的位置

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

Thanks in advance

Answer 1

我如何使用最后一个时间戳diff作为输入?(过渡矩阵?h2, controlParam ?)

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

您正在使用恒定速度预测模型，因此p_x(t+dt) = p_x(t) + v_x(t)·dt将以时间步长dt预测X中的位置(对于坐标也是如此)。在这种情况下，你的预测矩阵应该是:

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)

我留下了过程噪声盖。作为练习的公式。注意dt项的平方或不平方

2。我想要确定预测时间，例如想要在0.5秒或1.5秒内预测物体的位置

您可以遵循两种不同的方法:

1. 使用一个小的固定dt(例如0.02秒50Hz)，并在一个循环中计算预测，直到你达到你的目标(例如，从你的相机得到一个新的观察)。
2. 在线调整预测和过程噪声矩阵到所需的dt(在您的问题中为0,5/1,5秒)，并执行单个预测步骤。

如果你问的是如何预测你的相机的检测时间，这应该是一个不同的问题，我恐怕我不能帮助你:-)