卡尔曼滤波器总是第一次预测0,0

Question

下面的代码使用从底部到顶部扫描图像。然而，卡尔曼滤波器的预测总是在第一时间显示0,0。因此，它会画线从下到0,0。如何使路径（卡尔曼滤波器）更接近于实际的路径？

下面的代码和图像更新。

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

Answer 1

这一点已经在这里找到答案：Kalman filter always predicting origin

OpenCV的卡尔曼滤波器的实现不会让你设定的初始状态。你必须保存您的初始状态，然后当你调用kalman.correct你必须减去初始状态。当你骂kalman.predict你必须添加你的初始状态。

事情是这样的伪代码：

initialState = (y,x)
....

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

Answer 2

我设法通过改变statePost和statePre更改初始状态。在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

然后更新如常

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