function [estParams] = tee_2011(trajData, accel_MPU)
%TEE_2011 Summary of this function goes here
%   Here we only implement the six position version of the method as their
%   results showed little difference between using 6 and 12 positions. The
%   cardinal positions specifically are used. The linear model assumes a
%   nonorthogonality in which the x axis is the fixed reference to which y
%   and z are nonorthogonal. The method assumes g_true is known i.e. the
%   specific force measurements in the platform ref. frame. The task is to
%   calibrate the sensor measurements so they are expressed in the platform
%   frame as best as possible.

av_acc = computeAvAccels(trajData, accel_MPU);
g_true = trajData.g_true; % true/reference accelerometer measurements in the platform frame to aim for.

A_x = [1 g_true(5, 1)
    1 g_true(6, 1)];
A_y = [1 g_true(2, 2) g_true(2, 1)
    1 g_true(4, 2) g_true(4, 1)
    1 g_true(5, 2) g_true(5, 1)];
A_z = [1 g_true(1, 3) g_true(1, 2) g_true(1, 1)
    1 g_true(2, 3) g_true(2, 2) g_true(2, 1)
    1 g_true(5, 3) g_true(5, 2) g_true(5, 1)
    1 g_true(4, 3) g_true(4, 2) g_true(4, 1)];

v_x1 = av_acc(5, 1); v_x2 = av_acc(6, 1); v_x = [v_x1; v_x2];
v_y1 = av_acc(2, 2); v_y2 = av_acc(4, 2); v_y3 = av_acc(5, 2); v_y = [v_y1;v_y2;v_y3];
v_z1 = av_acc(1, 3); v_z2 = av_acc(2, 3); v_z3 = av_acc(5, 3); v_z4 = av_acc(4, 3); v_z = [v_z1; v_z2; v_z3; v_z4];

params_x = linsolve(A_x, v_x); % = b_x, k_x
params_y = linsolve(A_y, v_y); % = b_y, k_y, alpha*k_y
params_z = linsolve(A_z, v_z); % = b_z, k_z, gamma*k_z, beta*k_z

estParams.Bias = [params_x(1) params_y(1) params_z(1)];
estParams.AxesMisalignment = diag([params_x(2), params_y(2), params_z(2)]) * [1 0 0;asin(params_y(3)/params_y(2)) cos(params_y(3)/params_y(2)) 0;sin(params_z(4)/params_z(2)) sin(params_z(3)/params_z(2))*cos(params_z(4)/params_z(2)) cos(params_z(3)/params_z(2))*cos(params_z(4)/params_z(2))];

% Return individual calibration components:
estParams.ScaleFactors = [params_x(2); params_y(2); params_z(2)];
estParams.NonorthogAngles = [params_y(3)/params_y(2) params_z(4)/params_z(2) params_z(3)/params_z(2)];

end