python - 我对 openCV 卡尔曼滤波器的使用很接近，但没有用

Question

openCV 卡尔曼滤波器的使用记录很少，如果有任何工作示例 - 以及 C++ 中的示例。我移植了一个假定有效的简单 C++ 示例 ( Opencv kalman filter prediction without new observtion )。我的端口可以运行，但无法正常工作。

我做错了什么？

谷歌搜索提供了一些有效的 C++ 示例和一些无效的旧 Python 示例。 openCV 文档引用了 C++“Example of c calls to OpenCV's Kalman filter”，即不是很有用。

measurement = np.zeros((2,1),dtype=np.float32)
state = np.zeros((4,1),dtype=np.float32)           # (x, y, Vx, Vy)
kalman = cv2.KalmanFilter(4,2,0)

def initKalman(x,y):   # init to 0,0
    measurement[0][0] = x
    measurement[1][0] = y
    kalman.statePre = np.zeros((4,1),dtype=np.float32)
    kalman.statePre[0,0] = x
    kalman.statePre[1,0] = y
    kalman.statePost = np.zeros((4,1),dtype=np.float32)
    kalman.statePost[0,0] = x
    kalman.statePost[1,0] = y
    cv2.setIdentity(kalman.measurementMatrix)
    cv2.setIdentity(kalman.processNoiseCov, .01)
    cv2.setIdentity(kalman.measurementNoiseCov, .1)
    cv2.setIdentity(kalman.errorCovPost, .1)
    kalman.transitionMatrix = np.array([[1,0,1,0],
                                    [0,1,0,1],
                                    [0,0,1,0],
                                    [0,0,0,1]],np.float32)

def kalmanPredict():
    prediction = kalman.predict()
    predictPr = [prediction[0,0],prediction[1,0]]
    return predictPr


def kalmanCorrect(x,y):
    measurement[0,0] = x
    measurement[1,0] = y
    estimated = kalman.correct(measurement)
    return [estimated[0,0],estimated[1,0]]

def runK():
    initKalman(0,0)

    p = kalmanPredict();    # first time - should be the initial x,y, i.e., 0,0
    print("first",p)

    s = kalmanCorrect(10, 10);   
    print("C",s)            # should be (per example) 5,5 -- but I get 0,0

    p = kalmanPredict()
    print("P",p)            # should be (per example) 5,5 -- but I get 0,0

    s = kalmanCorrect(20, 20);
    print("C",s)            # should be (per example) 10,10 -- but I get 0,0

    p = kalmanPredict()
    print("P",p)            # should be (per example) 10,10 -- but I get 0,0

    s = kalmanCorrect(30, 30);  #  -- but I get 0,0
    print("C",s)

    p = kalmanPredict()     #  -- but I get 0,0
    print("P",p)

runK()

---- with the output ----
first [0.0, 0.0]
C [0.0, 0.0]
P [0.0, 0.0]
C [0.0, 0.0]
P [0.0, 0.0]
C [0.0, 0.0]
P [0.0, 0.0]

我期待 C++ 示例的结果。相反，我收到了全零，即不好的结果。

谢谢!!!!

Answer 1

即使您的代码看起来不错，但似乎 setidentity不像名字所暗示的那样工作。就像现在一样，它只会留下 0 的矩阵:

print (kalman.measurementMatrix )
cv2.setIdentity(kalman.measurementMatrix)
print (kalman.measurementMatrix )

给出:

[[0. 0. 0. 0.]
 [0. 0. 0. 0.]]

[[0. 0. 0. 0.]
 [0. 0. 0. 0.]]

您需要将函数的结果分配给变量，如文档中所述，mtx=cv.setIdentity(mtx[, s])。在您的代码中，它将是这样的:

kalman.measurementMatrix = cv2.setIdentity(kalman.measurementMatrix)

或者使用numpy eye函数

kalman.measurementMatrix = np.eye(2,M=4, dtype=np.float32)

修复 initKalman 函数中所有有问题的行，将导致如下结果:

def initKalman(x,y):   # init to 0,0
    measurement[0][0] = x
    measurement[1][0] = y
    kalman.statePre = np.zeros((4,1),dtype=np.float32)
    kalman.statePre[0,0] = x
    kalman.statePre[1,0] = y
    kalman.statePost = np.zeros((4,1),dtype=np.float32)
    kalman.statePost[0,0] = x
    kalman.statePost[1,0] = y
    kalman.measurementMatrix=cv2.setIdentity(kalman.measurementMatrix)
    kalman.processNoiseCov=cv2.setIdentity(kalman.processNoiseCov, .01)
    kalman.measurementNoiseCov=cv2.setIdentity(kalman.measurementNoiseCov, .1)
    kalman.errorCovPost=cv2.setIdentity(kalman.errorCovPost, .1)
    kalman.transitionMatrix = np.array([[1,0,1,0],
                                    [0,1,0,1],
                                    [0,0,1,0],
                                    [0,0,0,1]],np.float32)

这会产生以下结果:

first [0.0, 0.0]
C [6.774194, 6.774194]
P [10.0, 10.0]
C [16.875, 16.875]
P [23.538307, 23.538307]
C [27.827488, 27.827488]
P [36.32232, 36.32232]