add support for collision detection

Implement the algorithm described by Orbit in their coding presentation that detects when the robot collides with something and doesn't feed the swerve module positions into the pose estimator when this condition is detected. (Or, perhaps better, feeds in the swerve module positions with a great deal of uncertainty.)