// KalmanInitialized is called for initializing the initial estimate
public void KalmanInitialized(Matrixd stateTransMatrix, Matrixd controlMatrix, Matrixd inputVar, Matrixd procNoiseVector, Matrixd obsvMatrix, Matrixd measNoiseVector)
{
// assign the variables
if (stateTransMatrix.rows != Fd.rows || stateTransMatrix.columns != Fd.columns)
{
throw new ArgumentException("Can not assign the state transition matrix with different rows or columns.", nameof(stateTransMatrix));
}
else
{
Fd = stateTransMatrix;
}
if (controlMatrix.rows != Gd.rows || controlMatrix.columns != Gd.columns)
{
throw new ArgumentException("Can not assign the control matrix with different rows or columns.", nameof(controlMatrix));
}
else
{
Gd = controlMatrix;
}
if (inputVar.rows != ud.rows || inputVar.columns != ud.columns)
{
throw new ArgumentException("Can not assign the input variable with different rows or columns.", nameof(inputVar));
}
else
{
ud = inputVar;
}
if (procNoiseVector.rows != wd.rows || procNoiseVector.columns != wd.columns)
{
throw new ArgumentException("Can not assign the process noise vector with different rows or columns.", nameof(procNoiseVector));
}
else
{
wd = procNoiseVector;
}
if (obsvMatrix.rows != Hd.rows || obsvMatrix.columns != Hd.columns)
{
throw new ArgumentException("Can not assign the observation matrix with different rows or columns.", nameof(obsvMatrix));
}
else
{
Hd = obsvMatrix;
}
if (measNoiseVector.rows != vd.rows || measNoiseVector.columns != vd.columns)
{
throw new ArgumentException("Can not assign the measurement noise vector with different rows or columns.", nameof(measNoiseVector));
}
else
{
vd = measNoiseVector;
}
// construct the process noise uncertainty and measurement noise uncertainty
Qd = Matrixd.Mul(wd, Matrixd.Transpose(wd));
Rd = Matrixd.Mul(vd, Matrixd.Transpose(vd));
// calculate the initial estimate(initial state vector and estimate uncertainty) for updating iteration
KalmanPredictd();
}
// 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;
}