/// <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 **********/
}
private double[] CalculateDelta(double[] measurement)
{
//innovation vector (measurement error)
var stateMeasurement = MeasurementMatrix.Dot(state);
var measurementError = measurement.Subtract(stateMeasurement);
return(measurementError);
}
/// <summary>
/// Calculates Mahalanobis distance between the provided measurement and the predicted state.
/// <para>Covariance matrix is the <see cref="ResidualCovariance"/>.</para>
/// </summary>
/// <param name="measurement">Measurement.</param>
/// <param name="delta">The residual from the measurement and the current state.</param>
/// <returns>Mahalanobis distance.</returns>
public double CalculateMahalanobisDistance(TMeasurement measurement, out double[] delta)
{
CheckPrerequisites();
var m = measurementConvertFunc(measurement);
var stateMeasurement = MeasurementMatrix.Dot(state);
delta = m.Subtract(stateMeasurement);
var distance = Distance.Mahalanobis(m, stateMeasurement, ResidualCovarianceInv);
return(distance);
}
/// <summary>
/// Checks pre-conditions: matrix sizes.
/// </summary>
private void CheckPrerequisites()
{
/************************** TRANSITION MATRIX ***************************/
if (TransitionMatrix is null)
{
throw new Exception("Transition matrix cannot be null!");
}
if (TransitionMatrix.GetLength(0) != StateVectorDimension || TransitionMatrix.GetLength(1) != StateVectorDimension)
{
throw new Exception("Transition matrix dimensions are not valid!");
}
/************************** TRANSITION MATRIX ***************************/
/************************** CONTROL MATRIX ***************************/
if (ControlMatrix is null && ControlVectorDimension != 0)
{
throw new Exception("Control matrix can be null only if control vector dimension is set to 0!");
}
if (ControlMatrix is not null && (ControlMatrix.GetLength(0) != StateVectorDimension || ControlMatrix.GetLength(1) != ControlVectorDimension))
{
throw new Exception("Control matrix dimensions are not valid!");
}
/************************** CONTROL MATRIX ***************************/
/************************** MEASUREMENT MATRIX ***************************/
if (MeasurementMatrix is null)
{
throw new Exception("Measurement matrix cannot be null!");
}
if (MeasurementMatrix.GetLength(0) != MeasurementVectorDimension || MeasurementMatrix.GetLength(1) != StateVectorDimension)
{
throw new Exception("Measurement matrix dimesnions are not valid!");
}
/************************** MEASUREMENT MATRIX ***************************/
/************************** PROCES NOISE COV. MATRIX ***************************/
if (ProcessNoise is null)
{
throw new Exception("Process noise covariance matrix cannot be null!");
}
if (ProcessNoise.GetLength(0) != StateVectorDimension || ProcessNoise.GetLength(1) != StateVectorDimension)
{
throw new Exception("Process noise covariance matrix dimensions are not valid!");
}
/************************** PROCES NOISE COV. MATRIX ***************************/
/************************** MEASUREMENT NOISE COV. MATRIX ***************************/
if (MeasurementNoise is null)
{
throw new Exception("Measurement noise covariance matrix cannot be null!");
}
if (MeasurementNoise.GetLength(0) != MeasurementVectorDimension || MeasurementNoise.GetLength(1) != MeasurementVectorDimension)
{
throw new Exception("Measurement noise covariance matrix dimensions are not valid!");
}
/************************** MEASUREMENT NOISE COV. MATRIX ***************************/
}
