Ellipsoid Fit Implementation

This script aims to implement the ellipsoid fit method proposed in [1] to develop a method for magnetic compass calibration. As the algebraic analysis presented in section (IV) of the paper is not enough for the direct implementation, we will take the algebraic analysis further for the straightforward Python implementation.

[1] Liu, Y. X., Li, X. S., Zhang, X. J., & Feng, Y. B. (2014). Novel calibration algorithm for a three-axis strapdown magnetometer. Sensors, 14(5), 8485-8504.

Magnetic Calibration method

The measurement of a magnetic compass can be modeled as:

\[ h_m^b = Mh^b + b + n \]

Where \(b\) is a constant offset that shifts the output of the sensors, and \(M\) accounts for the the sensitivity of the individual axes of the sensor, the non-orthogonality and misalignment of the axes, and the sum of soft-iron errors fixed to the body frame, which serves to scale the sensor's output.

When the magnetic compass remains stationary and only changes direction, the magnitude of the true magnetic field \(||h^b||\) remains constant, and the locus of the true magnetic field measured \(h^b\) is a sphere. Meanwhile, the locus of the disturbed magnetic field measured \(h_m^b\) is an ellipsoid, and it can be expressed as follows:

\[ ||h^b||^2 = (h_m^b)^TAh_m^b - 2b^TAh_m^b + b^TAb + \tilde{n} \]

Where \(A = G^TG\), \(G = M^{-1}\), and \(\tilde{n} = 2(h_m^b-b)^TG^TGn + n^TG^TGn\). We can see that this is the expression of an ellipsoid in terms of \(h_m^b\). In other words, the measurements \((h_m^b)\) with errors are constrained to lie on an ellipsoid. Thus, the calibration of the magnetic compass is to seek ellipsoid-fitting methods to solve the coefficients of \(G\) and \(b\).

Since an ellipsoid is a kind of concoid, the ellipsoid equation can be expressed as a general equation of a concoid in the 3-D space as follows:

\[ F(a, h_m^b) = a(h_{mx}^b)^2 + b(h_{mx}^bh_{my}^b) + c (h_{my}^b)^2 + d(h_{mx}^bh_{mz}^b) + e(h_{my}^bh_{mz}^b) + j(h_{mz}^b)^2 + p(h_{mx}^b) + q(h_{my}^b) + r(h_{mz}^b) + s = 0\]

Where \(a = \begin{bmatrix} a && b && c && d && e && j && p && q && r && s\end{bmatrix}^T\). Moreover, the problem of fitting an ellipsoid into N data points \(h_m^b\) can be solved by minimizing the sum of squares of the algebraic distance:

\[ min \; \sum_{i}^nF_i(a, h_m^b)^2\]

We define the design matrix (we will use the notation \(h_j^i\) to denote the i-th sample of \(h_{mj}^b\)), where the i-th row is:

\[ S_i = \begin{bmatrix} (h_{x}^i)^2 && h_{x}^ih_{y}^i && (h_{y}^i)^2 && h_{x}^ih_{z}^i && h_{y}^ih_{z}^i && (h_{z}^i)^2 && h_{x}^i && h_{y}^i && h_{z}^i && 1 \end{bmatrix} \]

An additional property of the design matrix is that (S^TS) is symmetric:

\[ (S^TS)^T = (S)^T(S^T)^T = S^TS \; \rightarrow \; (S^TS)^T = S^TS \]

Now, the minimization problem can be presented as:

\[ min \; ||Sa||^2 \; \rightarrow \; min \; (Sa)^T(Sa) \; \rightarrow \; min \; a^TS^TSa\]

In order to avoid the trivial solution \(a = \mathbb{O}_{10}\), and recognizing that any multiple of a solution \(a\) represents the same concoid, the parameter vector \(a\) is constrained in someway. In order that the surface is fitted to be an ellipsoid in 3D, the parameters \(a\) must insure the matrix \(A\) to be either positive or negative definite, the equivalent constrained condition is:

\[4ac - b^2 > 0\]

The imposition of this inequality constraint is difficult in general; in this case, we have the freedom to arbitrarily scale the parameters, so we may simply incorporate the scaling into the constraint and impose the equality constraint \(\(4ac - b^2 = 1\)\), which can be expressed in the matrix form of \(a^TCa = 1\), as:

\[ C = \begin{bmatrix} C_1 && C_2 \\ C_3 && C_4 \end{bmatrix},\; \text{where:} \; C_1 = \begin{bmatrix} 0 && 0 && 2 \\ 0 && -1 && 0 \\ 2 && 0 && 0 \end{bmatrix}, \; C_2 = \mathbb{O}_{3 \times 7}, \; C_3 = \mathbb{O}_{7 \times 3}, \; C_4 = \mathbb{O}_{7 \times 7}\]

Notice that as \(C\) is a block matrix, \(C_1 = C_1^T\), and all the other blocks are zeros:

\[ C^T = \begin{bmatrix} C_1^T && C_3^T \\ C_2^T && C_4^T \end{bmatrix} \; \rightarrow \; C^T = \begin{bmatrix} C_1 && C_2 \\ C_3 && C_4 \end{bmatrix} = C\]

With this additional constraint, the optimization problem is:

\[ min \; a^TS^TSa \quad s.t. \quad a^TCa - 1= 0\]

Using the Lagrange method:

\[ \mathcal{L}(a) = a^TS^TSa - \lambda(a^TCa - 1= 0) \]

Differentiating with respect to \(a\) we find:

\[ \frac{\partial \mathcal{L}}{\partial a} = 0 \; \rightarrow \; a^T(S^TS + SS^T) - \lambda(a^T(C + C^T)) = 0 \; \rightarrow \; 2a^TS^TS - 2\lambda a^TC = 0 \; \rightarrow \; S^TSa = \lambda Ca\]

To solve this system, we can rewrite the matrix \(S^TS\) as a block matrix:

\[ S^TS = \begin{bmatrix} X_{11} && X_{12} \\ X_{21} && X_{22} \end{bmatrix}, \text{ where: } X_{11} \text{ is } 3 \times 3, \; X_{12} \text{ is } 3 \times 7, \; X_{21} \text{ is } 7 \times 3, \; X_{22} \text{ is } 7 \times 7\]

Furthermore, Using the definition that \(S^TS\) is symmetric, then: \(X_{21} = X_{12}^T\):

\[ S^TS = \begin{bmatrix} X_{11} && X_{12} \\ X_{12}^T && X_{22} \end{bmatrix}\]

We can also define: \(a_1 = \begin{bmatrix} a && b && c\end{bmatrix}^T\) and \(a_2 = \begin{bmatrix} d && e && j && p && q && r && s\end{bmatrix}^T\), such that: \(a = \begin{bmatrix} a_1 && a_2\end{bmatrix}^T\). Now, we can rewrite the system as:

\[ \begin{bmatrix} X_{11} && X_{12} \\ X_{12}^T && X_{22} \end{bmatrix}\begin{bmatrix} a_1 \\ a_2\end{bmatrix} = \lambda \begin{bmatrix} C_1 && C_2 \\ C_3 && C_4 \end{bmatrix}\begin{bmatrix} a_1 \\ a_2\end{bmatrix} \]

Considering that \(C_2, C_3, C_4\) are zero matrices, the first equation that we can get from this is:

\[ X_{11}a_1 + X_{12}a_2 = \lambda C_1a_1 \]

The second equation is:

\[ X_{12}^Ta_1 + X_{22}a_2 = 0 \]

If the data is not coplanar, the \(X_{22}\) will be non-singular:

\[ \rightarrow a_2 = -X_{22}^{-1}X_{12}^Ta_1 \]

Replacing this term in the first equation, and considering that \(C_1\) is non-singular as \(det(C_1) = 4\):

\[(X_{11} - \lambda C_1)a_1 + X_{12}(-X_{22}^{-1}X_{12}^Ta_1) = 0 \; \rightarrow \; C_1^{-1}(X_{11} - X_{12}X_{22}^{-1}X_{12}^T)a_1 = \lambda a_1 \]

It is proven that exactly one eigenvalue of the last system is positive. Let \(u_1\) be the eigenvector associated with the only positive eigenvalue of the general system, then we can get the full solution: \(u = \begin{bmatrix} u_1 && u_2\end{bmatrix}^T\). In the case that the matrix \((X_{11} - X_{12}X_{22}^{-1}X_{12}^T)\) is singular, the corresponding \(u_1\) can be replaced with the eigenvector associated with the largest eigenvalue.

With the previously defined equations, we are able to determine the values of: \(a = \begin{bmatrix} a && b && c && d && e && j && p && q && r && s\end{bmatrix}^T\), and with those values we need to recover the soft-iron and hard-iron, \(M\) and \(b\), respectively. By definition:

Soft-Iron

\[ A = \begin{bmatrix} a && b/2 && d/2 \\ b/2 && c && e/2 \\ d/2 && e/2 && j \end{bmatrix} \]

And we also defined that: \(G^TG = A\), with \(G = M^{-1}\). However, there are uncountable matrices \(G\) which can satisfy \(G^TG = A\). Thus, we select the x-axis of the sensors as the x-axis of the magnetic compass; thus we can obtain the unique \(G\) by singular value decomposition.

\[ G = U_G\Sigma_G V_G^T \; \rightarrow G^TG = (V_G\Sigma_G^TU_G^T)(U_G\Sigma_G V_G^T) \; \leftrightarrow \; G^TG = V_G\Sigma_G^T\Sigma_G V_G^T \]

As A is a symmetric matrix, then: \(A^T = A \; \rightarrow \; G^TG = GG^T\).

\[ G = U_G\Sigma_G V_G^T \; \rightarrow GG^T = (U_G\Sigma_G V_G^T)(V_G\Sigma_G^T U_G^T) \; \leftrightarrow \; GG^T = U_G\Sigma_G \Sigma_G^T U_G^T \]

If we do the singular value decomposition of A:

\[ V_G\Sigma_G^T\Sigma_G V_G^T = U_A \Sigma_A V_A^T \qquad \qquad U_G\Sigma_G\Sigma_G^T U_G^T = U_A \Sigma_A V_A^T \]

Then, as \(A\) is a symmetric matrix, then: \(U = V\), therefore:

\[ U_G = U_A \qquad \qquad V_G = V_A \qquad \qquad \Sigma_G = \Sigma_A^{1/2} \]

Then:

\[ G = U_G \Sigma_G V_G^T \quad \rightarrow \quad G = U_A \Sigma_A^{1/2} V_A^T \quad \rightarrow \quad M = (U_A \Sigma_A^{1/2} V_A^T)^{-1} \]

Hard-Iron

From the magnetic field magnitude we found that:

\[ ||h^b||^2 = (h_m^b)^TAh_m^b - 2b^TAh_m^b + b^TAb + \tilde{n} \]

The first term of that equation is related to the quadratic terms, while the second term is related to the linear terms:

\[ - 2b^TAh_m^b = \begin{matrix}h_{mx} \left(- 2 a b_{x} - b b_{y} - b_{z} e\right) + h_{my} \left(- b b_{x} - 2 b_{y} c - b_{z} d\right) + h_{mz} \left(- b_{x} e - b_{y} d - 2 b_{z} j\right)\end{matrix} \]

From the general equation of a concoid in the 3-D space, the linear terms are:

\[ p(h_{mx}^b) + q(h_{my}^b) + r(h_{mz}^b) \]

Therefore, we have the system:

\[ \begin{bmatrix} 2a && b && d \\ b && 2c && e \\ d && e && 2j \end{bmatrix} \begin{bmatrix} b_x \\ b_y \\ b_z \end{bmatrix} = \begin{bmatrix} p \\ q \\ r \end{bmatrix} \; \rightarrow \; \begin{bmatrix} b_x \\ b_y \\ b_z \end{bmatrix} = \begin{bmatrix} 2a && b && d \\ b && 2c && e \\ d && e && 2j \end{bmatrix} ^{-1} \begin{bmatrix} p \\ q \\ r \end{bmatrix} \; \leftrightarrow \; \begin{bmatrix} b_x \\ b_y \\ b_z \end{bmatrix} = (2A)^{-1} \begin{bmatrix} p \\ q \\ r \end{bmatrix}\]

MAGYC - Benchmark Methods - Ellipsoid Fit

This module contains ellipsoid fit appraches for magnetometer calibration.

Functions:

Name	Description
ellipsoid_fit	Standard ellipsoid fit method.
ellipsoid_fit_fang	Ellipsoid fit method by Fang et al.

ellipsoid_fit(magnetic_field)

The ellipsoid fit method is based on the fact that the error model of a magnetic compass is an ellipsoid, and a constraint least-squares method is adopted to estimate the parameters of an ellipsoid by rotating the magnetic compass in various random orientations.

For further details about the implementation, refer to Aleksandr Bazhin Github repository, where he ports to python [matlab's ellipsoid fit].(http://www.mathworks.com/matlabcentral/fileexchange/24693-ellipsoid-fit)

Parameters:

Name	Type	Description	Default
magnetic_field	ndarray or list	Magnetic field measurements in a 3xN or Nx3 numpy array or list.	required

Returns:

Name	Type	Description
hard_iron	ndarray	Hard iron bias.
soft_iron	ndarray	Soft iron matrix.
calibrated_magnetic_field	ndarray	Calibrated magnetic field measurements.

Raises:

Type	Description
TypeError	If the input is not a numpy array or a list.
ValueError	If the input is not a 3xN or Nx3 numpy array.

Source code in magyc/benchmark_methods/ellipsoidfit.py

def ellipsoid_fit(magnetic_field: Union[np.ndarray, list]) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
    """
    The ellipsoid fit method is based on the fact that the error model of a magnetic
    compass is an ellipsoid, and a constraint least-squares method is adopted to
    estimate the parameters of an ellipsoid by rotating the magnetic compass in
    various random orientations.

    For further details about the implementation, refer to Aleksandr Bazhin [Github
    repository](https://github.com/aleksandrbazhin/ellipsoid_fit_python), where he
    ports to python [matlab's ellipsoid fit].(http://www.mathworks.com/matlabcentral/fileexchange/24693-ellipsoid-fit)

    Args:
        magnetic_field (numpy.ndarray or list): Magnetic field measurements in a
            3xN or Nx3 numpy array or list.

    Returns:
        hard_iron (numpy.ndarray): Hard iron bias.
        soft_iron (numpy.ndarray): Soft iron matrix.
        calibrated_magnetic_field (numpy.ndarray): Calibrated magnetic field measurements.

    Raises:
        TypeError: If the input is not a numpy array or a list.
        ValueError: If the input is not a 3xN or Nx3 numpy array.
    """
    # Check if the input is a list and convert it to a numpy array
    if isinstance(magnetic_field, list):
        magnetic_field = np.array(magnetic_field)

    # Check if the input is a numpy array
    if not isinstance(magnetic_field, np.ndarray):
        raise TypeError("The input must be a numpy array or a list.")

    # Check if the input is a 3xN or Nx3 numpy array
    if magnetic_field.ndim != 2 or (magnetic_field.shape[0] != 3 and magnetic_field.shape[1] != 3):
        raise ValueError("The input must be a 3xN or Nx3 numpy array.")

    # Force the array to be a Nx3 numpy array
    if magnetic_field.shape[0] == 3:
        magnetic_field = magnetic_field.T

    # Compute magnetic field calibration
    x, y, z = magnetic_field[:, 0], magnetic_field[:, 1], magnetic_field[:, 2]

    d_matrix = np.array(
        [
            x * x + y * y - 2 * z * z,
            x * x + z * z - 2 * y * y,
            2 * x * y,
            2 * x * z,
            2 * y * z,
            2 * x,
            2 * y,
            2 * z,
            1 - 0 * x,
        ]
    )
    d2 = np.array(x * x + y * y + z * z).T
    u = np.linalg.solve(d_matrix.dot(d_matrix.T), d_matrix.dot(d2))
    a = np.array([u[0] + 1 * u[1] - 1])
    b = np.array([u[0] - 2 * u[1] - 1])
    c = np.array([u[1] - 2 * u[0] - 1])
    v = np.concatenate([a, b, c, u[2:]], axis=0).flatten()
    a_matrix = np.array(
        [[v[0], v[3], v[4], v[6]], [v[3], v[1], v[5], v[7]], [v[4], v[5], v[2], v[8]], [v[6], v[7], v[8], v[9]]]
    )

    center = np.linalg.solve(-a_matrix[:3, :3], v[6:9])

    translation_matrix = np.eye(4)
    translation_matrix[3, :3] = center.T

    r_matrix = translation_matrix.dot(a_matrix).dot(translation_matrix.T)

    evals, evecs = np.linalg.eig(r_matrix[:3, :3] / -r_matrix[3, 3])
    evecs = evecs.T

    radii = np.sqrt(1.0 / np.abs(evals))
    radii *= np.sign(evals)

    a, b, c = radii
    r = (a * b * c) ** (1. / 3.)
    D = np.array([[r/a, 0., 0.], [0., r/b, 0.], [0., 0., r/c]])
    transformation = evecs.dot(D).dot(evecs.T)

    hard_iron = center.reshape(3, 1)
    soft_iron = transformation.reshape(3, 3)

    # Calibrate magnetic field
    calibrated_magnetic_field = (np.linalg.inv(soft_iron) @ (magnetic_field.reshape(3, -1) - hard_iron.reshape(3, 1))).T

    return hard_iron.flatten(), soft_iron, calibrated_magnetic_field

ellipsoid_fit_fang(magnetic_field)

The ellipsoid fit method is based on the fact that the error model of a magnetic compass is an ellipsoid, and a constraint least-squares method is adopted to estimate the parameters of an ellipsoid by rotating the magnetic compass in various random orientations.

For further details about the implementation, refer to section (III) in J. Fang, H. Sun, J. Cao, X. Zhang, and Y. Tao, “A novel calibration method of magnetic compass based on ellipsoid fitting,” IEEE Transactions on Instrumentation and Measurement, vol. 60, no. 6, pp. 2053--2061, 2011.

Parameters:

Name	Type	Description	Default
magnetic_field	ndarray or list	Magnetic field measurements in a 3xN or Nx3 numpy array or list.	required

Returns:

Name	Type	Description
hard_iron	ndarray	Hard iron bias.
soft_iron	ndarray	Soft iron matrix.
calibrated_magnetic_field	ndarray	Calibrated magnetic field measurements.

Raises:

Type	Description
TypeError	If the input is not a numpy array or a list.
ValueError	If the input is not a 3xN or Nx3 numpy array.
RuntimeWarning	If no positive eigenvalues are found.

Source code in magyc/benchmark_methods/ellipsoidfit.py

def ellipsoid_fit_fang(magnetic_field: Union[np.ndarray, list]) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
    """
    The ellipsoid fit method is based on the fact that the error model of a magnetic
    compass is an ellipsoid, and a constraint least-squares method is adopted to
    estimate the parameters of an ellipsoid by rotating the magnetic compass in
    various random orientations.

    For further details about the implementation, refer to section (III) in J. Fang,
    H. Sun, J. Cao, X. Zhang, and Y. Tao, “A novel calibration method of magnetic
    compass based on ellipsoid fitting,” IEEE Transactions on Instrumentation
    and Measurement, vol. 60, no. 6, pp. 2053--2061, 2011.

    Args:
        magnetic_field (numpy.ndarray or list): Magnetic field measurements in a
            3xN or Nx3 numpy array or list.

    Returns:
        hard_iron (numpy.ndarray): Hard iron bias.
        soft_iron (numpy.ndarray): Soft iron matrix.
        calibrated_magnetic_field (numpy.ndarray): Calibrated magnetic field measurements.

    Raises:
        TypeError: If the input is not a numpy array or a list.
        ValueError: If the input is not a 3xN or Nx3 numpy array.
        RuntimeWarning: If no positive eigenvalues are found.
    """
    # Check if the input is a list and convert it to a numpy array
    if isinstance(magnetic_field, list):
        magnetic_field = np.array(magnetic_field)

    # Check if the input is a numpy array
    if not isinstance(magnetic_field, np.ndarray):
        raise TypeError("The input must be a numpy array or a list.")

    # Check if the input is a 3xN or Nx3 numpy array
    if magnetic_field.ndim != 2 or (magnetic_field.shape[0] != 3 and magnetic_field.shape[1] != 3):
        raise ValueError("The input must be a 3xN or Nx3 numpy array.")

    # Force the array to be a Nx3 numpy array
    if magnetic_field.shape[0] == 3:
        magnetic_field = magnetic_field.T

    # Compute magnetic field calibration
    # Design matrix (S)
    s = np.concatenate(
        [
            np.square(magnetic_field[:, [0]]),
            magnetic_field[:, [0]] * magnetic_field[:, [1]],
            np.square(magnetic_field[:, [1]]),
            magnetic_field[:, [0]] * magnetic_field[:, [2]],
            magnetic_field[:, [1]] * magnetic_field[:, [2]],
            np.square(magnetic_field[:, [2]]),
            magnetic_field[:, :],
            np.ones((magnetic_field.shape[0], 1)),
        ],
        axis=1,
    )

    # Block Matrices: S_11, S_12, S_22
    sTs = s.T @ s
    s_11, s_12, s_22 = sTs[:3, :3], sTs[:3, 3:], sTs[3:, 3:]

    # Constrain matrix C_11
    c_11 = np.array([[0, 0, 2], [0, -1, 0], [2, 0, 0]])

    # Ellipsoid Parameters Estimation
    eigenvals, eigenvecs = np.linalg.eig(np.linalg.inv(c_11) @ (s_11 - s_12 @ np.linalg.inv(s_22) @ s_12.T))

    if np.max(eigenvals) < 0:
        warnings.warn("No positive eigenvalues: max eigenvalue = {:.6f}".format(np.max(eigenvals)), RuntimeWarning)

    a_1 = -eigenvecs[:, [np.argmax(eigenvals)]]
    a_2 = -np.linalg.inv(s_22) @ s_12.T @ a_1
    a = np.concatenate([a_1, a_2], axis=0).flatten()

    # Determine A and b
    a_matrix = np.array([[a[0], a[1]/2, a[3]/2], [a[1]/2, a[2], a[4]/2], [a[3]/2, a[4]/2, a[5]]])
    hard_iron = np.linalg.inv(-2*a_matrix) @ np.vstack(a[6:9])

    # Determine G and M
    u, s, vh = np.linalg.svd(a_matrix)
    g = u @ np.sqrt(np.diag(s)) @ vh
    soft_iron = np.linalg.inv(g)

    # Calibrate magnetic field
    calibrated_magnetic_field = (np.linalg.inv(soft_iron) @ (magnetic_field.reshape(3, -1) - hard_iron.reshape(3, 1))).T

    return hard_iron.flatten(), soft_iron, calibrated_magnetic_field