First commit for Kalman Filter code