MatLab - Mean Suqared Difference

%The goal of the as described by the author of the MatLab code

    %The FdFd2F function evaluates the functional F, which is the mean squared difference between the corrected measured magnetic
    %field and the corrected reference field, and the first and second derivatives of F.  The updating or correction is done when
    %FdFd2F calls function CalcF.  CalcF applies the current values of the unknowns to the measurements and then evaluates F.

    %The first and second derivatives are found by brute force, applying small perturbations to the unknowns and re-evaluating F.
    %These perturbations form the vector pert in function FdFd2F.

    %The overall objective of the program is to find those values of the unknowns which minimize the value of F.

%Main Function Code
    % Set initial values for unknowns
    bBx = 0;
    bBy = 0;
    bBz = 0;
    sBx = 0;
    sBy = 0;
    sBz = 0;
    Torsion = 0;
    dBref = 0;
    dDref = 0;

    unknowns = [bBx bBy bBz sBx sBy sBz Torsion dBref dDref];
    unknown_header = 'Iter_num Bx_bias By_bias Bz_bias   Bx_sf   By_sf   Bz_sf Torsion   dBref   dDref  rmsdBD\n';
    unknown_header2 = 'Iter_num,Bx_bias,By_bias,Bz_bias,Bx_sf,By_sf,Bz_sf,Torsion,dBref,dDref,rmsdBD\n';
    fprintf(unknown_header);
    fprintf(fileID, unknown_header2);
    [F, dF, d2F, Bxcorr, Bycorr, Bzcorr, Brefcorr, Drefcorr] = FdFd2F_1_2(solve, Gx, ...
        Gy, Gz, Bxmeas, Bymeas, Bzmeas, Gref, Bref, Dref, active, unknowns, delta);

    rmsdBD = sqrt(F);
    unknown_format = '%8.0f%8.0f%8.0f%8.0f%8.5f%8.5f%8.5f%8.2f%8.0f%8.2f%8.0f\n';
    unknown_format2 = '%8.0f%c%8.0f%c%8.0f%c%8.0f%c%8.5f%c%8.5f%c%8.5f%c%8.2f%c%8.0f%c%8.2f%c%8.0f\n';
    fprintf(unknown_format, [iteration_count unknowns rmsdBD]);
    fprintf(fileID, unknown_format2, iteration_count,',',unknowns(1),',',unknowns(2),',',unknowns(3),',', ...
        unknowns(4),',',unknowns(5),',',unknowns(6),',',unknowns(7),',',unknowns(8),',',unknowns(9),',',rmsdBD);

    while iterate == true
        iteration_count = iteration_count + 1;
        adj = dF/d2F;
        for i = 1:length(solve) % update unknowns vector
            unknowns(solve(i)) = unknowns(solve(i)) - adj(i); % could shorten?
        end
        [F, dF, d2F, Bxcorr, Bycorr, Bzcorr, Brefcorr, Drefcorr] = FdFd2F_1_2(solve, Gx, ...
            Gy, Gz, Bxmeas, Bymeas, Bzmeas, Gref, Bref, Dref, active, unknowns, delta);
        rmsdBD = sqrt(F);
        [sdB, sdD, sBD] = QC_1_3(Gx, Gy, Gz, Bxcorr, Bycorr, Bzcorr, Brefcorr, Drefcorr, ...
            MBX, MBY, MBZ, MSX, MSY, MSZ, AMIL, MFI, MDI);
        if strcmp(BD_dim, '2D')
            [max_sBD, max_indexBD] = max(abs(sBD).*active'); % Find maximum MdBD among active stations
            max_sd = max_sBD;
            max_index = max_indexBD;
        else
            [max_sdB, max_indexB] = max(abs(sdB).*active'); % Find maximum MdB among active stations
            [max_sdD, max_indexD] = max(abs(sdD).*active'); % Find maximum MdD among active stations
            max_sd = max([max_sdB max_sdD]);
            if max_sd == max_sdB
                max_index = max_indexB;
            else
                max_index = max_indexD;
            end
        end
        if iteration_count >= iteration_limit
            iterate = false;
            if max(abs(abs(adj) - delta(solve))) <= 1; % test for convergence
                if max_sd > BD_lim; % if there is a bad active station
                    end_message = 'Bad station';
                else
                    end_message = 'Converged\n';
                end
            else
                end_message = 'Not converged\n';
            end
        else
            if max_sd > BD_lim; % if there is a bad active station
                active(max_index) = 0; % deactivate failed active station
            else
                if max(abs(abs(adj) - delta(solve))) <= 1; % test for convergence
                    end_message = 'Converged\n';
                    iterate = false;
                end
            end
        end
        fprintf(unknown_format, [iteration_count unknowns rmsdBD]);
        fprintf(fileID, unknown_format2, iteration_count,',',unknowns(1),',',unknowns(2),',',unknowns(3),',', ...
            unknowns(4),',',unknowns(5),',',unknowns(6),',',unknowns(7),',',unknowns(8),',',unknowns(9),',',rmsdBD);
    end

%FdFd2F_1_2 function
    function[F, dF, d2F, Bxcorr, Bycorr, Bzcorr, Brefcorr, Drefcorr] = FdFd2F_1_2(solve, Gx, Gy, Gz, Bxmeas, Bymeas, Bzmeas, ...
    Gref, Bref, Dref, active, unknowns, delta)

    % Version 1.0, 11-3-17
    % Version 1.1, 6-10-18 No changes
    % Version 1.2, 7-15-18 Call to calcF_1_2 instead of calcF

    % compute the functional
    [F, Bxcorr, Bycorr, Bzcorr, Brefcorr, Drefcorr] ...
        = calcF_1_2(Gx, Gy, Gz, Bxmeas, Bymeas, Bzmeas, Bref, Dref, active, unknowns);

    % Compute the vector of first derivatives and the diagonals of the Jacobian
    N = length(solve);
    dF(N) = zeros;
    d2F(N, N) = zeros;
    pert(9) = zeros;
    for i = 1:N
        pert(1:9) = zeros;
        pert(solve(i)) = delta(solve(i));
        plus = calcF_1_2(Gx, Gy, Gz, Bxmeas, Bymeas, Bzmeas, Bref, Dref, active, unknowns+pert);
        pert(solve(i)) = - delta(solve(i));
        minus = calcF_1_2(Gx, Gy, Gz, Bxmeas, Bymeas, Bzmeas, Bref, Dref, active, unknowns+pert);
        dF(i) = (plus - minus)/(2*delta(solve(i)));
        d2F(i, i) = (plus + minus - 2*F)/(delta(solve(i)))^2;
    end

    % Complete the Jacobian matrix of second derivatives
    N = length(solve);
    for i = 1:N-1
        for j = i+1:N
            pert(1:9) = zeros;
            pert(solve(i)) = delta(solve(i));
            pert(solve(j)) = delta(solve(j));
            pp = calcF_1_2(Gx, Gy, Gz, Bxmeas, Bymeas, Bzmeas, Bref, Dref, active, unknowns+pert);
            pert(solve(j)) = - delta(solve(j));
            pm = calcF_1_2(Gx, Gy, Gz, Bxmeas, Bymeas, Bzmeas, Bref, Dref, active, unknowns+pert);
            pert(solve(i)) = - delta(solve(i));
            mm = calcF_1_2(Gx, Gy, Gz, Bxmeas, Bymeas, Bzmeas, Bref, Dref, active, unknowns+pert);
            pert(solve(j)) = delta(solve(j));
            mp = calcF_1_2(Gx, Gy, Gz, Bxmeas, Bymeas, Bzmeas, Bref, Dref, active, unknowns+pert);
            d2F(i,j) = (pp + mm - pm - mp)/(4*delta(solve(i))*delta(solve(j)));
            d2F(j,i) = d2F(i,j);
        end
    end

%calcF_1_2 function
    function[F, Bxcorr, Bycorr, Bzcorr, Brefcorr, Drefcorr] ...
        = calcF_1_2(Gx, Gy, Gz, Bxmeas, Bymeas, Bzmeas, Bref, Dref, active, unknowns)

    % Version 1.0, 11-4-17
    % Version 1.1, 6-10-18 No changes
    % Version 1.2, 7-15-18 No changes

    bBx = unknowns(1);
    bBy = unknowns(2);
    bBz = unknowns(3);
    sBx = unknowns(4);
    sBy = unknowns(5);
    sBz = unknowns(6);
    Torsion = unknowns(7);
    dBref = unknowns(8);
    dDref = unknowns(9);

    Bxcorr = (Bxmeas - bBx)*cosd(Torsion)/(1 + sBx) + (Bymeas - bBy)*sind(Torsion)/(1 + sBy);
    Bycorr = (Bymeas - bBy)*cosd(Torsion)/(1 + sBy) - (Bxmeas - bBx)*sind(Torsion)/(1 + sBx);
    Bzcorr = (Bzmeas - bBz)/(1 + sBz);
    G = sqrt(Gx.^2 + Gy.^2 + Gz.^2);
    Bcorr = sqrt(Bxcorr.^2 + Bycorr.^2 + Bzcorr.^2);
    Dcorr = asind((Gx.*Bxcorr + Gy.*Bycorr + Gz.*Bzcorr)./(G.*Bcorr));
    Brefcorr = Bref - dBref;
    Drefcorr = Dref - dDref;
    dBD = sqrt(Bcorr.^2 + Brefcorr.^2 - 2*Bcorr.*Brefcorr.*cosd(Dcorr - Drefcorr));
    F = sum((dBD.*active.').^2)/sum(active); % this is the functional to be minimized (mean square dBD)