Cleaning GPS tracks with a factor graph on a phone

A bike ride of 1 h 16 min came out at 6.98 km once its track was cleaned with the phone's raw satellite measurements. Cleaned without them, it was 6.19 km. The raw track, straight from the chip, was 6.39 km. The cleaning that was meant to take noise out had made the track longer than the raw one.

walker, the walk, hike and ride tracker I make for Android, cleaned that ride. It keeps everything it knows on the phone, so every number it shows has to be worked out there too, and it counts everything from the cleaned track: History, Stats, goals and badges. The cleaner has to be right, not just pretty. It began as a Kalman smoother and grew into a small factor graph with a solver I wrote myself, and the ride was the graph's first real test. The maths that shaped it is here. The tuning is walker's own.

Why a raw track lies

A phone's GPS fix is noisy in ways that add up. Jitter turns a straight line into a longer zigzag. Spikes throw single fixes tens of metres off. GPS height is off by 5–15 m, and climb adds up every rise, so noise counts as climbing. Why your GPS track is longer than the walk shows what that does to the numbers you see, with this ride in it.

The worst one I met is the static hold. Some chips freeze the position and report 0 m/s while you walk on. My test phone, a OnePlus 7 Pro, did it on a walk. The fix stood still, then jumped to catch up. A naive cleaner reads that as a stop followed by a teleport.

First attempt: a Kalman smoother with Doppler

The cleaner before the graph was a constant-velocity Kalman filter forwards and a Rauch–Tung–Striebel pass backwards, with east and north as two separate axes. Besides position it measured the chip's Doppler velocity, from each fix's speed and bearing, and altitude had its own smoother.

Around it sat rules for spikes, impossible jumps and stops. The static hold broke it twice over. The frozen fix became a confident "velocity is nought", and the stop rule pinned it in place. The cleaned track drew a stop and a jump where I had walked steadily. The step counter knew better, and the filter had no way to hear it.

Why a graph

For a linear model with Gaussian noise, a factor graph and a Kalman filter plus RTS pass give identical answers. Both are least squares over the whole segment. The gain is in what a Kalman filter can't express easily. A robust loss lets the estimator itself down-weight a spike, so no threshold has to be decided beforehand. A nonlinear factor can constrain the length of the velocity, which is what speed from steps measures, and no linear factor can. An unknown shared by a whole segment, such as the stride or the barometer's offset and drift, can be solved along with the track. And a measurement can be switched off where it is wrong. A held fix simply has none.

Dellaert and Kaess's Factor Graphs for Robot Perception is a good introduction. I wrote the solver myself, which made sense only because this graph has a very particular shape.

What is in the graph

The graph is built per segment, between pauses, so nothing is smoothed across one.

The states and the cost

Each kept fix gets a horizontal state in local metres, $x_i = (e_i, n_i, v^e_i, v^n_i)$. That is east and north from the segment's first fix, and the velocity along them. A separate vertical chain holds each fix's height and rate of climb. The two share no factor, so they are solved apart. Beside the states sit the globals, one set per segment. For the plane that is the stride; for the height, the barometer's offset and drift.

Every measurement becomes a factor, a term that says how far the states are from what was measured. The solver looks for the states and globals that make the sum of all of them smallest:

$x$ is every state of the chain and $g$ the globals. Each factor $j$ has a residual $r_j$, what it measured against what the states say, and a covariance $\Sigma_j$, how far off it may be. $e_j$ is the residual's length in units of its noise, so $e_j = 1$ is one standard deviation whatever the factor measures. $\rho_j$ is the factor's loss. Most factors use plain least squares, $\rho(e) = e^2$. The ones that see spikes use a robust loss, which comes below. For the track, this means no fix is taken at its word. Each pulls on the solution by how sure it was, against every other measurement.

Motion between neighbours

What holds the chain together is the motion factor between neighbouring states. It says you moved at constant velocity from one fix to the next, and charges for any change:

$F_i$ carries state $i$ forward by $\Delta t_i$, the time from fix $i$ to fix $i+1$, at constant velocity. $I_2$ is the 2×2 identity, and $\otimes I_2$ repeats the block for east and north, which are independent and share one model. $Q_i$ is the factor's covariance, the $\Sigma_j$ above. It is the textbook continuous white-noise acceleration model. $q$ is the acceleration's spectral density: over $\Delta t_i$ the velocity picks up a variance of $q\,\Delta t_i$, the velocity's diagonal entry in $Q_i$. It is set from how quickly speed may change on foot or on a bike. The larger $q$, the more freely the track may bend between two fixes.

The old filter modelled that noise as a step of acceleration per interval, with variance $\sigma_a^2$: $\sigma_a^2 \begin{bmatrix} \Delta t^4/4 & \Delta t^3/2 \\ \Delta t^3/2 & \Delta t^2 \end{bmatrix}$. That matrix is the outer product of one vector, so it has rank 1. A Kalman filter only adds it, and doesn't mind. A graph needs its inverse. The white-noise block has determinant $q^2 \Delta t^4 / 12$, never nought, so it can be inverted.

The measurements

Five kinds of measurement pull on the states.

GPS position is a robust factor, weighted by the accuracy the fix reports, read on the cautious side.

Doppler velocity is robust too, with its error split along and across the direction of travel. Along it is the chip's speed accuracy; across it the bearing's, which grows with speed. That gives a full 2×2 covariance. The old smoother, with its two separate axes, could only take the diagonal. The graph takes the whole matrix.

Speed from steps applies on walking fixes. The speed is stride times cadence, within a tolerance. It is nonlinear, because it constrains the length of the velocity. The stride is a global, with a loose prior, so the graph learns it where Doppler is good.

Zero velocity applies only to a real stop, a few slow fixes in a row while the steps say you are not walking. It applies only without satellites, because they measure the velocity themselves.

Altitude comes from GPS as a robust factor, and from the barometer where the phone has one. Pressure becomes a height through the international standard atmosphere, but a change in the weather moves that by metres, so only its changes are trusted. The barometer's offset and drift are globals, and GPS altitude anchors them.

A per-segment unknown that nothing measures changes nothing, so one graph shape serves every track, with or without steps and pressure.

The held fix

With those factors, the static hold becomes simple. A fix that reports standing still while the step counter says walking is held. It gets no position and no Doppler factor, like the fourth state in Figure 1. The motion factors carry the track through and spread the catch-up jump over the hold.

Huber in place of the spike rules

The spike rules went. In their place, position, Doppler and GPS altitude use a Huber loss, which counts a residual in full while it is small and only linearly once it passes a threshold. In walker's scaling, for a threshold $k$, it is

$e$ is the factor's normalised residual from above. Up to $k$ standard deviations, a factor counts in full. Beyond it, its pull stays that of a residual of $k$, however far off it is. That is twice Huber's usual loss, which changes nothing about where the minimum is. $w(e)$ is the weight the solver uses, because it never evaluates $\rho$ itself. For the track, a spike no longer needs a rule to catch it. It pulls like any residual at the threshold, and the other measurements decide where the track goes.

Positions get one more rule. A position far beyond any honest error is left out entirely. That cut-off is what still lets walker's GPS diagnostics name a fix as left out, and the old spike and too-fast tests pass unchanged. I expected to need Cauchy; the tests never asked for it.

The solver: Levenberg–Marquardt over an arrow

Each state links only to its neighbours, plus a few globals. That sparseness is what makes the solver cheap. For a segment of $n$ states, the system for a step $\delta$ has this shape:

$A_i$ is the block of state $i$ with itself, $N \times N$ for a state of $N$ numbers. $L_i$ links state $i$ to state $i+1$; only factors between neighbours write it. $E_i$ links state $i$ to the $G$ globals, and $C$ is the globals' $G \times G$ block. $\delta_i$ is the step for state $i$, $\delta_g$ the step for the globals, and the $b$ are the right-hand side. Everything else is nought, which is the whole trick. The matrix is block-tridiagonal with a border, an arrow.

The solver eliminates the states one after another. What remains is a small system in the globals alone, their Schur complement, which it solves before it recovers the states backwards. Each state costs a few small Cholesky solves. No dense matrix is ever formed, and there is no linear algebra crate. The solver is hand-written Rust, like nearly all of walker. A segment costs $O(n\,(N + G)^3)$, linear in $n$. A dense solve of an hour at a fix a second would be $O\big((nN)^3\big)$: for 14,400 unknowns, about 10¹² operations.

Around that sits Levenberg–Marquardt, because the factors are not all linear. Each iteration linearises every factor where the states are and takes a damped step. A step that doesn't raise the cost is kept and the damping eases. One that does is thrown away and tried again with more damping. Nocedal and Wright's Numerical Optimization describes the trust-region version, which compares the drop in cost with the drop the linear model predicted. walker only asks whether the cost went down, the simplest rule that works.

Robust weights make the sum depend on itself, so the whole thing sits inside iteratively reweighted least squares. Each round sets every robust weight from its residual, then solves with the weights held. Rounds repeat until the weights settle. A factor left out is still looked at in the next round, so a fix dropped early can come back once the track has moved towards it.

Robust losses make the problem non-convex, so the start matters. The old Kalman smoother became the initialiser. Its answer is a least-squares solution, and the first round only has to take the outliers out.

What the tests said

First, the check. With plain Gaussian losses, the graph reproduces the old smoother's positions and altitude to within a millimetre. Then synthetic tests:

• a static hold at walking pace is walked through within 5 m, and the distance comes out within 5 %; without steps it stays a stop;
• a 100 m climb with noisy, biased GPS altitude counts within 10 % of what the true heights give, and at least twice as close as without the barometer.

Figure 3 shows the idea of the first test in one dimension.

These are tests on made-up tracks, not field results. My phone has no barometer, so that part has only been checked with synthetic hills and a simulated one.

The ride that got longer

Later, the phone's raw GNSS measurements joined the graph; a post of their own covers them. The state grew by a receiver clock bias and drift, each signal's bias against GPS L1 became a global, and the arrow kept its shape. On my OnePlus, BeiDou sits at −459 m, GLONASS at +1,141 m, and GPS L5 at −2,350 m. The exact method converts each position from local metres to Earth-centred coordinates at every evaluation. Done exactly, an hour with satellites was 13 times slower to clean, and walker linearises that conversion once per epoch instead. That is safe. A node moves by metres while solving, and the Earth's curve over 100 m is under a millimetre.

Then came the ride from the top of this post, the first real test. It was 1 h 16 min of recorded fixes, on a trip that ran longer, with a stop of over two hours. Cleaned with satellites, 6.98 km. Without them, 6.19 km. Raw, 6.39 km.

A probe split the extra 793 m. Only 62 m of it was where the receiver stood, so the stops weren't the cause. The weights were. Each pseudorange came with the chip's own σ, a few metres, and was treated as independent every second. Multipath errors last seconds to minutes, so a dozen ranges an epoch pulled every node around. Two changes followed:

• a floor on each range's σ that rises as the signal weakens;
• widening, so a run of epochs counts as one independent range per correlation time of tens of seconds.

The last row also holds a stop the chip reported, where the solved speed agrees it is one. Without satellites: 6,185 m and 78 moved. So 6.98 km became 6.22 km.

When I added every satellite system and band the phone measures, about 25–30 signals an epoch, the trap came back in a new form. The surroundings bend every signal of an epoch at once, so the position they give is off by metres however many satellites agree. Now an epoch's ranges, and its rates, together count as a few independent ones.

Both widenings rest on one fact. Information, one over the variance, adds up. Widen each of an epoch's ranges by how many there are, and their information together stays that of a few independent ranges, however many satellites are up. The widening over time rests on it too. A satellite's ranges within one correlation time together count as one. A fuller estimator would model the correlation, with a Gauss–Markov state per satellite or a full covariance per epoch. walker only widens each factor, which keeps every factor scalar and the arrow's shape intact.

Scoring without a truth

I also stopped tuning on length. Two scores need no ground truth. The first is how far the cleaned fixes lie from the mapped ways in the map regions on the phone, as a median and a 90th percentile. The second cleans the track with folds of satellites held out, scores the held-out ranges against where the track put each fix, and takes a robust σ of what is left. The map sees error across the way. The held-out satellites see error along it.

On the ride, fixes lay 2.37 m (median) and 5.98 m (p90) from the ways without satellites, and 2.24 m and 5.34 m with every signal. The held-out ranges' robust σ went from 7.61 m to 7.51 m. The ride now cleans to 6.25 km; the chip's speed over time says 6.13 km.

That's a modest gain. Satellites mostly cut the bad tails. Where the chip is good, its fix is already about as close to the way. On one of three shorter logs the median got slightly worse (2.63 m to 2.87 m) while the p90 improved. The 2021 winner of Google's Smartphone Decimeter Challenge used a factor graph too, and scored 1.62 m, but with carrier phase and pseudoranges corrected from reference stations. My phone gives no valid carrier phase, and walker uses no correction service.

A UKF for the matched ways

walker also matches tracks to OpenStreetMap paths on the phone, with a hidden Markov model and the Viterbi algorithm, which picks the likeliest sequence of ways. Projecting each sample onto the chosen ways jitters, and at a corner it can step backwards, so each matched run gets an unscented Kalman filter along the matched line. Its state is the distance along the line, the speed along it, and an offset across it. The offset covers a track that keeps to one side of a wide way, or a GPS bias, and fades back to nought.

It is unscented because the measurement, where the state puts a sample on the line, bends at every corner. The filter pushes a handful of sample states through it, its sigma points, and points on either side of a corner land on both legs and see the bend, where a linearisation would see only one. For the same reason the line runs on straight past its ends. When I clamped it instead, every sigma point that fell off an end landed on it, and a straight, noiseless run came out 6 m off. Figure 4 shows both. The run is then smoothed backwards, and a last step makes sure the distance only goes forward. Round a synthetic corner, that gives 1.5 m RMS against the bare projection's 3.3 m. The matched line never feeds back into the cleaned track, or downloading a map region would change your History.

What it costs on a phone

On my desktop, in a release build, 300 synthetic hour-long walks cleaned in 4.4 s through the graph against 318 ms through the old smoother. An hour with raw satellites took about 200 ms against 20 ms without them.

The phone is where it showed. On the OnePlus, cleaning two walks of 1.8 and 6.2 km with raw GNSS took 37 s, and it happened at every launch, because walker cleaned every activity from scratch. That was a debug build, and the solver's small by-value matrix functions need inlining. It overstates the release cost, but I haven't measured a release build on the phone yet.

The next milestone left the constant alone and fixed the pattern. walker now cleans each activity once and keeps the result, and cleans what is missing on up to four threads off the store thread, so the rest of the app doesn't wait for it. Measuring first found two speedups that leave the result identical bit for bit, and the ride went from 2.6 s to 1.2 s on my desktop, in a release build. That is built, not released yet, and I still haven't measured it on the phone.

There is no GPU in this. Ranges of about 2·10⁷ m need millimetres, which is f64, and the phone's Vulkan driver has no f64 shaders.

If I started again, I'd do two things sooner. I'd score against something the cleaner never sees rather than the distance it shows. And I'd measure on the phone early: the desktop said a couple of hundred milliseconds an hour, and the phone said 37 seconds.