It would be nice to set the initial state of the EKF by using the ROS parameter server rather than hardcoded values