Added motor thrust optimization

Added a hill-climbing based optimization method to determine the
per-motor thrust (after trying least squares). Works Ok for linear
acceleration but is unstable (too much spin) when enabled for angular
acceleration. I tried many different combinations of parameters. Right
now I have the basic angular acceleration with physically-based linear
acceleration and it works pretty well.