Real-Time Calibration of Magnetometers Using the RLS/ML Algorithm

Abstract

This study presents a new real-time calibration algorithm for three-axis magnetometers by combining the recursive least square (RLS) estimation and maximum likelihood (ML) estimation methods. Magnetometers are widely employed to determine the heading information by sensing the magnetic field of earth; however, they are vulnerable to ambient magnetic disturbances. This makes the calibration of a magnetometer inevitable before it is employed. In this paper, first, a complete measurement error model of the magnetometer is studied, and a simplified model is developed. Then, the real-time RLS algorithm is introduced and discussed in detail, and the unbiased optimal ML is utilized to improve the accuracy of the parameter estimation. The proposed algorithm is advantageous in correcting the parameters in real time and simultaneously obtaining unbiased parameter estimation. Finally, the simulation and experimental results demonstrate that both the accuracy and computational speed of the proposed algorithm is better than those of the widely used bath-processing method. Moreover, the proposed calibration method can be adopted for calibrating other three-axis sensors.

1. Introduction

With the rapid development of micro electromechanical system (MEMS) technology, developing accurate long-term positioning systems based on the MEMS inertial devices without any external signals may become possible. Currently, most magnetic and inertial measurement units (MIMU) based on the MEMS technology integrate three accelerometers, gyroscopes, and magnetometers each, assembled in three orthogonal directions. Due to the weak yaw observation of the conventional navigation systems, magnetometers are widely utilized for heading estimation based on the principle that the local magnetic field points to the north [1,2,3,4].

Nevertheless, the yaw information obtained by the magnetic sensor is usually vulnerable to magnetic field disturbances [5]. Further, the result obtained is sometimes worse with magnetometers than those without magnetometers, if the local magnetic field is contaminated. The magnetic disturbances can be classified into hard iron and soft iron disturbances. The hard iron disturbances are caused by ferromagnetic materials with a permanent magnetic field, e.g., magnets and speakers, which are time-invariant, whereas the soft iron disturbances are deflections or alterations in the existing magnetic field, which are caused by magnetized materials such as steel shield and batteries [2]. Moreover, magnetometers inherently possess non-orthogonality, scaling, and bias errors. Thus, magnetometer calibration is indispensable in realizing accurate orientation estimation.

The conventional magnetometer calibration methods assume that a reference sensor, which can provide accurate heading information, is available [6,7,8]. A well-known example of such an approach is compass swinging [7]. However, this method requires external equipment for assistance, making it expensive and impractical for a wide range of applications. Thus, magnetometer calibration using the information provided only by the MIMU has been increasingly popular. Magnetometer calibration without the aid of external equipment can generally be grouped to two categories: one that uses the data from only the magnetometer, and the other that performs calibration along with inertial sensors. In the first category, a classical method is used to formulate the calibration problem as an ellipsoid fitting problem based on the least-square (LS) method, similar to mapping an ellipsoid data to a sphere [9,10]. This approach has the advantage that solving an ellipsoid fitting problem is simple. However, it has some clear drawbacks: sufficient measured data must be available to avoid the emergence of singular matrix for the least-squares solution; determining the required volume of measured data is difficult. Another class is the TWOSTEP batch method designed by Gebre-Egziabher et al. and Zhang et al. [11,12], which is based on the differences between the actual and the measured unit vectors. In this method, first, the centering approximation is utilized to yield a good initial estimation based on the LS method. Second, the non-orthogonality and the scaling and bias errors are estimated using the bath Gauss-Newton method. However, the estimation result is not an optimal solution because of the existence of noise in the least square coefficient matrix. Furthermore, the magnetometer calibration problem can be formulated as a suboptimal maximum likelihood estimation (called NM) by considering the smallest parameter dimension [13,14]. This derivation is based on the fact that the magnitude of the calibrated measurements must be constant in a homogeneous magnetic field; further, the Gauss–Newton method is utilized to address the NM problem. However, this method has the following limitations: first, this method yields biased results because of the quadratic item of the noise, which has an imperative effect on the estimation. Second, the objective function of the NM is quartic in parameters, which complicates the calibration with multiple minima and maxima, thus necessitating a good estimation for the nonlinear solver. Wu et al. [15] proposed the use of quadratic unbiased optimal maximum likelihood (ML) estimation, which is superior to the conventional quartic suboptimal ML estimation both in accuracy and stability. It must be noted that all the above-mentioned methods are batch-processing methods. Many studies in the literature propose a real-time calibration algorithm for magnetometers, specifically the filtering algorithm, to address the problems in the above-mentioned methods [16,17]. Although the real-time calibration algorithm can estimate the parameters in real time and reduce computational loads, the filtering algorithm is still a biased estimation.

In the second category, the magnetometer calibration is performed with assistance from inertial sensors [18,19,20,21,22,23]. This approach has the advantage that it can perform an alignment estimation of the sensor axes between the inertial sensors and the magnetometer. However, it increases the complexity of the system and may introduce unnecessary errors from the inertial sensors (gyroscope/accelerometer). In actual inertial navigation systems, the alignment error between the sensor axes can be ignored by comparison with the other errors. Thus, magnetometer calibration using only the information itself is the main objective of this research.

To address the above-mentioned problems, this research proposes a combination of the recursive least square (RLS) estimation method and the optimal ML estimation algorithm to successfully realize magnetometer calibration utilizing only the magnetometer information. The proposed algorithm can estimate the parameters in real time and simultaneously obtain unbiased optimal estimation results. Moreover, this algorithm can adaptively detect the calibration implementation, which is discussed in detail in the following section. Notably, because of its similar measurement model, the proposed calibration algorithm can be applied to other three-axis sensors.

The following sections are organized as given below. In Section 2, a detailed description of our proposed RLS algorithm is presented. In Section 3, the quadratic optimal ML estimation is discussed in detail to further improve the accuracy. In Section 4, the results of numerous simulations and experiments performed in this study are provided and discussed. Finally, the conclusions are provided in Section 5.

2. Sensor Model and Initial Estimation

2.1. Sensor Model

Based on previous studies [13,18,24], the complete error model of the magnetometer is given by,

$$\begin{array}{lll}{\tilde{\mathit{m}}}^b& =& {\mathit{S}}_M{\mathit{C}}_{NO}\left({\mathit{C}}_{SI}{\mathit{C}}_n^b{\mathit{m}}^n+{\mathit{b}}_{HI}\right)+{\mathit{b}}_M+{\mathit{n}}_m\\ & =& \Vert {\mathit{m}}^n\Vert {\mathit{S}}_M{\mathit{C}}_{NO}{\mathit{C}}_{SI}{\mathit{C}}_n^b{\overline{\mathit{m}}}^n+{\mathit{S}}_M{\mathit{C}}_{NO}{\mathit{b}}_{HI}+{\mathit{b}}_M+{\mathit{n}}_m\\ & =& \mathit{P}{\mathit{C}}_n^b{\overline{\mathit{m}}}^n+{\mathit{h}}_m+{\mathit{n}}_m\end{array}$$

(1)

where $\begin{array}{ccc}\mathit{P}& =& {\mathit{m}}^n{\mathit{S}}_M{\mathit{C}}_{\mathit{NO}}{\mathit{C}}_{\mathit{SI}}\end{array}$, ${\mathit{h}}_m={\mathit{S}}_M{\mathit{C}}_{\mathit{NO}}{\mathit{b}}_{\mathit{HI}}{+\mathit{b}}_M={\left[\begin{array}{ccc}{h}_1& h_2& h_3\end{array}\right]}^T$, and ${\overline{\mathit{m}}}^n={\mathit{m}}^n/\Vert {\mathit{m}}^n\Vert$. ${\tilde{\mathit{m}}}^b={[{\tilde{\mathit{m}}}_x^b {\tilde{\mathit{m}}}_y^b {\tilde{\mathit{m}}}_z^b]}^T\in {ℝ}^3$ denotes the measured magnetic field vector, ${\mathit{n}}_m\in {ℝ}^3$ denotes the zero-mean Gaussian noise, ${\mathbf{m}}^n\in {ℝ}^3$ is the local geomagnetic field vector in the navigational frame, whose magnitude is constant when the external magnetic field does not change over time, and ${\overline{\mathbf{m}}}^n$ is the unit vector obtained by normalizing ${\mathit{m}}^n$. The attitude matrix ${\mathbf{C}}_{\mathrm{n}}^{\mathrm{b}}\in SO\left(3\right)$ transforms the magnetic field vector from the navigational frame (n-frame) to the body frame (b-frame), where $SO\left(3\right)$ denotes a set of $3\times 3$ orthogonal matrices. ${\mathbf{S}}_{\mathrm{M}}\in D\left(3\right)$ represents the scaling matrix, where $D\left(3\right)$ denotes a set of $3\times 3$ diagonal matrices and ${\mathbf{C}}_{\mathrm{NO}}$ is a non-orthogonal error matrix. ${\mathbf{m}}_{\mathrm{SI}}$ is given as ${\mathbf{m}}_{\mathrm{SI}}={\mathbf{C}}_{\mathrm{SI}}{\mathbf{C}}_{\mathrm{n}}^{\mathrm{b}}{\mathbf{m}}^{\mathrm{n}}$, where ${\mathbf{C}}_{\mathrm{SI}}\in {ℝ}^{3\times 3}$ is the soft iron transformation matrix. ${\mathbf{m}}_{\mathrm{SI}}$ and ${\mathbf{b}}_{\mathrm{HI}}$ denote the soft and hard iron effects, respectively, and ${\mathbf{b}}_{\mathrm{M}}$ is the null-shift of the magnetometer. In this paper, the navigational frame is defined as East-North-Up and the body frame is defined as Right-Forward-Up.

Clearly, the purpose of the three-axis magnetometer calibration is to obtain the optimal parameters $\mathit{P}$ and ${\mathit{h}}_m$. We assume ${\mathit{P}}^{-1}=\mathit{QR}$ based on the orthogonal-triangular ($\mathit{QR}$) decom-position, where $\mathit{Q}$ and $\mathit{R}$ denote the orthogonal and upper triangular matrices with positive diagonal entries [15]. The first item in Equation (1) can be written as,

$$\mathit{P}{\mathit{C}}_n^b{\overline{\mathit{m}}}^n={\mathit{R}}^{-1}{\mathit{Q}}^T{\mathit{C}}_n^b{\overline{\mathit{m}}}^n=\mathit{T}{\mathit{m}}^{b^{\prime }},$$

(2)

where ${\mathit{T}=\mathit{R}}^{-1}$, which is an upper triangular matrix, and ${\mathit{m}}^{b^{\prime }}={\mathit{Q}}^T{\mathit{C}}_n^b{\overline{\mathit{m}}}^n$, which is a unit vector. Finally, the magnetometer sensor model can be expressed as,

$${\tilde{\mathit{m}}}^b=\mathit{T}{\mathit{m}}^{b^{\prime }}+{\mathit{h}}_m+{\mathit{n}}_m,$$

(3)

and the calibrated magnetometer measurement can be expressed as,

$${\mathit{m}}^{b^{\prime }}={\mathit{T}}^{-1}\left({\tilde{\mathit{m}}}^b-{\mathit{h}}_m\right).$$

(4)

2.2. Proposed Initial Estimation Method

The most popular method to achieve a good initial estimation is the method based on the batch-processing LS [15,25,26]. This method requires a batch of data, which cannot correct the parameters in real time to improve the accuracy.

In this paper, a real-time initial estimation method based on the RLS is proposed. With the increase in the number of iterations, the state vector tends to converge, which indicates a good initial estimation. Moreover, to address the above-mentioned drawbacks of the LS method, the RLS can develop a real-time parameter estimation, which is implemented once the state covariance matrix converges well.

By performing a transformation and a modulus operation, Equation (3) can be written as,

$$\begin{array}{lll}{\Vert {\mathit{m}}^{b^{\prime }}\Vert }^2& =& \Vert \mathit{R}\left({\tilde{\mathit{m}}}^b-{\mathit{h}}_m-{\mathit{n}}_m\right){\Vert }^2\\ & =& {\Vert \mathit{R}\left({\tilde{\mathit{m}}}^b-{\mathit{h}}_m\right)\Vert }^2-2{\left({\tilde{\mathit{m}}}^b-{\mathit{h}}_m\right)}^{\mathrm{T}}{\mathit{R}}^{\mathrm{T}}\mathit{R}{\mathit{n}}_m+{\mathit{n}}_m^{\mathrm{T}}{\mathit{R}}^{\mathrm{T}}\mathit{R}{\mathit{n}}_m\\ & =& {\Vert \mathit{R}\left({\tilde{\mathit{m}}}^b-{\mathit{h}}_m\right)\Vert }^2+v\end{array}$$

(5)

where $v=-2{\left({\tilde{\mathit{m}}}^b-{\mathit{h}}_m\right)}^{\mathrm{T}}{\mathit{R}}^{\mathrm{T}}\mathit{R}{\mathit{n}}_m+{\mathit{n}}_m^{\mathrm{T}}{\mathit{R}}^{\mathrm{T}}\mathit{R}{\mathit{n}}_m$, which is not the exact Gaussian noise because it contains a quadratic item of ${\mathit{n}}_m$. By expanding Equation (5), the following equation is obtained:

$$\begin{array}{ccc}z& =& \mathit{Hx}+v\end{array},$$

(6)

where $\mathrm{H}=[{\tilde{\mathrm{m}}}_{\mathrm{y}}^{{\mathrm{b}}^2} \hspace{1em}{\tilde{\mathrm{m}}}_{\mathrm{z}}^{{\mathrm{b}}^2} \hspace{1em}{\tilde{\mathrm{m}}}_{\mathrm{x}}^{\mathrm{b}}·{\tilde{\mathrm{m}}}_{\mathrm{y}}^{\mathrm{b}} \hspace{1em}{\tilde{\mathrm{m}}}_{\mathrm{x}}^{\mathrm{b}}·{\tilde{\mathrm{m}}}_{\mathrm{z}}^{\mathrm{b}} \hspace{1em}{\tilde{\mathrm{m}}}_{\mathrm{y}}^{\mathrm{b}}·{\tilde{\mathrm{m}}}_{\mathrm{z}}^{\mathrm{b}} \hspace{1em}-{\tilde{\mathrm{m}}}_{\mathrm{x}}^{\mathrm{b}} \hspace{1em}-{\tilde{\mathrm{m}}}_{\mathrm{y}}^{\mathrm{b}} \hspace{1em}-{\tilde{\mathrm{m}}}_{\mathrm{z}}^{\mathrm{b}} \hspace{1em}1]$, $\mathrm{z}=-{\tilde{\mathrm{m}}}_{\mathrm{x}}^{{\mathrm{b}}^2}$. Assuming ${\mathbf{A}=\mathbf{R}}^{\mathrm{T}}\mathbf{R}$, which is a symmetric matrix, the state vector $\mathbf{x}$ can be expressed as,

$$\begin{array}{lll}\mathit{x}& =& {\left[\begin{array}{ccc}\begin{array}{ccc}\frac{a_{22}}{a_{11}}& \frac{a_{33}}{a_{11}}& \frac{{2a}_{12}}{a_{11}}\end{array}& \begin{array}{ccc}\frac{{2a}_{13}}{a_{11}}& \frac{{2a}_{23}}{a_{11}}& k_1\end{array}& \begin{array}{ccc}{k}_2& k_3& k_4\end{array}\end{array}\right]}^T\\ & =& {\left[\begin{array}{ccc}\begin{array}{ccc}{x}_1& x_2& x_3\end{array}& \begin{array}{ccc}{x}_4& x_5& x_6\end{array}& \begin{array}{ccc}{x}_7& x_8& x_9\end{array}\end{array}\right]}^T,\end{array}$$

(7)

where $a_{\mathit{ij}}\left(i,j=1,2,3\right)$ denotes the element in the $ith$ row and $jth$ column of matrix $\mathit{A}$, $x_i\left(i=1,2,\cdots ,9\right)$ denotes the $ith$ element of state vector $x$, and $k_i(i=1,2,3,4)$ denotes the expression consisting of $a_{\mathit{ij}}$ and $h_i\left(i=1,2,3\right)$. $k_i(i=1,2,3,4)$ can be written as,

$$\{\begin{array}{l}{k}_1=2h_1+2\frac{a_{12}}{a_{11}}{h}_2+2\frac{a_{13}}{a_{11}}{h}_3\\ k_2=2\frac{a_{12}}{a_{11}}{h}_1+2\frac{a_{22}}{a_{11}}{h}_2+2\frac{a_{23}}{a_{11}}{h}_3\\ k_3=2\frac{a_{13}}{a_{11}}{h}_1+2\frac{a_{23}}{a_{11}}{h}_2+2\frac{a_{33}}{a_{11}}{h}_3\\ k_4=h_1^2+\frac{a_{22}}{a_{11}}{h}_2^2+\frac{a_{33}}{a_{11}}{h}_3^2+2\frac{a_{12}}{a_{11}}{h}_1{h}_2+2\frac{a_{23}}{a_{11}}{h}_2{h}_3+2\frac{a_{13}}{a_{11}}{h}_1{h}_3-\frac{1}{a_{11}}{\Vert {\mathit{m}}^{b^{\prime }}\Vert }^2\end{array}$$

(8)

Then, the RLS method is utilized to update the state vector $\mathit{x}$. The updated equation is as follows:

$$\{\begin{array}{l}\mathit{K}={\mathit{P}}_{k-1}{\mathit{H}}_k^{\mathrm{T}}{\left({\mathit{H}}_k{\mathit{P}}_{k-1}{\mathit{H}}_k^{\mathrm{T}}+{\delta }_k\right)}^{-1}\\ {\widehat{\mathit{x}}}_k={\widehat{\mathit{x}}}_{k-1}+\mathit{K}\left(z_k-{\mathit{H}}_k{\widehat{\mathit{x}}}_{k-1}\right)\\ {\mathit{P}}_k=\left({\mathit{I}}_k-\mathit{K}{\mathit{H}}_k\right){\mathit{P}}_{k-1}\end{array}$$

(9)

where $\mathit{K}$ is the gain matrix, ${\delta }_k$ is the covariance value for measurement noise, and ${\mathit{I}}_k$ is a unit matrix of dimension $9\times 9$. Remarkably, the diagonal elements of $\mathit{P}$ tend to be stable when the state vector is convergent, based on the basic theory of filter. Therefore, by comparing the defined parameter $s$ with an experienced threshold associated with the used sensor, we can detect whether the state vector has converged, as follows:

$$\begin{array}{ccc}s& =& {{\displaystyle \sum }}_{i=1}^9\sqrt{P_{\mathit{ii}}}<\gamma i=1,2,\cdots ,9\end{array}.$$

(10)

It must be noted that the experienced threshold $\gamma$ in this paper can be obtained by experiments, which is introduced in Section 4.

Using Equations (7) and (8), ${\widehat{a}}_{ij}$ and ${\widehat{h}}_i$ can be obtained from the iteration estimation results of the state vector, as follows:

$$\{\begin{array}{l}{\widehat{h}}_2=\frac{\left({\widehat{x}}_4{\widehat{x}}_6-2{\widehat{x}}_8\right)\left({\widehat{x}}_3{\widehat{x}}_4-2{\widehat{x}}_5\right)-\left({\widehat{x}}_3{\widehat{x}}_6-2{\widehat{x}}_7\right)\left({\widehat{x}}_4^2-4{\widehat{x}}_2\right)}{{\left({\widehat{x}}_3{\widehat{x}}_4-2{\widehat{x}}_5\right)}^2-\left({\widehat{x}}_4^2-4{\widehat{x}}_2\right)\left({\widehat{x}}_3^2-4{\widehat{x}}_1\right)}\\ {\widehat{h}}_3=\frac{\left({\widehat{x}}_3{\widehat{x}}_6-2{\widehat{x}}_7\right)\left({\widehat{x}}_3^2-4{\widehat{x}}_1\right){\widehat{h}}_2}{\left({\widehat{x}}_3{\widehat{x}}_4-2{\widehat{x}}_5\right)}\\ {\widehat{h}}_1=\frac{{\widehat{x}}_4{\widehat{x}}_6-{\widehat{x}}_4{\widehat{h}}_2-{\widehat{x}}_4^2{\widehat{h}}_3}{2{\widehat{x}}_4}\\ {\widehat{a}}_{11}=\frac{{\Vert {\mathit{m}}^{b^{\prime }}\Vert }^2}{{\widehat{h}}_1^2+{\widehat{x}}_1{\widehat{h}}_2^2+{\widehat{x}}_2{\widehat{h}}_3^2+{\widehat{x}}_3{\widehat{h}}_1{\widehat{h}}_2+{\widehat{x}}_4{\widehat{h}}_1{\widehat{h}}_3+{\widehat{x}}_5{\widehat{h}}_2{\widehat{h}}_3-+{\widehat{x}}_9}\\ {\widehat{a}}_{22}={\widehat{a}}_{11}{\widehat{x}}_1\\ {\widehat{a}}_{33}={\widehat{a}}_{11}{\widehat{x}}_2\\ {\widehat{a}}_{12}=\frac{{\widehat{a}}_{11}{\widehat{x}}_3}{2}\\ {\widehat{a}}_{13}=\frac{{\widehat{a}}_{11}{\widehat{x}}_4}{2}\\ {\widehat{a}}_{23}=\frac{{\widehat{a}}_{11}{\widehat{x}}_5}{2}\end{array}$$

(11)

where the hatted quantities denote the final parameters of the estimated results; thus, $\widehat{\mathit{A}}$ and $\widehat{h}$ can be obtained. Further, $\widehat{\mathit{R}}=chol\left(\widehat{\mathit{A}}\right)$, where $chol(·)$ denotes the matrix Cholesky factorization.

Using the proposed RLS, the state vector and the state covariance matrix converge well to produce a good estimation result.

3. Performing Magnetometer Calibration

Although a good initial estimation can be obtained using the RLS method, it is important to note that this method is a biased estimation method because it contains a quadratic item of the noise in Equation (5). Thus, a quadratic optimal ML estimation is adopted to address the above-mentioned problems, which can be regarded unbiased estimations. The objective function can be described as,

$$f=\underset{{\theta }_{ml}}{\mathit{min}}{{\displaystyle {\sum }_{k=1}^N\Vert {\tilde{\mathit{m}}}_k^b-\mathit{T}{\mathit{m}}_k^{b^{\prime }}-{\mathit{h}}_m\Vert }}^2+{\lambda }_k\left({\Vert {\mathit{m}}_k^{b^{\prime }}\Vert }^2-1\right),$$

(12)

with variables ${\theta }_{\mathit{ml}}=\left\{{\mathit{T},\mathit{h}}_m,{\mathit{m}}_k^{b^{\prime }},{\lambda }_k\right\}$, where ${\lambda }_k$ is the Lagrangian coefficient for the norm constraint ${\mathit{m}}_k^{b^{\prime }}$. Due to its good initial estimation characteristic, the Gauss–Newton method is adopted in this paper. The updated equation can be written as,

$$\begin{array}{ccc}{\mathit{x}}_{k+1}& =& {\mathit{x}}_k-{\left[{\nabla }^2{f|}_x\right]}^{-1}\nabla {f|}_x\end{array},$$

(13)

where $\nabla f$ and ${\nabla }^{2}f$ denote the Jacobian vector and Hessian matrix, respectively. The state vector is given by $\begin{array}{ccc}\mathit{x}& =& \left[\begin{array}{ccc}\begin{array}{ccc}{\mathit{vec}}^{\mathrm{T}}\left(\mathit{T}\right)& {\mathit{h}}^{\mathrm{T}}& {\mathit{m}}_1^{b^{\prime }}\end{array}& \begin{array}{ccc}\cdots & {\mathit{m}}_N^{b^{\prime }}& {\lambda }_1\end{array}& \begin{array}{cc}\cdots & {\lambda }_N\end{array}\end{array}\right]\end{array}$, and the Jacobian vector by,

$$\begin{array}{ccc}\nabla {f|}_{\mathit{x}}& =& \left[\begin{array}{ccc}\nabla {f|}_{\mathit{T}}^{\mathrm{T}}& \nabla {f|}_{\mathit{h}}^{\mathrm{T}}& \begin{array}{cc}\nabla {f|}_{{\mathit{m}}_k^{b^{\prime }}}^{\mathrm{T}}& \nabla {f|}_{{\lambda }_k}^{\mathrm{T}}\end{array}\end{array}\right]\end{array} k=1,2,\cdots ,N.$$

(14)

Let ${\mathbf{V}}_{\mathrm{k}}={\tilde{\mathbf{m}}}_{\mathrm{k}}^{\mathrm{b}}-\mathbf{T}{\mathbf{m}}_{\mathrm{k}}^{\mathrm{b}}-{\mathbf{h}}_{\mathrm{m}}$; then

$$\begin{gathered} \begin{array}{ccc}\nabla {f|}_{\mathit{T}}^{\mathrm{T}}& =& -2{{\displaystyle \sum }}_{k=1}^N{\mathit{m}}_k^{b^{\prime }}\otimes {\mathit{v}}_k\end{array},\begin{array}{ccc}\nabla {f|}_{\mathit{h}}^{\mathrm{T}}& =& -2{{\displaystyle \sum }}_{k=1}^N{\mathit{v}}_k\end{array}, \\ \begin{array}{ccc}\nabla {f|}_{{\mathit{m}}_k^{b^{\prime }}}^{\mathrm{T}}& =& {-2\mathit{T}}^{\mathrm{T}}{\mathit{v}}_k{+2\lambda }_k{\mathit{m}}_k^{b^{\prime }}\end{array},\begin{array}{ccc}\nabla {f|}_{{\lambda }_k}^{\mathrm{T}}& =& \Vert {\mathit{m}}_k^{b^{\prime }}{\Vert }^2-1\end{array}. \end{gathered}$$

(15)

In addition, the Hessian matrix becomes,

$${\nabla }^2{f}^{|}{}_{\mathit{x}}=\left[\begin{array}{cccccc}{\mathit{H}}_{\mathit{T}\mathit{T}}& {\mathit{H}}_{\mathit{T}\mathit{h}}& {\mathit{H}}_{\mathit{T}{\mathit{m}}_k^{b^{\prime }}}& \cdots & {\mathit{0}}_{9\times 1}& \cdots \\ {\mathit{H}}_{\mathit{T}\mathit{h}}^{\mathrm{T}}& {\mathit{H}}_{\mathit{h}\mathit{h}}& {\mathit{H}}_{\mathit{h}{\mathit{m}}_k^{b^{\prime }}}& \cdots & {\mathit{0}}_{3\times 1}& \cdots \\ {\mathit{H}}_{\mathit{T}{\mathit{m}}_k^{b^{\prime }}}^{\mathrm{T}}& {\mathit{H}}_{\mathit{h}{\mathit{m}}_k^{b^{\prime }}}^{\mathrm{T}}& {\mathit{H}}_{{\mathit{m}}_k^{b^{\prime }}{\mathit{m}}_k^{b^{\prime }}}& \cdots & {\mathit{H}}_{{\lambda }_k{\mathit{m}}_k^{b^{\prime }}}& \cdots \\ ⋮& ⋮& ⋮& \ddots & ⋮& \ddots \\ {\mathit{0}}_{9\times 1}^{\mathrm{T}}& {\mathit{0}}_{3\times 1}^{\mathrm{T}}& {\mathit{H}}_{{\lambda }_k{\mathit{m}}_k^{b^{\prime }}}^{\mathrm{T}}& \cdots & \mathit{0}& \cdots \\ ⋮& ⋮& ⋮& \ddots & ⋮& \end{array}\right].$$

(16)

Further,

$$\begin{gathered} {\mathit{H}}_{\mathit{TT}}=2{{\displaystyle \sum }}_{k=1}^N\left({\mathit{m}}_k^{b^{\prime }}{\mathit{m}}_k^{b^{\prime }}{}^{\mathrm{T}}\right)\otimes {\mathit{I}}_3,{\mathit{H}}_{\mathit{Th}}=2{{\displaystyle \sum }}_{k=1}^N{\mathit{m}}_k^{b^{\prime }}\otimes {\mathit{I}}_3, \\ {\mathit{H}}_{\mathit{hh}}=2{N\mathit{I}}_3,{\mathit{H}}_{\mathit{T}{\mathit{m}}_k^{b^{\prime }}}=2(({\mathit{m}}_k^{b^{\prime }}\otimes {\mathit{I}}_3)\mathit{T}-{\mathit{I}}_3\otimes {\mathit{v}}_k), \\ {\mathit{H}}_{{\mathit{m}}_k^{b^{\prime }}{\mathit{m}}_k^{b^{\prime }}}=2{\mathit{T}}^{\mathrm{T}}\mathit{T}+2{\lambda }_k{\mathit{I}}_3, {\mathit{H}}_{\mathit{h}{\mathit{m}}_k^{b^{\prime }}}=2\mathit{T}, {\mathit{H}}_{{\lambda }_k{\mathit{m}}_k^{b^{\prime }}}=2{\mathit{m}}_k^{b^{\prime }}, \end{gathered}$$

(17)

where $\otimes$ denotes the Kronecker product, and $\mathit{A}\otimes \mathit{B}$ denotes the Kronecker product of $\mathit{A}$ and $\mathit{B}$ excluding the corresponding lower triangular rows and columns, i.e., the $2th$, $3th$, and $6th$ rows and columns are removed from the result of $\mathit{A}\otimes \mathit{B}$. For an optimal ML estimation, ${\mathit{T}}_{\mathit{init}}={\mathit{R}}_{\mathit{init}}^{-1}$, which can be obtained by the initial estimation; the initial Lagrangian coefficient ${\lambda }_{\mathit{init}}=0$, and the initial ${\mathit{m}}_k^{b^{\prime }}$ can be obtained from Equation (4).

4. Simulation and Experimental Results

4.1. Simulation Results

The performed simulations and their results are provided in this section to study the performance of the proposed algorithm. The parameters such as the geomagnetic field unit vector ${\overline{\mathit{m}}}_n={\left[\begin{array}{ccc}-0.0695& 0.6720& -0.7373\end{array}\right]}^{\mathrm{T}}$ in Suzhou city, and $\mathit{P}$ and ${\mathit{h}}_m$, used in the simulation for the measurement model (1) are given by,

$$\begin{array}{ccc}\mathit{P}& =& \left[\begin{array}{ccc}0.7& -0.8& 0.4\\ 1.1& 0.3& -0.1\\ -0.3& 0.6& 0.7\end{array}\right]\end{array}\begin{array}{ccc}{\mathit{h}}_m& =& \left[\begin{array}{c}0.5\\ 1.7\\ 2.6\end{array}\right]\end{array}.$$

(18)

Moreover, the standard deviation of the measurement noise is $\sigma =0.003$; the attitude matrix ${\mathit{C}}_n^b$ can be expressed as,

$${\mathit{C}}_n^b=\left[\begin{array}{ccc}\mathit{cos}\varphi \mathit{cos}\psi -sin\varphi \mathit{sin}\theta \mathit{sin}\psi & \mathit{cos}\varphi \mathit{sin}\psi +sin\varphi \mathit{sin}\theta \mathit{cos}\psi & -sin\varphi \mathit{cos}\theta \\ -\mathit{cos}\theta \mathit{sin}\psi & \mathit{cos}\theta \mathit{cos}\psi & \mathit{sin}\theta \\ sin\varphi \mathit{cos}\psi +\mathit{cos}\varphi \mathit{sin}\theta \mathit{sin}\psi & sin\varphi \mathit{sin}\psi -\mathit{cos}\varphi \mathit{sin}\theta \mathit{cos}\psi & \mathit{cos}\varphi \mathit{cos}\theta \end{array}\right],$$

(19)

where $\varphi ,\theta ,\mathrm{and}\psi$ denote the roll, pitch, and yaw angles, respectively, and are taken as,

$$\{\begin{array}{l}\begin{array}{ccc}\varphi & =& {20}^{°}\mathit{sin}\left(20\mathsf{\pi }k/N+\mathsf{\pi }/2\right)\end{array}\hfill \\ \begin{array}{ccc}\theta & =& {20}^{°}\mathit{sin}\left(20\mathsf{\pi }k/N\right)\end{array}\hfill \\ \begin{array}{ccc}\psi & =& {360}^{°}k/N\end{array},\hfill \end{array}$$

(20)

where $k=1,2,\cdots ,N$.

The ellipsoid and sphere fitting results obtained from 300 simulations of the measurement model (1) are shown in Figure 1. Moreover, the changes in the state vector $\mathit{x}$ with or without noise are plotted in Figure 2. The initial state vector ${\mathit{x}}_0={\left[\begin{array}{ccccccccc}1& 1& 0& 0& 0& 0& 0& 0& -1\end{array}\right]}^{\mathrm{T}}$ by assuming zero external magnetic field interference, i.e., $\mathit{A}$ is assumed as a unit matrix; ${\mathit{h}}_m$ is the zero vector in Equation (7). The initial covariance matrix for the state vector ${\mathit{P}}_0$ must be set sufficiently large to achieve a better estimation. If ${\mathit{P}}_0$ is very small, the state vector may converge well earlier. Thus, an optimal estimation may not be achieved in the following recursive algorithm. In this paper, the value is selected as ${\mathit{P}}_0=diag\left(\left[\begin{array}{c}\begin{array}{ccccccccc}1& 1& 2& 2& 2& 6& 6& 6& 1\end{array}\end{array}\right]\times \mathrm{10,000}\right)$. In Figure 2, the red, blue, and green lines represent the order of parameters, and the dotted line represents the real value. For example, in the top image in Figure 2a, the red, blue, and green lines denote $X1,X2$, and $X3$, respectively. A comparison of Figure 2a,b indicates that the state vector without noise is almost equal to the real vector calculated using the given parameters, whereas the state vector with noise is not equal to the real vector. This result clearly demonstrates that the RLS method is a biased estimation method.

Figure 3a plots the changes in the square root of the diagonal elements of the covariance matrix $\sqrt{P_{ii}}$ with noise, which clearly converges well after 200 iterative operations. Thus, the parameter $s$ at the 200th iteration can be considered the threshold $\gamma$ to detect if the state vector has converged. To obtain $\gamma$ sufficiently and reasonably, 80 Monte Carlo (MC) runs were performed. The variation curves of the MC runs were concurrent, as shown in Figure 3b. Further, this result agreed well with the expected results. Finally, the threshold is selected as the mean for all the 80 MC simulations.

Figure 4 plots the magnitudes of all the data points before and after the application of the iterative calibrated algorithm. The convergent point in the proposed estimation method is ${\widehat{\mathit{x}}}_{195}$, which was used to calculate the magnitudes of all the data points. As shown in Figure 4, the magnitude of the data points of the proposed algorithm wavers near 1 with a small error, which corresponds well with the theory above (shown in Equation (4)). Moreover, it must be noted that although the later 105 data points were not used in the calibration, their magnitudes were steady, which clearly demonstrates the superiority of the proposed algorithm.

To evaluate the performance of the algorithm better, 80 MC runs were adopted in the experiments below. The objective function value for each iteration in ML is presented in Figure 5. Further, the number of applied data points in ML is determined by the RLS, which has already been discussed above in detail. The estimation converges well within five iterations, and the ML initial objective value is zero because the initial ${\mathit{m}}_k^{b^{\prime }}$ is determined by Equation (4) and ${\lambda }_{\mathit{init}}=0$.

Moreover, the normal error metrics [15] were adopted in this paper. The non-orthogonal and scale factor matrices of the three-axis magnetometer could be obtained by decomposing $\mathit{R}=\mathit{M}\mathit{\Lambda }$, where $\mathit{\Lambda }$ is a diagonal matrix that makes the diagonal elements of $\mathit{M}$ be all 1. Several physical error metrics are defined as follows: average scale factor error, $e_s=\frac{1}{3}\Vert diag({\mathit{\Lambda }}^{-1}\widehat{\mathit{\Lambda }}-\mathit{I})\Vert \times 100\%$ (in percentage), average sensor orthogonal error, $e_o=\frac{180}{3\mathsf{\pi }}\Vert \mathit{vec}(\widehat{\mathit{M}}-\mathit{M})\Vert$ (in degree), and average hard-iron effect error, $e_o=\frac{1}{3}\Vert \widehat{\mathit{h}}-\mathit{h}\Vert$ (in Gauss). The hatted quantities denote the final estimation. The means and standard deviations of the RLS and the ML methods are provided in

Table 1. Mean (standard deviation) of three error metrics.

Methods	${\mathit{e}}_{\mathit{s}}(\%)$	${\mathit{e}}_{\mathit{o}}\left(\mathbf{deg}\right)$	${\mathit{e}}_{\mathit{h}}\left(\mathbf{Gauss}\right)$
RLS	0.0119 (0.0053)	0.4845 (0.2127)	0.0093 (0.0039)
RLS + ML	0.0047 (0.0030)	0.2035 (0.1140)	0.0040 (0.0023)

. From the table, it is seen that the ML method was slightly superior to the RLS method, especially in the average sensor orthogonal error, which indicates that utilizing the unbiased ML is essential to perform the estimation. Moreover, a comparison of the execution times of the conventional LS/ML [15] and the proposed RLS/ML using MATLAB is shown in Figure 6. With the increase in the number of data samples, the variation in the execution time of the proposed algorithm was less than that of the conventional method. This result demonstrated that the proposed method effectively shortens the computational time.

4.2. Experimental Results

To further evaluate the performance of the proposed algorithm, calibration experiments were conducted, which are discussed in this section. The algorithm was implemented in the ADIS16488BMLZ platform (ADI Inc.), which comprised of a triaxial gyroscope, triaxial accelerometer, triaxial magnetometer, and pressure sensor. The dynamic range, sensitivity, and noise density of the magnetometer are $\pm 2.5\mathrm{Gs}$, $0.1\mathrm{mGs}/\mathrm{LSB}$, and $0.042\mathrm{mgauss}/\sqrt{\mathrm{Hz}}$, respectively. Further, the magnetometer signals were sampled at 246 Hz. To validate the proposed algorithm, the sensor was placed in an environment amidst multiple magnetic disturbances from sources including a laptop and a ferromagnetic stair railing, and by the current carried by the sensor, as shown in Figure 7. Moreover, the experiment was conducted by the rotation of the magnetometer around central point because any change in the magnetometer positioning could affect the local magnetometer field.

The sensor measures 3000 data points for the experiments. Good calibration results were achieved when the magnetometer rotates fully in all directions, because it carries sufficient information about the ellipsoid surface. However, if the measured data do not sufficiently cover most of the ellipsoid surface, the calibration results may be poor. The proposed algorithm effectively addresses this problem. Thus, to demonstrate the performance of the proposed algorithm better, the measured data points were distributed on only a little part of the surface of the ellipsoid in this experiment, and not on most of the surface, as shown in Figure 8.

The real-time state vector estimation results and the parameters of the covariance matrix are shown in Figure 9 and Figure 10, respectively. The final number of data points was set as 1835, according to the above-mentioned condition. Moreover, the state vector and the covariance matrix tended to be stable after 1835 iterations, which agreed well with the simulation results above.

After obtaining the optimal parameters by applying the RLS method, the calibrated magnitude results could be obtained using Equation (4). Figure 11 plots the normalized magnitude result of the measured data, and the magnitude results of LS, LS/ML, and the proposed RLS and RLS/ML. It is clearly seen that all of the algorithms performed the calibration well because RLS and LS all could provide good initial estimation. In fact, it is usual that a good initial estimation could be obtained when the magnetic field was not destroyed heavily. In this test, even if only 1835 samples were required to perform the calibration, it was important to analyze the accuracy of the calibrated data in the whole 3000 samples. The magnitude results utilizing all of the 3000 points by RLS/ML was plotted in Figure 12. It is obvious that the range of the magnitudes was between 0.98 and 1.02, which also met the calibration requirement perfectly. Further, the corrected sphere fitting result by the proposed RLS/ML algorithm was plotted in Figure 13.

Finally, to compare the accuracy of the algorithms, the parameters of the reference method were utilized as the standard values. The reference calibrated parameters were obtained by utilizing all the data points, covering most of the ellipsoid surface in the same test environment. The LS/ML method was adopted to obtain the reference parameters, which has already been demonstrated to exhibit a reliable performance [15]. The reference calibrated parameters in the test are as follows:

$$\begin{array}{ccc}{\mathit{R}}_{\mathit{ref}}& =& \left[\begin{array}{ccc}2.3415& -0.0053& -0.0259\\ 0& 2.3613& -0.0255\\ 0& 0& 2.3938\end{array}\right]\end{array}\hspace{1em}\begin{array}{ccc}{\mathit{h}}_{\mathit{ref}}& =& \left[\begin{array}{c}0.0165\\ 0.0245\\ 0.2061\end{array}\right]\end{array}.$$

(21)

In order to evaluate the proposed algorithm sufficiently and reasonably, 20 experiments were conducted in many locations, including stairs, laboratory, open area, and so on.

Table 2. Mean (standard deviation) of three error metrics in the 20 experiments.

Methods	${\mathit{e}}_{\mathit{s}}(\%)$	${\mathit{e}}_{\mathit{o}}\left(\mathbf{deg}\right)$	${\mathit{e}}_{\mathit{h}}\left(\mathbf{Gauss}\right)$
LS	0.0388 (0.0402)	0.9062 (0.7369)	0.0134 (0.0134)
RLS	0.0269 (0.0244)	0.8019 (0.5461)	0.0115 (0.0098)
LS + ML	0.0310 (0.0301)	0.7314 (0.5830)	0.0111 (0.0105)
RLS + ML	0.0309 (0.0300)	0.7280 (0.5803)	0.0111 (0.0105)

lists the means and standard deviations of the 20 experiments by applying different algorithms. A decrease in the mean of all the error metrics could be seen from the table, especially of the average sensor orthogonal error, after applying the proposed RLS/ML method. It is clearly seen that the standard deviation of the error metrics was a little large generally. The reason for the situation was that the accuracy of the calibrated data varied greatly in different locations. In other words, the accuracy of the calibration is associated with the degree of magnetic field disturbance. For example, the calibration results of flat land were better than the stair surrounded by the ferromagnetic material. However, the maximum three error metrics (${\mathit{e}}_{\mathit{s}}, {\mathit{e}}_{\mathit{o}}, \mathrm{and} {\mathit{e}}_{\mathit{h}}$) by applying RLS/ML in the 20 experiments are 0.0657, 1.3860, and 0.0242 respectively, which were all acceptable.

Moreover, RLS/ML was slightly better than LS/ML in the table, not obvious, because RLS and LS all could provide good initial estimation in most experiments. In this situation, the proposed RLS/ML could effectively shorten the computational time due to the iterative operations of RLS, as is presented in Figure 6. This was also one of the main superiority of the proposed algorithm. For example, in the test above, the LS/ML performing the calibration including 3000 samples needed 282.1227 s, while the RLS/ML only needed 46.1559 s.

5. Discussion

Based on the analysis of the simulation and experimental results above, the proposed RLS/ML could perform the magnetometer calibration in many different locations surrounded by magnetic field disturbance. Compared with the traditional LS algorithm, the advantages of the proposed RLS/ML can be listed as:

①

Shortens the computational time due to the iterative operations of RLS;

②

Detects the calibration implementation adaptively by the parameter s in Equation (10);

③

Improves the accuracy of the calibration by utilizing ML algorithm.

Although LS/ML was almost equal to RLS/ML in the accuracy, RLS/ML could effectively shorten the computational time and save memory space, which is beneficial to the engineering application. This is also one of the main advantages of the proposed algorithm.

6. Conclusions

A real-time magnetometer calibration algorithm based on the RLS/ML method was proposed in this paper. First, a simplified model was derived from the measured error model of the magnetometer, and an observation equation was constructed by transformation. Then, real-time parameter estimation was performed using the proposed algorithm. Finally, numerous simulations and experiments were conducted to demonstrate the efficiency of the proposed algorithm. The proposed method was not only more accurate than the conventional batch-processing method, but also reduced greatly the computational time. Moreover, the estimation results were detected in real time by state covariance matrix analysis, which improved the flexibility of the system. Due to the simplicity of the proposed measurement model of the three-axis sensor, the proposed method could be adopted for calibrating other three-axis sensors.