formation control in matlab