Final design for the Kalman filter (Matlab). Minor cleanup.