Optimal Control Approach to Terrain Following Trajectory Generation