// semi-definite matrix, we can use the cholesky decomposition
DecompositionSolver solver = new CholeskyDecomposition(s).getSolver();
RealMatrix invertedS = solver.getInverse();
// 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);