Improved control stability; added initial conditions from live test; added delay line to emulate GPS latency in SITL script