Skip to content
Permalink
Browse files
  • Loading branch information
Gilles committed Dec 15, 2014
2 parents ed6109d + d5f586b commit 92a027bc73d5ef67c4b2ead2f3ef63f198be1e26
Showing 2 changed files with 15 additions and 4 deletions.
@@ -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).
">
<action dev="tn" type="fix" issue="MATH-1142" due-to="Arne Schwarz">
Improve performance of kalman gain calculation in "KalmanFilter" by
directly solving a linear system rather than computing the matrix
inverse.
</action>
<action dev="psteitz" type="fix" issue="MATH-1181">
Fixed integer overflow in KolmogorovSmirnovTest causing 2-sample test
to use exact method when the product of the sample sizes exceeds
@@ -20,6 +20,7 @@
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 void correct(final RealVector z)
.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

0 comments on commit 92a027b

Please sign in to comment.