^{1}

^{1}

^{1}

^{2}

^{1}

^{2}

Robust object tracking and maneuver estimation methods play significant role in the design of advanced driver assistant systems and self-driving cars. As an input to situation understanding and awareness, the performance of such algorithms influences the overall effectiveness of motion planning and plays high role in safety. The paper examines the suitability of different probabilistic state estimation methods, namely, the Extended Kalman Filter (EKF) and the more general Particle Filter (PF) with the addition of the Interacting Multiple Model (IMM) approach. These algorithms are not capable of predicting motion for long term in road traffic conditions, though their robustness and model classification capability are essential for the overall system. The performance is evaluated in road traffic scenarios where the tracked object imitates the motion characteristics of a road vehicle and is observed from a stationary sensor. The measurements are generated according to standard automotive radar models. The analysis conducted along two aspects emphasizes the different performance and scaling properties of the examined state estimation algorithms. The presented evaluation framework serves as a customizable method to test and develop advanced autonomous functions.

Nowadays, highly automated driver assistance systems and autonomous vehicles are in focus of attention and pose many different challenges that need to be solved. The architecture of the motion planning has multiple layers from route planning, through behavioral and tactical planning to local control [

A maneuvering vehicle can be effectively tracked by using a multimodel state estimator [

The paper is organized as follows. Section

In probabilistic estimation the state is considered as a random variable and its distribution is approximated. The approximation can be performed with various precision; however, the underlying mathematical structure, the Bayes theorem, can give a unified description of the different methods. The general problem is to estimate the state

Beside the fact that the Kalman filter is the optimal minimum mean squared error filter for linear-Gaussian systems, its usage is limited by the presuppositions of the model. Most real-life systems show nonlinearities and the Gaussian presumption cannot always be held. An approximate method is the Extended Kalman Filter (EKF) which, to handle nonlinearities, linearizes the system equations by first order Taylor expansion around the current state.

A general discrete time state space system model with additive noise has the form

Initialize variables:

A more general approach to handle nonlinearities or even non-Gaussian PDFs is to use the particle filter (PF), which is an umbrella term. It corresponds to a family of Monte Carlo sampling based sequential algorithms. It is a numerical method that approximates a function, in our context the posterior PDF by particles. The particles are weighted samples, drawn from the distribution; hence, the higher the particle number, the more accurate the approximation. The PDF of a state vector

The particle filter can be considered a Bayesian-type filter where the PDFs are numerically handled. At the predict stage samples are drawn from a pool, called the importance density. The choice of the importance density is crucial. An obvious choice is to use the state transitional density whose method is referred to as the bootstrap particle filter. The update stage involves the computation of the likelihoods for every particle which is realized by the evaluation of the PDF associated with the measurement model. The normed likelihood values give the updated particle weights and the point estimate for the current timestep can be obtained by the weighted sum of the particle states. The main steps of the bootstrap particle filter are summarized in Algorithm

Initialize particles:

Draw

The purpose of multimodel filtering is twofold. On one hand it helps giving a more precise state estimation if the correct system model is used and on the other hand it provides information on the actual mode of operation. To be effective at both at the same time, as will be seen, can be a contradictory requirement.

System model is the collective term for the process or motion model and the measurement or sensor model. The mode of operation or system mode refers to a certain kind of behavior that we identify, e.g., accelerating or turning. A given process model, if general enough, can account for multiple modes of operation. Mode change is the switching between two modes of operation. The mode history is the time sequence of the actual system modes. Mode uncertainty refers to the circumstance that we are not aware of the actual system mode.

One way of dealing with model uncertainties in an estimation problem is to use a number of plausible system model, compare their performances, and choose one result or combine several. The output of a filter associated with a certain mode is referred as the mode-conditioned estimate. There are numerous multiple model algorithms at hand [

The interacting multiple model method, proposed in [

The structure of the IMM estimator is depicted in Figure

Structure of the interacting multiple model algorithm.

Originally the IMM algorithm was working with Kalman filters [

Every filter has a unique Gaussian input parametrized by

The likelihood of a particle is

A point estimate is computed from the particles by a weighted sum:

The particle filter does not produce residual covariance like the EKF. To be integrated in the IMM framework, a model likelihood has to be derived. The estimated measurement is

If particle degeneration is an issue, one will perform some form of resampling after the update step [

To evaluate the filter performances a simulated environment is used. The tracked object is assumed to be a road vehicle with appropriate motion characteristics. The maneuvering vehicle is observed from a stationary sensor at the origin. The measurements are generated according to standard automotive radar models.

In this study the considered modes of operation are moving with constant velocity and turning with constant speed along a circle or a clothoid segment.

The CV and CT model both use the state vector

It would also be possible to use a simple CV motion model with states

The noise acting on the system is modeled as an additive Gaussian term. For the CV and CT model it is a white noise linear acceleration and white noise linear and angular acceleration, respectively.

The measurement vector comes from simulated radar observations with components

With the help of (

As it was pointed out in [

The vehicle is initialized with state vector

The vehicle moves along a road segment and its trajectory is segmented into straight lines and circular or clothoid arcs. Two trajectories were designed with different characteristics to help emphasize the nature of the filters. Trajectory1 (T1), as outlined in Table

Motion of the tracked object in the simulation: Trajectory1.

Vehicle motion | Turn rate | Duration [s] | |
---|---|---|---|

#1 | CV | — | 15 |

#2 | CT | | 15 |

#3 | CV | — | 20 |

#4 | Clothoid | | 40 |

#5 | CV | — | 10 |

Motion of the tracked object in the simulation: Trajectory2.

Vehicle motion | Turn rate | Duration [s] | |
---|---|---|---|

#1 | CV | — | 25 |

#2 | CT | | 15 |

#3 | CV | — | 30 |

#4 | Clothoid | | 20 |

#5 | CV | — | 10 |

The speed is constant throughout the whole simulation. The duration of the simulation is

The filter performances were examined along two parameters, the mode transition probability matrix

The mode transition matrix is adopted as

Parameter set of the simulation. 50

The performance of the filters was measured from two aspects of interest. First, the position error is calculated as the Euclidean distance between the real and the estimated coordinates. Second, as the measure of maneuver detection, the estimated mode is regarded and two measures are defined: from which the first is the ratio of time steps when the correct mode was estimated versus the total time; and the second method is area based and indicates the robustness of the output. The area under the estimated mode probability curve is divided by the length of time that mode was active.

A ratio to measure the effect of relative motion to measurement uncertainty on the position is defined in [

Numerical complexity of the compared algorithms has the following order. The single model EKF has the lowest requirements. The state space is of low dimension, the involved matrix operations can be computed directly, without sophisticated numerical methods. The IMM-EKF needs to run a filter for every considered mode of operation. In addition, the IMM algorithm poses overhead mainly in terms of creating Gaussian mixtures and computing covariance matrices. The IMM-PF is the most computationally expensive. The most demanding steps are sampling, evaluation of the likelihood function, and sample covariance computation. The particle number was held constant 1000 which can be considered as a mediocre quantity. The current implementation used parallelization wherever it was possible at the particle level.

Simulations were conducted for both trajectories. The IMM is implemented with EKF and PF and for comparison single model CV/CT EKF were used. 100 Monte Carlo runs were averaged out for every parameter setting meaning 75000 runs in total. Figures

Contour plot of MC averaged position RMSE with respect to filter hyperparameters. Case: EKF – T1.

Top: MC averaged correctness of estimated mode of operation versus parameter

Figures

MC averaged result of the estimated mode probabilities (left) and the position error (right). The vertical dotted lines indicate mode changings. The dashed line is the actual angular velocity of the tracked vehicle. Case: EKF – T1.

Result of a single MC run for the IMM-EKF (left) and CV/CT EKF. Case: EKF – T1.

Contour plot of MC averaged position RMSE with respect to filter hyperparameters. Case: EKF – T2.

Top: MC averaged correctness of estimated mode of operation versus parameter

MC averaged result of the estimated mode probabilities (left) and the position error (right). The vertical dotted lines indicate mode changings. The dashed line is the actual angular velocity of the tracked vehicle. Case: EKF – T2.

Result of a single MC run for the IMM-EKF (left) and CV/CT EKF. Case: EKF – T2.

Contour plot of MC averaged position RMSE with respect to filter hyperparameters. Case: PF - T1.

Top: MC averaged correctness of estimated mode of operation versus parameter

Contour plot of MC averaged position RMSE with respect to filter hyperparameters. Case: PF – T2.

Top: MC averaged correctness of estimated mode of operation versus parameter

The estimated mode at each timestep is acquired by selecting the most probable mode:

A general property of the IMM filters in all cases is that the position RMSE is lower for greater

The position RMSE of IMM-EKF exhibits negligible dependence on

The single model EKF with constant turn rate can be considered as an option in certain situations. If the tracked vehicle performs a great amount of maneuvering, the EKF can outperform the IMM in terms of position RMSE at the cost of providing no mode estimates. The EKF, in scenario T1, estimates the state significantly better than the IMM; on the contrary in T2 the IMM performs comparably while the EKF is worse. The particle filter implementations of the IMM have different characteristics. The position RMSE shows dependence on both hyperparameters. The mode of operation can be estimated with greater efficiency for certain parameters compared to the IMM-EKF. In scenario T1 the dependence of the RMSE on

The continuously increasing level of automation in road traffic and vehicles poses new requirements towards data processing algorithms [

It is essential to fine tune or, during operation, reconfigure the filters to help adaption to the actual conditions. The presented work can serve as a guideline creating a customized framework to analyze and help the development of multiple-model state and maneuver estimating algorithms for advanced vehicular functions. In the knowledge of the scaling properties and behavior of the algorithms parameter ranges for the optimal performance can be identified.

Dimension of state space

Dimension of measurement space

PDF of state vector

State vector at time

Process noise vector

Measurement vector at time

Measurement noise vector

State transition density

Likelihood function of

State transition matrix

Measurement matrix

Nonlinear state transition function

Nonlinear measurement function

Estimated state conditioned on mode

Estimated covariance conditioned on mode

Likelihood of mode

Residual covariance of mode

Mode transition probability matrix

Intensity of process noise

Process noise covariance

Measurement noise covariance

Process noise input matrix

Probability of mode

Mixing matrix.

No data were used to support this study. The study used MATLAB simulation to evaluate the methods; the source can be requested from the corresponding author.

The authors declare that there are no conflicts of interest regarding the publication of this paper.

The research reported in this paper was supported by the Higher Education Excellence Program of the Ministry of Human Capacities in the frame of Artificial Intelligence research area of Budapest University of Technology and Economics (BME FIKPMI/FM). The project has also been supported by the European Union, cofinanced by the European Social Fund (EFOP-3.6.3-VEKOP-16-2017-00001: talent management in autonomous vehicle control technologies).