mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
b8d6bc2eb1
The spiraling issue occurs when the vision rotation standard deviation is very high relative to the odometry rotation standard deviation and the vision measurements have a large rotation error. (Scaling the rotation component of a twist without scaling the translation component causes the direction of overall translation to change, leading to spiraling around (either towards or away) the vision measurement instead of moving towards it.) Using a transform instead of a twist avoids this issue. In general, scaling twist components is more mathematically correct than scaling transform components. However, although twists are correct for modeling uncertainty in an odometry-only pose estimate, they are not correct for the difference between the odometry-only pose estimate and a vision measurement. Since neither twists nor transforms are completely correct (and the pose estimator as a whole is not mathematically correct), but using transforms can guarantee that the pose estimate approaches the vision measurement (instead of potentially spiraling away), they are the least bad option.