/// <summary>
/// Predicts the next state using the current state and <paramref name="controlVector"/>.
/// </summary>
/// <param name="controlVector">Set of data for external system control.</param>
/// <summary>
/// <para>Estimates the subsequent model state.</para>
/// <para>
/// x'(k) = A * x(k-1) + B * u(k).
/// P'(k) = A * P(k-1) * At + Q
/// K(k) = P'(k) * Ht * (H * P'(k) * Ht + R)^(-1)
/// </para>
/// </summary>
public void Predict(double[] controlVector)
{
CheckPrerequisites();
//x'(k) = A * x(k-1)
//this.state = this.state.Multiply(this.TransitionMatrix);
state = TransitionMatrix.Dot(state);
//x'(k) = x'(k) + B * u(k)
if (controlVector is not null)
{
state = state.Add(ControlMatrix.Dot(controlVector));
}
//P'(k) = A * P(k-1) * At + Q
EstimateCovariance = TransitionMatrix.Multiply(EstimateCovariance).Multiply(TransitionMatrix.Transpose()).Add(ProcessNoise);
/******* calculate Kalman gain **********/
var measurementMatrixTransponsed = MeasurementMatrix.Transpose();
//S(k) = H * P'(k) * Ht + R
ResidualCovariance = MeasurementMatrix.Multiply(EstimateCovariance).Multiply(measurementMatrixTransponsed).Add(MeasurementNoise);
ResidualCovarianceInv = ResidualCovariance.Inverse();
//K(k) = P'(k) * Ht * S(k)^(-1)
KalmanGain = EstimateCovariance.Multiply(measurementMatrixTransponsed).Multiply(ResidualCovarianceInv);
/******* calculate Kalman gain **********/
}
