German Aerospace Center (DLR)
Institute of Communications and Navigation
Department Communications Systems
Cooperative Systems Group

---

Further Obervations and Thoughts on FootSLAM

---

This page serves to provide additional explanations, obervations and thoughts about FootSLAM (and its extensions) and we might use this page in the future to act as a kind of blog on the topic.

Improving the underlying human odometry

Since 2009 when we started work on FootSLAM we have been trying to improve the underlying step measurements (aka human odometry) from our foot mounted IMU. The first wave of improvement came from improving the ZUPT (see our IPIN 2010 paper. Our former visiting scientist Francisco Zampella recently presented a UKF implementation (at PLANS 2012, Session C1) that also uses Zero Angular Rate Updates (ZARUs) and Magnetic Field Angular Updates (MARUs). The basic idea behind the MARU is to use the short duration between samples from the 3-axis magnetometer to get additional information about the rotation of the sensor array (we have colocated mag. and inertial sensors), which helps estimate the gyro drifts. FootSLAM can cope with a surprising amount of drift on the odometry, but of course this comes at a cost of both convergence time and computational complexity (lots of particles ....). Reducing this drift will not make FootSLAM obsolete but will open up new areas of application, such as real-time mapping.

FootSLAM playback showing raw odometry, particle distribution and heading rate bias posterior

In some of these videos it take a few seconds before anything happens, and there are also moments where the pedestrian stopped for a while.

This video shows the raw human odometry (bottom right), the evolving FootSLAM map (bottom left), the particle distribution (top right) and the heading rate bias posterior (histogram on top left). The blue dots on the x,y particle viewer (top right) are the mean of the estimated position posterior at that time, and it is frozen on the display. We can observe to which extent the mean estimated position - conditioned only on measurements available at that time - deviates from what the eye can quickly recognize as the "true" track (e.g. compared to the map after it has converged). The histogram shows the estimate of the heading rate bias (HRB) which begins as a wide (prior) distribution and then converges during loop closures. We can see the effect of this HRB on the raw odometry which shows the typical spiralling. FootSLAM estimates the HRB state which is the main coloured part of the odometry error process. The odometry data set shown here is quite benign since the HRB is rather stable over the whole walk, but we observe this quite often. Furthermore, since the pedestrian ensured a few loop closures early in the walk the position estimate converges very early meaning "stable" SLAM pretty much from the start. We used 5000 particles for the particle filter, which isn't really very many. This video shows the same data processed with just 100 particles - it still converges well.

More videos of the MIT walks in the same format: Walk 1, Walk 3, Walk 4.

A FootSLAM video from data recorded in our building with some quite severe HRB: Walk Jan_17_16-03-30_CET_2012 with 30000 particles.

Realtime FootSLAM and more Experiments

FootSLAM is now running realtime. We have made significant advances in reducing the algorithm complexity (to be published later this year, we hope) and we have modified the particle filter to work with queues of timestamped measurements. We also plan to add to this site a gallery of FootSLAM maps to show the kinds of environments we have used it successfully. Update: the gallery is under construction. So far, we have not encountered an environment in which FootSLAM did not work - but then again we made no efforts to find one, and a large unrestricted building (no walls, etc) would be such an example where we know that it cannot work.

FeetSLAM experiments

We have undertaken two experiments to verify FeetSLAM, which is cooperative FootSLAM. One data set contained five walks, one of which was chosen to be so short that it would not converge with FootSLAM by itself (insufficient loop closure). However, as expected and hoped the data set converged when combined with the other walks using the iterative processing algorithm that is motivated by Turbo Decoding. We were grateful and happy that our talk at ION GNSS received the best presentation award in its session. This work is now being continued by our first full time PhD student Maria Garcia Puyol, whose thesis on the topic can be found here. Maria has produced some illustrative videos which can be found on the main FootSLAM page .



In addition to the data we collected at DLR we collected data while visiting the CSAIL at MIT in March 2011, the results of which are shown above. The area covered is significantly more extensive than the DLR data sets.

FootSLAM exposes error in our reference floor plan!

Our development of our first version of FeetSLAM have enabled us to improve accuracy but had left us with a disturbing contradiction between the resulting FootSLAM maps and what we thought was the true layout of our building. This is the first time we actually looked at where FootSLAM contradicted the known plan, and it turned out that the latter is plain wrong:



This is exciting because it's a firsthand example of how FootSLAM can be used to improve and correct existing floor plans. Our building interior walls are drywall constructions and have been changed over the last years - except these changes didn't make their way to the data reference we were using.

The "bent wire" analogy

It's essential to understand that in our particular flavour of FootSLAM the only actual state space we are exploring in our Bayesian Filter is the error state space of the human odometry (step measurements). All other state variables, such as the location of the pedestrian, as well as the learnt map, are conditioned on the error state of the odometry and are actually uniquely specified by it.

So, to draw an analogy, the FootSLAM estimator is processing the raw odometry track that looks like a bent piece of wire with many loops. (A human observer can sometimes recognize features of the underlying floor plan when looking at the raw odometry, especially if the walk was not too long.) Exploring the odometry error space is akin to bending and also slightly elongating or squashing the wire at each segment (step). The state space which needs to be explored increases exponentially over distance travelled.

Essentially, each particle of the RBPF holds its own modified piece of wire. For each particle we can now compute the map which might be a hexagon transition map, given this modified piece of wire (i.e. the corrected odometry). The estimator will reward those particles whose maps have the lowest entropy (see equation (18) of the PLANS paper).

If we reward the lowest entropy map, why wouldn't FootSLAM always find only the trivial "back-and-forth" map?

A FootSLAM hexagon map with minimal entropy might consist of two adjacent hexagons where the connecting edge has transition probability of one for both hexagons. A person "walking according to this map" would step repeatedly back and forth from one hexagon to the other across the edge for all eternity ...

The question we can ask now is: what stops the estimator from applying exactly that odometry error to the bent piece of wire which results in just this back and forth odometry, which in turn corresponds to this minimal entropy map?

Answer: Nothing! Except that for any real-world odometry that is not in fact measured from a true instance of such back and forth motion, the likelihood of drawing this odometry error (proposal function!) is next to zero. In a particle filter the posterior is modelled not just by the particles' weights, but also by their density in the state space. Hence, although we might have a lonesome particle sitting at the minimal entropy map, it will contribute next to no contribution to the posterior (given a sufficient number of particles).

What about 3D FootSLAM?

Well, extension is conceptially trivial: You could just put a lid and floor on each hexagon and stack layers of hexagons. The new structures would now have eight edges per hexagon instead of six. We might make these stacks a meter high or so, or we would adapt them to the floor separation for each building (which we would have to learn also). Alternatively, one could do simple 2D FootSLAM and switch over to the next level if we can reliably detect floor transitions. We are still exploring so many aspects of 2D FootSLAM that we have simply not yet implemented a 3D version. Update April 2012: this is now work in progress.

Implementation issues

Our FootSLAM implementation is based on the particle filtering and multi-sensor navigation framework which we have been using for our indoor positioning work and also our work on cooperative vehicle positioning. This grew out of a small experimental particle filter put together in 2005 and has been expanded ever since. Unfortunately, stronger spouts of growth tend to lead to cracks in the design and poorly documented code. But yes, you sometimes have to sacrifice speed to get results published with SW engineering standards. I (Patrick) have recently been commenting the FootSLAM part of the framework, but I have to force myself to reserve the time to do so.

We have used the Java language for our implementation and develop using Eclipse and Subversion. The particle filter is more or less generic and the developer "just" needs to provide state, proposal function and weight update classes. For FootSLAM this meant developing additional data structures to represent the maps: Hexagons, Hexagons maps, hexagon counts, with efficient data structures where possible. In addition, we need map visualization tools if we are to make sense of anything. The same basically applied to PlaceSLAM (PLANS 2010), WiSLAM (IPIN 2011) and the visualization of all three are interwoven. In terms of processing speed we can process roughly at real time for 10-15 minute walks and 25000-30000 particles on a single core of a 3 GHz PC. Memory consumption rises with the number of particles and the number of explored hexagons. It's actually quite fun to see memory usage drop after a loop closure. A run with 30000 to 50000 particles may require 1-2 GBytes of main memory for a 10-15 minute walk in our building.

Update: We have further improved the algorithm and will soon begin work on more efficient map stuctures based on trees to reduce the complexity during resampling.

Size of the hexagons

Why did we choose a hexagon radius of around 0.5 meters? Well, to address this question we must consider the spatial dependence / independence of the map. In the FootSLAM Bayesian Filter derivaton we assume that the map can be decomposed into independent elements (in our case: Hexagons with edge-crossing probabilities). We think that this assumption is roughly valid for typical building structures' physical dimensions, such as corridors, entrances, walkable office space, etc. In other words, we are assuming that if we know something about the limitations of human motion for one area of 1 meter diameter, then the adjacent areas should be statistically independent. Using significantly smaller hexagons would mean that we are not taking spatial dependence into account, and would be much more suboptimal. A larger hexagon area would lack the precision to represent typical building spaces.

Illustration of the step measurement error and coordinate systems of human odometry for FootSLAM

It was actually quite an effort to document the step errors for the ION GNSS 2009 and InsideGNSS papers. Since these are static documents the multitude of coordinate systems may confuse the reader. That's why there's an animated powerpoint version here which has been slightly modified and improved w.r.t. ION GNSS 2009 and SLAM Dance papers.