Fix odometry over the bump

[aidnem]

Tonight during shop testing, we saw some pretty bad accuracy immediately after crossing the bump. I suspect that the wheel freespin while midair caused us to have massively wrong odometry for a second, which took a few seconds to be corrected by vision.

Ideas to fix this:

• Detect certain conditions that cause odometry to be bad and set a flag that puts our full trust in vision. This will give us a very noisy pose estimate for a short period of time, but it will prevent our pose estimate from shooting potentially multiple meters off the field.
• Add a residual calculator to see how good our current pose estimate is:
  For the last 5 vision measurements, sum up their differences from the current pose estimate. These estimates should roughly cancel each other out and the resultant vector should have a tiny magnitude. If the magnitude of this residual is too high, it means that all of our vision measurements are on one side of our pose estimate, indicating that our pose estimate is wrong.

Code changes:

• Add constants for the maximum cumulative tilt (abs(roll) + abs(pitch) from the gyro) before distrusting odometry
• Add constant for the maximum z acceleration (to detect when we're free falling) before distrusting odometry
• Add constant for how long we should fully trust vision after one of these conditions is true

[aidnem]

@godmar I'm going to prototype this on a branch and we can test it out tonight. If our pose estimation is significantly better right after crossing the bump, we can merge it and then continue testing on the other branch from last night.

[aidnem]

We tested this out and it doesn't actually seem to be a huge deal.