clear;

trajDataTee = load('trajectories/tee_6.mat'); trajDataTee = trajDataTee.trajData; % load trajData struct
load('trajectories/sixposition_kinematic.mat'); % load trajData struct
addpath('six_position\');
warning('off');

n = 200; % runs of the simulation
g = 9.81;
k_calib = 0.3;

data_ferraris = zeros(n, 5); data_jurman = data_ferraris; data_tee = data_ferraris;
MSEs_ferraris = zeros(n, 9); MSEs_jurman = zeros(n, 12); MSEs_tee = MSEs_ferraris;

% Create kinematic variables from trajData structs:
[orient, acc, angvel] = kintrajvarsFromkintrajData(trajData);
[orientTee, accTee, angvelTee] = kintrajvarsFromkintrajData(trajDataTee);

crlbs = zeros(9, n); % no. of rows depends on the version of the SPM being used and how many parameters its estimating.

for i = 1:n

    % fsRange in m/s2
    % res in (m/s2)/LSB
    % typBias in m/s2
    % typSFcalibErrPerc % of FS
    % typNonOrthogAngleDeg in degrees
    % typMisalignAngleDeg in degrees
    % noiseDens in (m/s2)/√(Hz)
    % rdnWalk in (m/s2)*(√Hz)
    % biasInstab in m/s2

    % New accelerometer model parameters set to a normal distribution with
    % 3 stds corresponding to the bounds of the uniform distribution.
    accelParams.fsRange = 2*g; accelParams.res = 5.9875e-04; accelParams.typBiasX = (40/1000 * 9.81) / 3; accelParams.typBiasY = (40/1000 * 9.81) / 3;
    accelParams.typBiasZ = (40/1000 * 9.81) / 3; accelParams.typSFcalibErrPerc = 3/3; accelParams.typNonOrthogAngleDeg = 1/3; accelParams.typMisalignAngleDeg = 1/3;
    accelParams.noiseDens = 0.003; accelParams.rdnWalk = 2.2e-05; accelParams.biasInstab = 4.0e-04;

    % MPU-6050 IMU
    MPU_model = imuSensor('SampleRate', trajData.fs, 'Temperature', 20, 'IMUType', 'accel-gyro', 'ReferenceFrame', 'NED');
    % Create random accelerometer model based on typical values:
    [MPU_model.Accelerometer, scaleFactors, nonorthogAngles, misalignAngles] = genAccel(accelParams, 'normal');

    trueParams.Bias = MPU_model.Accelerometer.ConstantBias; trueParams.AxesMisalignment = MPU_model.Accelerometer.AxesMisalignment ./ 100;
    trueParams.ScaleFactors = scaleFactors; trueParams.NonorthogAngles = nonorthogAngles; trueParams.MisalignAngles = misalignAngles; % in rad

    % Run IMU model
    an_nb = acc; an_nb_tee = accTee; % acceleration of body frame rel. to nav. frame expressed in nav. frame.
    gn_nb = angvel; gn_nb_tee = angvelTee; % ang. velocity of body frame rel. to nav. frame expressed in nav. frame.
    qb_n = quaternion(compact(orient)); qb_n_tee = quaternion(compact(orientTee)); % orientation of body frame rel. to nav. frame
    [accel_MPU, ~] = MPU_model(an_nb, gn_nb, qb_n); [accel_MPU_tee, ~] = MPU_model(an_nb_tee, gn_nb_tee, qb_n_tee);

    trueAccels = computeTrueAccels(trajData, qb_n); crlbs(:, i) = computeCRLB(accelParams.noiseDens * sqrt(trajData.fs / 2), trueParams, trueAccels);

    % ferraris et al.
    estParamsFerraris = ferraris_1994(trajData, accel_MPU);
    [MASE_ferraris, MABE_ferraris, CE_ferraris, MSSE_ferraris, MSBE_ferraris] = computeCalibError(estParamsFerraris, trueParams, k_calib);
    data_ferraris(i,:) = [MASE_ferraris, MABE_ferraris, CE_ferraris, MSSE_ferraris MSBE_ferraris];
    MSEs_ferraris(i,:) = [(estParamsFerraris.ScaleFactors' - scaleFactors).^2 (estParamsFerraris.MisalignAngles - misalignAngles).^2 (estParamsFerraris.Bias - trueParams.Bias).^2];

    % Jurman et al.
    [estParamsJurman] = jurman_2007(trajData, accel_MPU);
    [MASE_jurman, MABE_jurman, CE_jurman, MSSE_jurman, MSBE_jurman] = computeCalibError(estParamsJurman, trueParams, k_calib);
    data_jurman(i,:) = [MASE_jurman, MABE_jurman, CE_jurman, MSSE_jurman, MSBE_jurman];
    MSEs_jurman(i,:) = [(estParamsJurman.ScaleFactors' - scaleFactors).^2 (estParamsJurman.NonorthogAngles - nonorthogAngles).^2 (estParamsJurman.MisalignAngles - misalignAngles).^2 (estParamsJurman.Bias - trueParams.Bias).^2];

    % Tee et al.
    estParamsTee = tee_2011(trajDataTee, accel_MPU_tee);
    [MASE_tee, MABE_tee, CE_tee, MSSE_tee, MSBE_tee] = computeCalibError(estParamsTee, trueParams, k_calib);
    data_tee(i,:) = [MASE_tee, MABE_tee, CE_tee, MSSE_tee, MSBE_tee];
    MSEs_tee(i,:) = [(estParamsTee.ScaleFactors' - scaleFactors).^2 (estParamsTee.NonorthogAngles - nonorthogAngles).^2 (estParamsTee.Bias - trueParams.Bias).^2];

    % disp(estParamsFerraris.AxesMisalignment); disp(' '); disp(estParamsFerraris.Bias); disp('-------------------------------');
    % disp(estParamsJurman.AxesMisalignment); disp(' '); disp(estParamsJurman.Bias); disp('-----------------------------------');
    % disp(estParamsTee.AxesMisalignment); disp(' '); disp(estParamsTee.Bias); disp('-----------------------------------');
    % disp(trueParams.AxesMisalignment); disp(' '); disp(trueParams.Bias); disp('-----------------------------------');

    nonorthogData(i,:) = [mean(abs(estParamsJurman.NonorthogAngles - nonorthogAngles)) mean(abs(estParamsTee.NonorthogAngles - nonorthogAngles))];
    misalignData(i,:) = [mean(abs(estParamsFerraris.MisalignAngles - misalignAngles)) mean(abs(estParamsJurman.MisalignAngles - misalignAngles))];

end

disp([mean(data_ferraris(:,3)) std(data_ferraris(:,3))]);
disp([mean(data_jurman(:,3)) std(data_jurman(:,3))]);
disp([mean(data_tee(:,3)) std(data_tee(:,3))]);

%% Results analysis
disp([mean(data_ferraris(:,3)) std(data_ferraris(:,3))]);
disp([mean(data_jurman(:,3)) std(data_jurman(:,3))]);
disp([mean(data_tee(:,3)) std(data_tee(:,3))]);

crlb = mean(crlbs, 2); % variance form so comparable with the var of the MSE

disp(mean(MSEs_ferraris, 'all'));
disp(mean(MSEs_jurman, 'all'));
disp(mean(MSEs_tee, 'all'));

MAE_nonorthog_Jurman = rad2deg(mean(nonorthogData(:,1))); MAE_nonorthog_Tee = rad2deg(mean(nonorthogData(:,2)));
MAE_misalign_Jurman = rad2deg(mean(misalignData(:,2))); MAE_misalign_Ferraris = rad2deg(mean(misalignData(:,1)));