modified q_formation_control