Try and change the max_corrections to 18 and angular_max to 5. If this doesn't help, I'm afraid I'm also stuck on this and it turns out that the navigate function and the send_commands function are the culprits. I've used threading for send_commands which improved timings slightly. Using threading for navigations is basically useless