Simple 2D Kalman filter for OpenCV3

kalman2d_opencv3.py

'''
kalman2d_opencv3 - Simple 2D Kalman filter for OpenCV3

Based on http://stackoverflow.com/questions/29012038/is-there-any-example-of-cv2-kalmanfilter-implementation

Copyright (C) 2017 Alex McClung

This code is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation, either version 3 of the
License, or (at your option) any later version.
This code is distributed in the hope that it will be useful,

MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with this code. If not, see <http://www.gnu.org/licenses/>.
'''

import cv2
import numpy as np

class Kalman2D():
    '''
    A class for 2D Kalman filtering for OpenCV3
    '''

    def __init__(self, measurementMatrix = np.array([[1,0,0,0],[0,1,0,0]],np.float32),
        transitionMatrix = np.array([[1,0,1,0],[0,1,0,1],[0,0,1,0],[0,0,0,1]],np.float32),
        processNoiseCov = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]],np.float32) * 0.03):

        self.kalman = cv2.KalmanFilter(4,2)

        self.meas=[] #history of measurements
        self.pred=[] #history of kalman predictions
        self.mp = np.array((2,1), np.float32) # measurement
        self.tp = np.zeros((2,1), np.float32) # tracked / prediction

        self.kalman.measurementMatrix = measurementMatrix
        self.kalman.transitionMatrix = transitionMatrix
        self.kalman.processNoiseCov = processNoiseCov

    def update(self,x,y):
        self.mp = np.array([[np.float32(x)],[np.float32(y)]])
        self.meas.append((x,y))
        self.kalman.correct(self.mp)
        self.tp = self.kalman.predict()
        self.pred.append((int(self.tp[0]),int(self.tp[1])))

    def get_estimate(self):
        return self.pred[-1]