Implementation based on the following two papers:
K. Garg and D. Panagou, “A Robust Coordination Protocol for Safe Multi-Agent Motion Planning,” 2018 AIAA Guidance, Navigation, and Control Conference, 2018.
D. Panagou, “A Distributed Feedback Motion Planning Protocol for Multiple Unicycle Agents of Different Classes,” IEEE Transactions on Automatic Control, vol. 62, no. 3, pp. 1178–1193, 2017.