KalmanFilter always predict 0,0 in first time

问题

The following code use to scan image from bottom to top. However, the prediction of Kalman filter always show 0,0 in first time. So that, it will draw line from bottom to 0,0. How to make path(Kalman filter) more similar to actual path?

The following code and image was updated.

import cv2
import matplotlib.pyplot as plt
import numpy as np

img = cv2.imread('IMG_4614.jpg',1)
img = cv2.resize(img, (600, 800))
hsv_image = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
low_yellow = np.array([18, 94, 140])
up_yellow = np.array([48, 255, 255])
hsv_mask = cv2.inRange(hsv_image, low_yellow, up_yellow)
hls_image = cv2.cvtColor(img, cv2.COLOR_BGR2HLS)
low_yellow = np.array([0, 170, 24])
up_yellow = np.array([54, 255, 255])
hls_mask = cv2.inRange(hls_image, low_yellow, up_yellow)
mask = np.logical_or(hsv_mask,hls_mask)

offset = 100
height, width, _ = img.shape
previousPos = h
currentPos = h - offset
finalImg = img.copy()
is_first = True

initState = np.array([[np.float32(int(width/2))], [np.float32(h)]], np.float32)
last_measurement = current_measurement = initState
last_prediction = current_prediction = np.array((2, 1), np.float32)
kalman = cv2.KalmanFilter(4, 2)
kalman.measurementMatrix = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], np.float32)
kalman.transitionMatrix = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32)


while currentPos >= 0:
    histogram = np.sum(mask[currentPos:previousPos,:], axis=0)
    areas = np.where(histogram > 40)
    if areas[0].size >= 2:
        bottomLeft = areas[0][0]
        topRight = areas[0][-1]

        x = int((topRight-bottomLeft) / 2 + bottomLeft)
        y = int((previousPos - currentPos) / 2 + currentPos)
        last_prediction = current_prediction
        last_measurement = current_measurement 
        current_measurement = np.array([[np.float32(x)], [np.float32(y)]], np.float32)
        lmx, lmy = last_measurement[0], last_measurement[1]
        cmx, cmy = current_measurement[0], current_measurement[1]

        cv2.rectangle(finalImg, (bottomLeft,previousPos), (topRight,currentPos), (0,255,0), 5)
        cv2.circle(finalImg,(x,y), 5, (0,0,255), -1)
        cv2.line(finalImg, (lmx, lmy), (cmx, cmy), (255, 0, 0),5) #actual path




        kalman.correct(current_measurement-initState)
        current_prediction = kalman.predict()

        lpx, lpy = last_prediction[0] + initState[0], last_prediction[1] + initState[1]
        cpx, cpy = current_prediction[0] + initState[0], current_prediction[1] + initState[1]
        cv2.line(finalImg, (lpx, lpy), (cpx, cpy), (255, 0, 255),5) # predict path  



        plt.figure(figsize=(10,10))  
        plt.imshow(cv2.cvtColor(finalImg, cv2.COLOR_BGR2RGB))
        plt.show()


    previousPos = currentPos
    currentPos = currentPos - offset

回答1:

This has already been answered here: Kalman filter always predicting origin

OpenCV Kalman filter implementation does not let you set the an initial state. You have to save your initial state and then when you call kalman.correct you have to subtract the initial state. And when you call kalman.predict you have to add your initial state.

Something like this pseudo-code:

initialState = (y,x)
....

kalman.correct(current_measurement - initialState)
...
prediction = kalman.predict()
prediction[0] = prediction[0] + initState[0]
prediction[1] = prediction[1] + initState[1]

回答2:

I managed to change the initial state by changing statePost and statePre. In init:

self.KF = cv2.KalmanFilter(nmbStateVars, nmbMeasts, nmbControlInputs)

A = self.KF.statePost
A[0:4] = self.measurement.reshape((4, 1))
# A[4:8] = 0.0
self.KF.statePost = A
self.KF.statePre = A

Then update as usual

self.updatedMeasts = self.KF.correct(self.measurement)

来源：https://stackoverflow.com/questions/51327207/kalmanfilter-always-predict-0-0-in-first-time