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

Question

openCV卡尔曼滤波器的使用很少有文献记载，只有少数（如果有的话）和C ++中的例子。我已经移植了一个假设工作的，简单的C ++示例（Opencv kalman filter prediction without new observtion）。我的端口运行，但无法正常工作。

我做错了什么？

谷歌搜索提供了一些有用的C ++示例和一些不起作用的旧Python示例。 openCV文档引用了C ++“对OpenCV的卡尔曼滤波器的c调用的示例”，即不太有用。

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]