function [a, currError] = lmicp_our_3d(model, data, a0, huber)
    maxdist = 10000;
    thresh = 0.0135; 
    a = a0;
    errors = [];
    
    % repeat step
    while (1)
        
        %transform the data using a0
        t = a(4:6)';
        r = eul2rotm(a(1:3));
        transData = (r*data' + repmat(t,1,length(data)))';

        %find correspondences: take closest point as correspondence - multiple
        %points can be mapped to same point, closest points outside of distance 
        %threshold maxdist are not mapped 

        [k,d] = dsearchn(model, transData);
        wi = double(d < maxdist);
        m = model(k, :);
        
        e_a = zeros(length(k), 1);
        J = zeros(length(k), length(a0));
        
        lambda = 0.001;
        theta = thresh.* ones(length(k), 1);
        
        while (lambda < 10^9)
        
            e_a = double(sqrt(wi).*(vecnorm(m' - (eul2rotm([a(1) a(2) a(3)])*data' + repmat([a(4) a(5) a(6)]',1,length(data))))'));
            
            if (huber)
                e_a = (((sqrt(e_a) < theta) .* (e_a)) + ((sqrt(e_a) >= theta) .* (2.*theta.*sqrt(e_a) - theta.^2)));
            end
            
            a_old = a;
            
            for j = 1:length(a0)
                da = 1e-3*ones(1, length(a0));
                da(j) = da(j)*(1 + abs(a(j)));
                a(j) = a(j) + da(j);
                
                %recalculate correspondences
                t = a(4:6)';
                r = eul2rotm(a(1:3));
                tD = (r*data' + repmat(t,1,length(data)))';
                [k,d] = dsearchn(model, tD);
                wi = double(d < maxdist);
                m = model(k, :);
                
                e_a1 = double(sqrt(wi).*(vecnorm(m' - (eul2rotm([a(1) a(2) a(3)])*data' + repmat([a(4) a(5) a(6)]',1,length(data))))'));
                
                if (huber)
                    e_a1 = (((sqrt(e_a1) < theta) .* (e_a1)) + ((sqrt(e_a1) >= theta) .* (2.*theta.*sqrt(e_a1) - theta.^2)));
                end
                
                J(:,j) = (e_a1 - e_a)/da(j);
                
                a = a_old;
            end
        
            ak = a - ((J'*J + lambda*eye(length(a0)))\(J'*e_a))';

            e_ak = double(sqrt(wi).*(vecnorm(m' - (eul2rotm([ak(1) ak(2) ak(3)])*data' + repmat([ak(4) ak(5) ak(6)]',1,length(data))))'));

            if (huber)
                e_ak = (((sqrt(e_ak) < theta) .* (e_ak)) + ((sqrt(e_ak) >= theta) .* (2.*theta.*sqrt(e_ak) - theta.^2)));
            end
            
            prevError = norm(e_a);
            currError = norm(e_ak);

            if (currError >= prevError)
                lambda = lambda * 3;
            else
                lambda = lambda / 3;
                a = ak;
                if (prevError - currError < 1e-3)
                    break;
                end
            end
            
        end

        if (a - ak < ones(1, length(a))*1e-3)
            break;
        end
        
     end 
   
end

