optimized formation control