我对 openCV 卡尔曼滤波器的使用已关闭，但无法正常工作

Posted 2023-02-21

【中文标题】我对 openCV 卡尔曼滤波器的使用已关闭，但无法正常工作

【英文标题】：My usage of the openCV Kalman filter is close, but not working

【发布时间】：2019-04-15 12:51:46

【问题描述】：

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++ 示例的结果。相反，我收到的都是零，即不是很好的结果。

谢谢！！！！

【参考方案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]