Kalman filter trajectory prediction always done normal to motion [closed]

edit

import cv2
import numpy as np
import numpy.linalg as la
#from umucv.kalman import kalman

def kalman(mu,P,F,Q,B,u,z,H,R):
    # mu, P : current state and its uncertainty
    # F, Q  : Dynamic system and its noise
    # B, u  : control model and the entrance
    # z     : observation
    # H, R  : Observation model and its noise

    mup = F @ mu + B @ u;
    pp  = F @ P @ F.T + Q;

    zp = H @ mup

    # if there is no observation we only do prediction

    if z is None:
        return mup, pp, zp

    epsilon = z - zp

    k = pp @ H.T @ la.inv(H @ pp @ H.T +R)

    new_mu = mup + k @ epsilon;
    new_P  = (np.eye(len(P))-k @ H) @ pp;
    return new_mu, new_P, zp

cap = cv2.VideoCapture('2.mp4')
loHue = 0
loSaturation = 50
loValue = 50
high_hue = 0
high_saturation = 255
high_value = 255

def low_hue(x):
    global loHue
    loHue = x 


def upper_hue (x):
     global high_hue
     high_hue = x



cv2.namedWindow('Trackbars', flags=cv2.WINDOW_OPENGL)
cv2.resizeWindow('Trackbars', 500, 30)
cv2.moveWindow('Trackbars', 500, 600)
cv2.createTrackbar('loHue', 'Trackbars', 0, 180, low_hue)

cv2.createTrackbar('upperHue', 'Trackbars', 0, 180, upper_hue)

cv2.setTrackbarPos('loHue', 'Trackbars', 3)
cv2.setTrackbarPos('upperHue', 'Trackbars', 9)



fps = 30
dt = 1 / fps
t = np.arange(0, 2.01, dt)
noise = 3

a = np.array([0, 900])
F = np.array([1, 0, dt, 0,
                0, 1, 0, dt,
                0 , 0, 1, 0,
                0, 0, 0, 1 ]).reshape(4,4)


B = np.array([dt**2/2, 0,
                0, dt**2/2,
                dt, 0,
                0, dt ]).reshape(4,2)


H = np.array([1,0,0,0,
                 0,1,0,0]).reshape(2,4)


mu = np.array([0,0,0,0])
P = np.diag([1000,1000,1000,1000])**2


sigmaM = 0.0001
sigmaZ = 3*noise

Q = sigmaM**2 * np.eye(4)
R = sigmaZ**2 * np.eye(2)

listCenterX=[]
listCenterY=[]
xe = []
xu = []
ye = []
yu = []
xp = []
yp = []
xpu = []
ypu = []

while(True):
    _, image = cap.read()
   if _ == False:
        break
   height, width, __ = image.shape
   roi_vertices = [
   (width / 4, height / 4.7),
   (width / 3.8, height / 1.8),
   (width / 1.64, height / 1.52),
   (width / 1.8, height / 4)

   ]
   vertices = np.array([roi_vertices], np.int32)

   mask1 = np.zeros_like(image)
   mask_colour = (255,255,255)
   cv2.fillPoly(mask1, vertices, mask_colour)
   masked_image = cv2.bitwise_and(image, mask1)



   blur_masked_image = cv2.GaussianBlur(masked_image, (3, 3), 2)
   hsv = cv2.cvtColor(blur_masked_image, cv2.COLOR_BGR2HSV)
   lower_limit = np.array([loHue,loSaturation,loValue])
   upper_limit = np.array([high_hue,high_saturation,high_value])
   mask2 = cv2.inRange(hsv, lower_limit, upper_limit)
   res = cv2.bitwise_and(image, blur_masked_image, mask = mask2)
   erode_element = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
   dilate_element = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
   erosion = cv2.erode(mask2, erode_element, iterations = 1)

   erosion = cv2.medianBlur(erosion,3)
   dilation = cv2.dilate(erosion, dilate_element, iterations = 2)
   copy_dilation = dilation.copy()

   _, contours, hierarchy = cv2.findContours(copy_dilation, cv2.RETR_CCOMP, cv2.CHAIN_APPROX_SIMPLE)
   center = None

   if len(contours) > 0:
       c = max(contours, key = cv2.contourArea)
       ((x, y), radius) = cv2.minEnclosingCircle(c)
       M = cv2.moments(c)
       center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))

       mu,P,pred = kalman(mu,P,F,Q,B,a,center,H,R)


       xe.append ...