Implementing IMU Measurements into the PoseEstimator

[DarkEden-coding]

Is your feature request related to a problem? Please describe.
We are experiencing significant drift and noise in our swerve drive odometry, particularly during matches where wheels may be degraded. This issue is exacerbated by rapid movements and collisions, leading to unreliable position estimates between vision updates.

Describe the solution you'd like
I propose utilizing high-quality Inertial Measurement Units (IMUs) to supplement or, in some cases, replace the current swerve odometry for filling in position gaps between vision system updates. The goal is to leverage IMU data for more robust and accurate short-term pose estimation, especially during conditions where wheel encoders are prone to error.

Describe alternatives you've considered
I have considered vision-based smoothing techniques, such as those found in Visual-SLAM algorithms (specifically the Visual-Odometry components). However, these methods are generally more complex to implement and are unlikely to provide better real-time performance compared to existing AprilTag vision systems for global localization.

Additional context
Given the increasing robustness of vision-based systems for primary position calculation, the role of traditional odometry is primarily to provide estimates when vision updates are unavailable. The proposed IMU integration aims to improve the quality of these interpolated estimates. Additionally, as more robust IMU hardware becomes available (such as the Pigeon 2), the noise related to older hardware is either less of an issue or a non-issue.

Let me know what you think of the feasibility, pros, cons, and issues related to this idea. I am happy to contribute to this project; however, I am currently working on another project that is taking most of my time atm.
It seems like this will not be too hard to implement, given that the pose estimator already uses a Kalman filter, and a Kalman filter is a very common, tried and true method for converting acceleration and or velocity measurements into position estimates, I have not looked too hard into WPILIB source code so let me know if I am wrong about that!

[DarkEden-coding]

I don't believe that the kit of parts includes an accelerometer; however, I believe that enough teams use one that it is a feature that is worth considering for WPILIB.

[calcmogul]

In my experience, accelerometers are so noisy that the Kalman filter determines that the optimal contribution to the state estimate is negligible. See the second half of this CD post:
https://www.chiefdelphi.com/t/personal-library-development-improving-upon-wpilib/424713/15

[DarkEden-coding]

Ah, interesting, that makes sense. Do you think there is any way to possibly use accelerometer data (even if it is not the best) when odometry data results in velocity spikes or some other identifiable erroneous result? I understand that if the accelerometer data is so noisy that it "overpowers" the acceleration that the robot is under that it won't be useful for even this. Also given that modern apriltag data is around 50ms per frame (limelight 3g ish hardware), would accelerometers be useful given that they would not have much time to drift between vision corrections?