[wpimath] Use Odometry for internal state in Pose Estimation (#4668)

This effectively replaces the Unscented Kalman Filter used for Pose Estimation with the Odometry model, and uses a recalculable Kalman gain matrix to update pose estimations whenever a vision measurement is added.

Notably, this change removes the need for the confusing generics used in Java, and the C++ implementation got quite a bit less complex as well.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java

@@ -134,6 +134,16 @@ public final class TimeInterpolatableBuffer<T> {
     }
   }
 
+  /**
+   * Grant access to the internal sample buffer. Used in Pose Estimation to replay odometry inputs
+   * stored within this buffer.
+   *
+   * @return The internal sample buffer.
+   */
+  public NavigableMap<Double, T> getInternalBuffer() {
+    return m_pastSnapshots;
+  }
+
   public interface InterpolateFunction<T> {
     /**
      * Return the interpolated value. This object is assumed to be the starting position, or lower