// KalmanInitializef is called for initializing the initial estimate
public void KalmanInitializef(Matrixf stateTransMatrix, Matrixf controlMatrix, Matrixf inputVar, Matrixf procNoiseVector, Matrixf obsvMatrix, Matrixf measNoiseVector)
{
// assign the variables
if (stateTransMatrix.rows != Ff.rows || stateTransMatrix.columns != Ff.columns)
{
throw new ArgumentException("Can not assign the state transition matrix with different rows or columns.", nameof(stateTransMatrix));
}
else
{
Ff = stateTransMatrix;
}
if (controlMatrix.rows != Gf.rows || controlMatrix.columns != Gf.columns)
{
throw new ArgumentException("Can not assign the control matrix with different rows or columns.", nameof(controlMatrix));
}
else
{
Gf = controlMatrix;
}
if (inputVar.rows != uf.rows || inputVar.columns != uf.columns)
{
throw new ArgumentException("Can not assign the input variable with different rows or columns.", nameof(inputVar));
}
else
{
uf = inputVar;
}
if (procNoiseVector.rows != wf.rows || procNoiseVector.columns != wf.columns)
{
throw new ArgumentException("Can not assign the process noise vector with different rows or columns.", nameof(procNoiseVector));
}
else
{
wf = procNoiseVector;
}
if (obsvMatrix.rows != Hf.rows || obsvMatrix.columns != Hf.columns)
{
throw new ArgumentException("Can not assign the observation matrix with different rows or columns.", nameof(obsvMatrix));
}
else
{
Hf = obsvMatrix;
}
if (measNoiseVector.rows != vf.rows || measNoiseVector.columns != vf.columns)
{
throw new ArgumentException("Can not assign the measurement noise vector with different rows or columns.", nameof(measNoiseVector));
}
else
{
vf = measNoiseVector;
}
// construct the process noise uncertainty and measurement noise uncertainty
Qf = Matrixf.Mul(wf, Matrixf.Transpose(wf));
Rf = Matrixf.Mul(vf, Matrixf.Transpose(vf));
// calculate the initial estimate(initial state vector and estimate uncertainty) for updating iteration
KalmanPredictf();
}
// 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;
}