Fix UKF negative variance issue (#133)

[jbrodovsky]

Summary

Fixes #133 - UKF test failure due to negative variance on diagonal

Problem

The test_ukf_with_degraded_gnss integration test was failing with the error:

Negative variance on diagonal: idx=3, val=-0.01953125

The root cause is that the standard UKF covariance update formula P = P - K*S*K' is numerically unstable and can produce negative eigenvalues due to floating-point rounding errors.

Solution

Added gentle diagonal regularization in the UKF update step (kalman.rs:303-309):

1. Use symmetrize() for robust covariance symmetrization
2. Add small diagonal regularization (eps = 1e-9) to prevent negative eigenvalues

This approach is more stable than aggressive eigenvalue flooring, which can cause the filter to become overconfident and diverge.

Test plan

• test_ukf_with_degraded_gnss now passes
• All 188 unit tests pass
• Integration tests pass
• No filter divergence observed

🤖 Generated with Claude Code

[gemini-code-assist]

Summary of Changes

Hello @jbrodovsky, I'm Gemini Code Assist¹! I'm currently reviewing this pull request and will post my feedback shortly. In the meantime, here's a summary to help you and other reviewers quickly get up to speed!

This pull request enhances the robustness and numerical stability of the Unscented Kalman Filter (UKF) implementation. By introducing a gentle regularization technique during the covariance update, it effectively prevents the occurrence of negative variance values, which were previously causing test failures and could lead to filter instability. The change ensures the filter maintains a positive semi-definite covariance matrix, crucial for accurate state estimation.

Highlights

• Fixes UKF Test Failure: Resolves an issue where the test_ukf_with_degraded_gnss integration test was failing due to negative variance on the diagonal of the covariance matrix.
• Addresses Numerical Instability: The root cause of the failure was identified as numerical instability in the standard UKF covariance update formula, which can lead to negative eigenvalues due to floating-point rounding errors.
• Implements Gentle Diagonal Regularization: The solution introduces gentle diagonal regularization in the UKF update step, utilizing a symmetrize() function for robust covariance symmetrization and adding a small diagonal regularization (1e-9) to prevent negative eigenvalues.
• Improved Stability: This approach is noted to be more stable than aggressive eigenvalue flooring, which could potentially cause the filter to become overconfident and diverge.

Using Gemini Code Assist

The full guide for Gemini Code Assist can be found on our documentation page, here are some quick tips.

Invoking Gemini

You can request assistance from Gemini at any point by creating a comment using either /gemini <command> or @gemini-code-assist <command>. Below is a summary of the supported commands on the current page.

Feature	Command	Description
Code Review	/gemini review	Performs a code review for the current pull request in its current state.
Pull Request Summary	/gemini summary	Provides a summary of the current pull request in its current state.
Comment	@gemini-code-assist	Responds in comments when explicitly tagged, both in pull request comments and review comments.
Help	/gemini help	Displays a list of available commands.

Customization

To customize Gemini Code Assist for GitHub experience, repository maintainers can create a configuration file and/or provide a custom code review style guide (such as PEP-8 for Python) by creating and adding files to a .gemini/ folder in the base of the repository. Detailed instructions can be found here.

Limitations & Feedback

Gemini Code Assist may make mistakes. Please leave feedback on any instances where its feedback is incorrect or counter productive. You can react with 👍 and 👎 on @gemini-code-assist comments. If you're interested in giving your feedback about your experience with Gemini Code Assist for Github and other Google products, sign up here.

You can also get AI-powered code generation, chat, as well as code reviews directly in the IDE at no cost with the Gemini Code Assist IDE Extension.

Footnotes

1. Review the Privacy Notices, Generative AI Prohibited Use Policy, Terms of Service, and learn how to configure Gemini Code Assist in GitHub here. Gemini can make mistakes, so double check it and use code with caution. ↩

[gemini-code-assist]

Code Review

This pull request addresses a numerical instability issue in the Unscented Kalman Filter's covariance update by introducing diagonal regularization. The approach is sound and correctly fixes the problem of negative variance. The code change is clear and well-targeted. I have one suggestion to improve the implementation by using a named constant for the regularization value and a more idiomatic nalgebra function to modify the matrix, which will improve readability and maintainability.

[gemini-code-assist]

The value 1e-9 is a magic number. It's better to define it as a constant with a descriptive name (e.g., COVARIANCE_REGULARIZATION) to improve readability and maintainability. This makes the purpose of the value explicit and easier to change if needed.

Additionally, the loop for adding the regularization term to the diagonal can be expressed more idiomatically and potentially more efficiently using nalgebra's diagonal_mut() and add_scalar_mut() methods.

        const COVARIANCE_REGULARIZATION: f64 = 1e-9;
        self.covariance.diagonal_mut().add_scalar_mut(COVARIANCE_REGULARIZATION);