6. Preprocessing sublayer 101
6.2. Map-based outlier handling
This approach can easily be extended to incorporate multiple unbiased pose sources by estimatingcˆb(s)for each of these sources. In general, we assume that the majority of sources are unbiased. We can easily determine whether a source is biased by observing all pose sources over a certain period of time and performing a simple version of random sample consensus.
6.2. Map-based outlier handling 109 question is how do we know where the vehicle should be driving. It turns out that we can gain information about this in multiple ways. First, in an automated vehicle we might be able to leverage the planned trajectory of the vehicle. Secondly, in automated platooning or similar applications the vehicle is often given a trajectory that it should follow. Thirdly, it is usually safe to assume that the vehicle is driving on a road. In the following we focus on the latter possibility. In the evaluations in Section 7.4.2 we show that this assumption is broadly satisfied. We employ maps with a precise road geometry as presented in Section 3.3.2. Specifically, we exploit the concept of center lines. They give us the geometric middle of each lane, which is close to where most vehicles are driving.
With this information in hand, our approach compares each global pose estimate to the center lines of a DLM and scales its information matrix accordingly. Our intuition is that a pose estimate is more likely corrupted as that the vehicle is driving far off the road. In this sense, we exploit the map information to judge the global pose estimates.
If we have detected a pose estimate as an outlier, we prefer to scale its information matrix to decrease its influence in the fusion over completely rejecting the outlier. This has two reasons. First, the decision to reject a pose estimate requires a hard criterion of what we consider an outlier. It is tough to design such a criterion with a low probability of false alarm. In contrast, our method of scaling provides a smooth separation between inliers and outliers. How much this information comes into play in the core estimator depends on the uncertainties of all other pose estimates. We see this approach therefore in the same line of thought as robust cost functions (cf. Section 3.4.3) which downscale the error terms instead of rejecting too high values. A similar effect is achieved when upscaling the information matrices.
Secondly, we note that our approach is a heuristic that judges independently of knowl-edge of the sensors’ and algorithms’ working principles and can falsely detect outliers.
Imagine the case of an emergency maneuver where the vehicle is forced to leave the road and come to a stop on the side of it. In this case, all of the pose sources will hint at this fact. If we were to reject all pose estimates because they are sufficiently far away from the road, then we have trouble estimating the vehicle’s pose. In contrast, we pro-pose to scale the uncertainties of this information such that we can compute the fused pose as being off the road and as being uncertain, which is a favorable behavior.
Our key idea consists of choosing an appropriate scaling function, computing the
µ0,Λ0 δ0
δ1
µ1,Λ1
(a) Two pose estimates are given of which the orange one is close and the red one is far away from the road. Initially, they both have the same covariance matrix.
µ0,Λ0 δ0
δ1
µ1,Λscal1
(b) After applying our scaling, the covari-ance matrix of the red pose estimate is scaled up. This reflects our semantic knowledge that we generally expect ve-hicles to drive on the road.
Figure 6.4.: Key idea of the map-based outlier handling: inappropriately small covari-ance matrices of pose estimates far away from the center line are scaled up.
distance of a pose estimate to the center line that it likely belongs to, and scaling the es-timate’s information matrix accordingly. Figure 6.4 illustrates this idea. In the following we detail the steps behind it.
6.2.1. Scaling the information matrix of a pose estimate
First of all, it is important to compute the distance of the pose estimate to the correspond-ing center line of the road. As detailed in Section 3.3.2, each segment of a DLM has one or more lanes. Each lane has a center line which is modeled as a polyline. Thus, for a given pose estimate we examine all center lines in its close vicinity to find the one that the pose estimate most likely belongs to. Once we identify this center line we compute the minimal lateral, longitudinal, and heading distance to it by orthogonal projection.
Figure 6.5 illustrates a pose estimate (blue), the corresponding center line (green), and the orthogonal projection on it. Note that we take heading difference as the difference between the pose estimate’s heading and the direction of the center line (by also taking into account the known driving direction). Usually, this comparison with a map allows
6.2. Map-based outlier handling 111
. . .
. ..
. . .
...
δ
Figure 6.5.: Computing the distance δ (dashed) of a pose estimate (blue) to the center line. The center line is depicted for both driving directions, indicated by arrow heads. The green part of the center line is the lane piece that has the shortest distance to the pose estimate. While lane pieces of the gray center line are closer to the pose estimate, the driving direction does not match and therefore the black center line is chosen.
us to get good information about the lateral deviation of the pose estimate to the center line, but the longitudinal and heading distances are less precise (see the discussion of Wijesoma et al. (2006) for the observability of path constrained vehicle localization).
We consider this when designing our scaling functions by choosing more conservative parameters for the longitudinal and heading distances. Overall, the lateral localization error is the most interesting error component and therefore a precise determination of the lateral distance is of foremost interest.
Consider the i-th pose estimate with its information matrix Λi. Let the minimal longitudinal, lateral, and heading distances of the pose estimate to the center line beδx, δy, and δθ. For now we assume that a scaling matrix S exists such that its Cholesky decomposition is
S =
sx(δx) 0 0 0 sy(δy) 0 0 0 sθ(δθ)
=LSL>S, (6.8)
wheresx,sy, andsθare scaling functions that depend upon the respective distances. We propose to scaleΛisuch that
Λscali =LSΛiL>S. (6.9) This slightly complicated notation allows us to express that all entries ofΛiare scaled.
The remaining question is how to definesx,sy, andsθ.
−3 −2 −1 0 1 2 3 0
0.5 1 1.5
distance to centerline δ [m]
scaling function s(δ)
Figure 6.6.: Scaling function as defined by (6.10) for different distances to the center line. The parameters areλ1 = 1.0,λ2 = 1.0,smin = 0.1, andsmax= 1.0.
6.2.2. Scaling functions
Different choices are possible for the scaling functions. Similar to robust cost functions, the optimal choice depends upon the concrete situation. The idea of robust cost func-tions is to limit the influence of outliers by reducing their weight in the optimization process. To this end, they scale large error terms less than the default quadratical scal-ing. See Section 3.4.3 for more information. Our scaling functions are similar in that they reduce the influence of potential outliers by increasing their covariance.
We present one parametrized scaling function that works well in our context. Drop-ping the indices on the scaling functionss and on the distancesδfor notational clarity we define
s(δ) =
smin exp
λ1−|δ|
λ2
≤smin
smax exp
λ1−|δ|
λ2
≥smax
exp
λ1−|δ|
λ2
else,
(6.10)
with parameters λ1, λ2, smin, and smax. This seemingly complex definition of s is quickly untangled when plotting the corresponding function. Figure 6.6 shows this function for a certain parameter set. We see that smin defines a minimum value below which the scaling does not drop. If we wanted to reject extremely improbable pose esti-mates, then we could set this value to zero. However, we will generally prefer to set this to a minimum value above zero to respect the probability of false alarm. Similarly,smax
defines the maximum value of the scaling. It will often make sense to limit this to1as the scaling would otherwise imply that the pose estimate contains more information than
6.3. Cross-correlated errors between pose sources 113