Метод Рунге-Кутта с автоматическим выбором шага

Здравствуйте, была задача написать программу, которая будет считать по методу Рунге-Кутта с автоматическим выбором шага с заданной погрешностью. (Выбираю xi из отрезка, считаю сначала с шагом h, потом с шагом h/2, проверяю выполнение условия, и либо снова уменьшаю шаг, либо перехожу на следующий x). Программа работает с ошибкой - значения функции после первого x выводятся неправильные и программа считает только 3 значения, почему-то последний x не выводит. Помогите, пожалуйста, найти ошибки

function f = f1(x,z)
    f=z^2*cos(x)
endfunction
yi = -1;
xi = 0;
a = 0;
b = 1.2;
eps = 0.0001;
h0 = 0.4;
n = (b-a)/h0;
for xi = a:h0:b
    while 1
       h = h0;
        k1 = h*f1(xi,yi);
        k2 = h*f1(xi+h/2,yi+k1/2);
        k3 = h*f1(xi+h/2,yi+k2/2);
        k4 = h*f1(xi+h,yi+k3);
        y_h = yi + (k1 + 2*k2 + 2*k3 + k4)/6;
        
        h = h/2;
        
        k1 = h*f1(xi,yi);
        k2 = h*f1(xi+h/2,yi+k1/2);
        k3 = h*f1(xi+h/2,yi+k2/2);
        k4 = h*f1(xi+h,yi+k3);
        y_hh = yi + (k1 + 2*k2 + 2*k3 + k4)/6; 
        if abs(y_hh-y_h) > eps then 
            h = h/2;
        else 
            break;
        end
        end 
disp(y_hh)
yi = y_hh;
h = h0  
end