Add updated gains and fixed yaw bug