@sevthjs/kalman-filter/dist/index.cjs

Kalman filter (and Extended Kalman Filter) Multi-dimensional implementation in Javascript

'use strict';

const simpleLinalg = require('simple-linalg');
const Matrix = require('@rayyamhk/matrix');

function _interopDefaultCompat (e) { return e && typeof e === 'object' && 'default' in e ? e.default : e; }

const simpleLinalg__default = /*#__PURE__*/_interopDefaultCompat(simpleLinalg);
const Matrix__default = /*#__PURE__*/_interopDefaultCompat(Matrix);

const registeredObservationModels = {};
const registeredDynamicModels = {};
function registerObservation(name, fn) {
  registeredObservationModels[name] = fn;
}
function registerDynamic(name, fn) {
  registeredDynamicModels[name] = fn;
}
function buildObservation(observation) {
  if (typeof registeredObservationModels[observation.name] !== "function") {
    throw new TypeError(
      `The provided observation model name (${observation.name}) is not registered`
    );
  }
  return registeredObservationModels[observation.name](observation);
}
function buildDynamic(dynamic, observation) {
  if (typeof registeredDynamicModels[dynamic.name] !== "function") {
    throw new TypeError(
      `The provided dynamic model (${dynamic.name}) name is not registered`
    );
  }
  return registeredDynamicModels[dynamic.name](dynamic, observation);
}

const {
  identity,
  diag,
  elemWise,
  matPermutation,
  padWithZeroCols,
  matMul,
  transpose,
  subSquareMatrix,
  subtract,
  invert,
  add,
  frobenius,
  trace,
  sum
} = simpleLinalg__default;

function constantPosition(dynamic, observation) {
  let { dimension } = dynamic;
  const observationDimension = observation.dimension;
  const { observedProjection } = observation;
  const { stateProjection } = observation;
  let { covariance } = dynamic;
  if (!dynamic.dimension) {
    if (observationDimension) {
      dimension = observationDimension;
    } else if (observedProjection) {
      dimension = observedProjection[0].length;
    } else if (stateProjection) {
      dimension = stateProjection[0].length;
    }
  }
  const transition = identity(dimension);
  covariance || (covariance = identity(dimension));
  return {
    ...dynamic,
    dimension,
    transition,
    covariance
  };
}

function constantSpeed(dynamic, observation) {
  const timeStep = dynamic.timeStep || 1;
  const { observedProjection } = observation;
  const { stateProjection } = observation;
  const observationDimension = observation.dimension;
  let dimension;
  if (stateProjection && Number.isInteger(stateProjection[0].length / 2)) {
    dimension = observation.stateProjection[0].length;
  } else if (observedProjection) {
    dimension = observedProjection[0].length * 2;
  } else if (observationDimension) {
    dimension = observationDimension * 2;
  } else {
    throw new Error("observedProjection or stateProjection should be defined in observation in order to use constant-speed filter");
  }
  const baseDimension = dimension / 2;
  const transition = identity(dimension);
  for (let i = 0; i < baseDimension; i++) {
    transition[i][i + baseDimension] = timeStep;
  }
  const arrayCovariance = new Array(baseDimension).fill(1).concat(new Array(baseDimension).fill(timeStep * timeStep));
  const covariance = dynamic.covariance || arrayCovariance;
  return {
    ...dynamic,
    dimension,
    transition,
    covariance
  };
}

function constantAcceleration(dynamic, observation) {
  const timeStep = dynamic.timeStep || 1;
  const { observedProjection } = observation;
  const { stateProjection } = observation;
  const observationDimension = observation.dimension;
  let dimension;
  if (stateProjection && Number.isInteger(stateProjection[0].length / 3)) {
    dimension = observation.stateProjection[0].length;
  } else if (observedProjection) {
    dimension = observedProjection[0].length * 3;
  } else if (observationDimension) {
    dimension = observationDimension * 3;
  } else {
    throw new Error("observedProjection or stateProjection should be defined in observation in order to use constant-speed filter");
  }
  const baseDimension = dimension / 3;
  const transition = identity(dimension);
  for (let i = 0; i < baseDimension; i++) {
    transition[i][i + baseDimension] = timeStep;
    transition[i][i + 2 * baseDimension] = 0.5 * timeStep ** 2;
    transition[i + baseDimension][i + 2 * baseDimension] = timeStep;
  }
  const arrayCovariance = new Array(baseDimension).fill(1).concat(new Array(baseDimension).fill(timeStep * timeStep)).concat(new Array(baseDimension).fill(timeStep ** 4));
  const covariance = dynamic.covariance || arrayCovariance;
  return {
    ...dynamic,
    dimension,
    transition,
    covariance
  };
}

function composition({ perName }, observation) {
  const { observedProjection } = observation;
  const observedDynamDimension = observedProjection[0].length;
  const dynamicNames = Object.keys(perName);
  const confs = {};
  let nextDynamicDimension = observedDynamDimension;
  let nextObservedDimension = 0;
  dynamicNames.forEach((k) => {
    const obsDynaIndexes = perName[k].obsDynaIndexes;
    if (typeof perName[k].name === "string" && perName[k].name !== k) {
      throw new Error(`${perName[k].name} and "${k}" should match`);
    }
    perName[k].name = k;
    const { dimension, transition, covariance, init: init2 } = buildDynamic(perName[k], observation);
    const dynamicIndexes = [];
    for (let i = 0; i < dimension; i++) {
      const isObserved = i < obsDynaIndexes.length;
      let newIndex;
      if (isObserved) {
        newIndex = nextObservedDimension;
        if (newIndex !== obsDynaIndexes[i]) {
          throw new Error("thsoe should match");
        }
        nextObservedDimension++;
      } else {
        newIndex = nextDynamicDimension;
        nextDynamicDimension++;
      }
      dynamicIndexes.push(newIndex);
    }
    confs[k] = {
      dynamicIndexes,
      transition,
      dimension,
      covariance,
      init: init2
    };
  });
  const totalDimension = dynamicNames.map((k) => confs[k].dimension).reduce((a, b) => a + b, 0);
  if (nextDynamicDimension !== totalDimension) {
    throw new Error("miscalculation of transition");
  }
  const init = {
    index: -1,
    mean: new Array(totalDimension),
    covariance: new Array(totalDimension).fill(0).map(() => new Array(totalDimension).fill(0))
  };
  dynamicNames.forEach((k) => {
    const {
      dynamicIndexes,
      init: localInit
    } = confs[k];
    if (typeof localInit !== "object") {
      throw new TypeError("Init is mandatory");
    }
    dynamicIndexes.forEach((c1, i1) => dynamicIndexes.forEach((c2, i2) => {
      init.covariance[c1][c2] = localInit.covariance[i1][i2];
    }));
    dynamicIndexes.forEach((c1, i1) => {
      init.mean[c1] = localInit.mean[i1];
    });
  });
  return {
    dimension: totalDimension,
    init,
    transition(options) {
      const { previousCorrected } = options;
      const resultTransition = new Array(totalDimension).fill(void 0).map(() => new Array(totalDimension).fill(0));
      dynamicNames.forEach((k) => {
        const {
          dynamicIndexes,
          transition
        } = confs[k];
        const options2 = {
          ...options,
          previousCorrected: previousCorrected.subState(dynamicIndexes)
        };
        const trans = transition(options2);
        dynamicIndexes.forEach((c1, i1) => dynamicIndexes.forEach((c2, i2) => {
          resultTransition[c1][c2] = trans[i1][i2];
        }));
      });
      return resultTransition;
    },
    covariance(options) {
      const { previousCorrected } = options;
      const resultCovariance = new Array(totalDimension).fill(void 0).map(() => new Array(totalDimension).fill(0));
      dynamicNames.forEach((k) => {
        const {
          dynamicIndexes,
          covariance
        } = confs[k];
        const options2 = {
          ...options,
          previousCorrected: previousCorrected.subState(dynamicIndexes)
        };
        const cov = covariance(options2);
        dynamicIndexes.forEach((c1, i1) => dynamicIndexes.forEach((c2, i2) => {
          resultCovariance[c1][c2] = cov[i1][i2];
        }));
      });
      return resultCovariance;
    }
  };
}

const huge = 1e6;
function constantPositionWithNull({ staticCovariance, obsDynaIndexes, init }) {
  const dimension = obsDynaIndexes.length;
  init || (init = {
    mean: new Array(obsDynaIndexes.length).fill(0).map(() => [0]),
    covariance: diag(new Array(obsDynaIndexes.length).fill(huge)),
    index: -1
  });
  if (staticCovariance && staticCovariance.length !== dimension) {
    throw new Error("staticCovariance has wrong size");
  }
  return {
    dimension,
    transition() {
      return identity(dimension);
    },
    covariance({ previousCorrected, index }) {
      const diffBetweenIndexes = index - previousCorrected.index;
      if (staticCovariance) {
        return staticCovariance.map((row) => row.map((element) => element * diffBetweenIndexes));
      }
      return identity(dimension);
    },
    init
  };
}

function constantSpeedDynamic(args, observation) {
  const { staticCovariance, avSpeed, center } = args;
  const observationDimension = observation.observedProjection[0].length;
  const dimension = 2 * observationDimension;
  if (center === void 0) {
    throw new TypeError("Center must be defined");
  }
  if (center.length !== observationDimension) {
    throw new TypeError(`Center size should be ${observationDimension}`);
  }
  if (avSpeed.length !== observationDimension) {
    throw new TypeError(`avSpeed size should be ${observationDimension}`);
  }
  const initCov = diag(center.map((c) => c * c / 3).concat(avSpeed.map((c) => c * c / 3)));
  const init = {
    mean: center.map((c) => [c]).concat(center.map(() => [0])),
    covariance: initCov,
    index: -1
  };
  const transition = (args2) => {
    const { getTime, index, previousCorrected } = args2;
    const dT = getTime(index) - getTime(previousCorrected.index);
    if (typeof dT !== "number" || Number.isNaN(dT)) {
      throw new TypeError(`dT (${dT}) should be a number`);
    }
    const mat = diag(center.map(() => 1).concat(center.map(() => 1)));
    for (let i = 0; i < observationDimension; i++) {
      mat[i][observationDimension + i] = dT;
    }
    if (Number.isNaN(mat[0][2])) {
      throw new TypeError("nan mat");
    }
    return mat;
  };
  const covariance = (args2) => {
    const { index, previousCorrected, getTime } = args2;
    const dT = getTime(index) - getTime(previousCorrected.index);
    if (typeof dT !== "number") {
      throw new TypeError(`dT (${dT}) should be a number`);
    }
    const sqrt = Math.sqrt(dT);
    if (Number.isNaN(sqrt)) {
      console.log({ lastPreviousIndex: previousCorrected.index, index });
      console.log(dT, previousCorrected.index, index, getTime(index), getTime(previousCorrected.index));
      throw new Error("Sqrt(dT) is NaN");
    }
    return diag(staticCovariance.map((v) => v * sqrt));
  };
  return {
    init,
    dimension,
    transition,
    covariance
  };
}

const safeDiv = function(a, b) {
  if (a === 0) {
    return 0;
  }
  if (b === 0) {
    return 1;
  }
  return a / b;
};
function shorttermConstantSpeed(options, observation) {
  const { typicalTimes } = options;
  if (!Array.isArray(typicalTimes)) {
    throw new TypeError("typicalTimes must be defined");
  }
  const constantSpeed = constantSpeedDynamic(options, observation);
  const { dimension, init } = constantSpeed;
  if (typicalTimes.length !== dimension) {
    throw new TypeError(`typicalTimes (${typicalTimes.length}) length is not as expected (${dimension})`);
  }
  const mixMatrix = function({
    ratios,
    aMat,
    bMat
  }) {
    return elemWise([aMat, bMat], ([m, d], rowIndex, colIndex) => {
      const ratio = rowIndex === colIndex ? ratios[rowIndex] : (ratios[rowIndex] + ratios[colIndex]) / 2;
      return ratio * m + (1 - ratio) * d;
    });
  };
  return {
    dimension,
    init,
    transition(options2) {
      const aMat = constantSpeed.transition(options2);
      const { getTime, index, previousCorrected } = options2;
      const dT = getTime(index) - getTime(previousCorrected.index);
      const ratios = typicalTimes.map((t) => Math.exp(-1 * dT / t));
      const bMat = diag(
        elemWise([init.mean, previousCorrected.mean], ([m, d]) => safeDiv(m, d)).reduce((a, b) => a.concat(b))
      );
      return mixMatrix({ ratios, aMat, bMat });
    },
    covariance(options2, observation2) {
      const { getTime, index, previousCorrected } = options2;
      const dT = getTime(index) - getTime(previousCorrected.index);
      const ratios = typicalTimes.map((t) => Math.exp(-1 * dT / t));
      const aMat = constantSpeed.covariance(
        options2
        /*, observation*/
      );
      return mixMatrix({ ratios, aMat, bMat: init.covariance });
    }
  };
}

const defaultDynamicModels = {
	__proto__: null,
	composition: composition,
	constantAcceleration: constantAcceleration,
	constantPosition: constantPosition,
	constantPositionWithNull: constantPositionWithNull,
	constantSpeed: constantSpeed,
	constantSpeedDynamic: constantSpeedDynamic,
	shorttermConstantSpeed: shorttermConstantSpeed
};

function checkShape(matrix, shape, title = "checkShape") {
  if (matrix.length !== shape[0]) {
    throw new Error(`[${title}] expected size (${shape[0]}) and length (${matrix.length}) does not match`);
  }
  if (shape.length > 1) {
    return matrix.forEach((m) => checkShape(m, shape.slice(1), title));
  }
}

function checkMatrix(matrix, shape, title = "checkMatrix") {
  if (!Array.isArray(matrix)) {
    throw new TypeError(`[${title}] should be a 2-level array matrix and is ${matrix}`);
  }
  for (const row of matrix) {
    if (!Array.isArray(row)) {
      throw new TypeError(`[${title}] 1-level array should be a matrix ${JSON.stringify(matrix)}`);
    }
  }
  if (matrix.reduce((a, b) => a.concat(b)).some((a) => Number.isNaN(a))) {
    throw new Error(
      `[${title}] Matrix should not have a NaN
In : 
` + matrix.join("\n")
    );
  }
  if (shape) {
    checkShape(matrix, shape, title);
  }
}

function debugValue(value) {
  if (value === void 0) {
    return "undefined";
  }
  let asStirng = "";
  asStirng = typeof value === "function" ? value.toString() : JSON.stringify(value);
  if (asStirng.length < 100) {
    return asStirng;
  }
  return asStirng.slice(0, 97) + "...";
}
class TypeAssert {
  constructor() {
    throw new Error("do not constuct me");
  }
  dummy() {
  }
  static assertNotArray(arg, name = "parameter") {
    if (Array.isArray(arg)) {
      throw new TypeError(`E001 ${name} cannot be an array. current value is ${debugValue(arg)}.`);
    }
  }
  static assertIsArray2D(arg, name = "parameter") {
    if (!Array.isArray(arg)) {
      throw new TypeError(`E002 ${name} is not an array. current value is ${debugValue(arg)}.`);
    }
    if (arg.length === 0) {
      return;
    }
    if (!Array.isArray(arg[0])) {
      throw new TypeError(`E003 ${name} must be an array of array. current value is ${debugValue(arg)}.`);
    }
  }
  static assertIsArray2DOrFnc(arg, name = "parameter") {
    if (typeof arg === "function") {
      return;
    }
    TypeAssert.assertIsArray2D(arg, name);
  }
  /**
      * ensure that the provided arg is a number, number[], or number[][]
      * @param arg
      * @param name
      * @returns
      */
  static assertIsNumbersArray(arg, name = "parameter") {
    if (typeof arg === "number") {
      return;
    }
    if (!TypeAssert.isArray(arg)) {
      throw new TypeError(`E004 ${name} is not an array. current value is ${debugValue(arg)}.`);
    }
    if (arg.length === 0) {
      return;
    }
    if (typeof arg[0] === "number") {
      return;
    }
    if (!TypeAssert.isArray(arg[0])) {
      throw new TypeError(`E005 ${name} is not an array of array. current value is ${debugValue(arg)}.`);
    }
    if (typeof arg[0][0] !== "number") {
      throw new TypeError(`E006 ${name} is not an array of array of number. current value is ${debugValue(arg)}.`);
    }
  }
  static isArray2D(obj) {
    if (!Array.isArray(obj)) {
      return false;
    }
    return Array.isArray(obj[0]);
  }
  static isArray1D(obj) {
    if (!Array.isArray(obj)) {
      return false;
    }
    return typeof obj[0] === "number";
  }
  static isArray(obj) {
    if (!Array.isArray(obj)) {
      return false;
    }
    return true;
  }
  static isFunction(arg) {
    if (typeof arg === "function") {
      return true;
    }
    return false;
  }
}

function polymorphMatrix(cov, opts = {}) {
  const { dimension, title = "polymorph" } = opts;
  if (typeof cov === "number" || Array.isArray(cov)) {
    if (typeof cov === "number" && typeof dimension === "number") {
      return diag(new Array(dimension).fill(cov));
    }
    if (TypeAssert.isArray2D(cov)) {
      let shape;
      if (typeof dimension === "number") {
        shape = [dimension, dimension];
      }
      checkMatrix(cov, shape, title);
      return cov;
    }
    if (TypeAssert.isArray1D(cov)) {
      return diag(cov);
    }
  }
  return cov;
}

const copy = (mat) => mat.map((a) => a.concat());
function sensor(options) {
  const { sensorDimension = 1, sensorCovariance = 1, nSensors = 1 } = options;
  const sensorCovarianceFormatted = polymorphMatrix(sensorCovariance, { dimension: sensorDimension });
  if (TypeAssert.isFunction(sensorCovarianceFormatted)) {
    throw new TypeError("sensorCovarianceFormatted can not be a function here");
  }
  checkMatrix(sensorCovarianceFormatted, [sensorDimension, sensorDimension], "observation.sensorCovariance");
  const oneSensorObservedProjection = identity(sensorDimension);
  let concatenatedObservedProjection = [];
  const dimension = sensorDimension * nSensors;
  const concatenatedCovariance = identity(dimension);
  for (let i = 0; i < nSensors; i++) {
    concatenatedObservedProjection = concatenatedObservedProjection.concat(copy(oneSensorObservedProjection));
    for (const [rIndex, r] of sensorCovarianceFormatted.entries()) {
      for (const [cIndex, c] of r.entries()) {
        concatenatedCovariance[rIndex + i * sensorDimension][cIndex + i * sensorDimension] = c;
      }
    }
  }
  return {
    ...options,
    dimension,
    observedProjection: concatenatedObservedProjection,
    covariance: concatenatedCovariance
  };
}

function nullableSensor(options) {
  const { dimension, observedProjection, covariance: baseCovariance } = buildObservation({ ...options, name: "sensor" });
  return {
    dimension,
    observedProjection,
    covariance(o) {
      const covariance = identity(dimension);
      const { variance } = o;
      variance.forEach((v, i) => {
        covariance[i][i] = v * baseCovariance[i][i];
      });
      return covariance;
    }
  };
}

const tolerance = 0.1;
const checkDefinitePositive = function(covariance, tolerance2 = 1e-10) {
  const covarianceMatrix = new Matrix__default(covariance);
  const eigenvalues = covarianceMatrix.eigenvalues();
  for (const eigenvalue of eigenvalues) {
    if (eigenvalue <= -tolerance2) {
      console.log(covariance, eigenvalue);
      throw new Error(`Eigenvalue should be positive (actual: ${eigenvalue})`);
    }
  }
  console.log("is definite positive", covariance);
};
const checkSymetric = function(covariance, title = "checkSymetric") {
  for (const [rowId, row] of covariance.entries()) {
    for (const [colId, item] of row.entries()) {
      if (rowId === colId && item < 0) {
        throw new Error(`[${title}] Variance[${colId}] should be positive (actual: ${item})`);
      } else if (Math.abs(item) > Math.sqrt(covariance[rowId][rowId] * covariance[colId][colId])) {
        console.log(covariance);
        throw new Error(`[${title}] Covariance[${rowId}][${colId}] should verify Cauchy Schwarz Inequality (expected: |x| <= sqrt(${covariance[rowId][rowId]} * ${covariance[colId][colId]}) actual: ${item})`);
      } else if (Math.abs(item - covariance[colId][rowId]) > tolerance) {
        throw new Error(
          `[${title}] Covariance[${rowId}][${colId}] should equal Covariance[${colId}][${rowId}]  (actual diff: ${Math.abs(item - covariance[colId][rowId])})  = ${item} - ${covariance[colId][rowId]}
${covariance.join("\n")} is invalid`
        );
      }
    }
  }
};
function checkCovariance(args, _title) {
  const { covariance, eigen = false } = args;
  checkMatrix(covariance);
  checkSymetric(covariance);
  if (eigen) {
    checkDefinitePositive(covariance);
  }
}

function correlationToCovariance({ correlation, variance }) {
  checkCovariance({ covariance: correlation });
  return correlation.map((c, rowIndex) => c.map((a, colIndex) => a * Math.sqrt(variance[colIndex] * variance[rowIndex])));
}

function covarianceToCorrelation(covariance) {
  checkCovariance({ covariance });
  const variance = covariance.map((_, i) => covariance[i][i]);
  return {
    variance,
    correlation: covariance.map((c, rowIndex) => c.map((a, colIndex) => a / Math.sqrt(variance[colIndex] * variance[rowIndex])))
  };
}

function sensorProjected({ selectedCovariance, totalDimension, obsIndexes, selectedStateProjection }) {
  if (!selectedStateProjection) {
    selectedStateProjection = new Array(obsIndexes.length).fill(0).map(() => new Array(obsIndexes.length).fill(0));
    obsIndexes.forEach((index1, i1) => {
      selectedStateProjection[i1][i1] = 1;
    });
  } else if (selectedStateProjection.length !== obsIndexes.length) {
    throw new Error(`[Sensor-projected] Shape mismatch between ${selectedStateProjection.length} and ${obsIndexes.length}`);
  }
  const baseCovariance = identity(totalDimension);
  obsIndexes.forEach((index1, i1) => {
    if (selectedCovariance) {
      obsIndexes.forEach((index2, i2) => {
        baseCovariance[index1][index2] = selectedCovariance[i1][i2];
      });
    }
  });
  const { correlation: baseCorrelation, variance: baseVariance } = covarianceToCorrelation(baseCovariance);
  const dynaDimension = selectedStateProjection[0].length;
  if (selectedStateProjection.length !== obsIndexes.length) {
    throw new Error(`shape mismatch (${selectedStateProjection.length} vs ${obsIndexes.length})`);
  }
  const observedProjection = matPermutation({
    outputSize: [totalDimension, dynaDimension],
    colIndexes: selectedStateProjection[0].map((_, i) => i),
    rowIndexes: obsIndexes,
    matrix: selectedStateProjection
  });
  return {
    dimension: totalDimension,
    observedProjection,
    covariance(o) {
      const { variance } = o;
      if (!variance) {
        return baseCovariance;
      }
      if (variance.length !== baseCovariance.length) {
        throw new Error("variance is difference size from baseCovariance");
      }
      const result = correlationToCovariance({ correlation: baseCorrelation, variance: baseVariance.map((b, i) => variance[i] * b) });
      return result;
    }
  };
}

const defaultObservationModels = {
	__proto__: null,
	sensor: sensor,
	sensorLocalVariance: nullableSensor,
	sensorProjected: sensorProjected
};

function arrayToMatrix(args) {
  const { observation, dimension } = args;
  if (!Array.isArray(observation)) {
    if (dimension === 1 && typeof observation === "number") {
      return [[observation]];
    }
    throw new TypeError(`The observation (${observation}) should be an array (dimension: ${dimension})`);
  }
  if (observation.length !== dimension) {
    throw new TypeError(`Observation (${observation.length}) and dimension (${dimension}) not matching`);
  }
  if (typeof observation[0] === "number" || observation[0] === null) {
    return observation.map((element) => [element]);
  }
  return observation;
}

function setDimensions(args) {
  const { observation, dynamic } = args;
  const { stateProjection } = observation;
  const { transition } = dynamic;
  const dynamicDimension = dynamic.dimension;
  const observationDimension = observation.dimension;
  if (dynamicDimension && observationDimension && Array.isArray(stateProjection) && (dynamicDimension !== stateProjection[0].length || observationDimension !== stateProjection.length)) {
    throw new TypeError("stateProjection dimensions not matching with observation and dynamic dimensions");
  }
  if (dynamicDimension && Array.isArray(transition) && dynamicDimension !== transition.length) {
    throw new TypeError("transition dimension not matching with dynamic dimension");
  }
  if (Array.isArray(stateProjection)) {
    return {
      observation: {
        ...observation,
        dimension: stateProjection.length
      },
      dynamic: {
        ...dynamic,
        dimension: stateProjection[0].length
      }
    };
  }
  if (Array.isArray(transition)) {
    return {
      observation,
      dynamic: {
        ...dynamic,
        dimension: transition.length
      }
    };
  }
  return { observation, dynamic };
}

function checkDimensions(args) {
  const { observation, dynamic } = args;
  const dynamicDimension = dynamic.dimension;
  const observationDimension = observation.dimension;
  if (!dynamicDimension || !observationDimension) {
    throw new TypeError("Dimension is not set");
  }
  return { observation, dynamic };
}

function buildStateProjection(args) {
  const { observation, dynamic } = args;
  const { observedProjection, stateProjection } = observation;
  const observationDimension = observation.dimension;
  const dynamicDimension = dynamic.dimension;
  if (observedProjection && stateProjection) {
    throw new TypeError("You cannot use both observedProjection and stateProjection");
  }
  if (observedProjection) {
    const stateProjection2 = padWithZeroCols(observedProjection, { columns: dynamicDimension });
    return {
      observation: {
        ...observation,
        stateProjection: stateProjection2
      },
      dynamic
    };
  }
  if (observationDimension && dynamicDimension && !stateProjection) {
    const observationMatrix = identity(observationDimension);
    return {
      observation: {
        ...observation,
        stateProjection: padWithZeroCols(observationMatrix, { columns: dynamicDimension })
      },
      dynamic
    };
  }
  return { observation, dynamic };
}

function extendDynamicInit(args) {
  const { observation, dynamic } = args;
  if (!dynamic.init) {
    const huge = 1e6;
    const dynamicDimension = dynamic.dimension;
    const meanArray = new Array(dynamicDimension).fill(0);
    const covarianceArray = new Array(dynamicDimension).fill(huge);
    const withInitOptions = {
      observation,
      dynamic: {
        ...dynamic,
        init: {
          mean: meanArray.map((element) => [element]),
          covariance: diag(covarianceArray),
          index: -1
        }
      }
    };
    return withInitOptions;
  }
  if (dynamic.init && !dynamic.init.mean) {
    throw new Error("dynamic.init should have a mean key");
  }
  const covariance = polymorphMatrix(dynamic.init.covariance, { dimension: dynamic.dimension });
  if (TypeAssert.isFunction(covariance)) {
    throw new TypeError("covariance can not be a function");
  }
  dynamic.init = {
    ...dynamic.init,
    covariance
  };
  return { observation, dynamic };
}

function toFunction(array, { label = "" } = {}) {
  if (typeof array === "function") {
    return array;
  }
  if (Array.isArray(array)) {
    return array;
  }
  throw new Error(`${label === null ? "" : label + " : "}Only arrays and functions are authorized (got: "${array}")`);
}

function uniq(array) {
  return array.filter(
    (value, index) => array.indexOf(value) === index
  );
}

const limit = 100;
function deepAssignInternal(args, step) {
  if (step > limit) {
    throw new Error(`In deepAssign, number of recursive call (${step}) reached limit (${limit}), deepAssign is not working on  self-referencing objects`);
  }
  const filterArguments = args.filter((arg) => arg !== void 0 && arg !== null);
  const lastArgument = filterArguments.at(-1);
  if (filterArguments.length === 1) {
    return filterArguments[0];
  }
  if (typeof lastArgument !== "object" || Array.isArray(lastArgument)) {
    return lastArgument;
  }
  if (filterArguments.length === 0) {
    return null;
  }
  const objectsArguments = filterArguments.filter((arg) => typeof arg === "object");
  let keys = [];
  for (const arg of objectsArguments) {
    keys = keys.concat(Object.keys(arg));
  }
  const uniqKeys = uniq(keys);
  const result = {};
  for (const key of uniqKeys) {
    const values = objectsArguments.map((arg) => arg[key]);
    result[key] = deepAssignInternal(values, step + 1);
  }
  return result;
}
function deepAssign(...args) {
  return deepAssignInternal(args, 0);
}

var __defProp$1 = Object.defineProperty;
var __defNormalProp$1 = (obj, key, value) => key in obj ? __defProp$1(obj, key, { enumerable: true, configurable: true, writable: true, value }) : obj[key] = value;
var __publicField$1 = (obj, key, value) => __defNormalProp$1(obj, typeof key !== "symbol" ? key + "" : key, value);
class State {
  constructor(args) {
    __publicField$1(this, "mean");
    __publicField$1(this, "covariance");
    __publicField$1(this, "index");
    this.mean = args.mean;
    this.covariance = args.covariance;
    this.index = args.index || void 0;
  }
  /**
  * Check the consistency of the State
  * @param {Object} options
  * @see check
  */
  check(options) {
    State.check(this, options);
  }
  /**
  * Check the consistency of the State's attributes
  * @param {State} state
  * @param {Object} [options={}]
  * @param {Array} [options.dimension=null] if defined check the dimension of the state
  * @param {String} [options.title=null] used to log error mor explicitly
  * @param {Boolean} options.eigen
  * @returns {Null}
  */
  static check(state, args = {}) {
    const { dimension, title, eigen } = args;
    if (!(state instanceof State)) {
      throw new TypeError(
        "The argument is not a state \nTips: maybe you are using 2 different version of kalman-filter in your npm deps tree"
      );
    }
    const { mean, covariance } = state;
    const meanDimension = mean.length;
    if (typeof dimension === "number" && meanDimension !== dimension) {
      throw new Error(`[${title}] State.mean ${mean} with dimension ${meanDimension} does not match expected dimension (${dimension})`);
    }
    checkMatrix(mean, [meanDimension, 1], title ? title + ".mean" : "mean");
    checkMatrix(covariance, [meanDimension, meanDimension], title ? title + ".covariance" : "covariance");
    checkCovariance({ covariance, eigen });
  }
  /**
  * Multiply state with matrix
  * @param {State} state
  * @param {Array.<Array.<Number>>} matrix
  * @returns {State}
  */
  static matMul(args) {
    const { state, matrix } = args;
    const covariance = matMul(
      matMul(matrix, state.covariance),
      transpose(matrix)
    );
    const mean = matMul(matrix, state.mean);
    return new State({
      mean,
      covariance,
      index: state.index
    });
  }
  /**
  * From a state in n-dimension create a state in a subspace
  * If you see the state as a N-dimension gaussian,
  * this can be viewed as the sub M-dimension gaussian (M < N)
  * @param {Array.<Number>} obsIndexes list of dimension to extract,  (M < N <=> obsIndexes.length < this.mean.length)
  * @returns {State} subState in subspace, with subState.mean.length === obsIndexes.length
  */
  subState(obsIndexes) {
    const state = new State({
      mean: obsIndexes.map((i) => this.mean[i]),
      covariance: subSquareMatrix(this.covariance, obsIndexes),
      index: this.index
    });
    return state;
  }
  /**
  * @typedef {Object} DetailedMahalanobis
  * @property {Array.<[Number]>} diff
  * @property {Array.<Array.<Number>>} covarianceInvert
  * @property {Number} value
  */
  /**
  * Simple Malahanobis distance between the distribution (this) and a point
  * @param {Array.<[Number]>} point a Nx1 matrix representing a point
  * @returns {DetailedMahalanobis}
  */
  rawDetailedMahalanobis(point) {
    const diff = subtract(this.mean, point);
    this.check();
    const covarianceInvert = invert(this.covariance);
    if (covarianceInvert === null) {
      this.check({ eigen: true });
      throw new Error(`Cannot invert covariance ${JSON.stringify(this.covariance)}`);
    }
    const diffTransposed = transpose(diff);
    const valueMatrix = matMul(
      matMul(diffTransposed, covarianceInvert),
      diff
    );
    const value = Math.sqrt(valueMatrix[0][0]);
    if (Number.isNaN(value)) {
      const debugValue = matMul(
        matMul(
          diffTransposed,
          covarianceInvert
        ),
        diff
      );
      console.log({
        diff,
        covarianceInvert,
        this: this,
        point
      }, debugValue);
      throw new Error("mahalanobis is NaN");
    }
    return {
      diff,
      covarianceInvert,
      value
    };
  }
  /**
  * Malahanobis distance is made against an observation, so the mean and covariance
  * are projected into the observation space
  * @param {KalmanFilter} kf kalman filter use to project the state in observation's space
  * @param {Observation} observation
  * @param {Array.<Number>} obsIndexes list of indexes of observation state to use for the mahalanobis distance
  * @returns {DetailedMahalanobis}
  */
  detailedMahalanobis(args) {
    const { kf, observation, obsIndexes } = args;
    if (observation.length !== kf.observation.dimension) {
      throw new Error(`Mahalanobis observation ${observation} (dimension: ${observation.length}) does not match with kf observation dimension (${kf.observation.dimension})`);
    }
    let correctlySizedObservation = arrayToMatrix({ observation, dimension: observation.length });
    TypeAssert.assertIsArray2D(kf.observation.stateProjection, "State.detailedMahalanobis");
    const stateProjection = kf.getValue(kf.observation.stateProjection, {});
    let projectedState = State.matMul({ state: this, matrix: stateProjection });
    if (Array.isArray(obsIndexes)) {
      projectedState = projectedState.subState(obsIndexes);
      correctlySizedObservation = obsIndexes.map((i) => correctlySizedObservation[i]);
    }
    return projectedState.rawDetailedMahalanobis(correctlySizedObservation);
  }
  /**
  * @param {Object} options @see detailedMahalanobis
  * @returns {Number}
  */
  mahalanobis(options) {
    const result = this.detailedMahalanobis(options).value;
    if (Number.isNaN(result)) {
      throw new TypeError("mahalanobis is NaN");
    }
    return result;
  }
  /**
  * Bhattacharyya distance is made against in the observation space
  * to do it in the normal space see state.bhattacharyya
  * @param {KalmanFilter} kf kalman filter use to project the state in observation's space
  * @param {State} state
  * @param {Array.<Number>} obsIndexes list of indexes of observation state to use for the bhattacharyya distance
  * @returns {Number}
  */
  obsBhattacharyya(options) {
    const { kf, state, obsIndexes } = options;
    TypeAssert.assertIsArray2D(kf.observation.stateProjection, "State.obsBhattacharyya");
    const stateProjection = kf.getValue(kf.observation.stateProjection, {});
    let projectedSelfState = State.matMul({ state: this, matrix: stateProjection });
    let projectedOtherState = State.matMul({ state, matrix: stateProjection });
    if (Array.isArray(obsIndexes)) {
      projectedSelfState = projectedSelfState.subState(obsIndexes);
      projectedOtherState = projectedOtherState.subState(obsIndexes);
    }
    return projectedSelfState.bhattacharyya(projectedOtherState);
  }
  /**
  * @param {State} otherState other state to compare with
  * @returns {Number}
  */
  bhattacharyya(otherState) {
    const { covariance, mean } = this;
    const average = elemWise([covariance, otherState.covariance], ([a, b]) => (a + b) / 2);
    let covarInverted;
    try {
      covarInverted = invert(average);
    } catch (error) {
      console.log("Cannot invert", average);
      throw error;
    }
    const diff = subtract(mean, otherState.mean);
    return matMul(transpose(diff), matMul(covarInverted, diff))[0][0];
  }
}

var __defProp = Object.defineProperty;
var __defNormalProp = (obj, key, value) => key in obj ? __defProp(obj, key, { enumerable: true, configurable: true, writable: true, value }) : obj[key] = value;
var __publicField = (obj, key, value) => __defNormalProp(obj, typeof key !== "symbol" ? key + "" : key, value);
const defaultLogger = {
  info: (...args) => console.log(...args),
  debug() {
  },
  warn: (...args) => console.log(...args),
  error: (...args) => console.log(...args)
};
class CoreKalmanFilter {
  constructor(options) {
    __publicField(this, "dynamic");
    __publicField(this, "observation");
    __publicField(this, "logger");
    const { dynamic, observation, logger = defaultLogger } = options;
    this.dynamic = dynamic;
    this.observation = observation;
    this.logger = logger;
  }
  // | number[]
  getValue(fn, options) {
    return typeof fn === "function" ? fn(options) : fn;
  }
  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;
  }
  /**
  This will return the predicted covariance of a given previousCorrected State, this will help us to build the asymptoticState.
  * @param {State} previousCorrected
  * @returns{Array.<Array.<Number>>}
  */
  getPredictedCovariance(options = {}) {
    let { previousCorrected, index } = options;
    previousCorrected || (previousCorrected = this.getInitState());
    const getValueOptions = { previousCorrected, index, ...options };
    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(
      dynCov,
      covariancePrevious
    );
    checkMatrix(covariance, [this.dynamic.dimension, this.dynamic.dimension], "predicted.covariance");
    return covariance;
  }
  predictMean(o) {
    const mean = this.predictMeanWithoutControl(o);
    if (!this.dynamic.constant) {
      return mean;
    }
    const { opts } = o;
    const control = this.dynamic.constant(opts);
    checkMatrix(control, [this.dynamic.dimension, 1], "dynamic.constant");
    return add(mean, control);
  }
  predictMeanWithoutControl(args) {
    const { opts, transition } = args;
    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
  * @returns{State} predicted State
  */
  predict(options = {}) {
    let { previousCorrected, index } = options;
    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 = {
      ...options,
      previousCorrected,
      index
    };
    const transition = this.getValue(this.dynamic.transition, getValueOptions);
    const mean = this.predictMean({ transition, opts: getValueOptions });
    const covariance = this.getPredictedCovariance(getValueOptions);
    const predicted = new State({ mean, covariance, index });
    this.logger.debug("Prediction done", predicted);
    if (Number.isNaN(predicted.mean[0][0])) {
      throw new TypeError("nan");
    }
    return predicted;
  }
  /**
   * This will return the new correction, taking into account the prediction made
   * and the observation of the sensor
   * param {State} predicted the previous State
   * @param options
   * @returns kalmanGain
   */
  getGain(options) {
    let { predicted, stateProjection } = options;
    const getValueOptions = {
      index: predicted.index,
      ...options
    };
    TypeAssert.assertIsArray2DOrFnc(this.observation.stateProjection, "CoreKalmanFilter.getGain");
    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
    );
    const innovationCovariance = add(noiselessInnovation, obsCovariance);
    const optimalKalmanGain = matMul(
      matMul(predicted.covariance, stateProjTransposed),
      invert(innovationCovariance)
    );
    return optimalKalmanGain;
  }
  /**
   * This will return the corrected covariance of a given predicted State, this will help us to build the asymptoticState.
   * @param {State} predicted the previous State
   * @returns{Array.<Array.<Number>>}
   */
  getCorrectedCovariance(options) {
    let { predicted, optimalKalmanGain, stateProjection } = options;
    const identity$1 = identity(predicted.covariance.length);
    if (!stateProjection) {
      TypeAssert.assertIsArray2D(this.observation.stateProjection, "CoreKalmanFilter.getCorrectedCovariance");
      const getValueOptions = {
        index: predicted.index,
        ...options
      };
      stateProjection = this.getValue(this.observation.stateProjection, getValueOptions);
    }
    optimalKalmanGain || (optimalKalmanGain = this.getGain({ stateProjection, ...options }));
    return matMul(
      subtract(identity$1, matMul(optimalKalmanGain, stateProjection)),
      predicted.covariance
    );
  }
  getPredictedObservation(args) {
    const { opts, stateProjection } = args;
    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
  * @param {State} predicted the previous State
  * @param {Array} observation the observation of the sensor
  * @returns{State} corrected State of the Kalman Filter
  */
  correct(options) {
    const { predicted, observation } = options;
    State.check(predicted, { dimension: this.dynamic.dimension });
    if (!observation) {
      throw new Error("no measure available");
    }
    const getValueOptions = {
      observation,
      predicted,
      index: predicted.index,
      ...options
    };
    TypeAssert.assertIsArray2DOrFnc(this.observation.stateProjection, "CoreKalmanFilter.correct");
    const stateProjection = this.getValue(this.observation.stateProjection, getValueOptions);
    const optimalKalmanGain = this.getGain({
      predicted,
      stateProjection,
      ...options
    });
    const innovation = subtract(
      observation,
      this.getPredictedObservation({ stateProjection, opts: getValueOptions })
    );
    const mean = add(
      predicted.mean,
      matMul(optimalKalmanGain, innovation)
    );
    if (Number.isNaN(mean[0][0])) {
      console.log({ optimalKalmanGain, innovation, predicted });
      throw new TypeError("Mean is NaN after correction");
    }
    const covariance = this.getCorrectedCovariance(
      {
        predicted,
        optimalKalmanGain,
        stateProjection,
        ...options
      }
    );
    const corrected = new State({ mean, covariance, index: predicted.index });
    this.logger.debug("Correction done", corrected);
    return corrected;
  }
}

const buildDefaultDynamic = function(dynamic) {
  if (typeof dynamic === "string") {
    return { name: dynamic };
  }
  return { name: "constant-position" };
};
const buildDefaultObservation = function(observation) {
  if (typeof observation === "number") {
    return { name: "sensor", sensorDimension: observation };
  }
  if (typeof observation === "string") {
    return { name: observation };
  }
  return { name: "sensor" };
};
const setupModelsParameters = function(args) {
  let { observation, dynamic } = args;
  if (typeof observation !== "object" || observation === null) {
    observation = buildDefaultObservation(observation);
  }
  if (typeof dynamic !== "object" || dynamic === null) {
    dynamic = buildDefaultDynamic(
      dynamic
      /*, observation*/
    );
  }
  if (typeof observation.name === "string") {
    observation = buildObservation(observation);
  }
  if (typeof dynamic.name === "string") {
    dynamic = buildDynamic(dynamic, observation);
  }
  const withDimensionOptions = setDimensions({ observation, dynamic });
  const checkedDimensionOptions = checkDimensions(withDimensionOptions);
  const buildStateProjectionOptions = buildStateProjection(checkedDimensionOptions);
  return extendDynamicInit(buildStateProjectionOptions);
};
const modelsParametersToCoreOptions = function(modelToBeChanged) {
  const { observation, dynamic } = modelToBeChanged;
  TypeAssert.assertNotArray(observation, "modelsParametersToCoreOptions: observation");
  return deepAssign(modelToBeChanged, {
    observation: {
      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), { label: "dynamic.transition" }),
      covariance: toFunction(polymorphMatrix(dynamic.covariance, { dimension: dynamic.dimension }), { label: "dynamic.covariance" })
    }
  });
};
class KalmanFilter extends CoreKalmanFilter {
  /**
  * @typedef {Object} Config
  * @property {DynamicObjectConfig | DynamicNonObjectConfig} dynamic
  * @property {ObservationObjectConfig | ObservationNonObjectConfig} observation
  */
  /**
  * @param {Config} options
  */
  // constructor(options: {observation?: ObservationConfig, dynamic?: DynamicConfig, logger?: WinstonLogger} = {}) {
  constructor(options = {}) {
    const modelsParameters = setupModelsParameters(options);
    const coreOptions = modelsParametersToCoreOptions(modelsParameters);
    super({ ...options, ...coreOptions });
  }
  // previousCorrected?: State, index?: number,
  correct(options) {
    const coreObservation = arrayToMatrix({ observation: options.observation, dimension: this.observation.dimension });
    return super.correct({
      ...options,
      observation: coreObservation
    });
  }
  /**
  * Performs the prediction and the correction steps
  * @param {State} previousCorrected
  * @param {<Array.<Number>>} observation
  * @returns {Array.<Number>} the mean of the corrections
  */
  filter(options) {
    const predicted = super.predict(options);
    return this.correct({
      ...options,
      predicted
    });
  }
  /**
      * Filters all the observations
      * @param {Array.<Array.<Number>>} observations
      * @returns {Array.<Array.<Number>>} the mean of the corrections
      */
  filterAll(observations) {
    let previousCorrected = this.getInitState();
    const results = [];
    for (const observation of observations) {
      const predicted = this.predict({ previousCorrected });
      previousCorrected = this.correct({
        predicted,
        observation
      });
      results.push(previousCorrected.mean.map((m) => m[0]));
    }
    return results;
  }
  /**
  * 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
  */
  asymptoticStateCovariance({ limitIterations = 100, tolerance = 1e-6 } = {}) {
    let previousCorrected = super.getInitState();
    const results = [];
    for (let i = 0; i < limitIterations; i++) {
      const predicted = new State({
        mean: [],
        covariance: super.getPredictedCovariance({ previousCorrected })
      });
      previousCorrected = new State({
        mean: [],
        covariance: super.getCorrectedCovariance({ predicted })
      });
      results.push(previousCorrected.covariance);
      if (frobenius(previousCorrected.covariance, results[i - 1]) < tolerance) {
        return results[i];
      }
    }
    throw new Error("The state covariance does not converge asymptotically");
  }
  /**
  * 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
  */
  asymptoticGain({ tolerance = 1e-6 } = {}) {
    const covariance = this.asymptoticStateCovariance({ tolerance });
    const asymptoticState = new State({
      // We create a fake mean that will not be used in order to keep coherence
      mean: Array.from({ length: covariance.length }).fill(0).map(() => [0]),
      covariance
    });
    return super.getGain({ predicted: asymptoticState });
  }
}

function getCovariance({ measures, averages }) {
  const l = measures.length;
  const n = measures[0].length;
  if (l === 0) {
    throw new Error("Cannot find covariance for empty