// KalmanCorrectf is called for correcting the predict of the pre-state(first update for every iteration)
public void KalmanCorrectf(Matrixf measurement)
{
// update the output vector(measurement result)
if (measurement.rows != zf.rows || measurement.columns != zf.columns)
{
throw new ArgumentException("Can not update the measurement with different rows or columns.", nameof(measurement));
}
else
{
zf = measurement;
}
// compute the kalman gain and correct the discrete state and estimate uncertainty
Kf = Matrixf.Mul(Matrixf.Mul(Pf, Matrixf.Transpose(Hf)), Matrixf.Inverse(Matrixf.Mul(Matrixf.Mul(Hf, Pf), Matrixf.Transpose(Hf)) + Rf));
xf = xf + Matrixf.Mul(Kf, (zf - Matrixf.Mul(Hf, xf)));
Pf = Matrixf.Mul(Matrixf.Mul((Matrixf.Identity(xf.rows, xf.rows) - Matrixf.Mul(Kf, Hf)), Pf), Matrixf.Transpose(Matrixf.Identity(xf.rows, xf.rows) - Matrixf.Mul(Kf, Hf))) + Matrixf.Mul(Matrixf.Mul(Kf, Rf), Matrixf.Transpose(Kf));
}
// KalmanPredictf is called for predicting the state(second update for every iteration)
public void KalmanPredictf()
{
// predict the discrete state and covariance
xf = Matrixf.Mul(Ff, xf) + Matrixf.Mul(Gf, uf);
Pf = Matrixf.Mul(Matrixf.Mul(Ff, Pf), Matrixf.Inverse(Ff)) + Qf;
}