[Feature Request] Mahalanobis distance

[Chris7462]

Thank you for providing this excellent library and easy-to-use API for IEKF.

I am currently exploring the invariant_extended_kalman_filter.h and was wondering if it's possible to implement a Mahalanobis distance gating for the IEKF update. This would allow us to perform the following check:

if (iekf.mahalanobis_dist(measurement_model, y) < ThresholdValue) {
  iekf.update(measurement_model, y);
} else {
  // Output a warning that y could be an outlier and isn't fused in the IEKF
}