Covariance Matrix Divergence (NaN/Inf State Estimates)

Why EKF Covariance Diverges (NaN/Inf)
The primary cause of divergence in an Extended Kalman Filter (EKF) during highly dynamic currents is **floating-point roundoff error**. 

Under high transient currents, the Jacobian matrix $H$ changes rapidly. The standard covariance update equation:

P = (eye(size(P)) - K * H) * P;
is numerically unstable. It does not guarantee that the covariance matrix $P$ remains symmetric and positive-definite. Once a diagonal element of $P$ becomes slightly negative due to rounding, the eigenvalues flip, error feedback becomes positive rather than negative, and the filter diverges to `NaN` or `Inf`.

Solution 1: Use the Joseph Form Update (Recommended)
Replace the standard update with the Joseph formulation. While mathematically equivalent, it is structurally guaranteed to yield a symmetric positive-definite matrix because it sums two quadratic forms.

Replace:
P = (eye(2) - K * H) * P_pred;
With:
I_KH = eye(size(P_pred)) - K * H;
P = I_KH * P_pred * I_KH' + K * R * K';

Solution 2: Force Symmetry and Add a Positive-Definite Floor
Ensure the matrix is symmetric at the end of every execution step, and add a tiny diagonal perturbation (numeric floor) to prevent the covariance from collapsing to absolute zero.

% 1. Force symmetry
P = 0.5 * (P + P');

% 2. Add numeric floor (epsilon) to prevent division by zero / negative eigenvalues
P = P + eye(size(P)) * 1e-9;

Solution 3: Handle Covariance Windup (Limit the Eigenvalues)
If the measurement noise $R$ is very small compared to the process noise $Q$, the filter trusts the measurement completely, driving the covariance $P$ down to zero. When $P$ becomes too small, the filter stops updating its states and ignores new measurements.

Implement covariance clamping/bounding:

% Clamp eigenvalues of P
[V, D] = eig(P);
diag_D = diag(D);

% Enforce upper and lower bounds on eigenvalues
diag_D = max(1e-6, min(1e2, diag_D)); 

P = V * diag(diag_D) * V';