My implementation of the KalmanFilter