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