Addressing Behavior Model Inaccuracies for Safe Motion Control in Uncertain Dynamic Environments

Uncertainties in the environment and behavior model inaccuracies compromise the state estimation of a dynamic obstacle and its trajectory predictions, introducing biases in estimation and shifts in predictive distributions. In this letter, we propose a novel algorithm <monospace xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">SIED-MPC</monospace>, which synergistically integrates Simultaneous State and Input Estimation (SSIE) and Distributionally Robust Model Predictive Control (DR-MPC) using model confidence evaluation. The SSIE process produces unbiased state estimates and optimal <italic xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">input gap</i> estimates to assess the confidence of the behavior model, defining the ambiguity radius for DR-MPC to handle predictive distribution shifts. The proposed method produces safe inputs with an adequate level of conservatism. Our algorithm demonstrated a reduced collision rate and computation time in autonomous driving simulations through improved state estimation.