Added a basic implementation of the Kalman filter