Simple Kalman Filter

kalman.py

import os
import cv2
import time
import numpy as np
from matplotlib import pyplot as plt

size = 1000
im = np.ones((size,size,3))

def plot(p,sz=3,col=(0,255,0)):
    cv2.circle(im,(int(p[:,0]),int(p[:,1])),sz,color=col,thickness=3)
    cv2.imshow('plot',im)


if __name__ == '__main__':
    
    x0 = np.array([[20.,180.,1.]]).T
    m = 0.5
    c = 0
    F = np.array([[1,0,1],[m,0,c],[0,0,1]]) # model to move along y = mx+c
  
    H = np.array([[1,0,0],[0,1,0]])
    
    x = x0 # true initial position
    xh = np.array([[0,0,1]]).T # guessed initial position: (0,0)
    P = np.array([[20,0,0],[0,20,0],[0,0,20]]) # guess initial error covariance
    Q = np.array([[5,0,0],[0,20,0],[0,0,0]])
    R = np.array([[0,0],[0,0]])
    I = np.eye(3)

    for _ in range(50):
        # plot true position -- linear model + noise
        x = np.matmul(F,x) + np.array([[np.abs(np.random.normal(0,5)),np.random.normal(0,50),1]]).T
        z = np.matmul(H,x)
        plot(z.T,sz=2,col=(0,255,0))
        
        # predict -- plot guessed position
        xh = np.matmul(F,xh)
        P = np.matmul(np.matmul(F,P),F.T) + Q
        zh = np.matmul(H,xh)
        plot(zh.T,sz=1,col=(255,0,0))

        # update
        y = z - zh
        K = np.matmul(np.matmul(P,H.T),np.linalg.pinv(R + 1e-4 + np.matmul(np.matmul(H,P),H.T)))
        xh = xh + np.matmul(K,y)
        P = np.matmul(I-np.matmul(K,H),P)
        time.sleep(0.1)
        #im = np.ones((size,size,3)) # clear screen
        cv2.waitKey(1)
    cv2.destroyAllWindows()