GRAVITY_TURN

The issue seems to be occuring earlier than that. Tgo is around 59 seconds before suddently going crazy, with the fr term blowing up too. Very strange behaviour as the plot for Tgo is linear up until this point.

My implementation is almost identical to yours, but in MATLAB. I'm getting strange results - I believe it converges as Tgo is consistenly reducing, but the fr term begins to go crazy and ends up being >1, clearly impossible! Did you ever experience behaviour like this? It should be near zero as my velocity vector is near the local horizontal and the vehicle isn't far off the target insertion altitude, but instead it begins to rise hence adding to the radial component.