KalmanFilter总是在第一时间预测0,0 [英] KalmanFilter always predict 0,0 in first time

问题描述

以下代码用于从下至上扫描图像.但是，卡尔曼滤波器的预测在第一时间始终显示为0,0.这样，它将从底部到0,0画一条线.如何使路径(卡尔曼滤波器)更类似于实际路径?

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

推荐答案

此问题已在此处得到解答: 卡尔曼过滤器始终可以预测来源

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

OpenCV卡尔曼过滤器实现不允许您设置初始状态. 您必须保存您的初始状态，然后在调用kalman.correct时必须减去初始状态.当您呼叫kalman.predict时，您必须添加初始状态.

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.

类似这样的伪代码:

initialState = (y,x)
....

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

这篇关于KalmanFilter总是在第一时间预测0,0的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！