Update Robot Pose

[SeanErn]

Put Odometry and Vision poses on smart dashboard from multiple locations and verify accuracy
Plot Odometry and Vision poses with driving slowly and aggressively
Log Odometry and Vision poses
Update robot pose with vision pose and verify somewhat smooth transitions