卡尔曼滤波器总是在第一时间预测 0,0

以下代码用于从下到上扫描图像。然而，卡尔曼滤波器的预测在第一次时总是显示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

这已经在这里得到了回答：卡尔曼滤波器始终预测原点 https://stackoverflow.com/questions/50863397/kalman-filter-always-predicting-origin/50868636#50868636

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]