Deduplicate common functionality in pose estimators

[calcmogul]

The odometry classes are wrappers around a kinematics object that converts from wheel movements to chassis movements, and a call to Pose2d.exp(). The pose estimators are wrappers around an odometry object, measurement latency compensation, and a Kalman filter for fusing pose measurements from computer vision that correctly represents the probability distribution in the Lie group SE(2).

The number of odometry and pose estimator classes is getting unwieldy (3 odometry in C++, 3 pose estimator in C++, 3 odometry in Java, 3 pose estimator in Java).

There's enough common complexity in the pose estimator classes to refactor into a RobotPoseEstimator class, which would make the pose estimators easier to maintain. The odometry classes are pretty simple as it is though, and the common functionality is already refactored into Pose2d.exp().

[srimanachanta]

How do you think we should handle the update call? We can create a RobotPoseEstimator that combined vision data with an update method that handles the Pose2d::exp call but then what will handle the rest of the math required for odometry? Although each odometry system is very similar, each one does things slightly differently (such as checking previous distances for tank drive).

[calcmogul]

The pose estimators will still need wrapper classes for each drivetrain that handle the drivetrain-specific kinematics, but the common logic for the Kalman filter can be refactored out in the same way the odometry classes have the pose update refactored into Pose2d::exp(). That should cut the class size at least in half.

An abstract base class could deduplicate setVisionMeasurementStdDevs(), but I'm not sure what to do about addVisionMeasurement() or the InterpolationRecord. addVisionMeasurement() has drivetrain-specific code buried in the middle. Moving addVisionMeasurement() to the base class and delegating with private inheritance is an option, though it feels a bit strange: https://godbolt.org/z/bE6rbE7fa.

The functional approach would be making a general pose estimator class that takes higher-order functions in the constructor, which would be composed within the drivetrain-specific pose estimator class.

[bruingineer]

here is a very crude initial attempt at just taking out the filter matrices my forked branch

all the pose estimators have an odometry object, but the function signatures are all different. So I'm not sure how to correctly consolidate that.