/// <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 **********/
}
/// <summary>
/// Calculates entropy from the error covariance matrix.
/// </summary>
public double CalculateEntropy()
{
if (EstimateCovariance is null || EstimateCovariance.GetLength(0) != EstimateCovariance.GetLength(1))
{
throw new ArgumentException("Error covariance matrix (P) must have the same number of rows and columns.");
}
var stateVectorDim = EstimateCovariance.GetLength(0);
var errorCovDet = EstimateCovariance.Determinant();
return(0.5f * ((stateVectorDim * Log(4 * PI)) + Log(errorCovDet)));
}
/// <summary>
/// Corrects the state error covariance based on innovation vector and Kalman update.
/// </summary>
/// <param name="innovationVector">The difference between predicted state and measurement.</param>
public void Correct(double[] innovationVector)
{
if (innovationVector is null)
{
throw new ArgumentNullException(nameof(innovationVector));
}
CheckPrerequisites();
if (innovationVector.Length != MeasurementVectorDimension)
{
throw new Exception("PredicitionError error vector (innovation vector) must have the same length as measurement.");
}
//correct state using Kalman gain
state = state.Add(KalmanGain.Dot(innovationVector));
var identity = Matrix.Identity(StateVectorDimension);
EstimateCovariance = (identity.Subtract(KalmanGain.Multiply(MeasurementMatrix))).Multiply(EstimateCovariance.Transpose());
}
