function [params] = least_squares_adjustment(l, initCalibParams, max_iter, tol)
    % Initial guess for parameters [s_x, s_y, s_z, b_x, b_y, b_z, theta_{yz}, theta_{zx}, theta_{zy}, a1, a2, a3]
% Define initial parameters
s = [initCalibParams(4); initCalibParams(5); initCalibParams(6)]; % Scale factors
theta = [initCalibParams(1); initCalibParams(2); initCalibParams(3)]; % Non-orthogonalities
b = [initCalibParams(7); initCalibParams(8); initCalibParams(9)]; % Biases

% Iterative least-squares adjustment
n_meas = length(l); % Number of measurement sets

for iter = 1:max_iter

    if iter == 1
        C_x = eye(9) * 1; % initialisation of C_x matrix;     % xy        xz        yz
        x_old = [b(1); b(2); b(3); s(1); s(2); s(3); theta(1); theta(2); theta(3)];
    else
        C_x = inv(N);
        x_old = x_new;
    end

    b = x_old(1:3); s = x_old(4:6); theta = x_old(7:9);

    g_mat = computeG(l, b, s, theta); % n_meas x 3 matrix
    
    % Design matrices (like Jacobians?):
    A = computeA(l, b, s, theta);
    B = computeB(g_mat, b, s, theta); % n_meas x 9 matrix

    w = zeros(n_meas, 1); % misclosure vector
    for i_row = 1:n_meas
        w(i_row) = f_g(l(i_row, :), b, s, theta); % fill misclosure vector
    end

    C_l = computeC_l(l);

    u = A' * inv(B * C_l * B') * w;

    N = A' * inv(B * C_l * B') * A + inv(C_x);
    
    % l_vec = reshape(l',[],1); % l as column vector

    delta = -inv(N) * u;

    x_new = x_old + delta;

    w_new = zeros(n_meas, 1); % misclosure vector
    for i_row = 1:n_meas
        w_new(i_row) = f_g(l(i_row, :), x_new(1:3), x_new(4:6), x_new(7:9)); % fill misclosure vector
    end

    r = w_new - w; % make residual vector

    % Check for convergence
    if norm(r) < tol
        break;
    end
end
          % thetas    scale factors      biases
params = [x_new(7:9)' x_new(4:6)' x_new(1:3)'];