Fix UKF negative variance by adding diagonal regularization

The standard UKF covariance update P = P - K*S*K' is numerically
unstable and can produce negative eigenvalues due to floating-point
rounding errors, causing the health monitor to fail with "Negative
variance on diagonal" errors.

This fix adds gentle diagonal regularization (eps = 1e-9) after the
covariance update to prevent negative eigenvalues without drastically
affecting filter behavior. The approach uses symmetrize() followed by
small diagonal additions, which is more stable than aggressive
eigenvalue flooring that can cause filter divergence.

Fixes #133

🤖 Generated with [Claude Code](https://claude.com/claude-code)

Co-Authored-By: Claude <noreply@anthropic.com>

Author: jbrodovsky

core/src/kalman.rs

@@ -300,7 +300,13 @@ impl NavigationFilter for UnscentedKalmanFilter {
         self.mean_state[7] = wrap_to_2pi(self.mean_state[7]);
         self.mean_state[8] = wrap_to_2pi(self.mean_state[8]);
         self.covariance -= &k * &s * &k.transpose();
-        self.covariance = 0.5 * (&self.covariance + self.covariance.transpose());
+        // Ensure covariance remains positive semi-definite with gentle regularization
+        self.covariance = symmetrize(&self.covariance);
+        // Add small diagonal regularization to prevent negative eigenvalues
+        let eps = 1e-9;
+        for i in 0..self.state_size {
+            self.covariance[(i, i)] += eps;
+        }
     }
     fn get_estimate(&self) -> DVector<f64> {
         self.mean_state.clone()