Implemented matlab script fot the Kalman filter. Minor cleanup.