The weighting process in the LPF that uses no motion models for weighting is based on binary decisions: if a particle crosses a wall its weight is set to zero otherwise it is set to one and weighted solely by the likelihood functions of the other sensors. In this article, the weighting functions for the LPF are based on new angular PDFs. Weighting with other sensors' likelihoods will still happen. The angular PDFs are derived from a map-based diffusion algorithm that can also be used as a movement model [27]. In this article, the diffusion algorithm taken from [14] is applied, which is extended for using maps with different degrees of accessibility and for handling floor-plans in three dimensions [27].
The principle of the computation of the 2D-diffusion matrix is described in Section 3.1. Section 3.2 describes the calculation of the new angular PDFs. In practice and in our implementation, the angular PDFs are pre-computed and stored to reduce the computational effort during position estimation.
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:
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:
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:
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:
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
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):
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:
and the diffusion values of at least one neighboring point (direct neighborhood) is greater than the threshold T:
then, the position (k, l) is part of the set of contour lines:
Walls are included in this computation, since for a point on the wall the following equation holds:
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:
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:
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:
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:
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.