如何根据测量维度设置opencv卡尔曼滤波器的测量矩阵[opencv+Python]

2024-05-19 20:12:14 发布

您现在位置:Python中文网/ 问答频道 /正文

我正在开发一个跟踪应用程序,其中我使用opencv卡尔曼滤波器验证我当前的位置测量。我使用来自this问题的代码:
首先,我计算运动物体在(x,y)处的速度(v)和加速度(a)。这4个值用作我的卡尔曼状态。我启动卡尔曼滤波器如下:
np.eye(n,m)生成维度为nxm的单位矩阵):

def initKalman(init_state, fps):
    kalman = cv.KalmanFilter(4, 4, 2)
    kalman.transitionMatrix = np.array([[1., 0., 1/fps, 0.],
                                        [0., 1., 0., 1/fps],
                                        [0., 0., 1., 0.],
                                        [0, 0., 0., 1.]])

    kalman.measurementMatrix = 1. * np.eye(2, 4)
    kalman.measurementNoiseCov = 1e-3 * np.eye(2, 2)
    kalman.processNoiseCov = 1e-5 * np.eye(4, 4) 
    kalman.errorCovPost = 1e-1 * np.eye(4, 4)
    kalman.statePost = init_state.reshape(4, 1)

    return kalman


kinematics = np.array((velocity, acceleration), dtype=np.float32)
kalman_state = np.concatenate((point, kinematics))
kalman_filter = initKalman(kalman_state, fps = 15)

在操作过程中,校正如下所示:

def correct_kalman(kalman, state):
    measurement = (np.dot(kalman.measurementNoiseCov, np.random.randn(2, 1))).reshape(-1)
    measurement = np.dot(kalman.measurementMatrix, state) + measurement
    return kalman.correct(measurement)

kinematics = np.array((velocity, acceleration), dtype=np.float32)
kalman_state = np.concatenate((point, kinematics))
correct_kalman(kalman_filter, kalman_state)

这似乎是工作女巫是伟大的,但我试图理解为什么。据我所知,它不应该起作用,因为在correct_kalman()中,速度和加速度都在这个代码行中:
measurement = np.dot(kalman.measurementMatrix, state) + measurement 因为measurementmatrix只有2 x 4。(事实上,如果我将加速度和速度设置为0,则过滤器的行为不会改变。)
例如,以kalman_state = np.array([10., 20., 25., 75.])为例,用measurementMatrix = 1. * np.eye(2, 4)
计算点积 那么measurement = np.dot(kalman.measurementMatrix, kalman_state)就是

>>> measurement
array([10., 20.])

v和a不见了。
因此,我将我的measurementMatrix和measurementNoiseCov更改为4 x 4维度,并使用np.random.randn(4, 1)对我的校正进行了相应的调整,但现在卡尔曼滤波器非常缓慢,落后于测量

如果不使用v和a,为什么第一种方法有效?
除了迭代调整值外,我如何以更有针对性的方式更改度量矩阵

谢谢你的帮助


Tags: 代码nparray速度dot加速度measurementstate