A human motion model based on maps for navigation systems

2.1 Motivation for a motion model based on maps and related work

Map matching is widespread used in navigation systems for vehicles and pedestrians. Map matching [15, 16] in general is the concept in which tracking data are related to maps. In this study, the objective is to improve the location estimation by "snapping" the measurements to the nearest path (polyline) in the map [15, 17]. For instance, in [18–21], road maps are used in different systems for different applications like vehicle navigation, pedestrian navigation with mobile devices, vehicles in parking garages, etc. In these applications, it is assumed that the vehicle/pedestrian can only follow streets on the map. Here, it can be assumed that the vehicle heading is the same as the heading of the road segment, which is known from the map [18].

In our applications, this assumption does not hold and more than only road maps are of interest because in indoor navigation the size of the rooms varies and pedestrians are not only following road maps with equally sized "lines". Here, we have to consider more accurate floor-plans where walls will restrict the motion. In addition, other obstacles like tables or cupboards could be considered since they are also hindering the movement of the pedestrian.

Floor-plans are used in many applications in a rather simple way. In [11, 22], the particles are weighted by zero when the path is crossing a wall. With this, particles that are crossing walls are eliminated. In [9], similar values were used for weighting regarding the floor-plans: a probability of zero (actually a very small value to allow a small fraction of particles to cross walls in the case of very inaccurate measurements or particle depletion) is applied when a particle's displacement crosses a wall. Otherwise, particles are weighted solely by the product of the likelihoods of other sensors and by a very simple motion model that might reward slower speeds or smaller angular changes (the weighting from the floor-plan is thus effectively very close to unity for all particles not crossing walls).

In this article, we propose a weighting function for PF-based positioning estimators that takes consideration of the heading distribution at each location and which is based on known maps. The principle is similar to the so-called movement models based map matching where the map is used to restrict the otherwise probabilistic movement of the tracked object. The main objective of this article is to increase the robustness of sequential Bayesian positioning estimators through proposing a motion model that awards higher weights for particles that follow motion which is compatible with walls and so the more constrained the heading options at that location are. In other words, particles that follow a path that do not cross walls will be rewarded more when in areas with more limited angular options.

To illustrate this, let us assume that--at the beginning of our LPF estimation--particles were distributed equally inside and outside a building since the starting position of the pedestrian is known with only a very large uncertainty. In addition, we assume that the area outside the building is an open area where the pedestrian can walk everywhere. First, we investigate the traditional case of using only floor-plans for weighting (no proper transition model): particles that are inside the building will obtain high (unity) weights if they do not cross walls. Particles outside the building will never obtain a very low or zero weight since they never cross walls. For the case where the tracked pedestrian is inside the building, a significant portion of the group of particles inside the building will cross walls and as time elapses will be eliminated more and more as a result of the resampling step. On the other hand, all the particles outside the building will have high weights since they are not crossing walls at all. Resampling will result in increasing the number of particles outside the building and decreasing the number of particles inside the building. This will result in divergence of the algorithm over time. Even without resampling and a very large number of particles, the posterior distribution will tend toward the outside group, since the density of particles will be much higher outside.

Second, we examine the case of using an accurate motion model that incorporates the knowledge of maps and floor-plans. Particles inside the building will obtain high weights if they do not cross walls and are weighted with the motion model. Particles that are outside the building will obtain moderate weights for all headings--the angular probability density function (PDF) is equally distributed in this case. For the case where the pedestrian is inside the building, the measurements will follow a path through the walkable areas within the building. Accordingly, particles inside the building will obtain higher weights compared to the ones outside the building. Resampling will result in increasing the number of particles inside the building and an improvement of performance and reliability.

There exist other techniques to reduce the heading error of a PF system. For instance in [11], the authors apply a backtracking PF, where the state estimates are refined based on particle trajectory histories. A Backtracking PF recalculates the previous state estimation without invalid trajectories to improve performance. Since in our case all paths are possible except across walls/obstacles--no invalid trajectories exist in our simulation except when crossing walls--backtracking will not help us to improve performance. Borestein et al. [23] proposed to compensate the heading drift by a heuristic heading reduction (HDR) algorithm that makes use of the fact that many corridors or paths are straight. With HDR, the gyro biases are corrected when it is detected that a person walks a straight path to reduce the heading error. However, in our simulations we do not assume long straight paths. The pedestrian very often enters rooms, stands still, and turns around, so that the assumption for long straight runs does not hold. In [24], it is proposed to compensate the heading drift of an INS/EKF framework by a combination of a compass, the HDR, and zero angular rate update (ZARU) [25]. In these simulations, floor-plans were not known. It is shown that the ZARU and HDR alone will not improve performance and only the combination of the two methods with a compass will improve the results. In our system, we actually use a compass but the assumption for long straight runs does not hold. In addition, we want to show the influence of known floor-plans and how it can help to reduce the possible heading drift.

Finally, there exist techniques to include the height for better positioning. In [26], a barometer height estimation with topographic maps (outdoor environment) is investigated and it is shown that it can improve performance in a GPS-INS-based system. In the simulations of this article, the pedestrians walk only through the ground floor so that it is not necessary to measure the height. However, in a building with more than one floor, the height has to be considered. The motion model can then be extended to the 3D case as described in [27] and measurements of the height can be included in the overall system design.

2.2 Cascaded estimation architecture

In many applications, strapdown inertial sensors are integrated into a navigation system using a direct/indirect extended Kalman filter together with a strapdown navigation computer [9, 28, 29]. However, we use the cascaded approach proposed in [9] because of the following reasons: the Kalman filter is based on pure kinematic relations between velocity, position, attitude, and sensor errors. In this study, the dynamics of the tracked object (e.g., a person traveling by foot) are not considered. In addition, the prior knowledge about the object dynamics coming from accelerometer and gyroscope cannot be exploited, because no likelihood function is used to incorporate these measurements.

To overcome this problem, the cascaded estimation architecture as illustrated in Figure 1 proposed in [9] is taken. Here, a lower-level Kalman filter is used to process the high-rate (typically >100 Hz) data of the foot-mounted inertial system. With the upper fusion filter, further prior dynamic knowledge about the pedestrian can be integrated at a much lower temporal rate (typically at around 1 Hz).

The lower Kalman filter estimates the foot displacement (one human step of one foot) which also includes the heading change of the foot (and hence the body) per step. These values are taken as measurements within the upper main fusion filter. The measurements, here referred to as the step-measurements, enter the algorithm via a Gaussian likelihood function along with the measurements and likelihoods of further available sensors. In the upper filter, nonlinear properties of human motion (by means of a dedicated movement model) and other nonlinear effects such as building plans can be considered. For the upper level fusion filter, a PF [13, 30] is applied since it can process sensors and models that are highly nonlinear.

The main focus of this article is the motion/transition model that is based on the knowledge of maps and floor-plans (see Figure 1). In [9], it was proposed to use a proper movement model at this place for weighting in a LPF. Here, a very simple movement model drawn from mutually uncorrelated zero-white Gaussian noise processes, the variances of which are adapted to the movement of a pedestrian, is used for weighting. Instead of this simple movement model, an angular weighting function based on maps is used in this article. The main error process of the whole system is the heading drift; therefore, we focus only on weighting possible headings. The proposed motion model in our case is used for weighting particles in the PF of Figure 1. However, it can also be used in applications when prediction of heading is needed--e.g., in a movement model--in an indoor/outdoor environment with known floor-plans and maps where the possible headings are reduced because of obstacles and walls.

When no reliable odometry measurements are available, a movement model is needed in our simulations. In this case, a normal PF is used instead of the LPF. In our simulations, this was only the case at the very beginning of the simulation runs. Here, a simple movement model like the one drawn from mutually uncorrelated zero-mean white Gaussian noise processes, or more accurate movement model as of [27], can be used.

The PF will perform sensor fusion roughly every second or when triggered to do so by a specific sensor. In our case, we will perform an update cycle at the latest once every second and also upon each step-measurement.

3.1 A 2D-Diffusion matrix based on maps

The diffusion algorithm is derived from the principle of gas diffusion in space studied in thermodynamics and is commonly used for path finding of robots [31]. The idea is to have a source continuously effusing gas that disperses in free space and which becomes absorbed by walls and other obstacles. In [14], the diffusion model is used with the central assumption to have a source effusing gas which is one of the possible destination points. Here, a path finder (following the gradient) is needed for finding the path to that destination point. In contrast, we assume that the source of the gas is the current waypoint in this article, and we calculate an angular PDF from the gas distribution around this point. Accordingly, the path-finding algorithm is not needed anymore.

To keep the model's complexity low, the diffusion matrix is confined to a rectangular area. The central assumption for defining the weighting function is that the possible headings follow the gas distribution, if the current waypoint is the source of the gas. Topographical maps and floor-plans contain useful information that influences pedestrian movement such as the different types of areas which have different degrees of accessibility. Examples of these areas are forests, fields, streets, ways, meadows, coppices, flowerbeds, houses, walls, etc.

Typically, people do not walk through less accessible areas like cultivated fields. Most probably people stay on dedicated paths or streets (e.g., on the pedestrian sidewalk). Walls are not passable, whereas houses may be entered through doors. Inside, not only house floor-plans are used, but also more detailed maps could be considered: The areas where many kinds of furniture stand (tables, cupboards, etc.) are not accessible. On the other hand, chairs are accessible. Therefore, the idea is that additionally to floor-plan maps are included in the motion model to handle the degree of accessibility. To handle the degree of accessibility, we define the layout map matrix L--which is considered in the computation of the diffusion matrix--in a new way:

(1)

where N _x × N _y is the size of the rectangular area. In our case of computing weights from the diffusion values, a square area is used. For inaccessible points (e.g., walls and closed areas), the values of the layout map matrix are set to be zero. For the accessible areas, the layout map matrix will have different values depending on the accessibility. According to the accessibility of a specific area, the values v lie between 1 and 255. The most accessible areas will have a value v of 1, whereas the least accessible area will have a value v of 255. We chose the values to be between 0 and 255 because of the memory-efficient representation of a single-byte value. These values give reasonable values in the diffusion matrix.

The diffusion process with these newly defined values of the layout map matrix is as follows: the point that represents the source effusing gas is the current waypoint (x _m , y _m ). We use a sliding square window, where the current waypoint is the middle point of that window:

(2)

where N _x = Ny and N _x is odd-numbered. For each waypoint, a so-called diffusion matrix D _m is pre-computed. The diffusion matrix for a particular waypoint contains the values for the gas concentration at each possible waypoint when gas effused from that source point. For this, a filter F of size n × n is applied:

(3)

The diffusion is expressed by a convolution of the diffusion matrix D _m with the filter matrix F element-wise multiplied by the layout map matrix L:

(4)

Here, the values l _{i, j} represent a weighting of the diffusion values according to their accessibility at the location (i, j).

Constantly refreshing the source is represented by forcing

(5)

at the waypoint. Equation 4 is evaluated repeatedly until the entire matrix is filled with values that are greater than zero (except for walls and closed areas):

(6)

Figure 2 shows the layout map matrix adequate for our simulation environment. The walls are depicted in black, not easily reachable forest area is marked with dark gray, and flowerbed areas are drawn in light gray. The area where people may walk is drawn in white. In addition, the stairs area is marked in blue. The diffusion results after reaching steady state are given in Figure 3, where the gas concentration is high in the dark red area and low in the blue area. One can see that gas coming from the source (waypoint) close to the center of the area effuses faster in the white areas (dark red color) and slower in the dark gray areas. In addition, gas will not flow in closed rooms of the building.

By using maps, one can easily handle restricted areas, forests walls, etc. In addition, one can precisely define areas where a person may stand and where not both in indoor and outdoor environments.

3.2 A motion model based on the diffusion algorithm

The computation of the diffusion matrix is the prerequisite for the computation of an angular PDF. Instead of using pre-defined destination points and calculating the directions to a specified destination point--as it is the case when applying the diffusion movement model for weighting [27], the source of the gas is, in our case, the actual waypoint. The advantage of taking the actual waypoint as the source of the gas is that we can obtain a weighting function directly from the gas distribution. Another advantage is that the path-finding algorithm is not needed anymore and the weighting is totally independent of any notion of destination points such as those used in the movement model presented in [27]. In addition, we can in practice restrict the rectangular area to a small area around the actual position, so that the computational effort is much lower. Finally, one can consider storing the PDF values during runtime instead of pre-computing the whole area.

The motion model is directly derived from the gas distribution. Figure 4 shows the gas distribution from one waypoint within a cutout of the floor-plan of Figure 2. One can see that the gas is restricted to the areas where it can flow. Walls are restricting the gas from flowing. From this diagram, we can choose a threshold for obtaining a contour line of the gas distribution. From this contour line, we directly obtain the angular weighting function using the distance from the waypoint to the contour line. When the gas is reaching a wall, the contour ends at the wall and the distance is equal to the distance to the wall. Figure 5 shows the polar diagram for the weighting function. The weight is higher for the directions where the persons may walk. Since it is possible to stay in front of a wall (not crossing), a small distance is applied for the directions pointing toward the wall for the case that the waypoint is close to the wall. When particles actually cross a wall, their weights are set to a very small value just as in the standard approach.

The angular PDF is obtained as follows: the contour line of the diffusion matrix represents our weighting function. Therefore, we have to determine this contour line first. Here, we specify for the diffusion area a set c of N _c contour-line points . The contour line points can be obtained by checking all the diffusion values to be below a certain threshold T. If a diffusion value at (k, l) is below that threshold:

(7)

and the diffusion values of at least one neighboring point (direct neighborhood) is greater than the threshold T:

(8)

then, the position (k, l) is part of the set of contour lines:

(9)

Walls are included in this computation, since for a point on the wall the following equation holds:

(10)

Figure 6 shows the contour line of the diffusion values marked in dark red (T was set to 0.0001, 0.001, and 0.01, respectively). Here, the size of the square window could be reduced when the threshold is increased.

The value of the angular PDF for an angle α is obtained via the distance of the current waypoint (x _m , y _m ) to the contour line point that lies in the direction of that angle α. Here, α is the absolute angle when drawing a line from the contour point to the waypoint (x _m , y _m ) in a coordinate system where (x _m , y _m ) represents the middle point. The distance b between the current waypoint and the point of the contour line (k, l) is defined as:

(11)

The values for the non-normalized weighting function are obtained by the maximum of possible distances to points of the contour line with a specified angle:

(12)

where φ(k, l) is the absolute angle between the contour point C(k, l) and the actual waypoint (x _m , y _m ).

In addition, it is checked if the direct line of the waypoint to the contour line points crosses a wall. The contour line points that cross a wall are not considered in the computation of the weighting function, since directions to points behind a wall should not be favored.

Finally, the weighting function is normalized:

(13)

In our simulation, we used discrete values for angle α. The angle bin size was 5° and we had 72 different values for computing the weighting function. These values seemed to be sufficient for obtaining a smooth weighting function.

From the angular PDF in Figure 5, one can see that angles in the direction to floors are favored and angles showing toward walls receive a lower weight. This reflects the pedestrian behavior: for a walking person it is more probable to walk through doors, large rooms, and floors than to walk directly to the walls. To adapt the histogram to the speed of the pedestrian, the following equation is applied:

(14)

where S is the step length of the particle. The motivation for power-relationship is that the weight update in a PF is multiplicative over time steps. Since we want the weighting above to take into account only the traveled heading, we need to normalize the weighting to a certain distance traveled. Otherwise, particles traveling a given distance in a larger number of shorter steps would be weighted more often than a particle traveling the distance in fewer steps.

In the case of really crossing a wall, the weight is set to a very small value. For the case when almost all of the particles cross a wall--this might happen very rarely--no weighting is applied, because we suspect an erroneous event such depletion and will count on the particle cloud to spread again and be constrained correctly by walls in the sequel.