function [estParams] = rohac_2014(trajData, accel_MPU, initCalibParams, maxIter)
% rohac_2014(trajData, accel_MPU, initCalibParams, maxIter)
% rng default % for reproductability

%% Get accelerations for each position
av_acc = computeAvAccels(trajData, accel_MPU);

%% Optimisation
% Initialise parameters:
initCalibParams = [initCalibParams.alpha initCalibParams.beta initCalibParams.gamma initCalibParams.s_xx initCalibParams.s_yy initCalibParams.s_zz, ...
                   initCalibParams.b_x initCalibParams.b_y initCalibParams.b_z]; % supply in vector form for the optimisation algorithm

options = optimoptions('fminunc', 'Algorithm', 'quasi-newton', 'MaxIterations', maxIter, 'Display', 'off');
problem.options = options;
problem.x0 = initCalibParams;
problem.objective = @(calibParams)rmse_loss(av_acc, calibParams);
problem.solver = 'fminunc';

calibParams = fminunc(problem);

%% Results
% MABE and MASE computation:
T_est = makeTmatrix(calibParams(1), calibParams(2), calibParams(3));

SF_est = [calibParams(4) 0 0;0 calibParams(5) 0;0 0 calibParams(6)]; % matrix of scale factors
S_est = SF_est * T_est; % axis sensitivites matrix
b_est = [calibParams(7) calibParams(8) calibParams(9)];

% Return the estimated parameters
estParams.Bias = b_est; estParams.AxesMisalignment = S_est;

% Return individual calibration components:
estParams.ScaleFactors = [calibParams(4) calibParams(5) calibParams(6)]; estParams.NonorthogAngles = [calibParams(1) calibParams(2) calibParams(3)];
end