Switched to nested PID loops for control, results are promising so far.