wouterbulten/slacjs

class 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 = 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;
    }
}

export default KalmanFilter;