我正在开发一个跟踪应用程序,其中我使用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,为什么第一种方法有效?
除了迭代调整值外,我如何以更有针对性的方式更改度量矩阵
谢谢你的帮助
目前没有回答
相关问题 更多 >
编程相关推荐