Debugged Kalman filter (test now passes)