Is your feature request related to a problem? Please describe. Build and analyze the current state of the EKF in the matlab code and detail what you find in your response to this issue.