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);
}