Trajectory Tracking Control and State Estimation for Highly Automated Vehicles: A System Framework Implemented on UShift II

Trajectory tracking control and state estimation are core functionalities of highly automated vehicles and must operate reliably under strict real-time constraints as well as in the presence of model uncertainties and limited sensor availability. In particular, low-speed maneuvers pose additional challenges due to weakly excited vehicle dynamics and system delays. This paper presents an integrated, real-time capable framework for trajectory tracking control and state estimation, developed within the UShift II research project and implemented on the highly automated vehicle platform. The framework combines nonlinear model predictive control (NMPC) for trajectory tracking with extended Kalman filter based state estimation within a modular system architecture. The NMPC is based on a vehicle model designed for low-speed automated driving maneuvers and explicitly accounts for actuator constraints. Trajectories are tracked based on local planned reference trajectories while ensuring smooth and physically feasible control inputs for underlying control. For state estimation, an extended Kalman filter is employed to fuse measurements from GNSS, inertial sensors, steering angle sensors, and wheel-speed-based odometry, providing consistent estimates of the vehicle states required by the model predictive controller. Particular emphasis is placed on robustness and computational efficiency in order to meet the real-time execution requirements on the target hardware. The proposed framework has been implemented and evaluated within the UShift II project. Experimental results show the real-time capability and reliable performance of the system in selected scenarios. The presented framework shows how model-based control and estimation methods can be systematically integrated into a practical and real-time capable overall system for highly automated vehicles.