Finished the SBA* MEAQR planner, working on getting the ARE solver to work on the quadrotor system.