A Nonlinear Model Predictive Control Algorithm For An Unmanned Ground Vehicle On Variable Terrain