/// <summary>
/// Updates the state of the system based on the given noisy measurements,
/// a description of how those measurements relate to the system, and a
/// covariance <c>Matrix</c> to describe the noise of the system.
/// </summary>
/// <param name="z">The measurements of the system.</param>
/// <param name="H">Measurement model.</param>
/// <param name="R">Covariance of measurements.</param>
/// <exception cref="System.ArgumentException">Thrown when given matrices
/// are of the incorrect size.</exception>
public void Update(Matrix <double> z, Matrix <double> H, Matrix <double> R)
{
KalmanFilter.CheckUpdateParameters(z, H, R, this);
// Fiddle with the matrices
Matrix <double> HT = H.Transpose();
Matrix <double> RI = R.Inverse();
// Perform the update
y = y + (HT * RI * z);
J = J + (HT * RI * H);
}
/// <summary>
/// Updates the state of the system based on the given noisy measurements,
/// a description of how those measurements relate to the system, and a
/// covariance <c>Matrix</c> to describe the noise of the system.
/// </summary>
/// <param name="z">The measurements of the system.</param>
/// <param name="H">Measurement model.</param>
/// <param name="R">Covariance of measurements.</param>
/// <exception cref="System.ArgumentException">Thrown when given matrices
/// are of the incorrect size.</exception>
public void Update(Matrix <double> z, Matrix <double> H, Matrix <double> R)
{
KalmanFilter.CheckUpdateParameters(z, H, R, this);
// We need to use transpose of H a couple of times.
Matrix <double> Ht = H.Transpose();
Matrix <double> I = Matrix <double> .Build.DenseIdentity(x.RowCount, x.RowCount);
Matrix <double> S = (H * P * Ht) + R; // Measurement covariance
Matrix <double> K = P * Ht * S.Inverse(); // Kalman Gain
P = (I - (K * H)) * P; // Covariance update
x = x + (K * (z - (H * x))); // State update
}