// KalmanCorrectd is called for correcting the predict of the pre-state(first update for every iteration)
public void KalmanCorrectd(Matrixd measurement)
{
// update the output vector(measurement result)
if (measurement.rows != zd.rows || measurement.columns != zd.columns)
{
throw new ArgumentException("Can not update the measurement with different rows or columns.", nameof(measurement));
}
else
{
zd = measurement;
}
// compute the kalman gain and correct the discrete state and estimate uncertainty
Kd = Matrixd.Mul(Matrixd.Mul(Pd, Matrixd.Transpose(Hd)), Matrixd.Inverse(Matrixd.Mul(Matrixd.Mul(Hd, Pd), Matrixd.Transpose(Hd)) + Rd));
xd = xd + Matrixd.Mul(Kd, (zd - Matrixd.Mul(Hd, xd)));
Pd = Matrixd.Mul(Matrixd.Mul((Matrixd.Identity(xd.rows, xd.rows) - Matrixd.Mul(Kd, Hd)), Pd), Matrixd.Transpose(Matrixd.Identity(xd.rows, xd.rows) - Matrixd.Mul(Kd, Hd))) + Matrixd.Mul(Matrixd.Mul(Kd, Rd), Matrixd.Transpose(Kd));
}
// KalmanPredictd is called for predicting the state(second update for every iteration)
public void KalmanPredictd()
{
// predict the discrete state and covariance
xd = Matrixd.Mul(Fd, xd) + Matrixd.Mul(Gd, ud);
Pd = Matrixd.Mul(Matrixd.Mul(Fd, Pd), Matrixd.Inverse(Fd)) + Qd;
}