Implement the algorithm described by Orbit in their coding presentation that detects when the robot skids 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.)
More details are available on this CD thread.
Implement the algorithm described by Orbit in their coding presentation that detects when the robot skids 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.)
More details are available on this CD thread.