I tightened up the PID gains by increasing the P term until I started getting some oscillations, then backed them down. First flights in full navigation mode - the red line.
Sorry Tom, I didn't try to break your altitude record. Everything is working very well!
I did bust the flight line but there was nobody else around! I walked out onto the field to do the takeoff on the grass so I could establish the home position away from the line. I just underestimated 70 meters by a little!
I attached the *.KML files also.