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.} 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.} selectedIndexes list of indexes of observation state to use for the mahalanobis distance + * @returns {Number} + */ + mahalanobis(options) { + return this.detailedMahalanobis(options).value; } }