My usage of the openCV Kalman filter is close, but not working

Question

Usage of the openCV Kalman filter is sparsely documented with few, if any working examples - and those in C++. I've ported over a supposedly working, simple C++ example (Opencv kalman filter prediction without new observtion). My port runs, but does not work correctly.

What am I doing incorrectly?

Googling has provided some working C++ examples and some non-working, older Python examples. The openCV documentation refers to a C++ "Example of c calls to OpenCV's Kalman filter", i.e., not very useful.

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]

I was expecting the results from the C++ example. Instead, I received all zeros, i.e., not good results.

Thanks!!!!

Answer 1

Even though your code looks ok, it seems that setidentity does not work like as the name may suggest. As it is now, it will only leave the matrix with 0:

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

gives:

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

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

You need to assign the result of the function to the variable, as explained in the documentation, mtx=cv.setIdentity(mtx[, s]). In your code it will be something like this:

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

or use the numpy eye function

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

Doing the fix to all of the problematic lines in the initKalman function, will result in something like this:

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)

And this yields the following result:

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]