Initial implementation of KalmanFilter and test case