Loiter yaw problem

@Leonardthall gave me a very useful insight here

Plotting the roll and pitch we can see that the roll and pitch are non zero when the copter is holding position. (But they are zero on arming and takeoff)

I’ll make new tests trying to remove that offset, thanks!