fix observation function for EKF