Title: Optimal Control Algorithm Based on an Initial Costate Search