Aggressive driving with model predictive path integral control
iSAM2: Incremental smoothing and mapping using the Bayes tree