Kalman GPS filtering

[gfwilliams]

There was a post recently which mentioned the distance measurements in the run app from GPS differing from that in the Recorder app.

It's come up a few times in the past, and basically it's because the GPS location includes some inaccuracy and can 'wobble around' over time, and that wobble adds to the distance.

It's been suggested that the way to get rid of this wobble and smooth things out is to use a Kalman filter, which builds its own internal idea about where on earth you are and only updates it based on how accurate your GPS signal is. As far as I can tell in its simplest form it is basically a weighted average where the weight depends how how much you trust the GPS signal.

I thought I'd have a play based on some previously contributed Kalman code which kind of worked at some point for the BangleRun app: https://github.com/espruino/BangleApps/blob/2e8b9ac2cbd0ea70248bbd2ece53ae37203eab7e/apps/banglerun/src/gps.ts#L94

Here are the results based on me turning the GPS on in the office and then leaving home. Red = original, Blue = Kalman:

You can see that initially the red value is pretty useless as satellites are found and the fix changes, and the filter tries to smooth this out. I'm obviously not walking through the middle of all those houses, but to the north side (but there's nothing we can do with the raw data!).

The main thing is it's correcting less when it has less accurate data, but IMO it's not really enough, and I don't know enough to really be sure how to change things. My gut feeling says ideally if GPS says we're 10m away from the current position but it's only accurate to ~25m we shouldn't be updating the location at all - but that's not what is happening right now. I'm not sure I can get it much better or if it's worth including (a weighted average of lat/lon would be way faster and probably just as good).

I've pasted my code below if anyone fancies having a look. I've disabled the velocity element (which I believe just kept track of velocity in the same way, and used it to add higher uncertainty to the Kalman filter when going faster) as I didn't have any trust it was doing anything useful.

Once you have data (see the comment) you can just run it in Node.js in the desktop and view the GPX traces in a viewer to see how it's working.

/*

// to make the log file on a Bangle...

Bangle.removeAllListeners("GPS");
Bangle.setGPSPower(1)
var fil = require("Storage").open("gps.log","w");
Bangle.on("GPS",g => {
  fil.write(JSON.stringify(g)+",\n");
});

... then download with the Web IDE and add `[` and `]` at the beginning and end of the file

*/
var gpslog = JSON.parse(require("fs").readFileSync("gps_log.json"));
// gpslog=gpslog.splice(0,100); // restrict what we work on?
const EARTH_RADIUS = 6371008.8;
const POS_ACCURACY = 5; // note GPS 'hdop' to actual accuracy isn't possible, multplying by 5 seems the best
// https://gis.stackexchange.com/questions/97774/how-can-i-convert-horizontal-dilution-of-position-to-a-radius-of-68-confidence
//const VEL_ACCURACY = 0.001;


var state = {
  time : undefined, // last update time
  dt : 0, // time since last fix
  lat : undefined, lon : undefined, alt : undefined, distance : 0
  // internally x,y,z,v,pError,vError
};

function onGPS(fix) {
    if (state.time ===undefined) {
    state.time = fix.time;
    state.lat = fix.lat;
    state.lon = fix.lon;
    state.alt = fix.alt;
    state.v = fix.speed;
  }
  const dt = (fix.time - state.time) / 1000;

  state.time = fix.time;
  state.dt += dt;

  if (!fix.fix) {
    return;
  }

  const r = EARTH_RADIUS + fix.alt;
  const toRAD = Math.PI/180;
  const x = r * Math.cos(fix.lat*toRAD) * Math.cos(fix.lon*toRAD);
  const y = r * Math.cos(fix.lat*toRAD) * Math.sin(fix.lon*toRAD);
  const z = r * Math.sin(fix.lat*toRAD);
  //const v = state.v;

  if (!state.x) {
    state.x = x;
    state.y = y;
    state.z = z;
    //state.v = v;
    state.pError = 1;
    //state.vError = 1;
    return;
  }

  const dx = x - state.x;
  const dy = y - state.y;
  const dz = z - state.z;
  //const dv = v - state.v;
  const dpMag = Math.sqrt(dx * dx + dy * dy + dz * dz);
  //const dvMag = Math.abs(dv);

  //state.pError += state.v * state.dt;
  state.dt = 0;

  const pError = dpMag + fix.hdop * fix.hdop * POS_ACCURACY;
  //const vError = dvMag + fix.hdop * VEL_ACCURACY;

  const pGain = 0.5 * state.pError / (state.pError + pError);
  //const vGain = state.vError / (state.vError + vError);
  //console.log(pGain, pError);
//  console.log(vGain);

  state.x += dx * pGain;
  state.y += dy * pGain;
  state.z += dz * pGain;
  //state.v += dv * vGain;
  state.pError += (pError - state.pError) * pGain;
  //state.vError += (vError - state.vError) * vGain;
  
  const pMag = Math.sqrt(state.x * state.x + state.y * state.y + state.z * state.z);

  state.lat = Math.asin(state.z / pMag) / toRAD;
  state.lon = Math.atan2(state.y, state.x) / toRAD;
  state.alt = pMag - EARTH_RADIUS;
  state.distance += dpMag * pGain;
}

function gpxStart(name) {
  return `<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
<gpx xmlns="http://www.topografix.com/GPX/1/1" version="1.1" creator="Wikipedia"
    xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
    xsi:schemaLocation="http://www.topografix.com/GPX/1/1 http://www.topografix.com/GPX/1/1/gpx.xsd">
    <trk>
    <name>${name}</name>
    <trkseg>;\n`;
  }
function gpxPt(fix) {
 return `<trkpt lat="${fix.lat}" lon="${fix.lon}">
  <ele>${fix.alt}</ele>
  <time>${fix.time.toISOString()}</time>
 </trkpt>\n`
}
function gpxEnd() {
  return `</trkseg>
  </trk>
</gpx>\n`;
}

var gpxBefore = gpxStart("Before");
var gpxKalman = gpxStart("Kalman");

gpslog.forEach(fix => {
  fix.time = new Date(fix.time);
  onGPS(fix);
  gpxBefore += gpxPt(fix);
  gpxKalman += gpxPt(state);
});

gpxBefore += gpxEnd();
gpxKalman += gpxEnd();
require("fs").writeFileSync("gps_log.orig.gpx", gpxBefore);
require("fs").writeFileSync("gps_log.kalman.gpx", gpxKalman);

[nxdefiant]

Kalman Filter..it is a few years ago since i last played with one, but I'll add my 2 cents anyway.

As far as I can tell in its simplest form it is basically a weighted average where the weight depends how how much you trust the GPS signal.

It also should contain a prediction step of the next position which is hard to guess without an external direction and speed source.

My gut feeling says ideally if GPS says we're 10m away from the current position but it's only accurate to ~25m we shouldn't be updating the location at all

Thats what the covariance sensor (co-)variance is for. Unfortunately we do not seem to have any. Best case would be a separate variance for lat and lon. I see you somehow used hdop.

For both reasons I believe a kalman is not optimal here. Since it is raining all the time and I'm lazy I put my watch for a quick test stationary on the balcony and did some measurements.

First fix is on the right, my actual position is more to the left.

This is the result of above script which looks pretty much the same:

I belive there is room for improvement, so I searched for "node kalman filter" and https://www.npmjs.com/package/kalman-filter
was the first result so I converted above script to it:

var gpslog = JSON.parse(require("fs").readFileSync("gps_log.json"));
const POS_ACCURACY = 5; // note GPS 'hdop' to actual accuracy isn't possible, multplying by 5 seems the best
// https://gis.stackexchange.com/questions/97774/how-can-i-convert-horizontal-dilution-of-position-to-a-radius-of-68-confidence

const {KalmanFilter} = require('kalman-filter');
const kFilter = new KalmanFilter({
	observation: {dimension: 2, 
		covariance: (opts) => {
			const cov = opts.index == 0 ? 1e-6 : hdop * POS_ACCURACY; // assume initial pos good
			return [[cov, 0], [0, cov]];
		}
	},
	dynamic: {name: 'constant-position', covariance: [1e-4, 1e-4]}
});
let previousCorrected = null;

let state = {};
let hdop = 1;

function onGPS(fix) {
	const observation = [fix.lat, fix.lon]; // TODO: values should be converted to metric
	hdop = fix.hdop;
	previousCorrected = kFilter.filter({previousCorrected, observation});
	state.lat = previousCorrected.mean[0][0];
	state.lon = previousCorrected.mean[1][0];
}

function gpxStart(name) {
  return `<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
<gpx xmlns="http://www.topografix.com/GPX/1/1" version="1.1" creator="Wikipedia"
    xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
    xsi:schemaLocation="http://www.topografix.com/GPX/1/1 http://www.topografix.com/GPX/1/1/gpx.xsd">
    <trk>
    <name>${name}</name>
    <trkseg>;\n`;
  }
function gpxPt(fix) {
 return `<trkpt lat="${fix.lat}" lon="${fix.lon}">
 </trkpt>\n`
}
function gpxEnd() {
  return `</trkseg>
  </trk>
</gpx>\n`;
}

var gpxBefore = gpxStart("Before");
var gpxKalman = gpxStart("Kalman");

gpslog.forEach(fix => {
  fix.time = new Date(fix.time);
  onGPS(fix);
  gpxBefore += gpxPt(fix);
  gpxKalman += gpxPt(state);
});

gpxBefore += gpxEnd();
gpxKalman += gpxEnd();
require("fs").writeFileSync("gps_log.orig.gpx", gpxBefore);
require("fs").writeFileSync("gps_log.kalmanlib.gpx", gpxKalman);

Since the watch its stationary the prediction step I used is prev pos = next pos. For the variance I see you did hdop*5 so I did the same. I skipped degree to metric conversion because the movement is minimal. After all it is pretty dirty (units do not match) and should not be used as it is, but this is the result:

It is a bit cleaner, so yes there is some room for improvement. Also my version above obviously does not run on espruino. It is just a quick test. The position does converge faster if the initial state would be the real position.

[gfwilliams]

Thanks! I do wonder whether in this case the Kalman you're using is better purely because you're feeding Lat/Lon values in (0..360) and the covariance is super high in comparison - so it just converges much more slowly?

But yes, I think the intention was that the GPS velocity would have been used in the prediction step, but that's not what actually happened in the Kalman implementation I had there. For running I guess there's a possibility that step count could be used to estimate the velocity, but doing that reliably feels like a minefield (eg stride length has to vary not just per runner, but if you're going up/downhill).

I'd be pretty sure the GPS has its own Kalman/similar implementation internally, so it is debatable whether adding one on top helps. I don't like the idea of just using a running average when displaying distance travelled, but to be honest everyone who has complained about the increased distance actually seems kind of happy with the values they get when the averaging is applied.

[nxdefiant]

Thanks! I do wonder whether in this case the Kalman you're using is better purely because you're feeding Lat/Lon values in (0..360) and the covariance is super high in comparison - so it just converges much more slowly?

Not sure, I tried values from 0.5 to 500 for POS_ACCURACY in your version and it always circles around the position in the end.
Also yes: The covariances of both the sensor and the process noise control in relation how much the position converges. 1e-6 for process noise together with hdop*5 gave me the most beautiful picture for my test case. Obviously this does not mean these are generally ideal values. Finally if I would correct the unit of the position to meters I would "only" need to change the process noise to get the same result.

For running I guess there's a possibility that step count could be used to estimate the velocity, but doing that reliably feels like a minefield (eg stride length has to vary not just per runner, but if you're going up/downhill).

Should make a nice graduation thesis :)