Mahalanobis Distance

Overview

The Mahalanobis distance is a statistical metric that measures the distance between a measurement and an object track while considering their uncertainties. Unlike Euclidean distance, it accounts for scaling, correlations, and variances of different dimensions, making it well-suited for probabilistic tracking and association tasks.

It is computed as:

\[d_{mahalanobis} = \sqrt{(\pmb{x_b} - \pmb{x_a})^T \cdot \pmb{P}_{ab}^{-1} \cdot (\pmb{x_b} - \pmb{x_a})}\]

where \(\pmb{x_a}\) and \(\pmb{x_b}\) are two states (e.g., a track position and a measurement), and \(\pmb{P}_{ab}\) is the combined covariance matrix:

\[\pmb{P}_{ab} = \pmb{P}_a + \pmb{P}_b - \pmb{P}_{a,b}\]

where \(\pmb{P}_a\) and \(\pmb{P}_b\) are the individual covariances, and \(\pmb{P}_{a,b}\) is the cross-covariance.

Warning

The function assumes \(\pmb{P}_{ab}\) is invertible. If numerical instability occurs, a pseudo-inverse or regularization may be used. This can lead to higher and sometimes inconsistent computation times.

Limitations

Requires an invertible covariance matrix.
Computational heavy.

Selection Note

The Mahalanobis distance is a good choice for applications where uncertainty and correlations matter, and slower computation is acceptable. This distance calculation requires heavy matrix computation, including regularization for stability, and may not produce a result if the covariance matrices are badly conditioned.

Code Documentation

The Mahalanobis distance is implemented in Ufil as part of its association process. It is used to create a cost matrix that helps in optimal measurement-to-track assignments. The function is located in hypothesizer_functions.hpp.

Function Definition

namespace ufil
{
namespace association
{

template<typename TrackType>
void mahalanobis(
const TrackType & track, const typename TrackType::MeasurementType & measurement,
ufil::type::Scalar & distance)
{
const auto & state = track.currentState();
const auto H = ufil::generateMeasurmentMatrix<typename TrackType::MeasurementType,
    typename TrackType::StateType>();

// Compute the average covariance
const ufil::type::SquareMatrix<TrackType::MeasurementType::Size> covariance =
    0.5 * (H * state.covariance() * H.transpose() + measurement.covariance());

// Check if the covariance matrix is invertible using a small threshold
if (covariance.determinant() < 1e-6) {
    distance = ufil::scalar::ScalarMax;  // use a max distance value to indicate a problem
    return;
}

// Compute the difference vector
const auto difference =
    ufil::difference<typename TrackType::MeasurementType,
    typename TrackType::StateType>(measurement, state);

// Compute Mahalanobis distance
// Invert covariance using Eigenvalue decomposition for better stability
Eigen::SelfAdjointEigenSolver<ufil::type::DynamicMatrix> solver(covariance);
if (solver.info() != Eigen::Success) {
    // If decomposition fails, return a max distance
    distance = ufil::scalar::ScalarMax;
    return;
}

// Calculate the Mahalanobis distance: sqrt(difference^T * inv(covariance) * difference)
const ufil::type::DynamicMatrix inv_covariance =
    solver.eigenvectors() * solver.eigenvalues().cwiseInverse().asDiagonal() *
    solver.eigenvectors().transpose();
distance = std::sqrt(difference.transpose() * inv_covariance * difference);
}

}  // namespace association
}  // namespace ufil

Example Usage

#include <iostream>
#include <ufil_object_tracking/ufil_object_tracking.hpp>

// Get track and measurement

ufil::type::Scalar distance;
ufil::association::mahalanobis(track, measurement, distance);
std::cout << "Mahalanobis Distance: " << distance << std::endl;

Parameters

track: The track containing the estimated state.
measurement: The measurement to compare against the track’s state.
distance: Output parameter storing the computed Mahalanobis distance.