Merge pull request #60 from piercus/extended

feat: extended kalman filter with fn parameters

Author: piercus

.github/workflows/release.yml

@@ -11,10 +11,10 @@ jobs:
     steps:
       - name: Checkout
         uses: actions/checkout@v3
-      - name: Use Node.js 12
+      - name: Use Node.js 18
         uses: actions/setup-node@v3
         with:
-          node-version: 12
+          node-version: 18
       - run: npm ci
       - run: npm run build  
       - name: Semantic Release

.github/workflows/test.yml

@@ -9,14 +9,14 @@ jobs:
 
     strategy:
       matrix:
-        node-version: [10.x, 12.x, 14.x, 15.x]
+        node-version: [18.x]
 
     steps:
       - uses: actions/checkout@v3
       - name: Use Node.js ${{ matrix.node-version }}
         uses: actions/setup-node@v3
         with:
-          node-version: ${{ matrix.node-version }}
+          node-version: ${{ matrix.node-version }}       
       - run: npm ci
       - run: npm run build --if-present
       - run: npm test

README.md

@@ -363,7 +363,7 @@ const kFilter = new KalmanFilter({
 });
 ```
 
-### Extended Kalman Filter
+## Play with Kalman Filter
 
 In order to use the Kalman-Filter with a dynamic or observation model which is not strictly a [General linear model](https://en.wikipedia.org/wiki/General_linear_model), it is possible to use `function` in following parameters :
 * `observation.stateProjection`
@@ -375,7 +375,6 @@ In this situation this `function` will return the value of the matrix at each st
 
 In this example, we create a constant-speed filter with non-uniform intervals;
 
-
 ```js
 const {KalmanFilter} = require('kalman-filter');
 
@@ -446,6 +445,16 @@ const kFilter = new KalmanFilter({
 	}
 });
 ```
+### Extended
+
+If you want to implement an [extended kalman filter](https://en.wikipedia.org/wiki/Extended_Kalman_filter)
+
+You will need to put your non-linear functions in the following parameters
+
+* `observation.fn`
+* `dynamic.fn`
+
+See an example in `test/issues/56.js`
 
 ## Use your kalman filter
 
@@ -457,7 +466,7 @@ const observations = [[0, 2], [0.1, 4], [0.5, 9], [0.2, 12]];
 // batch kalman filter
 const results = kFilter.filterAll(observations);
 ```
-### Online usage (run it online, forward step only)
+### Online filter
 
 When using online usage (only the forward step), the output of the `filter` method is an instance of the ["State"](/lib/state.js) class.
 

lib/core-kalman-filter.js

@@ -7,54 +7,54 @@ const getIdentity = require('../lib/linalgebra/identity.js');
 const State = require('./state.js');
 const checkMatrix = require('./utils/check-matrix.js');
 /**
-* @callback ObservationCallback
+* @callback PreviousCorrectedCallback
 * @param {Object} opts
 * @param {Number} opts.index
 * @param {Number} opts.previousCorrected
 */
 
 /**
-* @typedef {Object} ObservationConfig
-* @property {Number} dimension
-* @property {Array.Array.<Number>> | ObservationCallback} stateProjection,
-* @property {Array.Array.<Number>> | ObservationCallback} covariance
-*/
-
-/**
-* @callback DynamicCallback
+* @callback PredictedCallback
 * @param {Object} opts
 * @param {Number} opts.index
 * @param {State} opts.predicted
 * @param {Observation} opts.observation
 */
 
 /**
-* @typedef {Object} DynamicConfig
+* @typedef {Object} ObservationConfig
 * @property {Number} dimension
-* @property {Array.Array.<Number>> | DynamicCallback} transition,
-* @property {Array.Array.<Number>> | DynamicCallback} covariance
+* @property {PredictedCallback} [fn=null] for extended kalman filter only, the non-linear state to observation function
+* @property {Array.Array.<Number>> | PreviousCorrectedCallback} stateProjection the matrix to transform state to observation (for EKF, the jacobian of the fn)
+* @property {Array.Array.<Number>> | PreviousCorrectedCallback} covariance the covariance of the observation noise
 */
 
+/**
+* @typedef {Object} DynamicConfig
+* @property {Number} dimension dimension of the state vector
+* @property {PreviousCorrectedCallback} [fn=null] for extended kalman filter only, the non-linear state-transition model
+* @property {Array.Array.<Number>> | PredictedCallback} transition the state-transition model (or for EKF the jacobian of the fn)
+* @property {Array.Array.<Number>> | PredictedCallback} covariance the covariance of the process noise
+*/
+
+/**
+* @typedef {Object} CoreConfig
+* @property {DynamicConfig} dynamic the system's dynamic model
+* @property {ObservationConfig} observation the system's observation model
+* @property {Object} [logger=defaultLogger] a Winston-like logger
+*/
 const defaultLogger = {
 	info: (...args) => console.log(...args),
 	debug: () => {},
 	warn: (...args) => console.log(...args),
 	error: (...args) => console.log(...args)
 };
-
 /**
-* @class
-* @property {DynamicConfig} dynamic the system's dynamic model
-* @property {ObservationConfig} observation the system's observation model
-*@property logger a Winston-like logger
+ * @param {CoreConfig} options
 */
 class CoreKalmanFilter {
-	/**
-	* @param {DynamicConfig} dynamic
-	* @param {ObservationConfig} observation the system's observation model
-	*/
-
-	constructor({dynamic, observation, logger = defaultLogger}) {
+	constructor(options) {
+		const {dynamic, observation, logger = defaultLogger} = options;
 		this.dynamic = dynamic;
 		this.observation = observation;
 		this.logger = logger;
@@ -66,11 +66,13 @@ class CoreKalmanFilter {
 
 	getInitState() {
 		const {mean: meanInit, covariance: covarianceInit, index: indexInit} = this.dynamic.init;
+
 		const initState = new State({
 			mean: meanInit,
 			covariance: covarianceInit,
 			index: indexInit
 		});
+		State.check(initState, {title: 'dynamic.init'});
 		return initState;
 	}
 
@@ -85,10 +87,13 @@ class CoreKalmanFilter {
 		previousCorrected = previousCorrected || this.getInitState();
 
 		const getValueOptions = Object.assign({}, {previousCorrected, index}, options);
-		const d = this.getValue(this.dynamic.transition, getValueOptions);
-		const dTransposed = transpose(d);
-		const covarianceInter = matMul(d, previousCorrected.covariance);
-		const covariancePrevious = matMul(covarianceInter, dTransposed);
+		const transition = this.getValue(this.dynamic.transition, getValueOptions);
+
+		checkMatrix(transition, [this.dynamic.dimension, this.dynamic.dimension], 'dynamic.transition');
+
+		const transitionTransposed = transpose(transition);
+		const covarianceInter = matMul(transition, previousCorrected.covariance);
+		const covariancePrevious = matMul(covarianceInter, transitionTransposed);
 		const dynCov = this.getValue(this.dynamic.covariance, getValueOptions);
 
 		const covariance = add(
@@ -100,6 +105,14 @@ class CoreKalmanFilter {
 		return covariance;
 	}
 
+	predictMean({opts, transition}) {
+		if (this.dynamic.fn) {
+			return this.dynamic.fn(opts);
+		}
+
+		const {previousCorrected} = opts;
+		return matMul(transition, previousCorrected.mean);
+	}
 	/**
 	This will return the new prediction, relatively to the dynamic model chosen
 	* @param {State} previousCorrected State relative to our dynamic model
@@ -115,16 +128,14 @@ class CoreKalmanFilter {
 		}
 
 		State.check(previousCorrected, {dimension: this.dynamic.dimension});
-
-		const getValueOptions = Object.assign({}, {
+		const getValueOptions = Object.assign({}, options, {
 			previousCorrected,
 			index
-		}, options);
-		const d = this.getValue(this.dynamic.transition, getValueOptions);
+		});
 
-		checkMatrix(d, [this.dynamic.dimension, this.dynamic.dimension], 'dynamic.transition');
+		const transition = this.getValue(this.dynamic.transition, getValueOptions);
 
-		const mean = matMul(d, previousCorrected.mean);
+		const mean = this.predictMean({transition, opts: getValueOptions});
 
 		const covariance = this.getPredictedCovariance(getValueOptions);
 
@@ -146,8 +157,10 @@ class CoreKalmanFilter {
 		stateProjection = stateProjection || this.getValue(this.observation.stateProjection, getValueOptions);
 		const obsCovariance = this.getValue(this.observation.covariance, getValueOptions);
 		checkMatrix(obsCovariance, [this.observation.dimension, this.observation.dimension], 'observation.covariance');
-
 		const stateProjTransposed = transpose(stateProjection);
+
+		checkMatrix(stateProjection, [this.observation.dimension, this.dynamic.dimension], 'observation.stateProjection');
+
 		const noiselessInnovation = matMul(
 			matMul(stateProjection, predicted.covariance),
 			stateProjTransposed
@@ -187,6 +200,15 @@ class CoreKalmanFilter {
 		);
 	}
 
+	getPredictedObservation({opts, stateProjection}) {
+		if (this.observation.fn) {
+			return this.observation.fn(opts);
+		}
+
+		const {predicted} = opts;
+		return matMul(stateProjection, predicted.mean);
+	}
+
 	/**
 	This will return the new correction, taking into account the prediction made
 	and the observation of the sensor
@@ -209,8 +231,9 @@ class CoreKalmanFilter {
 
 		const innovation = sub(
 			observation,
-			matMul(stateProjection, predicted.mean)
+			this.getPredictedObservation({stateProjection, opts: getValueOptions})
 		);
+
 		const mean = add(
 			predicted.mean,
 			matMul(optimalKalmanGain, innovation)

lib/kalman-filter.js

@@ -12,6 +12,18 @@ const State = require('./state.js');
 const modelCollection = require('./model-collection.js');
 const CoreKalmanFilter = require('./core-kalman-filter.js');
 
+/**
+ * @typedef {String} DynamicNonObjectConfig
+ */
+/**
+ * @typedef {DynamicConfig} DynamicObjectConfig
+ * @property {String} name
+ */
+/**
+ * @param {DynamicNonObjectConfig} dynamic
+ * @returns {DynamicObjectConfig}
+ */
+
 const buildDefaultDynamic = function (dynamic) {
 	if (typeof (dynamic) === 'string') {
 		return {name: dynamic};
@@ -20,6 +32,17 @@ const buildDefaultDynamic = function (dynamic) {
 	return {name: 'constant-position'};
 };
 
+/**
+ * @typedef {String | Number} ObservationNonObjectConfig
+ */
+/**
+ * @typedef {ObservationConfig} ObservationObjectConfig
+ * @property {String} name
+ */
+/**
+ * @param {ObservationNonObjectConfig} observation
+ * @returns {ObservationObjectConfig}
+ */
 const buildDefaultObservation = function (observation) {
 	if (typeof (observation) === 'number') {
 		return {name: 'sensor', sensorDimension: observation};
@@ -35,8 +58,9 @@ const buildDefaultObservation = function (observation) {
 *This function fills the given options by successively checking if it uses a registered model,
 * it builds and checks the dynamic and observation dimensions, build the stateProjection if only observedProjection
 *is given, and initialize dynamic.init
-*@param {DynamicConfig} options.dynamic
-*@param {ObservationConfig} options.observation
+*@param {DynamicObjectConfig | DynamicNonObjectConfig} options.dynamic
+*@param {ObservationObjectConfig | ObservationNonObjectConfig} options.observation
+* @returns {CoreConfig}
 */
 
 const setupModelsParameters = function ({observation, dynamic}) {
@@ -63,29 +87,38 @@ const setupModelsParameters = function ({observation, dynamic}) {
 };
 
 /**
-*Returns the corresponding model without arrays as values but only functions
-*@param {ObservationConfig} observation
-*@param {DynamicConfig} dynamic
-*@returns {ObservationConfig, DynamicConfig} model with respect of the Core Kalman Filter properties
+* @typedef {Object} ModelsParameters
+* @property {DynamicObjectConfig} dynamic
+* @property {ObservationObjectConfig} observation
+*/
+
+/**
+* Returns the corresponding model without arrays as values but only functions
+* @param {ModelsParameters} modelToBeChanged
+* @returns {CoreConfig} model with respect of the Core Kalman Filter properties
 */
 const modelsParametersToCoreOptions = function (modelToBeChanged) {
 	const {observation, dynamic} = modelToBeChanged;
 	return deepAssign(modelToBeChanged, {
 		observation: {
-			stateProjection: toFunction(polymorphMatrix(observation.stateProjection)),
-			covariance: toFunction(polymorphMatrix(observation.covariance, {dimension: observation.dimension}))
+			stateProjection: toFunction(polymorphMatrix(observation.stateProjection), {label: 'observation.stateProjection'}),
+			covariance: toFunction(polymorphMatrix(observation.covariance, {dimension: observation.dimension}), {label: 'observation.covariance'})
 		},
 		dynamic: {
-			transition: toFunction(polymorphMatrix(dynamic.transition)),
-			covariance: toFunction(polymorphMatrix(dynamic.covariance, {dimension: dynamic.dimension}))
+			transition: toFunction(polymorphMatrix(dynamic.transition), {label: 'dynamic.transition'}),
+			covariance: toFunction(polymorphMatrix(dynamic.covariance, {dimension: dynamic.dimension}), {label: 'dynamic.covariance'})
 		}
 	});
 };
 
 class KalmanFilter extends CoreKalmanFilter {
 	/**
-	* @param {DynamicConfig} options.dynamic
-	* @param {ObservationConfig} options.observation the system's observation model
+	* @typedef {Object} Config
+	* @property {DynamicObjectConfig | DynamicNonObjectConfig} dynamic
+	* @property {ObservationObjectConfig | ObservationNonObjectConfig} observation
+	*/
+	/**
+	* @param {Config} options
 	*/
 	constructor(options = {}) {
 		const modelsParameters = setupModelsParameters(options);
@@ -117,11 +150,7 @@ class KalmanFilter extends CoreKalmanFilter {
 *@returns {Array.<Array.<Number>>} the mean of the corrections
 */
 	filterAll(observations) {
-		const {mean: meanInit, covariance: covarianceInit, index: indexInit} = this.dynamic.init;
-		let previousCorrected = new State({
-			mean: meanInit,
-			covariance: covarianceInit,
-			index: indexInit});
+		let previousCorrected = this.getInitState();
 		const results = [];
 		for (const observation of observations) {
 			const predicted = this.predict({previousCorrected});
@@ -138,8 +167,9 @@ class KalmanFilter extends CoreKalmanFilter {
 	/**
 	* Returns an estimation of the asymptotic state covariance as explained in https://en.wikipedia.org/wiki/Kalman_filter#Asymptotic_form
 	* in practice this can be used as a init.covariance value but is very costful calculation (that's why this is not made by default)
+	* @param {Number} [limitIterations=1e2] max number of iterations
 	* @param {Number} [tolerance=1e-6] returns when the last values differences are less than tolerance
-	* @return {<Array.<Array.<Number>>>} covariance
+	* @return {Array.<Array.<Number>>} covariance
 	*/
 	asymptoticStateCovariance(limitIterations = 1e2, tolerance = 1e-6) {
 		let previousCorrected = super.getInitState();
@@ -167,7 +197,7 @@ class KalmanFilter extends CoreKalmanFilter {
 	/**
 	* Returns an estimation of the asymptotic gain, as explained in https://en.wikipedia.org/wiki/Kalman_filter#Asymptotic_form
 	* @param {Number} [tolerance=1e-6] returns when the last values differences are less than tolerance
-	* @return {<Array.<Array.<Number>>>} gain
+	* @return {Array.<Array.<Number>>} gain
 	*/
 	asymptoticGain(tolerance = 1e-6) {
 		const covariance = this.asymptoticStateCovariance(tolerance);

lib/linalgebra/add.js

@@ -1,7 +1,7 @@
 const elemWise = require('./elem-wise');
 /**
 * Add matrixes together
-* @param {...<Array.<Array.<Number>>} args list of matrix
+* @param {...Array.<Array.<Number>>} args list of matrix
 * @returns {Array.<Array.<Number>>} sum
 */
 module.exports = function (...args) {