@bencevans/kalman-filter/src/index.ts

Kalman Filter

interface KalmanFilterParameters {
  initialEstimate: number;
  initialErrorInEstimate: number;
}

interface KalmanFilterUpdateParameters {
  measurement: number;
  errorInMeasurement: number;
}

export default class KalmanFilter {
  private previousEstimate: number;
  private previousErrorInEstimate: number;

  constructor({
    initialEstimate,
    initialErrorInEstimate
  }: KalmanFilterParameters) {
    this.previousEstimate = initialEstimate;
    this.previousErrorInEstimate = initialErrorInEstimate;
  }

  update({ measurement, errorInMeasurement }: KalmanFilterUpdateParameters) {
    const kalmanGain =
      this.previousErrorInEstimate /
      (this.previousErrorInEstimate + errorInMeasurement);
    const currentEstimate =
      this.previousEstimate +
      kalmanGain * (measurement - this.previousEstimate);
    const currentErrorInEstimate =
      (1 - kalmanGain) * this.previousErrorInEstimate;

    this.previousEstimate = currentEstimate;
    this.previousErrorInEstimate = currentErrorInEstimate;

    return [currentEstimate, currentErrorInEstimate];
  }
}

interface KalmanFilterArrayParameters {
  initialEstimate: Array<number>;
  initialErrorInEstimate: number;
}

interface KalmanFilterArrayUpdateParameters {
  measurement: Array<number>;
  errorInMeasurement: number;
}

export class KalmanFilterArray {
  private filters: Array<KalmanFilter>;

  constructor({
    initialEstimate,
    initialErrorInEstimate
  }: KalmanFilterArrayParameters) {
    this.filters = initialEstimate.map(
      initialEstimate =>
        new KalmanFilter({
          initialEstimate,
          initialErrorInEstimate
        })
    );
  }

  update({
    measurement,
    errorInMeasurement
  }: KalmanFilterArrayUpdateParameters) {
    let errorInEstimate;
    const estimates = this.filters.map((filter, index) => {
      const [estimate, _errorInEstimate] = filter.update({
        measurement: measurement[index],
        errorInMeasurement: errorInMeasurement
      });

      errorInEstimate = _errorInEstimate;
      return estimate;
    });

    return [estimates, errorInEstimate];
  }
}