diff --git a/third-party/kalman.js b/third-party/kalman.js index 47a18da..1b285b7 100644 --- a/third-party/kalman.js +++ b/third-party/kalman.js @@ -11,79 +11,78 @@ -class KalmanFilter { +function KalmanFilter(params) { + /** + * Create 1-dimensional kalman filter + * @param {Number} options.R Process noise + * @param {Number} options.Q Measurement noise + * @param {Number} options.A State vector + * @param {Number} options.B Control vector + * @param {Number} options.C Measurement vector + * @return {KalmanFilter} + */ - /** - * Create 1-dimensional kalman filter - * @param {Number} options.R Process noise - * @param {Number} options.Q Measurement noise - * @param {Number} options.A State vector - * @param {Number} options.B Control vector - * @param {Number} options.C Measurement vector - * @return {KalmanFilter} - */ - constructor({R = 1, Q = 1, A = 1, B = 0, C = 1} = {}) { + this.R = typeof params.R == "undefined" ? 1 : params.R; // noise power desirable + this.Q = typeof params.Q == "undefined" ? 1 : params.Q; // noise power estimated - this.R = R; // noise power desirable - this.Q = Q; // noise power estimated - - this.A = A; - this.C = C; - this.B = B; - this.cov = NaN; - this.x = NaN; // estimated signal without noise - } - - /** - * Filter a new value - * @param {Number} z Measurement - * @param {Number} u Control - * @return {Number} - */ - filter(z, u = 0) { - - if (isNaN(this.x)) { - this.x = (1 / this.C) * z; - this.cov = (1 / this.C) * this.Q * (1 / this.C); - } - else { - - // Compute prediction - const predX = (this.A * this.x) + (this.B * u); - const predCov = ((this.A * this.cov) * this.A) + this.R; - - // Kalman gain - const K = predCov * this.C * (1 / ((this.C * predCov * this.C) + this.Q)); - - // Correction - this.x = predX + K * (z - (this.C * predX)); - this.cov = predCov - (K * this.C * predCov); - } - - return this.x; - } - - /** - * Return the last filtered measurement - * @return {Number} - */ - lastMeasurement() { - return this.x; - } - - /** - * Set measurement noise Q - * @param {Number} noise - */ - setMeasurementNoise(noise) { - this.Q = noise; - } - - /** - * Set the process noise R - * @param {Number} noise - */ - setProcessNoise(noise) { - this.R = noise; - } + this.A = typeof params.A == "undefined" ? 1 : params.A; + this.C = typeof params.C == "undefined" ? 1 : params.C; + this.B = typeof params.B == "undefined" ? 0 : params.B; + this.cov = NaN; + this.x = NaN; // estimated signal without noise } + +/** +* Filter a new value +* @param {Number} z Measurement +* @param {Number} u Control +* @return {Number} +*/ +KalmanFilter.prototype.filter = function (z, u) { + if (typeof u == "undefined") + u = 0; + if (isNaN(this.x)) { + this.x = (1 / this.C) * z; + this.cov = (1 / this.C) * this.Q * (1 / this.C); + } + else { + + // Compute prediction + const predX = (this.A * this.x) + (this.B * u); + const predCov = ((this.A * this.cov) * this.A) + this.R; + + // Kalman gain + const K = predCov * this.C * (1 / ((this.C * predCov * this.C) + this.Q)); + + // Correction + this.x = predX + K * (z - (this.C * predX)); + this.cov = predCov - (K * this.C * predCov); + } + + return this.x; +} + +/** +* Return the last filtered measurement +* @return {Number} +*/ +KalmanFilter.prototype.lastMeasurement = function () { + return this.x; +} + +/** +* Set measurement noise Q +* @param {Number} noise +*/ +KalmanFilter.prototype.setMeasurementNoise = function (noise) { + this.Q = noise; +} + +/** +* Set the process noise R +* @param {Number} noise +*/ +KalmanFilter.prototype.setProcessNoise = function (noise) { + this.R = noise; +} +