Improve initial trajectory