function [crlb] = computeCRLB_SPM_FULL(sigma, trueParams, us)
%For method of Jurman/ Hiller et al.
%   sigma - std of the accelerometer measurements

M = length(us); % number of positions

N = 100; % samples in the averaging?

s_x = trueParams.AxesMisalignment(1, 1); s_y = trueParams.AxesMisalignment(2, 2); s_z = trueParams.AxesMisalignment(3, 3);
alpha = trueParams.NonorthogAngles(1); beta = trueParams.NonorthogAngles(2); gamma = trueParams.NonorthogAngles(3);
phi = trueParams.MisalignAngles(1); theta = trueParams.MisalignAngles(2); psi = trueParams.MisalignAngles(3);
b_x = trueParams.Bias(1); b_y = trueParams.Bias(2); b_z = trueParams.Bias(3);

As = zeros(12, 12, M); % to store A matrices below


for m = 1:M % for each position in the data

    u = us(m, :); u_x = u(1); u_y = u(2); u_z = u(3); % true specific force values in platform frame.

    dbyds_x = [u_z*sin(theta) + u_x*cos(psi)*cos(theta) - u_y*cos(theta)*sin(psi); 0; 0];
    dbyds_y = [0; u_x*(cos(phi)*sin(psi) + alpha*cos(psi)*cos(theta) + cos(psi)*sin(phi)*sin(theta)) - u_y*(sin(phi)*sin(psi)*sin(theta) - cos(phi)*cos(psi) + alpha*cos(theta)*sin(psi)) - u_z*(cos(theta)*sin(phi) - alpha*sin(theta)); 0];
    dbyds_z = [0; 0; u_y*(cos(psi)*sin(phi) + gamma*(cos(phi)*cos(psi) - sin(phi)*sin(psi)*sin(theta)) - beta*cos(theta)*sin(psi) + cos(phi)*sin(psi)*sin(theta)) + u_z*(cos(phi)*cos(theta) + beta*sin(theta) - gamma*cos(theta)*sin(phi)) + u_x*(sin(phi)*sin(psi) + gamma*(cos(phi)*sin(psi) + cos(psi)*sin(phi)*sin(theta)) + beta*cos(psi)*cos(theta) - cos(phi)*cos(psi)*sin(theta))];
    dbydalpha = [0; s_y*u_z*sin(theta) - s_y*u_y*cos(theta)*sin(psi) + s_y*u_x*cos(psi)*cos(theta); 0];
    dbydbeta = [0; 0; s_z*u_z*sin(theta) - s_z*u_y*cos(theta)*sin(psi) + s_z*u_x*cos(psi)*cos(theta)];
    dbydgamma = [0; 0; s_z*u_x*(cos(phi)*sin(psi) + cos(psi)*sin(phi)*sin(theta)) + s_z*u_y*(cos(phi)*cos(psi) - sin(phi)*sin(psi)*sin(theta)) - s_z*u_z*cos(theta)*sin(phi)];
    dbydphi = [0; - s_y*u_x*(sin(phi)*sin(psi) - cos(phi)*cos(psi)*sin(theta)) - s_y*u_y*(cos(psi)*sin(phi) + cos(phi)*sin(psi)*sin(theta)) - s_y*u_z*cos(phi)*cos(theta); u_x*(s_z*(cos(phi)*sin(psi) + cos(psi)*sin(phi)*sin(theta)) - gamma*s_z*(sin(phi)*sin(psi) - cos(phi)*cos(psi)*sin(theta))) + u_y*(s_z*(cos(phi)*cos(psi) - sin(phi)*sin(psi)*sin(theta)) - gamma*s_z*(cos(psi)*sin(phi) + cos(phi)*sin(psi)*sin(theta))) - u_z*(s_z*cos(theta)*sin(phi) + gamma*s_z*cos(phi)*cos(theta))];
    dbydtheta = [s_x*u_z*cos(theta) - s_x*u_x*cos(psi)*sin(theta) + s_x*u_y*sin(psi)*sin(theta); u_x*(s_y*cos(psi)*cos(theta)*sin(phi) - alpha*s_y*cos(psi)*sin(theta)) - u_y*(s_y*cos(theta)*sin(phi)*sin(psi) - alpha*s_y*sin(psi)*sin(theta)) + u_z*(alpha*s_y*cos(theta) + s_y*sin(phi)*sin(theta)); u_y*(beta*s_z*sin(psi)*sin(theta) + s_z*cos(phi)*cos(theta)*sin(psi) - gamma*s_z*cos(theta)*sin(phi)*sin(psi)) - u_x*(s_z*cos(phi)*cos(psi)*cos(theta) + beta*s_z*cos(psi)*sin(theta) - gamma*s_z*cos(psi)*cos(theta)*sin(phi)) + u_z*(beta*s_z*cos(theta) - s_z*cos(phi)*sin(theta) + gamma*s_z*sin(phi)*sin(theta))];
    dbydpsi = [- s_x*u_x*cos(theta)*sin(psi) - s_x*u_y*cos(psi)*cos(theta); u_x*(s_y*(cos(phi)*cos(psi) - sin(phi)*sin(psi)*sin(theta)) - alpha*s_y*cos(theta)*sin(psi)) - u_y*(s_y*(cos(phi)*sin(psi) + cos(psi)*sin(phi)*sin(theta)) + alpha*s_y*cos(psi)*cos(theta)); u_x*(s_z*(cos(psi)*sin(phi) + cos(phi)*sin(psi)*sin(theta)) + gamma*s_z*(cos(phi)*cos(psi) - sin(phi)*sin(psi)*sin(theta)) - beta*s_z*cos(theta)*sin(psi)) - u_y*(s_z*(sin(phi)*sin(psi) - cos(phi)*cos(psi)*sin(theta)) + gamma*s_z*(cos(phi)*sin(psi) + cos(psi)*sin(phi)*sin(theta)) + beta*s_z*cos(psi)*cos(theta))];
    dbydb_x = [1; 0; 0]; dbydb_y = [0; 1; 0]; dbydb_z = [0; 0; 1];

    A1_1 = dot(dbyds_x, dbyds_x); A2_1 = dot(dbyds_y, dbyds_x); A3_1 = dot(dbyds_z, dbyds_x); A4_1 = dot(dbydalpha, dbyds_x); A5_1 = dot(dbydbeta, dbyds_x);
    A1_2 = dot(dbyds_x, dbyds_y); A2_2 = dot(dbyds_y, dbyds_y); A3_2 = dot(dbyds_z, dbyds_y); A4_2 = dot(dbydalpha, dbyds_y); A5_2 = dot(dbydbeta, dbyds_y);
    A1_3 = dot(dbyds_x, dbyds_z); A2_3 = dot(dbyds_y, dbyds_z); A3_3 = dot(dbyds_z, dbyds_z); A4_3 = dot(dbydalpha, dbyds_z); A5_3 = dot(dbydbeta, dbyds_z);
    A1_4 = dot(dbyds_x, dbydalpha); A2_4 = dot(dbyds_y, dbydalpha); A3_4 = dot(dbyds_z, dbydalpha); A4_4 = dot(dbydalpha, dbydalpha); A5_4 = dot(dbydbeta, dbydalpha);
    A1_5 = dot(dbyds_x, dbydbeta); A2_5 = dot(dbyds_y, dbydbeta); A3_5 = dot(dbyds_z, dbydbeta); A4_5 = dot(dbydalpha, dbydbeta); A5_5 = dot(dbydbeta, dbydbeta);
    A1_6 = dot(dbyds_x, dbydgamma); A2_6 = dot(dbyds_y, dbydgamma); A3_6 = dot(dbyds_z, dbydgamma); A4_6 = dot(dbydalpha, dbydgamma); A5_6 = dot(dbydbeta, dbydgamma);
    A1_7 = dot(dbyds_x, dbydphi); A2_7 = dot(dbyds_y, dbydphi); A3_7 = dot(dbyds_z, dbydphi); A4_7 = dot(dbydalpha, dbydphi); A5_7 = dot(dbydbeta, dbydphi);
    A1_8 = dot(dbyds_x, dbydtheta); A2_8 = dot(dbyds_y, dbydtheta); A3_8 = dot(dbyds_z, dbydtheta); A4_8 = dot(dbydalpha, dbydtheta); A5_8 = dot(dbydbeta, dbydtheta);
    A1_9 = dot(dbyds_x, dbydpsi); A2_9 = dot(dbyds_y, dbydpsi); A3_9 = dot(dbyds_z, dbydpsi); A4_9 = dot(dbydalpha, dbydpsi); A5_9 = dot(dbydbeta, dbydpsi);
    A1_10 = dot(dbyds_x, dbydb_x); A2_10 = dot(dbyds_y, dbydb_x); A3_10 = dot(dbyds_z, dbydb_x); A4_10 = dot(dbydalpha, dbydb_x); A5_10 = dot(dbydbeta, dbydb_x);
    A1_11 = dot(dbyds_x, dbydb_y); A2_11 = dot(dbyds_y, dbydb_y); A3_11 = dot(dbyds_z, dbydb_y); A4_11 = dot(dbydalpha, dbydb_y); A5_11 = dot(dbydbeta, dbydb_y);
    A1_12 = dot(dbyds_x, dbydb_z); A2_12 = dot(dbyds_y, dbydb_z); A3_12 = dot(dbyds_z, dbydb_z); A4_12 = dot(dbydalpha, dbydb_z); A5_12 = dot(dbydbeta, dbydb_z);

    A6_1 = dot(dbydgamma, dbyds_x); A7_1 = dot(dbydphi, dbyds_x); A8_1 = dot(dbydtheta, dbyds_x); A9_1 = dot(dbydpsi, dbyds_x); A10_1 = dot(dbydb_x, dbyds_x);
    A6_2 = dot(dbydgamma, dbyds_y); A7_2 = dot(dbydphi, dbyds_y); A8_2 = dot(dbydtheta, dbyds_y); A9_2 = dot(dbydpsi, dbyds_y); A10_2 = dot(dbydb_x, dbyds_y);
    A6_3 = dot(dbydgamma, dbyds_z); A7_3 = dot(dbydphi, dbyds_z); A8_3 = dot(dbydtheta, dbyds_z); A9_3 = dot(dbydpsi, dbyds_z); A10_3 = dot(dbydb_x, dbyds_z);
    A6_4 = dot(dbydgamma, dbydalpha); A7_4 = dot(dbydphi, dbydalpha); A8_4 = dot(dbydtheta, dbydalpha); A9_4 = dot(dbydpsi, dbydalpha); A10_4 = dot(dbydb_x, dbydalpha);
    A6_5 = dot(dbydgamma, dbydbeta); A7_5 = dot(dbydphi, dbydbeta); A8_5 = dot(dbydtheta, dbydbeta); A9_5 = dot(dbydpsi, dbydbeta); A10_5 = dot(dbydb_x, dbydbeta);
    A6_6 = dot(dbydgamma, dbydgamma); A7_6 = dot(dbydphi, dbydgamma); A8_6 = dot(dbydtheta, dbydgamma); A9_6 = dot(dbydpsi, dbydgamma); A10_6 = dot(dbydb_x, dbydgamma);
    A6_7 = dot(dbydgamma, dbydphi); A7_7 = dot(dbydphi, dbydphi); A8_7 = dot(dbydtheta, dbydphi); A9_7 = dot(dbydpsi, dbydphi); A10_7 = dot(dbydb_x, dbydphi);
    A6_8 = dot(dbydgamma, dbydtheta); A7_8 = dot(dbydphi, dbydtheta); A8_8 = dot(dbydtheta, dbydtheta); A9_8 = dot(dbydpsi, dbydtheta); A10_8 = dot(dbydb_x, dbydtheta);
    A6_9 = dot(dbydgamma, dbydpsi); A7_9 = dot(dbydphi, dbydpsi); A8_9 = dot(dbydtheta, dbydpsi); A9_9 = dot(dbydpsi, dbydpsi); A10_9 = dot(dbydb_x, dbydpsi);
    A6_10 = dot(dbydgamma, dbydb_x); A7_10 = dot(dbydphi, dbydb_x); A8_10 = dot(dbydtheta, dbydb_x); A9_10 = dot(dbydpsi, dbydb_x); A10_10 = dot(dbydb_x, dbydb_x);
    A6_11 = dot(dbydgamma, dbydb_y); A7_11 = dot(dbydphi, dbydb_y); A8_11 = dot(dbydtheta, dbydb_y); A9_11 = dot(dbydpsi, dbydb_y); A10_11 = dot(dbydb_x, dbydb_y);
    A6_12 = dot(dbydgamma, dbydb_z); A7_12 = dot(dbydphi, dbydb_z); A8_12 = dot(dbydtheta, dbydb_z); A9_12 = dot(dbydpsi, dbydb_z); A10_12 = dot(dbydb_x, dbydb_z);

    A11_1 = dot(dbydb_y, dbyds_x); A12_1 = dot(dbydb_z, dbyds_x);
    A11_2 = dot(dbydb_y, dbyds_y); A12_2 = dot(dbydb_z, dbyds_y);
    A11_3 = dot(dbydb_y, dbyds_z); A12_3 = dot(dbydb_z, dbyds_z);
    A11_4 = dot(dbydb_y, dbydalpha); A12_4 = dot(dbydb_z, dbydalpha);
    A11_5 = dot(dbydb_y, dbydbeta); A12_5 = dot(dbydb_z, dbydbeta);
    A11_6 = dot(dbydb_y, dbydgamma); A12_6 = dot(dbydb_z, dbydgamma);
    A11_7 = dot(dbydb_y, dbydphi); A12_7 = dot(dbydb_z, dbydphi);
    A11_8 = dot(dbydb_y, dbydtheta); A12_8 = dot(dbydb_z, dbydtheta);
    A11_9 = dot(dbydb_y, dbydpsi); A12_9 = dot(dbydb_z, dbydpsi);
    A11_10 = dot(dbydb_y, dbydb_x); A12_10 = dot(dbydb_z, dbydb_x);
    A11_11 = dot(dbydb_y, dbydb_y); A12_11 = dot(dbydb_z, dbydb_y);
    A11_12 = dot(dbydb_y, dbydb_z); A12_12 = dot(dbydb_z, dbydb_z);


    As(:, :, m) = [A1_1 A1_2 A1_3 A1_4 A1_5 A1_6 A1_7 A1_8 A1_9 A1_10 A1_11 A1_12
        A2_1 A2_2 A2_3 A2_4 A2_5 A2_6 A2_7 A2_8 A2_9 A2_10 A2_11 A2_12
        A3_1 A3_2 A3_3 A3_4 A3_5 A3_6 A3_7 A3_8 A3_9 A3_10 A3_11 A3_12
        A4_1 A4_2 A4_3 A4_4 A4_5 A4_6 A4_7 A4_8 A4_9 A4_10 A4_11 A4_12
        A5_1 A5_2 A5_3 A5_4 A5_5 A5_6 A5_7 A5_8 A5_9 A5_10 A5_11 A5_12
        A6_1 A6_2 A6_3 A6_4 A6_5 A6_6 A6_7 A6_8 A6_9 A6_10 A6_11 A6_12
        A7_1 A7_2 A7_3 A7_4 A7_5 A7_6 A7_7 A7_8 A7_9 A7_10 A7_11 A7_12
        A8_1 A8_2 A8_3 A8_4 A8_5 A8_6 A8_7 A8_8 A8_9 A8_10 A8_11 A8_12
        A9_1 A9_2 A9_3 A9_4 A9_5 A9_6 A9_7 A9_8 A9_9 A9_10 A9_11 A9_12
        A10_1 A10_2 A10_3 A10_4 A10_5 A10_6 A10_7 A10_8 A10_9 A10_10 A10_11 A10_12
        A11_1 A11_2 A11_3 A11_4 A11_5 A11_6 A11_7 A11_8 A11_9 A11_10 A11_11 A11_12
        A12_1 A12_2 A12_3 A12_4 A12_5 A12_6 A12_7 A12_8 A12_9 A12_10 A12_11 A12_12];

end

A = sum(As, 3);
J = (N / sigma^2) .* A;

crlb = diag(inv(J));

end