Improvements for the mixed-integer UAV demos

This fixes one important bug in the UAV demos from the ICRA 2015 paper, in which we were computing an inaccurate measure of the snap of the trajectory. The bug ended up being insignificant, and the resulting trajectories are nearly the same before and after the fix, but it's still good to take care of. 

I've also fixed a few of the numerical issues that were making it hard to solve for 5th-degree polynomial trajectories, so we can now reliably compute the smooth minimum-snap trajectories for all three sample environments (office, forest, and simple forest). That means that we can also run the full TVLQR-stabilized simulations of the quadrotor on all of those environments as well. 


good catch.  :)