Thrust controller implemented. Does not match paper, but hopefully is just some bug waiting to be destroyed.