Active safety systems must be used to manipulate the dynamics of autonomous vehicles to ensure safety. To this end, accurate vehicle information, such as the longitudinal and lateral velocities, is crucial. Measuring these states, however, can be expensive, and the measurements can be polluted by noise. The available solutions often resort to Bayesian filters, such as the Kalman filter, but can be vulnerable and erroneous when the underlying assumptions do not hold. With its clear merits in handling nonlinearities and uncertainties, moving horizon estimation (MHE) can potentially solve the problem and is thus studied for vehicle state estimation. This paper designs an unscented Kalman filter, standard MHE, modified MHE, and recursive least squares MHE to estimate critical vehicle states, respectively. All the estimators are formulated based upon a highly nonlinear vehicle model that is shown to be locally observable. The convergence rate, accuracy, and robustness of the four estimation algorithms are comprehensively characterized and compared under three different driving maneuvres. For MHE-based algorithms, the effects of horizon length and optimization techniques on the computational efficiency and accuracy are also investigated.
Read full abstract