From d5f586bd4e1a20fc09a33aaa5c2b9320f4a2aa72 Mon Sep 17 00:00:00 2001 From: Thomas Neidhart Date: Mon, 15 Dec 2014 16:50:50 +0100 Subject: [PATCH] [MATH-1142] Improve performance of kalman gain calculation of KalmanFilter. Thanks to Arne Schwarz. --- src/changes/changes.xml | 5 +++++ .../apache/commons/math3/filter/KalmanFilter.java | 14 ++++++++++---- 2 files changed, 15 insertions(+), 4 deletions(-) diff --git a/src/changes/changes.xml b/src/changes/changes.xml index 034e13c7e..d9d8cf55b 100644 --- a/src/changes/changes.xml +++ b/src/changes/changes.xml @@ -79,6 +79,11 @@ Users are encouraged to upgrade to this version as this release not 2. A few methods in the FastMath class are in fact slower that their counterpart in either Math or StrictMath (cf. MATH-740 and MATH-901). "> + + Improve performance of kalman gain calculation in "KalmanFilter" by + directly solving a linear system rather than computing the matrix + inverse. + Fixed integer overflow in KolmogorovSmirnovTest causing 2-sample test to use exact method when the product of the sample sizes exceeds diff --git a/src/main/java/org/apache/commons/math3/filter/KalmanFilter.java b/src/main/java/org/apache/commons/math3/filter/KalmanFilter.java index 824eb6443..2f204bf1e 100644 --- a/src/main/java/org/apache/commons/math3/filter/KalmanFilter.java +++ b/src/main/java/org/apache/commons/math3/filter/KalmanFilter.java @@ -20,6 +20,7 @@ import org.apache.commons.math3.exception.DimensionMismatchException; import org.apache.commons.math3.exception.NullArgumentException; import org.apache.commons.math3.linear.Array2DRowRealMatrix; import org.apache.commons.math3.linear.ArrayRealVector; +import org.apache.commons.math3.linear.CholeskyDecomposition; import org.apache.commons.math3.linear.MatrixDimensionMismatchException; import org.apache.commons.math3.linear.MatrixUtils; import org.apache.commons.math3.linear.NonSquareMatrixException; @@ -359,16 +360,21 @@ public class KalmanFilter { .multiply(measurementMatrixT) .add(measurementModel.getMeasurementNoise()); - // invert S - RealMatrix invertedS = MatrixUtils.inverse(s); - // Inn = z(k) - H * xHat(k)- RealVector innovation = z.subtract(measurementMatrix.operate(stateEstimation)); // calculate gain matrix // K(k) = P(k)- * H' * (H * P(k)- * H' + R)^-1 // K(k) = P(k)- * H' * S^-1 - RealMatrix kalmanGain = errorCovariance.multiply(measurementMatrixT).multiply(invertedS); + + // instead of calculating the inverse of S we can rearrange the formula, + // and then solve the linear equation A x X = B with A = S', X = K' and B = (H * P)' + + // K(k) * S = P(k)- * H' + // S' * K(k)' = H * P(k)-' + RealMatrix kalmanGain = new CholeskyDecomposition(s).getSolver() + .solve(measurementMatrix.multiply(errorCovariance.transpose())) + .transpose(); // update estimate with measurement z(k) // xHat(k) = xHat(k)- + K * Inn