Conversation
…odel instead of differentiating it The extended filter pushes the covariance through a Jacobian, a straight line drawn at one point, which needs the model to be differentiable, needs the derivative written out, and discards everything beyond first order. The unscented filter picks 2n + 1 sigma points whose mean and covariance are exactly those of the estimate, sends each through the real model and reads the new mean and covariance off where they land. The square root that spreads them is a Cholesky factor, so a step costs about what the extended filter does, with no Jacobian anywhere, and the result is right to second order. The defaults are alpha = 1, beta = 2, kappa = 0. The alpha = 1e-3 often quoted puts the sigma points almost on the mean and compensates with a central weight near minus a million, exact on paper and six digits lost in floating point. After an update the covariance P - K S K' is symmetrised explicitly, and a covariance that is no longer positive definite is refused with an ArithmeticException instead of being given a square root that does not exist. The innovation covariance is inverted with the existing matrix.InverseOfMatrix. Tests: the sigma points reproduce the mean and covariance of the estimate exactly, the mean and variance of x^2 come out exact where the extended filter misses both, a linear model agrees with the extended filter to 1e-9 over 500 steps, and a target seen only as a range is tracked without writing a derivative. Signed-off-by: alxkm <19151554+alxkm@users.noreply.github.com>
Codecov Report❌ Patch coverage is
Additional details and impacted files@@ Coverage Diff @@
## master #7622 +/- ##
============================================
+ Coverage 81.49% 81.62% +0.12%
- Complexity 8078 8161 +83
============================================
Files 836 837 +1
Lines 25466 25656 +190
Branches 4966 5011 +45
============================================
+ Hits 20754 20941 +187
Misses 3924 3924
- Partials 788 791 +3 ☔ View full report in Codecov by Harness. 🚀 New features to boost your workflow:
|
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Adds the unscented Kalman filter: a Kalman filter for a nonlinear model that samples the model instead of differentiating it. It completes the pair with the
ExtendedKalmanFiltermerged in #7615.The extended filter pushes the covariance through a Jacobian, which is a straight line drawn at one point. That needs the model to be differentiable, needs somebody to write the derivative, and throws away everything the model does beyond first order. The unscented filter takes the opposite route: it picks a small deterministic set of sigma points whose mean and covariance are exactly those of the current estimate, sends every one of them through the real model, and reads the new mean and covariance off where they land.
The square root is a Cholesky factor, so a step costs
2n + 1evaluations of the model and one factorisation, roughly what the extended filter costs, with no Jacobian anywhere. The payoff is accuracy: the transformed mean and covariance are right to second order for any model, where the extended filter is right to first.That claim is tested rather than asserted. For
x^2withx ~ N(3, 0.5^2)the true mean ismu^2 + sigma^2 = 9.25and the true variance4 mu^2 sigma^2 + 2 sigma^4 = 9.125. The unscented filter returns both exactly; the extended filter returns 9.0 for each, missing thesigma^2the spread contributes to the mean and the2 sigma^4of the variance. The test runs both and checks both.A few decisions that are worth a reviewer's attention:
alpha = 1,beta = 2,kappa = 0, not thealpha = 1e-3often quoted. That value puts the sigma points almost on top of the mean and compensates with a central weight of about minus a million, which is exact on paper and a loss of six digits in floating point.beta = 2is optimal for a Gaussian prior and is what makes the variance of the quadratic above come out exact. All three stay configurable, and parameters that would collapse the sigma points onto the mean are refused at construction.P - K S K'is symmetric in exact arithmetic but not after rounding, and a covariance that has drifted out of symmetry eventually fails the Cholesky factorisation of the next step.ArithmeticExceptionthat says so, instead of carrying on with a square root that does not exist.matrix.InverseOfMatrix, as in the extended filter, on a defensive copy because it overwrites its input, with the result checked for finiteness.UnscentedKalmanFilterTestcovers 24 cases. Besides the quadratic: the sigma points are checked to reproduce the mean and the covariance of the estimate exactly; on a linear model the filter agrees with the extended filter, state and covariance, to 1e-9 at every one of 500 steps, which is the statement that it reduces to the ordinary Kalman filter; a target seen only as a range is tracked and its never-measured velocity recovered to within 0.2 without a single derivative written; the covariance stays symmetric and its variances positive over 500 steps of a model with asinin it; the mean weights sum to one for four different spreads; and prediction through the identity adds exactly the process noise.Checklist
clang-format -i --style=file path/to/your/file.java