%% scalar calibration of Swarm data (not corrected for dB_Sun disturbances, hence larger rms data misfit of 0.69 nT)
clear all
a = 6371.2;
rad = pi/180;

% file from swarm-diss.eo.esa.int/Level1b/Entire_mission_data/MAGx_CA
filename_in = 'SW_OPER_MAGA_CA_1B_20240521T000000_20240521T235959_0701_MDR_MAG_CA.cdf';
info = cdfinfo(filename_in)

data = cdfread(filename_in, 'CombineRecords', 1, 'variable', {'Timestamp', 'Latitude', 'Longitude', ...
     'Radius', 'B_pre', 'F', 'T_CDC', 'T_CSC', 'T_EU'});
t = todatenum(data{1}) - 730486;
r = double(data{4})/1e3; % radius in km
theta = 90-double(data{2});
phi = double(data{3});
B_pre = double(data{5});
F = double(data{6});
T_CDC = double(data{7});
T_CSC = double(data{8});
T_EU = double(data{9});

t_dt = datetime(t+datenum(2000,1,1),'ConvertFrom','datenum');

clf
subplot(1,2,1)
F_VFM = sqrt(B_pre(:,1).^2 + B_pre(:,2).^2 + B_pre(:,3).^2);
plot(theta, F-F_VFM, '.')
xlabel '\theta [deg]'
ylabel '\DeltaF = F_{ASM} - F_{VFM, pre-calibrated} [nT]'
grid on

% perform scalar calibration, including scale-dependency on sensor temperature

% initial model vector
m_i  = [0; 0; 0; % offsets
    1; 1; 1; % scale values
    0; 0; 0; % non-orthogonalities
    0; 0; 0]; % scale dependence on sensor temperature S_{S,i},i = 1-3

% a-priori model vector. Not used since a-priori weights are set to zero
m_prior = m_i;
m_weights = zeros(size(m_i));
iC_prior = diag(m_weights); % a-priori model covariance matrix

clear wG G w
w = ones(size(F)); % start with unit weights in first iteration
Max_iter = 20;
for iter = 1:Max_iter		% Do the non-linear least-squares iteration ...
    [F_calib, G] = F_calib_I_T(m_i, B_pre, [], T_CSC);

    delta_d = F - F_calib;

    rms = sqrt(((w'.*delta_d') * (w.*delta_d)) / sum(w));
    fprintf(2,'Iteration: %3i rms (nT): %8.3g\n', iter, rms)
    w = min(1.5./abs(delta_d ./ rms), 1); % new robust weights after Huber
    wG = w.*G;
    % delta_m = (G'*wG + iC_prior)\(wG'*delta_d + iC_prior*(m_prior-m_i));
    delta_m = inv(G'*wG)*wG'*delta_d; % inversion without a-priori information
    m_i = m_i + delta_m; % model update
end

[F_calib, ~, B_calib] = F_calib_I_T(m_i, B_pre, [], T_CSC); % also return calibrated vector components
subplot(1,2,2)
plot(theta, F-F_calib, '.')
xlabel '\theta [deg]'
ylabel '\DeltaF = F_{ASM} - F_{VFM, fully-calibrated} [nT]'
grid on

% print 9 model parameters (offsets, scale-values non-orthogonalities)
fprintf(2, '%6.1f %6.1f %6.1f    %6.5f %6.5f %6.5f    %6.3f %6.3f %6.3f  %8.2f\n', [m_i(1:6); m_i(7:9)/rad; rms])

%% estimation of Euler angles by comparing B_VFM with field model
clear all
a = 6371.2;
rad = pi/180;

% read CHAOS field model (from https://www.spacecenter.dk/files/magnetic-models/CHAOS-8/_downloads/d6e664af116d216141471e2468fd3fcb/CHAOS-8.5.mat)
load('/nio/m/mmod/CHAOS-8.5.mat')

% Swarm L1b LR file from swarm-diss.eo.esa.int/Level1b/Entire_mission_data/MAGx_LR
filename_in = 'SW_OPER_MAGA_LR_1B_20240521T000000_20240521T235959_0701_MDR_MAG_LR.cdf';
info = cdfinfo(filename_in)

Step = 15; % downsample from 1 Hz to 15 sec values
data = cdfread(filename_in, 'CombineRecords', 1, 'variable', {'Timestamp', 'Latitude', 'Longitude', ...
     'Radius', 'B_VFM', 'B_NEC', 'q_NEC_CRF'});
t = todatenum(data{1}(1:Step:end)) - 730486;
r = double(data{4}(1:Step:end))/1e3; % radius in km
theta = 90-double(data{2}(1:Step:end));
phi = double(data{3}(1:Step:end));
B_VFM = double(data{5}(1:Step:end,:));
B_NEC = double(data{6}(1:Step:end,:));
q_NEC_CRF = double(data{7}(1:Step:end,:));

% calculate model values
B_mod = synth_values(r, theta, phi, pp, t); % core field up to degree n = 20
B_mod = B_mod + synth_values(r, theta, phi, g(1:40*(40+2))); % add crustal field up to n = 40
% [B_magn_1, B_magn_2] = synth_values_MMA(r, theta, phi, t, s_MMA); 
% B_mod = B_mod + B_magn_1 + B_magn_2; % add magnetospheric and induced field 
B_mod_NEC = [-B_mod(:,2) +B_mod(:,3) -B_mod(:,1)];

B_mod_CRF = qvxform(qconj(q_NEC_CRF), B_mod_NEC); % rotate model field into CRF reference frame

% only non-polar data
theta_d = gg2gm(theta, phi);
idx = find(abs(90-theta_d) < 60);

Max_iter = 25;
% alpha = 11.8; beta = 283.8074; gamma = 347.4419; % starting values
alpha = 0; beta = 270; gamma = 360; % convergence even with these starting values
m_i = [alpha; beta; gamma];
N_data = 3*length(idx);
for iter = 1:Max_iter
    [R_VFM, R_a, R_b, R_c] = euler_123_SCARF(m_i(1)*rad, m_i(2)*rad, m_i(3)*rad); % rotation matrix VFM -> CRF

    B_mod_VFM = mat_mul_mat_3(R_VFM', B_mod_CRF(idx,:));
    dB_mod_dalpha = mat_mul_mat_3(R_a'*rad,   B_mod_CRF(idx,:));
    dB_mod_dbeta  = mat_mul_mat_3(R_b'*rad,   B_mod_CRF(idx,:));
    dB_mod_dgamma = mat_mul_mat_3(R_c'*rad,   B_mod_CRF(idx,:));

    delta_d_vec = [B_mod_VFM(:,1) - B_VFM(idx,1); ...
                   B_mod_VFM(:,2) - B_VFM(idx,2); ...
                   B_mod_VFM(:,3) - B_VFM(idx,3)];
    G = [dB_mod_dalpha; dB_mod_dbeta; dB_mod_dgamma];

    if iter > 1
        rms = sqrt(((w'.*delta_d_vec') * (w.*delta_d_vec)) / sum(w));
        w = min(1.5./abs(delta_d_vec./rms),1);  % robust weights
    else
        w = ones(size(delta_d_vec));
    end
    iC_y = spdiags(w, 0, N_data, N_data);
    wG = iC_y*G;
    delta_m = (G'*wG)\(wG'*delta_d_vec);
    m_i = m_i + delta_m;
    fprintf(2,'Iteration: %3i, %6.2f %6.2f %6.2f rms (nT):%10.4f\n', iter, mod(m_i, 360), rms_std(delta_d_vec, w))
end
fprintf(2,'%6.2f %6.2f %6.2f %8.4f %8.4f %8.4f %6.2f\n', alpha, beta, gamma, mod(m_i, 360), rms_std(delta_d_vec, w));

q_A_estimated = dcm2q(R_VFM); % convert Euler angle matrix to quaternions

% apply estimated Euler angles and rotate from B_VFM to B_NEC
B_NEC_1 = qvxform(qmult(q_A_estimated, q_NEC_CRF), B_VFM);
% apply Swarm operational Euler angles and rotate from B_VFM to B_NEC
q_A = [-0.013237940900440 0.618884832920591 0.022645867500753 0.785043619926120]; % Swarm Alpha
B_NEC_2 = qvxform(qmult(q_A, q_NEC_CRF), B_VFM);

clf
subplot(3,1,1)
plot(theta, B_NEC, '.', theta, B_NEC_1, 'o')
xlabel '\theta [deg]'
ylabel 'B_{NEC} [nT]'
grid on
subplot(3,1,2)
plot(theta, B_NEC-B_NEC_1, '.')
xlabel '\theta [deg]'
ylabel '\DeltaB_{NEC} [nT]'
grid on
subplot(3,1,3)
plot(theta, B_NEC-B_NEC_2, '.')
xlabel '\theta [deg]'
ylabel '\DeltaB_{NEC} [nT]'
grid on


%% vector-vector calibration: estimation of 12 calibration parameters
clear all
a = 6371.2;
rad = pi/180;

% read CHAOS field model
load('/nio/m/mmod/CHAOS-8.5.mat')

filename_in = 'SW_OPER_MAGA_LR_1B_20240521T000000_20240521T235959_0701_MDR_MAG_LR.cdf';
info = cdfinfo(filename_in)

Step = 15; % downsample from 1 Hz to 15 sec values
data = cdfread(filename_in, 'CombineRecords', 1, 'variable', {'Timestamp', 'Latitude', 'Longitude', ...
     'Radius', 'B_VFM', 'B_NEC', 'q_NEC_CRF'});
t = todatenum(data{1}(1:Step:end)) - 730486;
r = double(data{4}(1:Step:end))/1e3; % radius in km
theta = 90-double(data{2}(1:Step:end));
phi = double(data{3}(1:Step:end));
B_VFM = double(data{5}(1:Step:end,:));
B_NEC = double(data{6}(1:Step:end,:));
q_NEC_CRF = double(data{7}(1:Step:end,:));

% calculate model values
B_mod = synth_values(r, theta, phi, pp, t); % core field up to degree n = 20
B_mod = B_mod + synth_values(r, theta, phi, g(1:40*(40+2))); % add crustal field up to n = 40
% [B_magn_1, B_magn_2] = synth_values_MMA(r, theta, phi, t, s_MMA); 
% B_mod = B_mod + B_magn_1 + B_magn_2; % add magnetospheric and induced field 
B_mod_NEC = [-B_mod(:,2) +B_mod(:,3) -B_mod(:,1)];

B_mod_CRF = qvxform(qconj(q_NEC_CRF), B_mod_NEC); % rotate model field into CRF reference frame

% only non-polar data
theta_d = gg2gm(theta, phi);
idx = find(abs(90-theta_d) < 60);

% rotate B_mod_NEC to B_mod_CRF
B_mod_CRF = qvxform(qconj(q_NEC_CRF), B_mod_NEC);

m_i = [0 0 0  1 0 0  0 1 0  0 0 1]'; % the 12 basic parameters
m_prior = m_i; m_weights = zeros(size(m_i)); iC_prior = diag(m_weights);

Step = 1;
Max_iter = 15;
w = ones(numel(B_VFM(idx,:)), 1); % start with unit weights in first iteration
for iter = 1:Max_iter % Do the non-linear least-squares iteration ...
    [B_CRF, G] = B_calib_I_T(m_i, B_VFM(idx,:), [], []);

    delta_d_vec = [B_mod_CRF(idx,1) - B_CRF(:,1); ...
        B_mod_CRF(idx,2) - B_CRF(:,2); ...
        B_mod_CRF(idx,3) - B_CRF(:,3)];

    rms = sqrt(((w'.*delta_d_vec') * (w.*delta_d_vec)) / sum(w));
    fprintf(2,'Iteration: %3i rms (nT): %6.2f\n', iter, rms)
    w = min(1.5./abs(delta_d_vec ./ rms), 1); % new robust weights after Huber
    wG = repmat(w, 1, length(m_i)).*G;
    delta_m = (G'*wG + iC_prior)\(wG'*delta_d_vec + iC_prior*(m_prior-m_i));
    m_i = m_i + Step*delta_m;
end % iteration

B_CRF = B_calib_I_T(m_i, B_VFM, [], []); % apply calibration parameters to all data

[b, S, u, Euler] = decompose_A(m_i(1:12));
fprintf(2, '%10s & %8.1f & %8.1f & %8.1f\n', 'b [nT] ', b)
fprintf(2, '%10s & %8.6f & %8.6f & %8.6f\n', 'S [1] ', S)
fprintf(2, '%10s & %8.3f & %8.3f & %8.3f\n', 'u [deg]', u)
fprintf(2, '%10s & %8.3f & %8.3f & %8.3f\n', 'Euler [deg] ', Euler)

% note: offsets, scale values and non-orthogonalities are expected to be
% close to ideal values (0, resp 1) since calculated from already calibrated
% magnetic field B_VFM

% apply estimated Euler angles and rotate from B_VFM to B_NEC
R_VFM = euler_123_SCARF(Euler(1)*rad, Euler(2)*rad, Euler(3)*rad); % rotation matrix VFM -> CRF
q_A_estimated = dcm2q(R_VFM); % convert Euler angle matrix to quaternions
B_NEC_1 = qvxform(qmult(q_A_estimated, q_NEC_CRF), B_VFM);

% apply Swarm operational Euler angles and rotate from B_VFM to B_NEC
q_A = [-0.013237940900440 0.618884832920591 0.022645867500753 0.785043619926120]; % Swarm Alpha
B_NEC_2 = qvxform(qmult(q_A, q_NEC_CRF), B_VFM);

clf
subplot(3,1,1)
plot(theta, B_NEC, '.', theta, B_NEC_1, 'o')
xlabel '\theta [deg]'
ylabel 'B_{NEC} [nT]'
grid on
subplot(3,1,2)
plot(theta, B_NEC-B_NEC_1, '.')
xlabel '\theta [deg]'
ylabel '\DeltaB_{NEC} [nT]'
grid on
subplot(3,1,3)
plot(theta, B_NEC-B_NEC_2, '.')
xlabel '\theta [deg]'
ylabel '\DeltaB_{NEC} [nT]'
grid on

