debuggin EKF... specified error case
debugging EKF. removed const from predict_state and predict_observation