function [du K Kv Ku] = ilqr_iterate(x0, xf, u, Qf, Q, R, dt)
    % Se crea vector de acciones de contrl
    du = zeros(size(u));    
    % Simular en bucle abierto
    x = ilqr_sim(x0, u, dt); 
    % Obtener tamany de x e u
    N = length(x); Nx = length(x0); Nu = length(u(1,:));
    % Creación de matrices de ganancias K, Kv y Ku
    K = zeros(Nu, Nx, N-1); Kv = zeros(Nu, Nx, N-1); Ku = zeros(Nu, Nu, N-1);
    % Creación de matrices S y v
    S = zeros(Nx, Nx, N); v = zeros(N, Nx);
    % dx = x - x0
    dx = 0.000001*ones(size(x));  %0.001
    % Creación de Matrices A y B
    A = zeros(Nx, Nx, N-1); B = zeros(Nx, Nu, N-1);
    for i = 1:N-1
        % Linear Time Variant System LTV
        [A(:,:,i) B(:,:,i)] = ilqr_linearization_robot_arm(x(i,:), u(i,:), dt);
    end
    % Condicion final para las matrices S y v
    S(:,:,N) = Qf;  v(N,:) = (Qf*(x(N,:) - xf)')';
    % Bucle para calcular ecuaciones de Riccati    
    for k = N-1:-1:1 
%         dx(k,:) = x(k+1,:) - x(k,:);
        K(:,:,k) =  inv(B(:,:,k)' * S(:,:,k+1) * B(:,:,k) + R) * B(:,:,k)' * S(:,:,k+1) * A(:,:,k);
        Kv(:,:,k) = inv(B(:,:,k)' * S(:,:,k+1) * B(:,:,k) + R) * B(:,:,k)';
        Ku(:,:,k) = inv(B(:,:,k)' * S(:,:,k+1) * B(:,:,k) + R) * R;
        S(:,:,k) = A(:,:,k)' * S(:,:,k+1) * (A(:,:,k) - B(:,:,k) * K(:,:,k)) + Q;
        v(k,:) = ((A(:,:,k) - B(:,:,k) * K(:,:,k))' * v(k+1,:)' - K(:,:,k)' * R * u(k,:)' + Q * x(k,:)')';
        du(k,:) = (-1) * (K(:,:,k)* dx(k,:)' + Kv(:,:,k) * v(k+1,:)' + Ku(:,:,k) * u(k,:)')';
    end  

end

