Covariance Matrix Divergence (NaN/Inf State Estimates)
My Extended Kalman Filter for SOC estimation works during discharge, but the state covariance matrix P diverges and outputs NaN values during high C-rate regenerative charging. How do I prevent covariance windup?
Neeta Dsouza answered .
2026-06-29
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 symmetryP = 0.5 * (P + P');
% 2. Add numeric floor (epsilon) to prevent division by zero / negative eigenvaluesP = 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 eigenvaluesdiag_D = max(1e-6, min(1e2, diag_D));
P = V * diag(diag_D) * V';