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

Trajectory tracking control and vehicle 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. This paper presents an integrated, real-time capable framework for trajectory tracking control and vehicle 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 an extended Kalman filter (EKF) for multi-sensor 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. The EKF fuses measurements from global navigation satellite system (GNSS), inertial sensors, and wheel-speed-based odometry, providing consistent estimates of the vehicle states under varying sensor availability. Particular emphasis is placed on robustness and computational efficiency in order to meet the real-time execution requirements on the target hardware. The complete framework is implemented on automotive-grade real-time hardware and validated on the U-Shift II vehicle platform. Experimental results demonstrate reliable localization performance, smooth and accurate trajectory tracking, and deterministic real-time execution, confirming the suitability of the proposed approach for practical low-speed automated driving applications.