public void Correct(double[,] dz)
{
Matrix z = new DenseMatrix(dz);
// Innovation residual:
// S = HP'H + R
Matrix s = H.Multiply(P0).Multiply(H.Transpose()).Add(R) as Matrix;
// Optimal Kalman gain:
// K = P'HS^-1
Matrix k = P0.Multiply(H.Transpose()).Multiply(s.Inverse()) as Matrix;
// A posteriori state estimate:
// X = x' + K(z - Hx')
State = X0.Add(k.Multiply(z.Subtract(H.Multiply(X0)))) as Matrix;
// A posteriori covariance estimate:
// P = (I - kH)P'
Matrix I = new DiagonalMatrix(P0.RowCount, P0.RowCount, 1);
Covariance = I.Subtract(k.Multiply(H)).Multiply(P0) as Matrix;
}
