Hierarchical Nonlinear Moving Horizon Estimation of Vehicle Lateral Speed and Road Friction Coefficient

Abstract

In this paper, we address nonlinear moving horizon estimation (NMHE) of vehicle lateral speed, as well as the road friction coefficient, using measured signals from sensors common to modern series-production automobiles. Due to nonlinear vehicle dynamics, a standard nonlinear moving horizon formulation leads to non-convex optimization problems, and numerical optimization algorithms can be trapped in undesirable local minima, leading to incorrect solutions. To address the challenge of non-convex cost functions, we propose an estimator with a two-level hierarchy. At the high level, a grid search combined with numerical optimization aims to find reference estimates that are sufficiently close to the global optimum. The reference estimates are refined at the low level leading to high-precision solutions. Our algorithm ensures that the estimates converge to the true values for the nominal model without the need for accurate initialization. Our design is tested in simulation with both the nominal model as well as a high-fidelity model of Autonomoose, the self-driving car of the University of Waterloo.