Question

稀疏地记录了openCV卡尔曼过滤器的使用，很少有（如果有的话）工作示例-以及C ++中的例子。我已经移植了一个据说可以运行的简单C ++示例（Opencv kalman filter prediction without new observtion）。我的端口可以运行，但是不能正常工作。

我做错了什么？

Google搜索提供了一些有效的C ++示例和一些无效的旧Python示例。 openCV文档引用了C ++“对OpenCV的Kalman过滤器的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]

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

1 个答案: