function [GtG, Gty, w_B23, dB_rtp, dB_B23, B_synth] ...
    = do_slave_vector(t, r, theta, phi, Est, Ist, options, model, params, m_i, N_coeff, source, ...
    R_rtp, B, e_B, e_2, e_3, n, chi_t, psi_t, A, N_para, index_current, N_data_vec_all)

% $Rev: 6585 $
% $Date: 2014-03-01 23:02:33 +0000 (lø, 01 mar 2014) $

rad = pi/180;
N_data_vec = length(t);

for i_source = 1:length(params.dt_Euler);
    N_Euler(i_source) = length(model.t_break_Euler{i_source})-1;
end
N_Euler_cumsum = [0 cumsum(N_Euler)];

clear GtG Gty
GtG = zeros(N_para, N_para); Gty = zeros(N_para, 1);
w_B23 = zeros(N_data_vec, 3);
dB_rtp = zeros(N_data_vec, 3);
dB_B23 = zeros(N_data_vec, 3);
B_synth = zeros(N_data_vec, 3);

for i=1:options.N_Data_Frac:N_data_vec
    index = [i:min(i+options.N_Data_Frac-1, N_data_vec)]';
    [A_r, A_theta, A_phi] = create_G_3(t(index), r(index), theta(index), phi(index), Est(index), Ist(index), model, params);
    B_synth(index, 1) = A_r     * m_i(1:N_coeff);
    B_synth(index, 2) = A_theta * m_i(1:N_coeff);
    B_synth(index, 3) = A_phi   * m_i(1:N_coeff);
    
    % add the Euler angles
    A_Euler_a = []; A_Euler_b = []; A_Euler_c = [];
    A_Euler_tmp = zeros(length(index), max(N_Euler));
    B_rtp = zeros(length(index), 3);
    
    for i_source = 1:length(params.dt_Euler);
        if any(source(index) == i_source) % does this data piece contain data from data set # i_source ?
            ibreak = zeros( N_Euler(i_source)+1, 1);
            dB_dalpha = zeros(length(index), 3); dB_dbeta = zeros(length(index), 3); dB_dgamma = zeros(length(index), 3);
            for it = 1: N_Euler(i_source)
                ind = find(model.t_break_Euler{i_source}(it) <= t(index) & t(index) <= model.t_break_Euler{i_source}(it+1) & source(index) == i_source);
                if ~isempty(ind > 0); ibreak(it+1) = ind(end); else ibreak(it+1) = ibreak(it); end
            end
            ibreak(end) = length(index);
            for it = 1: N_Euler(i_source)
                % rotate B from VFM frame to NEC frame
                A_Euler_tmp(:,it) = [zeros(ibreak(it)-ibreak(1), 1); ones(ibreak(it+1)-ibreak(it), 1); zeros(ibreak(end)-ibreak(it+1), 1)];
                alpha = m_i(N_coeff +                         N_Euler_cumsum(i_source) + it);
                beta  = m_i(N_coeff +   N_Euler_cumsum(end) + N_Euler_cumsum(i_source) + it);
                gamma = m_i(N_coeff + 2*N_Euler_cumsum(end) + N_Euler_cumsum(i_source) + it);
                
                % R_VFM is matrix which transforms from magnetometer (VFM)
                % to star imager (STR) frame
                [R_VFM, R_a, R_b, R_c] = euler_123_SCARF(alpha*rad, beta*rad, gamma*rad); % rotation matrix VFM -> STR
                ind_2 = find(model.t_break_Euler{i_source}(it) <= t(index) & t(index) <= model.t_break_Euler{i_source}(it+1) & source(index) == i_source);
                
                % R_rtp is matrix which transforms from STR (star imager attitude) frame to (r, theta, phi) frame
                B_rtp(ind_2,:)     = mat_mul_mat_3(mat_mul_mat(R_rtp(index(ind_2),:,:), +R_VFM, 1, 1),   [B(index(ind_2),1) B(index(ind_2),2) B(index(ind_2),3)]);
                dB_dalpha(ind_2,:) = mat_mul_mat_3(mat_mul_mat(R_rtp(index(ind_2),:,:), -R_a*rad, 1, 1), [B(index(ind_2),1) B(index(ind_2),2) B(index(ind_2),3)]);
                dB_dbeta(ind_2,:)  = mat_mul_mat_3(mat_mul_mat(R_rtp(index(ind_2),:,:), -R_b*rad, 1, 1), [B(index(ind_2),1) B(index(ind_2),2) B(index(ind_2),3)]);
                dB_dgamma(ind_2,:) = mat_mul_mat_3(mat_mul_mat(R_rtp(index(ind_2),:,:), -R_c*rad, 1, 1), [B(index(ind_2),1) B(index(ind_2),2) B(index(ind_2),3)]);
            end
            A_Euler_a = [A_Euler_a repmat(reshape(dB_dalpha, 3*length(index), 1), 1,  N_Euler(i_source)).*repmat(A_Euler_tmp(:,1: N_Euler(i_source)), 3, 1)];
            A_Euler_b = [A_Euler_b repmat(reshape(dB_dbeta,  3*length(index), 1), 1,  N_Euler(i_source)).*repmat(A_Euler_tmp(:,1: N_Euler(i_source)), 3, 1)];
            A_Euler_c = [A_Euler_c repmat(reshape(dB_dgamma, 3*length(index), 1), 1,  N_Euler(i_source)).*repmat(A_Euler_tmp(:,1: N_Euler(i_source)), 3, 1)];
        else
            A_Euler_a = [A_Euler_a zeros(3*length(index),  N_Euler(i_source))];
            A_Euler_b = [A_Euler_b zeros(3*length(index),  N_Euler(i_source))];
            A_Euler_c = [A_Euler_c zeros(3*length(index),  N_Euler(i_source))];
        end
    end
    
    F = sqrt(B(:,1).^2 + B(:,2).^2 + B(:,3).^2);
    
    % model residuals in (r, theta, phi) frame 
    dB_rtp(index, :)    = B_rtp - B_synth(index, :);
    
    % model residuals in B23= (B, nxB, nx(nxB)) frame (cf. R. Holme 2000 and N. Olsen, OIFM, 2000)
    dB_B23(index,1) = dB_rtp(index, 1).*e_B(index,1) +dB_rtp(index, 2).*e_B(index,2) +dB_rtp(index,3).*e_B(index,3);
    dB_B23(index,2) = dB_rtp(index, 1).*e_2(index,1) +dB_rtp(index, 2).*e_2(index,2) +dB_rtp(index,3).*e_2(index,3);
    dB_B23(index,3) = dB_rtp(index, 1).*e_3(index,1) +dB_rtp(index, 2).*e_3(index,2) +dB_rtp(index,3).*e_3(index,3);

    cos_kappa_2 = ((B_rtp(:,1).*n(index,1) + B_rtp(:,2).*n(index,2) + B_rtp(:,3).*n(index,3))./F(index)).^2;
    sin_kappa_2 = 1 - cos_kappa_2;
    
    % data covariance matrix in B23 frame 
    C_tilde_B = params.sigma_csc_2*ones(length(index), 1);
    C_tilde_2 = params.sigma_csc_2 + F(index).^2.*(psi_t(index).^2.*cos_kappa_2 + chi_t(index).^2.*sin_kappa_2);
    C_tilde_3 = params.sigma_csc_2 + psi_t(index).^2.*F(index).^2;
    
    % weights (robust or unit weights) 
    if strcmp(options.robust, 'Tuckey')
        w_B23(index, 1) = tuckey_w(1./sqrt(C_tilde_B).*dB_B23(index, 1), params.c_Tuckey); % Tuckey
        w_B23(index, 2) = tuckey_w(1./sqrt(C_tilde_2).*dB_B23(index, 2), params.c_Tuckey); % Tuckey
        w_B23(index, 3) = tuckey_w(1./sqrt(C_tilde_3).*dB_B23(index, 3), params.c_Tuckey); % Tuckey
    elseif strcmp(options.robust, 'LS')
        w_B23(index,:) = ones(size(dB_B23(index,:))); % unit weights
    else
        w_B23(index,1) = min(1.5./abs(1./sqrt(C_tilde_B).*dB_B23(index,1)),1); % Huber weights
        w_B23(index,2) = min(1.5./abs(1./sqrt(C_tilde_2).*dB_B23(index,2)),1); % Huber weights
        w_B23(index,3) = min(1.5./abs(1./sqrt(C_tilde_3).*dB_B23(index,3)),1); % Huber weights
    end
    
    % A is matrix that transforms from (r, theta, phi) frame to B23 frame
    K = length(index);
    A_tmp = [[spdiags(e_B(index,1), 0, K, K) spdiags(e_B(index,2), 0, K, K) spdiags(e_B(index,3), 0, K, K)];
             [spdiags(e_2(index,1), 0, K, K) spdiags(e_2(index,2), 0, K, K) spdiags(e_2(index,3), 0, K, K)];
             [spdiags(e_3(index,1), 0, K, K) spdiags(e_3(index,2), 0, K, K) spdiags(e_3(index,3), 0, K, K)]];
    
    % A_tmp is the part of A which corresponds to present junk of data
%     A_tmp = A([index_current(index); N_data_vec_all+index_current(index); 2*N_data_vec_all+index_current(index)],[index_current(index); N_data_vec_all+index_current(index); 2*N_data_vec_all+index_current(index)]);
    
    % Inverse of data covariance matrix in B23 frame, including weights 
    C_y_1 = A_tmp'*spdiags([w_B23(index,1).*sin(theta(index)); w_B23(index,2).*sin(theta(index)); w_B23(index,3).*sin(theta(index))]./[C_tilde_B; C_tilde_2; C_tilde_3], 0, 3*length(index), 3*length(index))*A_tmp; % inverse of data covariance matrix
    
    G_tmp = [[A_r; A_theta; A_phi] A_Euler_a A_Euler_b A_Euler_c];
    Gty = Gty + (C_y_1*G_tmp)'*[dB_rtp(index,1); dB_rtp(index,2); dB_rtp(index,3)];
    GtG = GtG + G_tmp'*A_tmp'*A_tmp*C_y_1*G_tmp;
end

function C = mat_mul_mat_3(A, B)
% C = mat_mul_mat(A, B):           C = A*B
% multiplication with N x 3 matrix B
C = zeros(size(B));
for i=1:3
    for k=1:3
        C(:,i) = C(:,i) + A(:,i,k).*B(:,k);
    end
end

