I want to know why this code doesn't work well..?

The formula was found in the paper, and the rest was designed to help this guidance work well, so I don't know why I can't follow the path well.. It's really frustrating and I want to know why and how to solve it.

function [rollCmd, velCmd] = Guidance(state)
d2r = pi/180;
east = state(1);     % n_Xav
north = state(2);    % n_Yav
psi = state(3) * d2r * 1000   % n_psi_av
velCmd = 50;
Kint = 0.5;
% 항로점
wp = [0,     0;
    2817, -2724;
    5130, -4213;
    4697, -4807;
    3539, -4337;
    2817, -2724];
persistent nw
if isempty(nw)
    nw = 1;
end
% 새로운 웨이포인트 갱신
Xi = wp(nw, 1);
Yi = wp(nw, 2);
Xf = wp(nw+1, 1);  
Yf = wp(nw+1, 2); 
N = size(wp, 1);

n_LOS = sqrt((Xf - east)^2 + (Yf - north)^2)  
n_psi_leg = atan2(Xf - Xi, Yf - Yi);  

n_psi_LOS = atan2(Xf - east, Yf - north);  
zeta = atan2(Yf - Yi, Xf - Xi);  
eta = n_psi_LOS - n_psi_leg; 
Op = n_LOS * (1 - Kint);
Lp = Op * cos(eta);
Xvir = Xf - Lp * cos(zeta)
Yvir = Yf - Lp * sin(zeta)
n_psi_vLOS = atan2(Xvir - east, Yvir - north);  
n_vLOS = sqrt((Xvir - east)^2 + (Yvir - north)^2);  
n_psi_CMD = n_psi_vLOS - psi
n_XTRK = abs(n_vLOS * sin(n_psi_LOS - n_psi_leg)) * sign(n_psi_vLOS - psi);
if n_LOS < 3
    if nw < 5
        nw = nw + 1;      
    end
end
rollCmd = (n_psi_CMD - psi)*0.5*d2r;

[Voss]

1. What is the intent of the Guidance function?
2. How is its behavior different than intended?
3. Are you able to provide some typical input to the Guidance function, or a typical Simulink model that uses it, so that someone can attempt to reproduce the problem (after answering #1 and #2 describing what the problem is)?
4. What is the paper you referred to?
5. How was that plot created?