wpilibsuite / allwpilib

Official Repository of WPILibJ and WPILibC
https://wpilib.org/
Other
1.08k stars 614 forks source link

`SwerveDrivePoseEstimator` returns infinity occasionally #6415

Open spacey-sooty opened 7 months ago

spacey-sooty commented 7 months ago

Describe the bug When using SwerveDrivePoseEstimator we will get infinite values returned occasionally.

To Reproduce I can't really provide good reproduction instructions for this as its being tested on our robot.

Expected behavior SwerveDrivePoseEstimator should return normal/correct values.

Desktop (please complete the following information):

Additional context I'll try do a reproduction project for this if I get time.

PeterJohnson commented 7 months ago

Can you log both the inputs and outputs to the estimator? That would enable us to reproduce the input sequence.

spacey-sooty commented 7 months ago

:+1: I can do that next time I've with the robot (submitting a bug report from something we noticed last night)

spacey-sooty commented 7 months ago

Haven't had an opportunity to add an logging for the inputs and outputs but it appears to return infinity when the values get very close to 0. This could be that they are rounding to 0 then getting divided?

nobody5050 commented 7 months ago

I can reproduce. We had a similar issue with our robot code and ended up solving it through a combination of heavily trusting vision and throwing out any values outside of the field dimensions.

KangarooKoala commented 7 months ago

Could you please send the code that reproduced the issue? I'm glad that there's a workaround, but I'd like to see if there's a root cause that can/should be fixed.