// K(k) = P(k)- * H' * S^-1
RealMatrix kalmanGain = errorCovariance.multiply(measurementMatrixT).multiply(invertedS);
// update estimate with measurement z(k)
// xHat(k) = xHat(k)- + K * Inn
stateEstimation = stateEstimation.add(kalmanGain.operate(innovation));
// update covariance of prediction error
// P(k) = (I - K * H) * P(k)-
RealMatrix identity = MatrixUtils.createRealIdentityMatrix(kalmanGain.getRowDimension());
errorCovariance = identity.subtract(kalmanGain.multiply(measurementMatrix)).multiply(errorCovariance);