Using Vision to update Position and Heading

[Keon-San]

Goal: The navigation subsystem should have position and heading reset whenever possible from the vision subsystem.

Why: Wheel slipping means that our position and heading data will inevitably be different from our robot position and heading, We can use vision to reset this. While not 100% accurate, it is better than not correcting it.

How: The navigation update command might be able to query vision.