kalman.js

/**
 * Kalman Filters
 *
 * Uses a series of measurements observed over time, containing
 * statistical noise and other inaccuracies, and produces a more
 * accurate estimate.
 *
 *
 * http://www.cs.unc.edu/~welch/kalman/Levy1997/index.html
 */

function radians(angle) {
  return angle * (Math.PI / 180);
}

const calculateGreatCircleDistance = (locationA, locationZ) => {
  const lat1 = locationA.latitude;
  const lon1 = locationA.longitude;
  const lat2 = locationZ.latitude;
  const lon2 = locationZ.longitude;

  // http://www.movable-type.co.uk/scripts/latlong.html
  const p1 = radians(lat1);
  const p2 = radians(lat2);
  const deltagamma = radians(lon2 - lon1);
  const R = 6371e3; // gives d in metres
  const d =
    Math.acos(
      Math.sin(p1) * Math.sin(p2) +
        Math.cos(p1) * Math.cos(p2) * Math.cos(deltagamma)
    ) * R;

  return isNaN(d) ? 0 : d;
};

const KALMAN_CONST = 500;

function kalmanIteration(lastLocation, location) {
  const accuracy = Math.max(location.accuracy, 1);
  const result = Object.assign({}, location, lastLocation);

  if (!lastLocation) {
    result.variance = Math.pow(accuracy, 2);
  } else {
    const timestampInc = location.timestamp - lastLocation.timestamp;

    if (timestampInc > 0) {
      // We can tune the velocity and particularly the coefficient at the end
      const velocity =
        (calculateGreatCircleDistance(location, lastLocation) / timestampInc) *
        KALMAN_CONST;

      result.variance += (timestampInc * velocity * velocity) / 1000;
    }

    const k = result.variance / (result.variance + accuracy * accuracy);

    result.latitude += k * (location.latitude - lastLocation.latitude);
    result.longitude += k * (location.longitude - lastLocation.longitude);
    result.variance = (1 - k) * result.variance;
  }

  return Object.assign({}, location, result);
}

export function kalmanFilter(rawData) {
  return rawData.reduce(kalmanIteration, null);
}