[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/algorithms.md

@@ -0,0 +1,88 @@
+# Algorithms
+
+## Closed form Kalman gain for continuous Kalman filter with A = 0 and C = I
+
+### Derivation
+
+Model is
+
+```
+  dx/dt = Ax + Bu
+  y = Cx + Du
+```
+
+where A = 0, B = 0, C = I, and D = 0.
+
+The optimal cost-to-go is the P that satisfies
+
+```
+  AᵀP + PA − PBR⁻¹BᵀP + Q = 0
+```
+
+Let A = Aᵀ and B = Cᵀ for state observers.
+
+```
+  AP + PAᵀ − PCᵀR⁻¹CP + Q = 0
+```
+
+Let A = 0, C = I.
+
+```
+  −PR⁻¹P + Q = 0
+```
+
+Solve for P. P, Q, and R are all diagonal, so this can be solved element-wise.
+
+```
+  −pr⁻¹p + q = 0
+  −pr⁻¹p = −q
+  pr⁻¹p = q
+  p²r⁻¹ = q
+  p² = qr
+  p = sqrt(qr)
+```
+
+Now solve for the Kalman gain.
+
+```
+  K = PCᵀ(CPCᵀ + R)⁻¹
+  K = P(P + R)⁻¹
+  k = p(p + r)⁻¹
+  k = sqrt(qr)(sqrt(qr) + r)⁻¹
+  k = sqrt(qr)/(sqrt(qr) + r)
+```
+
+Multiply by sqrt(q/r)/sqrt(q/r).
+
+```
+  k = q/(q + r sqrt(q/r))
+  k = q/(q + sqrt(qr²/r))
+  k = q/(q + sqrt(qr))
+```
+
+### Corner cases
+
+For q = 0 and r ≠ 0,
+
+```
+  k = 0/(0 + sqrt(0))
+  k = 0/0
+```
+
+Apply L'Hôpital's rule to k with respect to q.
+
+```
+  k = 1/(1 + r/(2sqrt(qr)))
+  k = 2sqrt(qr)/(2sqrt(qr) + r)
+  k = 2sqrt(0)/(2sqrt(0) + r)
+  k = 0/r
+  k = 0
+```
+
+For q ≠ 0 and r = 0,
+
+```
+  k = q / (q + sqrt(0))
+  k = q / q
+  k = 1
+```