Optimal Motion Planning In Gps-Denied Environments Using Nonlinear Model Predictive Horizon