Update(
Matrix z,
Matrix H,
Matrix R
)
{
KalmanFilter.CheckUpdateParameters(z, H, R, this);
// Fiddle with the matrices
Matrix HT = Matrix.Transpose(H);
Matrix RI = R.Inverse();
// Perform the update
this.y = this.y + (HT * RI * z);
this.J = this.J + (HT * RI * H);
}
Update(
Matrix z,
Matrix H,
Matrix R
)
{
KalmanFilter.CheckUpdateParameters(z, H, R, this);
// We need to use transpose of H a couple of times.
Matrix Ht = Matrix.Transpose(H);
Matrix I = Matrix.Identity(x.RowCount, x.RowCount);
Matrix S = (H * P * Ht) + R; // Measurement covariance
Matrix K = P * Ht * S.Inverse(); // Kalman Gain
this.P = (I - (K * H)) * P; // Covariance update
this.x = this.x + (K * (z - (H * this.x))); // State update
}
