Added a number of tests to SimpleMultiRotoCopterTest. Issues remain with
things like pitching/rolling and altitude hold.