From d3fe87f20c9742f5f494556d3205ed9d1539f4b6 Mon Sep 17 00:00:00 2001
From: Pierre Colle <piercus@gmail.com>
Date: Wed, 23 Sep 2020 23:15:41 +0200
Subject: [PATCH] feat: detailedMahalanobis

---
 lib/state.js | 44 ++++++++++++++++++++++++++++++++++++++------
 1 file changed, 38 insertions(+), 6 deletions(-)

diff --git a/lib/state.js b/lib/state.js
index 53372d6..598ebe8 100644
--- a/lib/state.js
+++ b/lib/state.js
@@ -68,28 +68,60 @@ class State {
 		// }
 	}
 
-	mahalanobis({kf, observation}) {
+	/**
+	* @param {KalmanFilter} kf kalman filter use to project the state in observation's space
+	* @param {Observation} observation
+	* @param {Array.<Number>} selectedIndexes list of indexes of observation state to use for the mahalanobis distance
+	*/
+	detailedMahalanobis({kf, observation, selectedIndexes}) {
 		const correctlySizedObservation = arrayToMatrix({observation, dimension: observation.length});
 		const stateProjection = kf.observation.stateProjection();
 		const diff = sub(matMul(stateProjection, this.mean), correctlySizedObservation);
-		const diffTransposed = transpose(diff);
 
 		const covarianceInObservationSpace = matMul(
 			matMul(stateProjection, this.covariance),
 			transpose(stateProjection)
 		);
+		let covarianceInvert;
+		let diffSelected;
+		if (selectedIndexes) {
+			const selectedCov = selectedIndexes.map(s1 => selectedIndexes.map(s2 => covarianceInObservationSpace[s1][s2]));
+			covarianceInvert = invert(selectedCov);
+			diffSelected = selectedIndexes.map(s1 => diff[s1]);
+		} else {
+			covarianceInvert = invert(covarianceInObservationSpace);
+			diffSelected = diff;
+		}
+
+		const diffTransposed = transpose(diffSelected);
+
 		// Console.log('covariance in obs space', covarianceInObservationSpace);
-		const covarianceInvert = invert(covarianceInObservationSpace);
-		// Console.log('invert cov', covarianceInvert);
-		return Math.sqrt(
+
+		const value = Math.sqrt(
 			matMul(
 				matMul(
 					diffTransposed,
 					covarianceInvert
 				),
-				diff
+				diffSelected
 			)
 		);
+
+		return {
+			diff: diffSelected,
+			covarianceInvert,
+			value
+		};
+	}
+
+	/**
+	* @param {KalmanFilter} kf kalman filter use to project the state in observation's space
+	* @param {Observation} observation
+	* @param {Array.<Number>} selectedIndexes list of indexes of observation state to use for the mahalanobis distance
+	* @returns {Number}
+	*/
+	mahalanobis(options) {
+		return this.detailedMahalanobis(options).value;
 	}
 }