Energy preserving formation flight control