Complete vision / sensor fusion

[phurley]

Using something like http://pdocs.kauailabs.com/sf2/ or something less generic developed in house. It will be necessary for a given camera frame:

1. Based on when it was captured
2. Our angle when/since it was captured
3. Change in encoders since it was captured
4. Develop our newly estimated position
5. Compare to current range data from ultrasonics
6. If everything looks good, let the driver know we can take over

If we are given control

1. Generate a new motion profile for our current position, continuing to update based on above camera, angle, ultrasonic fusion
2. Follow path, updating profile based on any new valid frames that arrive

This is hard, and we may not get it done, but it is our reach goal, and could be the difference between being good and great

[phurley]

This is largely done, but we need to complete our auton use of this data and then try to integrate for teleop too.