In the previous chapter, we evaluated an AMM based behavior identification model running three different elemental filter motion models, each corresponding to a maneu- ver. The motion model corresponding to straight behavior constituted of Constant Velocity,
whereas the ones corresponding to left and right lane changes were based upon sinusoidal wave geometry with the maneuver length (𝐿) assumed as a parameter. Hence, the states assumed were[𝑥 𝑣𝑥 𝑦 𝑣𝑦
]′
. We defined the length of the maneuver as the longitudinal distance when starting from the origin lane and ending at the center of the target lane. This assumption aided in evaluating the filter parameters and understanding the intuition of filter tuning. For prolonged observation however, more information is required by the filters to identify the behavior correctly in the form of the maneuver initiation point (𝑥𝑚𝑖𝑝). 𝑥𝑚𝑖𝑝 is
defined as the longitudinal position at which the traffic participant has accepted the gap and initiated the maneuver, as shown in Fig. 6.1. This was implicitly assumed as 𝑥𝑚𝑖𝑝 = 0in the
previous chapter. For prolonged observations however, information about both 𝑥𝑚𝑖𝑝 and 𝐿
have to be obtained at run-time. In this section, two approaches are explored for achieving this requirement. In the first approach, 𝑥𝑚𝑖𝑝 is assumed as a parameter and 𝐿 is modeled as
a state. In the second approach, 𝑥𝑚𝑖𝑝 is modeled as a state in the motion model while 𝐿 is
assumed as a parameter. The following subsections describe the results of the approaches in detail.
6.1.1
Maneuver initiation point assumed as a parameter and maneu-
ver length as a state
Figure 6.2: Evolution of estimated maneuver length (green) with 𝑃0 = 100
The premise for this approach is the idea that if the maneuver initiation point can be obtained through a separate means (such as a curve fit, or lateral lane measurements), the maneuver length can be estimated online. Here, three motion models were considered, Constant Velocity for straight motion and sinusoidal waveforms for each of the left and right lane changes. The states for the lane change motion models were subsequently augmented as:
x=[𝑥 𝑣𝑥 𝑦 𝑣𝑦 𝐿
]′
(6.1) with motion equations as described in section 3.1.5.2.
In this case, initial values of the states 𝑥, 𝑣𝑥, 𝑦, and 𝑣𝑦 for the filter can be obtained
Figure 6.3: Evolution of estimated maneuver length (green) with 𝑃0 = 2500
tecture, with high level of initial confidence (i.e., very low initial estimate covariances P). However, 𝐿 cannot be measured directly and will have an uncertainty associated with it at the beginning of the maneuver as to the kind of maneuver that will be executed. Hence it has to be initiated with a nominal value of L, and with high initial covariance (𝑃0). If initial
covariance of L is set to a very low value, then it takes longer for the filter to converge to an approximate maneuver length. In Fig. 6.2, the measurements were generated with a maneu- ver length of 100 m, and the EKF initial state estimate was set to 150 m, with a covariance of 100 𝑚2, i.e. a standard deviation of 10 m. The convergence time for the maneuver length
estimate to settle within 5% band (less than 105 m) was 10.7 secs. With 2500 𝑚2 (50 m
standard deviation) initial covariance for the initial value of L = 150 m and process noise of 𝑄 = 10, it took less than 5 secs (Fig. 6.3).
For the initial length of the maneuver set as 100 m, process noise 𝑄 = 0.0025 and measurement noise 𝑅 = 0.0025, 𝑃0= diag
([
100 100 100 100 1𝑒5 ])
Figure 6.4: Probabilistic weights for maneuvers with AMM estimating maneuver length 𝐿 maneuver length of 60 m for a right lane change, the probabilistic weights of the AMM were as shown in Fig. 6.4. The evolution of the estimate of the maneuver length for the elemental filter running the right lane change motion model was as shown in Fig. 6.5, converging rather quickly. Not surprisingly, the evolution of the estimated 𝐿 for the elemental filter running the left lane change model deviates largely from the ground truth as shown in Fig. 6.6. Hence, one needs to be careful when combining the estimates as per the filter weights and should rather not be included in the combined estimates. Since the weights of the left filter fall to zero within a second, the influence of the maneuver length is only there at the initial stages of the maneuver.
Figure 6.5: Evolution of estimated maneuver length for right lane change elemental filter
6.1.2
Maneuver initiation point as a state and maneuver length as a
parameter
The premise for this approach is the idea that if the right set of candidate lane change behaviors, each with a different maneuver length, is assumed then it is possible to approx- imate the maneuver initiation point. Again, three elemental filters were considered for this case. First filter employed a constant longitudinal velocity and zero lateral velocity mo- tion model for the straight maneuver, the other two filters employed sinusoidal waveforms motion model with the states augmented as:
x= [
𝑥 𝑣𝑥 𝑦 𝑣𝑦 𝑥𝑚𝑖𝑝
]′
(6.2)