function  [F_calib, dF_dm, B_calibrated] = B_calib(m, E)
%
%  calibrated F = |B| as a function of the magnetometer output E
%

S1 = diag(1./m(4:6));               % 1/scale values

[P1, P1_x, P1_y, P1_z] = non_ortho_1(m(7), m(8), m(9)); % Inverse matrix of non-orthogonalities


E_tmp = zeros(size(E));
E_tmp(:,1) = E(:,1) - m(1);	% E - offsets
E_tmp(:,2) = E(:,2) - m(2);
E_tmp(:,3) = E(:,3) - m(3);

M = S1'*(P1'*P1)*S1;
F_calib = ...
    sqrt(E_tmp(:,1).*(M(1,1)*E_tmp(:,1) + M(1,2)*E_tmp(:,2) + M(1,3)*E_tmp(:,3)) ...
    +E_tmp(:,2).*(M(2,1)*E_tmp(:,1) + M(2,2)*E_tmp(:,2) + M(2,3)*E_tmp(:,3)) ...
    +E_tmp(:,3).*(M(3,1)*E_tmp(:,1) + M(3,2)*E_tmp(:,2) + M(3,3)*E_tmp(:,3)));

if nargout > 2 % also return calibrated magnetic field vector
    SE_tmp = [E_tmp(:,1)./m(4) E_tmp(:,2)./m(5) E_tmp(:,3)./m(6)];
    B_calibrated = [[M(1,1)*SE_tmp(:,1) + M(1,2)*SE_tmp(:,2) + M(1,3)*SE_tmp(:,3)] ...
        [M(2,1)*SE_tmp(:,1) + M(2,2)*SE_tmp(:,2) + M(2,3)*SE_tmp(:,3)] ...
        [M(3,1)*SE_tmp(:,1) + M(3,2)*SE_tmp(:,2) + M(3,3)*SE_tmp(:,3)]];
    varargout{1} = B_calibrated;
end

% Jacobian matrix
d_offsets_1 = -(2*M(1,1)*E_tmp(:,1) + M(2,1)*E_tmp(:,2) + M(3,1)*E_tmp(:,3))./F_calib(:);
d_offsets_2 = -(M(1,2)*E_tmp(:,1) + 2*M(2,2)*E_tmp(:,2) + M(3,2)*E_tmp(:,3))./F_calib(:);
d_offsets_3 = -(M(1,3)*E_tmp(:,1) + M(2,3)*E_tmp(:,2) + 2*M(3,3)*E_tmp(:,3))./F_calib(:);

% % general solution
% S1_x = zeros(3,3); S1_x(1,1) = -1/m(4); M = S1_x'*(P1'*P1)*S1 + S1'*(P1*P1')*S1_x;
% d_scale_1  =  (E_tmp(:,1).*(M(1,1)*E_tmp(:,1) + M(1,2)*E_tmp(:,2) + M(1,3)*E_tmp(:,3)) ...
%               +E_tmp(:,2).*(M(2,1)*E_tmp(:,1) + M(2,2)*E_tmp(:,2) + M(2,3)*E_tmp(:,3)) ...
%               +E_tmp(:,3).*(M(3,1)*E_tmp(:,1) + M(3,2)*E_tmp(:,2) + M(3,3)*E_tmp(:,3)))./(2*F_calib);
% S1_y = zeros(3,3); S1_y(2,2) = -1/m(5);  M   = S1_y'*(P1'*P1)*S1 + S1'*(P1*P1')*S1_y;
% d_scale_2  =  (E_tmp(:,1).*(M(1,1)*E_tmp(:,1) + M(1,2)*E_tmp(:,2) + M(1,3)*E_tmp(:,3)) ...
%               +E_tmp(:,2).*(M(2,1)*E_tmp(:,1) + M(2,2)*E_tmp(:,2) + M(2,3)*E_tmp(:,3)) ...
%               +E_tmp(:,3).*(M(3,1)*E_tmp(:,1) + M(3,2)*E_tmp(:,2) + M(3,3)*E_tmp(:,3)))./(2*F_calib);
% S1_z = zeros(3,3); S1_z(3,3) = -1/m(6); M   = S1_z'*(P1'*P1)*S1 + S1'*(P1*P1')*S1_z;
% d_scale_3  =  (E_tmp(:,1).*(M(1,1)*E_tmp(:,1) + M(1,2)*E_tmp(:,2) + M(1,3)*E_tmp(:,3)) ...
%               +E_tmp(:,2).*(M(2,1)*E_tmp(:,1) + M(2,2)*E_tmp(:,2) + M(2,3)*E_tmp(:,3)) ...
%               +E_tmp(:,3).*(M(3,1)*E_tmp(:,1) + M(3,2)*E_tmp(:,2) + M(3,3)*E_tmp(:,3)))./(2*F_calib);
% simplified version, for diagonal sensitivity matrix
d_scale_1   = -1/m(4)*E_tmp(:,1).*(M(1,1)*E_tmp(:,1) + M(2,1)*E_tmp(:,2) + M(3,1)*E_tmp(:,3))./F_calib(:);
d_scale_2   = -1/m(5)*E_tmp(:,2).*(M(1,2)*E_tmp(:,1) + M(2,2)*E_tmp(:,2) + M(3,2)*E_tmp(:,3))./F_calib(:);
d_scale_3   = -1/m(6)*E_tmp(:,3).*(M(1,3)*E_tmp(:,1) + M(2,3)*E_tmp(:,2) + M(3,3)*E_tmp(:,3))./F_calib(:);

M   = S1'*(P1_x'*P1)*S1 + S1'*(P1'*P1_x)*S1;
d_non_or_1  =  (E_tmp(:,1).*(M(1,1)*E_tmp(:,1) + M(1,2)*E_tmp(:,2) + M(1,3)*E_tmp(:,3)) ...
    +E_tmp(:,2).*(M(2,1)*E_tmp(:,1) + M(2,2)*E_tmp(:,2) + M(2,3)*E_tmp(:,3)) ...
    +E_tmp(:,3).*(M(3,1)*E_tmp(:,1) + M(3,2)*E_tmp(:,2) + M(3,3)*E_tmp(:,3)))./(2*F_calib);

M   = S1'*(P1_y'*P1)*S1 + S1'*(P1'*P1_y)*S1;
d_non_or_2  =  (E_tmp(:,1).*(M(1,1)*E_tmp(:,1) + M(1,2)*E_tmp(:,2) + M(1,3)*E_tmp(:,3)) ...
    +E_tmp(:,2).*(M(2,1)*E_tmp(:,1) + M(2,2)*E_tmp(:,2) + M(2,3)*E_tmp(:,3)) ...
    +E_tmp(:,3).*(M(3,1)*E_tmp(:,1) + M(3,2)*E_tmp(:,2) + M(3,3)*E_tmp(:,3)))./(2*F_calib);

M   = S1'*(P1_z'*P1)*S1 + S1'*(P1'*P1_z)*S1;
d_non_or_3  =  (E_tmp(:,1).*(M(1,1)*E_tmp(:,1) + M(1,2)*E_tmp(:,2) + M(1,3)*E_tmp(:,3)) ...
    +E_tmp(:,2).*(M(2,1)*E_tmp(:,1) + M(2,2)*E_tmp(:,2) + M(2,3)*E_tmp(:,3)) ...
    +E_tmp(:,3).*(M(3,1)*E_tmp(:,1) + M(3,2)*E_tmp(:,2) + M(3,3)*E_tmp(:,3)))./(2*F_calib);

dF_dm  =  [ d_offsets_1 d_offsets_2 d_offsets_3 ...   % design matrix of offsets
    d_scale_1   d_scale_2   d_scale_3   ...           %               of scale values
    d_non_or_1  d_non_or_2  d_non_or_3];              %               of non-orthogonalities

end
