function [a, currError] = lmicp_our(model, data, a0, huber)
    maxdist = 10000;
    thresh = 0.025; %0.0135; 
    a = a0;
    
    % repeat step
    while (1)
        
        %transform the data using a0
        t = a(2:3)';
        r = [cos(a(1)) sin(a(1)); -sin(a(1)) cos(a(1))];
        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); %this matlab function does exactly what we need
        wi = double(d < maxdist);
        m = model(k, :);
        
        % preallocating matrices
        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) % One of the convergence criteria.. exit if lambda is large
        
            e_a = double(sqrt(wi).*(vecnorm(m' - ([cos(a(1)) sin(a(1)); -sin(a(1)) cos(a(1))]*data' + repmat([a(2) a(3)]',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;
            
            %Calculation of Jacobian based on backwards difference
            for j = 1:length(a0)
                da = 5e-3*ones(1, length(a0));
                da(j) = da(j)*(1 + abs(a(j)));
                a(j) = a(j) + da(j);
                
                %recalculate correspondences for more accurate solution
                t = a(2:3)';
                r = [cos(a(1)) sin(a(1)); -sin(a(1)) cos(a(1))];
                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' - ([cos(a(1)) sin(a(1)); -sin(a(1)) cos(a(1))]*data' + repmat([a(2) a(3)]',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
        
            % Calculate new set of parameters
            ak = a - ((J'*J + lambda*eye(length(a0)))\(J'*e_a))';

            % Calculate new objective function value
            e_ak = double(sqrt(wi).*(vecnorm(m' - ([cos(ak(1)) sin(ak(1)); -sin(ak(1)) cos(ak(1))]*data' + repmat([ak(2) ak(3)]',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) % We are diverging... increase lambda
                lambda = lambda * 3;
            else % error is getting smaller
                lambda = lambda / 3;
                a = ak;
                if (prevError - currError < 1e-3) %If the error got smaller by a very small amount, we can stop
                    break;
                end
            end
            
        end

        if (a - ak < ones(1, length(a))*1e-3) % ICP convergence criteria, change in parameters is close to zero
            break;
        end
        
     end 
   
end

