The Regulated Pure Pursuit (RPP) controller — a widely used trajectory-tracking algorithm in ROS 2's Nav2 stack — selects its lookahead distance using a simple velocity-based heuristic. This assumes the robot's pose is known perfectly, which breaks down as localization uncertainty (from noisy odometry, IMU drift, and sparse position corrections) grows over time.
I designed an SNMPC-RPP controller that replaces only the lookahead-selection module of RPP, leaving the rest of its architecture (velocity regulation, collision checking) untouched. At each 5 Hz control cycle, it predicts lateral and heading tracking error forward in time (3 time steps in future), propagates the robot's pose-error covariance through that prediction, and solves a constrained optimization (cost function + chance constraints + box constraints on lookahead) using the derivative-free COBYLA method to choose the optimal lookahead distance.
Validated in ROS 2 / Gazebo across three operating speeds (0.5, 1.0, and 1.5 m/s) on a rectangular reference circuit, the controller showed its biggest advantage exactly where it matters most: at higher speed, where uncertainty accumulates fastest, SNMPC-RPP reduced cross-track MAE by over 40% and RMSE by over 50% relative to baseline RPP, while keeping the rest of the robot's motion just as smooth.