Updated EKF equations
Added range measurements simulation node
Updated launch files for sbnav testing
Updates to sb navigation filter