fix: add index and fix the options for extended filter

psantos9 · Sep 23, 2020 · 278313e · 278313e
1 parent fedfbc6
commit 278313e
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 19 deletions.
diff --git a/README.md b/README.md
@@ -299,9 +299,9 @@ const kFilter = new KalmanFilter({
 	observation: {
 		dimension: 2,
 		/**
-		* @param {State} predictedState
-		* @param {Array.<Number>} observation
-		* @param {Number} index		
+		* @param {State} opts.predicted
+		* @param {Array.<Number>} opts.observation
+		* @param {Number} opts.index
 		*/
 		stateProjection: function(opts){
 			return [
@@ -310,9 +310,9 @@ const kFilter = new KalmanFilter({
 			]
 		},
 		/**
-		* @param {State} predictedState
-		* @param {Array.<Number>} observation
-		* @param {Number} index
+		* @param {State} opts.predicted
+		* @param {Array.<Number>} opts.observation
+		* @param {Number} opts.index
 		*/		
 		covariance: function(opts){
 			return [
@@ -326,8 +326,8 @@ const kFilter = new KalmanFilter({
 	dynamic: {
 		dimension: 4, //(x, y, vx, vy)
 		/**
-		* @param {State} previousCorrected
-		* @param {Number} index
+		* @param {State} opts.previousCorrected
+		* @param {Number} opts.index
 		*/
 		transition: function(opts){
 			const dT = intervals[opts.index];
@@ -342,8 +342,8 @@ const kFilter = new KalmanFilter({
 			]
 		},
 		/**
-		* @param {State} previousCorrected
-		* @param {Number} index
+		* @param {State} opts.previousCorrected
+		* @param {Number} opts.index
 		*/		
 		covariance: function(opts){
 			const dT = intervals[opts.index];

diff --git a/lib/core-kalman-filter.js b/lib/core-kalman-filter.js
@@ -102,23 +102,24 @@ class CoreKalmanFilter {
 	* @returns{State} predicted State
 	*/
 
-	predict({previousCorrected} = {}) {
+	predict({previousCorrected, index} = {}) {
 		previousCorrected = previousCorrected || this.getInitState();
 
+		if (typeof (index) !== 'number' && typeof (previousCorrected.index) === 'number') {
+			index = previousCorrected.index + 1;
+		}
+
 		State.check(previousCorrected, {dimension: this.dynamic.dimension});
 
-		const getValueOptions = {previousCorrected, index: previousCorrected.index};
+		const getValueOptions = {
+			previousCorrected,
+			index
+		};
 		const d = this.getValue(this.dynamic.transition, getValueOptions);
 
 		const mean = matMul(d, previousCorrected.mean);
 
 		const covariance = this.getPredictedCovariance({previousCorrected});
-		let index;
-		if (typeof (previousCorrected.index) === 'number') {
-			index = previousCorrected.index + 1;
-		} else {
-			index = null;
-		}
 
 		const predicted = new State({mean, covariance, index});
 		this.logger.debug('Prediction done', predicted);
@@ -179,7 +180,7 @@ class CoreKalmanFilter {
 			throw (new Error('no measure available'));
 		}
 
-		const getValueOptions = {predicted, index: predicted.index};
+		const getValueOptions = {observation, predicted, index: predicted.index};
 		const stateProj = this.getValue(this.observation.stateProjection, getValueOptions);
 
 		const optimalKalmanGain = this.getGain({predicted, stateProjection: stateProj});