Magnetometer
Sources:
- A way to calibrate a magnetometer, Teslabs Engineering
- Calibration of a magnetometer in combination with inertial sensors, Manon Kok, Thomas B. Schon, and others.
- Least squares ellipsoid specific fitting, Qingde Li and John G.Griffiths
Spherical Magnetometer Representation
A magnetic field measured in any 3-dimensional plane with a magnetometer can be represented as: $$ \bold{h} = R_{x}(\phi) R_{y}(\theta) R_{z}(\psi) h_0 $$ Where $h_0$ represents the Earth’s magnetic field, which is spherical in its nature. Therefore, if our samples taken from the magnetometer are taken in the shape of a sphere, we are able to extrapolate more information from our sensors. $$ \bold{h} \bold{h}^{T} = \bold{F}^2 $$
Noise Sources & Measurement Model
Similarly to the Accelerometer’s calibration algorithm, the Magnetometer may be modeled as having scale-factor noise, $S$, non-orthogonality of the sensor axis, $N$, and an internal sensor bias or offset, $b_m$.
$$ \bold{S} = \begin{bmatrix} s_x & 0 & 0 \\ 0 & s_y & 0 \\ 0 & 0 & s_z \end{bmatrix} \qquad N = \begin{bmatrix} n_x \\ n_y \\ n_z \end{bmatrix} \qquad b_m = \begin{bmatrix} b_{mx} \\ b_{my} \\ b_{mz} \end{bmatrix} $$
However, the magnetometer differs from the Accelerometer in that it may have additional sources of noise from external magnetic fields. We are able to model any sort of Hard-Iron (permanent) sources of noise as a bias, $b_h$, and Soft-Iron (induced) sources of noise as a 3x3 matrix, $A$.
$$ b_h = \begin{bmatrix} b_{hx} \\ b_{hy} \\ b_{hz} \end{bmatrix} \qquad \bold{A}_{i} = \begin{bmatrix} a_{00} & a_{01} & a_{02} \\ a_{01} & a_{11} & a_{12} \\ a_{02} & a_{21} & a_{22} \\ \end{bmatrix} $$
Therefore, we can combine these sources of noise into the following equation:
$$ h_m = \bold{S}\bold{N} \left( \bold{A}_{i} h_{true} + b_h \right) + b_m $$
Where $h_m$ represents the sensor’s measured magnetic field, and $h_{true}$ represents the true magnetic field, without noise sources. We are able to re-write this equation as:
$$ h_m = \bold{S} \bold{N} \bold{A}_{i} h_{true} + \bold{S}\bold{N}b_h + b_m = \bold{A} h_{true} + \bold{b} $$ $$ \bold{A} = \bold{S} \bold{N} \bold{A}_{i} \qquad \bold{b} = \bold{S} \bold{N} b_h + b_m $$
Now we can re-write the equation for $h_m$ as follows: $$ h_m = \bold{A} h_{true} + \bold{b} \qquad \Longrightarrow \qquad h_{true} = \bold{A}^{-1} \left( h_{m} - \bold{b} \right) $$
Now, we must write this equation in the quadratic form(spherical representation) as described by $\bold{h}\bold{h}^{T} = \bold{F}^{2}$: $$ h_{m}^{T} \bold{A}^{-T} \bold{A}^{-1}h_{m} - 2h_{m}^{T}\bold{A}^{-T} \bold{A}^{-1}\bold{b} + \bold{b}^{T} \bold{A}^{-T} \bold{A}^{-1}\bold{b} - \bold{F}^2 = 0 $$ $$ = h_{m}^{T} \bold{M} h_{m} + h_{m} \bold{n} + \bold{d} = 0 $$ Where: $$ \bold{M} = \bold{A}^{-T} \bold{A}^{-1} \qquad \bold{n} = -2 \bold{A}^{-T} \bold{A}^{-1} \bold{b} \qquad \bold{d} = \bold{b}^{T} \bold{A}^{-T} \bold{A}^{-1} \bold{b} - \bold{F}^2 $$
Using techniques found in Calibration of a magnetometer in combination with inertial sensors, we can come to the solutions:
$$ b = -\bold{M}^{-1} \bold{n} \qquad \qquad A^{-1} = real\left( \frac{\bold{F}}{\sqrt{ \bold{n}^{T} \bold{M}^{-1} \bold{n}- \bold{d}}}\bold{M}^{1/2} \right) $$
Which are then used to solve for our calibrated values of the magnetometer:
$$ h_{cal} = h_{true} = \bold{A}^{-1} \left( h_{m} - \bold{b} \right) $$
Calibration Algorithm
The equations refered to in this section are found in Least squares ellipsoid specific fitting.
The Magnetometer calibration technique involves taking unfiltered and uncalibrated data and fitting the data points to a quadratic surface using multiple matrices.
$$ Surface = ax^2 + by^2 + cz^2 +2fyz + 2gxz + 2hxy + 2px + 2qy + 2rz + d = 0 $$
$$ D = \begin{bmatrix} x^2 & y^2 & z^2 & 2yz & 2xz & 2xy & 2x & 2y & 2z & 1 \end{bmatrix} $$
$$ v = \begin{bmatrix} a & b & c & f & g &h & p & q & r & d \end{bmatrix}^{T} $$
$$ C = \begin{pmatrix} -1 & \frac{k}{2}-1 & \frac{k}{2}-1 & \phantom{-}0 & \phantom{-}0 & \phantom{-}0 \\ \frac{k}{2}-1 & -1 & \frac{k}{2}-1 & \phantom{-}0 & \phantom{-}0 & \phantom{-}0 \\ \frac{k}{2}-1 & \frac{k}{2}-1 & -1 & \phantom{-}0 & \phantom{-}0 & \phantom{-}0 \\ \phantom{-}0 & \phantom{-}0 & \phantom{-}0 & -k & \phantom{-}0 & \phantom{-}0 \\ \phantom{-}0 & \phantom{-}0 & \phantom{-}0 & \phantom{-}0 & -k & \phantom{-}0 \\ \phantom{-}0 & \phantom{-}0 & \phantom{-}0 & \phantom{-}0 & \phantom{-}0 & -k \\ \end{pmatrix} $$
In our case, we are going to set $k = 4$.
$$ \boldsymbol{DD}^Tv = \lambda\boldsymbol{C}v $$ $$ v^T\boldsymbol{C}v = 1 $$
Using Equation (11), solve for $S = DD^T$.
$$ \boldsymbol{S} = \boldsymbol{DD}^T = \begin{bmatrix} S_\mathrm{0} & S_\mathrm{1} \\ S_\mathrm{2} & S_\mathrm{3} \\ \end{bmatrix} \qquad \boldsymbol{v} = \begin{bmatrix} v_1 \\ v_2 \\ \end{bmatrix} $$ Where the element $S_{2}$ is equal to the transpose of $S_{1}$, written as: $S_{2} = S_{1}^{T}$.
$$ \left( S_\mathrm{0} - \lambda\boldsymbol{C} \right)v_1 + S_\mathrm{1}v_2 = 0 $$
Next, solve for the Eigenvalue and Eigenvector of the system to determine $v_1$ as shown below:
$$ \lambda v_\mathrm{1} = \boldsymbol{C}^{-1} \left( S_{0} - S_{1} S^{-1}_{3} S_2 \right) v_1 $$
Next we solve for $v_2$ using equation (13), as shown below:
$$ S_\mathrm{2} v_{1} + S_\mathrm{3} v_{2} = 0 \qquad \Longrightarrow \qquad v_2 = - S_{3}^{-1} S_2 v_1 $$
Now, we can find $\boldsymbol{v}$ by concactenating $v_1$ and $v_2$, as done in Equation (11).
The values in $\boldsymbol{v}$ by indices are then assigned to vectors $\bold{M}$, $\bold{n}$, and $\bold{d}$.
$$ \bold{M} = \begin{bmatrix} v_0 & v_5 & v_4 \\ v_5 & v_1 & v_3 \\ v_4 & v_3 & v_2 \\ \end{bmatrix} \qquad \bold{n} = \begin{bmatrix} v_6 \\ v_7 \\ v_8 \\ \end{bmatrix} \qquad \bold{d} = \begin{bmatrix} v_9 \\ \end{bmatrix} \qquad $$
Using the values of $\bold{M}$, $\bold{n}$, and $\bold{d}$, we can determine matrices $A_m^{-1}$ and $b_m$ that will be used in calibrating magnetometer readings. The multiplication between matrices used in $\bold{A}_m^{-1}$ and $\bold{b}_m$ is dot multiplication, not matrix multiplication. $$ \bold{b}_m = -\bold{M}^{-1}\bold{n} $$
$$ \bold{A}_m^{-1} = real\left( \frac{\bold{F}}{\sqrt{\bold{n}^{T}\bold{M}^{-1}\bold{n}-\bold{d}}}\bold{M}^{1/2} \right) = real \left( \frac{1}{\sqrt{\bold{n}^{T} \bold{M}^{-1} \bold{n} - \bold{d}}}\bold{M}^{1/2} \right) $$
Where $F$ is the magnitude, commonly normalized to 1.
Now that we have determined the matrices used for calibration, we can use the equation below to determine the calibrated readings from the Magnetometer.
$$ h_{cal} = \bold{A}_m^{-1} (h_{raw} - \bold{b}_m) $$
Where $h_{cal}$ are the calibrated readings, and $h_{raw}$ are the raw sensor readings.
The code to this algorithm is found below:
def Calibrate_Mag(magX, magY, magZ):
x2 = (magX ** 2)
y2 = (magY ** 2)
z2 = (magZ ** 2)
yz = 2*np.multiply(magY, magZ)
xz = 2*np.multiply(magX, magZ)
xy = 2*np.multiply(magX, magY)
x = 2*magX
y = 2*magY
z = 2*magZ
d_tmp = np.ones(len(magX))
d = np.expand_dims(d_tmp, axis=1)
D = np.array([x2, y2, z2, yz, xz, xy, x, y, z, d])
D = D[:,:, 0]
C1 = np.array([[-1, 1, 1, 0, 0, 0],
[1, -1, 1, 0, 0, 0],
[1, 1, -1, 0, 0, 0],
[0, 0, 0, -4, 0, 0],
[0, 0, 0, 0, -4, 0],
[0, 0, 0, 0, 0, -4]])
# Equation 11 --- S = D(D.T)
#D_T = np.transpose(D, (1, 0, 2))
S = np.matmul(D, D.T)
print("S Shape: ", S.shape)
S11 = S[:6, :6]
S12 = S[:6, 6:]
S21 = S[6:, :6]
S22 = S[6:, 6:]
print(S22.shape)
# Equation 15
tmp1 = np.matmul(S12, np.matmul(np.linalg.inv(S22), S12.T))
tmp = np.matmul(np.linalg.inv(C1), S11 - tmp1)
eigenValue, eigenVector = np.linalg.eig(tmp)
v1 = eigenVector[:, np.argmax(eigenValue)]
if v1[0] < 0: v1 = -v1
# Equation 13
v2 = np.matmul(-np.matmul(np.linalg.inv(S22), S12.T), v1)
# Equation 11 (part 2)
v = np.concatenate([v1, v2]).T
M = np.array([[v[0], v[5], v[4]],
[v[5], v[1], v[3]],
[v[4], v[3], v[2]]])
n = np.array([[v[6]],
[v[7]],
[v[8]]])
d = v[9]
Minv = np.linalg.inv(M)
b = -np.dot(Minv, n)
Ainv = np.real(1 / np.sqrt(np.dot(n.T, np.dot(Minv, n)) - d) * linalg.sqrtm(M))
return Minv, b, Ainv
Plotting this content should result to something similar as shown below. It appears that I extended the magnetometer alittle bit more than average in parts of the rotation of the IMU.
Here’s some of the code used to plot the magnetometer calibration.
fig = plt.figure(1)
ax = fig.add_subplot(111, projection='3d')
ax.scatter(mX, mY, mZ, s=5, color='r')
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
u = np.linspace(0, 2 * np.pi, 100)
v = np.linspace(0, np.pi, 100)
x = np.outer(np.cos(u), np.sin(v))
y = np.outer(np.sin(u), np.sin(v))
z = np.outer(np.ones(np.size(u)), np.cos(v))
ax.plot_wireframe(x, y, z, rstride=10, cstride=10, alpha=0.5)
ax.plot_surface(x, y, z, alpha=0.3, color='b')
print("A_inv: ")
print(Ainv)
print()
print("b")
print(b)
print()
calibratedX = np.zeros(mX.shape)
calibratedY = np.zeros(mY.shape)
calibratedZ = np.zeros(mZ.shape)
totalError = 0
for i in range(len(mX)):
h = np.array([[mX[i], mY[i], mZ[i]]]).T
hHat = np.matmul(Ainv, h-b)
hHat = hHat[:, :, 0]
calibratedX[i] = hHat[0][0]
calibratedY[i] = hHat[0][1]
calibratedZ[i] = hHat[0][2]
mag = np.dot(hHat.T, hHat)
err = (mag[0][0] - 1)**2
totalError += err
print("Total Error: %f" % totalError)
fig2 = plt.figure(2)
ax2 = fig2.add_subplot(111, projection='3d')
ax2.scatter(calibratedX, calibratedY, calibratedZ, s=1, color='r')
ax2.set_xlabel('X')
ax2.set_ylabel('Y')
ax2.set_zlabel('Z')
# plot unit sphere
u = np.linspace(0, 2 * np.pi, 100)
v = np.linspace(0, np.pi, 100)
x = np.outer(np.cos(u), np.sin(v))
y = np.outer(np.sin(u), np.sin(v))
z = np.outer(np.ones(np.size(u)), np.cos(v))
ax2.plot_wireframe(x, y, z, rstride=10, cstride=10, alpha=0.5)
ax2.plot_surface(x, y, z, alpha=0.3, color='b')
plt.show()