P4 - Account for wheel slippage in ekf localization

[1999michael]

Description:

• When Caffeine tries to run into an obstacle/wall or can't go up the ramps, the wheels will slip, i.e. wheels move but the robot doesn't. This will throw off the ekf localization. We need to be able to account for wheel slippage (bad values in encoder when fusing odometry with ekf localization).

Goals:

• Still have a good estimate of odometry even when wheel slippage occurs.

Acceptance Criteria:

• Local/global odometry remain fairly accurate when Caffeine is trying to go into a wall
• Local/global odometry remain fairly accurate when Caffeine is trying to go onto a ramp (but doesn't have enough power)

Notes:

• This is one of two issues related to fixes to be implemented for ekf location (other issue is lost visual odometry). You will need to remove situations where visual odometry is lost when testing for this issue.
• Note that the solution does not have to be restricted to changes directly related to ekf localization, as long as its reasonable

[1999michael]

LOWER PRIORITY
Reasoning: This will only be an issue if wheels are slipping for a prolonged period of time (which shouldn't happen as long as we don't run into obstacles/walls for long periods of time). If we do run into obstacles/walls for long period of time, we should focus on nav_stack rather than ekf.