The Accelerometer calibration model is based upon the error associated with the offset bias, $b_a$, the magnitude scale factors, $\bold{S}$, and the internal accelerometer axis misalignment, $\bold{T}$. This is represented in the equation below:
$$ a_\mathrm{i} = \boldsymbol{S}\boldsymbol{T}^{-1}\boldsymbol{g}_i + b_a $$
The static calibration algorithm requires us to take measurements in 6 different orientations of the sensors: +x, -x, +y, -y, +z, -z.
For the calibration process, the raw accelerometer sensor measurements ,$a_\mathrm{raw}$, are substituted for values of gravity being measured in the 6 possible static positions. This equation is re-written below in matrix form:
$$ \begin{bmatrix} a_x \\ a_y \\ a_z \end{bmatrix} = \begin{bmatrix} k_x & 0 & 0 \\ 0 & k_y & 0 \\ 0 & 0 & k_z \\ \end{bmatrix} \begin{bmatrix} 1 & -a_\mathrm{yz} & a_\mathrm{zy} \\ 0 & 1 & -a_\mathrm{zx} \\ 0 & 0 & 1 \end{bmatrix}^{-1} \begin{bmatrix} g_x \\ g_y \\ g_z \end{bmatrix} + \begin{bmatrix} b_x \\ b_y \\ b_z \end{bmatrix} $$
This equation can be written into the following three equations, which are iteratively used to solve for the matrix coefficients.
$$ \begin{bmatrix} a_\mathrm{z1} \\ a_\mathrm{z2} \end{bmatrix} = \begin{bmatrix} 1 & g_\mathrm{z1} \\ 1 & g_\mathrm{z2} \end{bmatrix} \begin{bmatrix} b_z \\ k_z \end{bmatrix} \Longleftrightarrow \begin{bmatrix} b_z \\ k_z \end{bmatrix} = \begin{bmatrix} 1 & g_\mathrm{z1} \\ 1 & g_\mathrm{z2} \end{bmatrix}^\mathrm{-1} \begin{bmatrix} a_\mathrm{z1} \\ a_\mathrm{z2} \end{bmatrix} $$ $$ \begin{bmatrix} a_\mathrm{y1} \\ a_\mathrm{y2} \\ a_\mathrm{y3} \end{bmatrix} = \begin{bmatrix} 1 & g_\mathrm{y1} & g_\mathrm{z1} \\ 1 & g_\mathrm{y2} & g_\mathrm{z2} \\ 1 & g_\mathrm{y3} & g_\mathrm{z3} \end{bmatrix} \begin{bmatrix} b_y \\ k_y \\ k_\mathrm{yzx} \end{bmatrix} \Longleftrightarrow \begin{bmatrix} b_y \\ k_y \\ k_\mathrm{yzx} \end{bmatrix} = \begin{bmatrix} 1 & g_\mathrm{y1} & g_\mathrm{z1} \\ 1 & g_\mathrm{y2} & g_\mathrm{z2} \\ 1 & g_\mathrm{y3} & g_\mathrm{z3} \end{bmatrix}^{-1} \begin{bmatrix} a_\mathrm{y1} \\ a_\mathrm{y2} \\ a_\mathrm{y3} \end{bmatrix} $$ $$ \begin{bmatrix} a_\mathrm{x1} \\ a_\mathrm{x2} \\ a_\mathrm{x3} \\ a_\mathrm{x4} \end{bmatrix} = \begin{bmatrix} 1 & g_\mathrm{x1} & g_\mathrm{y1} & -g_\mathrm{z1} \\ 1 & g_\mathrm{x2} & g_\mathrm{y2} & -g_\mathrm{z2} \\ 1 & g_\mathrm{x3} & g_\mathrm{y3} & -g_\mathrm{z3} \\ 1 & g_\mathrm{x4} & g_\mathrm{y4} & -g_\mathrm{z4} \\ \end{bmatrix} \begin{bmatrix} b_x \\ k_x \\ k_\mathrm{xyz} \\ k_\mathrm{xzy} \end{bmatrix} \Longleftrightarrow \begin{bmatrix} b_x \\ k_x \\ k_\mathrm{xyz} \\ k_\mathrm{xzy} \end{bmatrix} \begin{bmatrix} 1 & g_\mathrm{x1} & g_\mathrm{y1} & -g_\mathrm{z1} \\ 1 & g_\mathrm{x2} & g_\mathrm{y2} & -g_\mathrm{z2} \\ 1 & g_\mathrm{x3} & g_\mathrm{y3} & -g_\mathrm{z3} \\ 1 & g_\mathrm{x4} & g_\mathrm{y4} & -g_\mathrm{z4} \\ \end{bmatrix}^{-1} \begin{bmatrix} a_\mathrm{x1} \\ a_\mathrm{x2} \\ a_\mathrm{x3} \\ a_\mathrm{x4} \end{bmatrix} $$
In order to solve for these systems of equations, each 2, 3, and 4 pick combinations of the 6 different static accelerometer axis options (+x, -x, +y, -y, +z, -z) are solved for iteratively. Each static axis is measured for 500 data points using any MCU, such as an Arduino, Raspberry Pi, STM, Nordic, etc.
There are a total of 15 combinations for pick 2 (z-axis), 20 combinations for pick 3 (y-axis), and 15 combinations for pick 4 (x-axis), which can be determined by calculating that the rank(A) = pick count. The solved for values determined by each combination are averaged together to get average values for the solved variables on the left hand side of the equations above. Using these solved for averaged values, the following remaining coefficients can be solved for:
$$ a_\mathrm{zx} = \frac{k_\mathrm{yzx}}{k_y} \qquad a_\mathrm{yz} = \frac{k_\mathrm{xyz}}{k_x} \qquad a_\mathrm{zy} = \frac{k_\mathrm{xyz}}{k_x} + a_\mathrm{yz}a_\mathrm{zx}\qquad $$
These values can then be substituted back into the $S$, $T$, and $b_a$ matrices. Afterwards, we can find the calibrated values of the Accelerometer using the equation:
$$ S = \begin{bmatrix} k_x & 0 & 0 \\ 0 & k_y & 0 \\ 0 & 0 & k_z \end{bmatrix} \qquad T = \begin{bmatrix} 1 & -a_\mathrm{yz} & a_\mathrm{zy} \\ 0 & 1 & -a_\mathrm{zx} \\ 0 & 0 & 1 \\ \end{bmatrix} \qquad b_a = \begin{bmatrix} b_x \\ b_y \\ b_z \\ \end{bmatrix} \qquad $$
$$$$
$$ a_\mathrm{cal} = \boldsymbol{S}^{-1}\boldsymbol{T}(a_\mathrm{raw} - b_a) $$
def Calibrate_Accel(y0, y1, y2, y3, y4, y5):
idx = [0, 1, 2, 3, 4, 5]
y = np.array([y0, y1, y2, y3, y4, y5])
p2_x = np.zeros((2, 1))
p3_x = np.zeros((3, 1))
p4_x = np.zeros((4, 1))
p2_x = np.delete(p2_x, 0, axis=1)
p3_x = np.delete(p3_x, 0, axis=1)
p4_x = np.delete(p4_x, 0, axis=1)
#**********************************************************************************************
#Pick 2 --> z-axis Index
p2 = 2
pick2 = [comb for comb in itertools.combinations(idx, p2)]
p2_idx = 2
p2_count = 0
for p in pick2:
# Produce Gravity Equivalence array
g1 = [round(num) for num in y[p[0]]]
g2 = [round(num) for num in y[p[1]]]
A = np.array([[1.0, g1[2]], [1.0, g2[2]]])
if (np.linalg.matrix_rank(A) == p2):
Ainv = np.linalg.inv(A)
tmp_p2_y = np.array([y[p[0]][p2_idx], y[p[1]][p2_idx]])
tmp_p2_x = np.matmul(Ainv, tmp_p2_y)
tmp_p2_x = np.array([[tmp_p2_x[0]], [tmp_p2_x[1]]])
p2_x = np.append(p2_x, tmp_p2_x, 1)
p2_count += 1
p2_x_mean = np.mean(p2_x, axis=1)
#**********************************************************************************************
#Pick 3 --> y-axis Index
p3 = 3
pick3 = [comb for comb in itertools.combinations(idx, p3)]
p3_idx = 1
p3_count = 0
for p in pick3:
# Produce Gravity Equivalence array
g1 = [round(num) for num in y[p[0]]]
g2 = [round(num) for num in y[p[1]]]
g3 = [round(num) for num in y[p[2]]]
A = np.array([[1.0, g1[1], g1[2]], [1.0, g2[1], g2[2]], [1.0, g3[1], g3[2]]])
if (np.linalg.matrix_rank(A) == p3):
Ainv = np.linalg.inv(A)
tmp_p3_y = np.array([y[p[0]][p3_idx], y[p[1]][p3_idx], y[p[2]][p3_idx]])
tmp_p3_x = np.matmul(Ainv, tmp_p3_y)
tmp_p3_x = np.array([[tmp_p3_x[0]], [tmp_p3_x[1]], [tmp_p3_x[2]]])
p3_x = np.append(p3_x, tmp_p3_x, 1)
p3_count += 1
p3_x_mean = np.mean(p3_x, axis=1)
#**********************************************************************************************
#Pick 4 --> x-axis Index
p4 = 4
pick4 = [comb for comb in itertools.combinations(idx, p4)]
p4_idx = 0
p4_count = 0
for p in pick4:
# Produce Gravity Equivalence array
g1 = [round(num) for num in y[p[0]]]
g2 = [round(num) for num in y[p[1]]]
g3 = [round(num) for num in y[p[2]]]
g4 = [round(num) for num in y[p[3]]]
A = np.array([[1.0, g1[0], g1[1], -g1[2]], [1.0, g2[0], g2[1], -g2[2]],
[1.0, g3[0], g3[1], -g3[2]], [1.0, g4[0], g4[1], -g4[2]]])
if (np.linalg.matrix_rank(A) == p4):
Ainv = np.linalg.inv(A)
tmp_p4_y = np.array([y[p[0]][p4_idx], y[p[1]][p4_idx], y[p[2]][p4_idx], y[p[3]][p4_idx]])
tmp_p4_x = np.matmul(Ainv, tmp_p4_y)
tmp_p4_x = np.array([[tmp_p4_x[0]], [tmp_p4_x[1]], [tmp_p4_x[2]], [tmp_p4_x[3]]])
p4_x = np.append(p4_x, tmp_p4_x, 1)
p4_count += 1
p4_x_mean = np.mean(p4_x, axis=1)
bz = p2_x_mean[0]
kz = p2_x_mean[1]
by = p3_x_mean[0]
ky = p3_x_mean[1]
kyzx = p3_x_mean[2]
bx = p4_x_mean[0]
kx = p4_x_mean[1]
kxyz = p4_x_mean[2]
kxzy = p4_x_mean[3]
azx = kyzx / ky
ayz = kxyz / kx
azy = (kxzy / kx) + (ayz*azx)
S = np.array([ [kx, 0, 0], [0, ky, 0], [0, 0, kz] ])
T = np.array([ [1, -ayz, azy], [0, 1, -azx], [0, 0, 1]])
b = np.array([ bx, by, bz ])
Sinv = np.linalg.inv(S)
return S, T, b, Sinv