EKF: added function for updating from a re-observed feature through
distance and heading. This version probably still has a bug related to
computations involving heading.
Converted testBadINSGoodVINS() to use
updateFromReobservedFeatureThroughDistanceHeading(). Verified to be
correct now. You can see (1) the estimate improving due to the V-INS,
and (2) that the correction gets stronger over time (noticable through
the heading correction; probably because there is more certainty in a
feature's location). The corrections can be seen in the log with the tag
"EKFTests"