[wpimath] Improve EKF numerical stability (#4093)

The Joseph form of the error covariance update equation is more
numerically stable when the Kalman gain isn't optimal. Numerical
instability and filter divergence can occur if the user goes long time
periods between updates and the error covariance becomes ill-conditioned
(the ratio between the largest and smallest eigenvalue gets too large).

wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java

@@ -366,7 +366,13 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
     // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
     m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, h.apply(m_xHat, u))));
 
-    // Pₖ₊₁⁺ = (I − KC)Pₖ₊₁⁻
-    m_P = Matrix.eye(m_states).minus(K.times(C)).times(m_P);
+    // Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
+    // Use Joseph form for numerical stability
+    m_P =
+        Matrix.eye(m_states)
+            .minus(K.times(C))
+            .times(m_P)
+            .times(Matrix.eye(m_states).minus(K.times(C)).transpose())
+            .plus(K.times(discR).times(K.transpose()));
   }
 }