Two-Step System Identification And Primitive-Based Motion Planning For Control Of Small Unmanned Aerial Vehicles