[MATH-1142] Improve performance of kalman gain calculation of KalmanFilter. Thanks to Arne Schwarz.

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).
 ">
+      <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 

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