An Optimized Two-Step Magnetic Correction Strategy by Means of a Lagrange Multiplier Estimator with an Ellipsoid Constraint

The geomagnetic field is as fundamental a constituent of passive navigation as Earth’s gravity. In cases where no other external attitude reference is available, for the direct heading angle estimation by a typical magnetic compass, a two-step optimized correction algorithm is proposed to correct the model coefficients caused by hard and soft iron nearby. Specifically, in Step 1, a Levenberg-Marquardt (L-M) fitting estimator with an ellipsoid constraint is applied to solve the hard magnetic coefficients. In Step 2, a Lagrange multiplier estimator is used to deal with the soft magnetic iron circumstance. The essential attribute of “the two-step” lies in its eliminating the coupling effects of hard and soft magnetic fields, and their mutual interferences on the pure geomagnetic field. Under the conditions of non-deterministic magnetic interference sources with noise, the numerical simulation by referring to International Geomagnetic Reference Field (IGRF), and the laboratory tests based upon the turntable experiments with Honeywell HMR3000 compass (Honeywell, Morristown, NJ, USA) conducted, the experimental results indicate that, in the presence of the variation of multi-magnetic interferences, the RMSE (Root Mean Square Error) value of the estimated total magnetic flux density by the proposed two-step estimator falls to 0.125 μT from its initial 2.503 μT, and the mean values of the heading angle error estimates are less than 1°. The proposed solution therefore, exhibits ideal convergent properties, fairly meeting the accuracy requirements of non-tactical level navigation applications.


Introduction
Magnetic compasses are usually considered effective measuring units for heading angle estimates, the accuracy of which, in general, will determine the performances of the whole navigation system [1,2]. In practical measurements, the measured data derived from uncontrolled environmental magnetic fields will lead to large error in evaluating the relevant attitude parameters [3,4]. Among the error sources, hard iron (mainly refers to the permanent magnets, the magnetized iron or steel) and soft iron (mainly refers to the iron which is easily magnetized once being placed in a magnetic field environment, especially the pure iron) interferences around the carriers, are generally the main error − (am 2 +bmn+cn 2 +dmt+ent+ft 2 −1) f ] , X = x 2 x y 2 y z xy xz yz 1 T . Let k (i) (i = 1, 2, . . . , 9) indicate the elements of known vector k (being evaluated beforehand by L-M fitting method), the hard iron interference matrix H i = m n t T with respect to k, therefore, can be calculated from the ternary equations set below: (1) m − k (6) n − k (7) t k (4) = −k (6) m − 2k (3) n − k (8) t k (5) = −k (7) m − 2k (8) n + 2t (7) 3.

2nd Step Correction for Soft Magnetic Coefficients with Lagrange Multiplier Estimator
Let X = H m − H i = [x y z ] being a collection of data points after hard iron interference compensation, so that Equation (3) reduces to: Equation (8) in this case can be rewritten by substituting Equation (4), it follows that: ax 2 + cy 2 + f y 2 + bx y + dx z + ey y = 1 (9) Equation (9) represents an ellipsoid with the following constraint: where, I ≡ a + c + f , J ≡ ac + c f + a f − b 2 − d 2 − e 2 , q is a unknown positive integer.
For each X = [x y z ], define a new vector in augmented form, X i = x i 2 y i 2 z i 2 x i y i x i z i y i z i 1 T , where i = 1, 2, . . . , N representing the measuring set number.
To estimate the coefficients a, b, c, d, e and f in Equation (9) T be a coefficient vector in general augmented form, whose values are to be determined. Thus, rewrite Equation (9) in the following matrix form, and clearly it can be expanded to a set of linear equations: where D = [ X 1 ' X 2 ' . . . X i "]∈ R 7×N , N ≥ 10. Denote the geometric distance (the minimum distance between the measured X and the optimal fitted ellipsoid) by the matrix below: Minimizing Equation (12) under the constraint indicated in Equation (10) is our direct objective. To achieve this, rewrite Equation (10) in terms of v T Cv = 1, and we get: The robust solution to the above optimization problem, therefore, turns to a matter for skilled mathematic iteration.
Construct the simultaneous equations by typical Lagrange multiplier model: where λ is the scalar Lagrange multiplier, and L represents "Lagrange function of". Differentiate the Lagrange function L with respective of v and λ respectively, and let ∂L/∂v = 0 and ∂L/∂λ = 0, we obtain: Note here, Equation (15) has only one solution when q > 3, which denotes the general eigenvector associated with the unique positive eigenvalue of a generalized eigenvalue equation Solving for Equations (15) and (16), we reconstruct DD T and v in the following forms: where, the matrixes P 11 , P 12 , P 22 are of size 6 × 6, 6 × 1, and 1 × 1 respectively.
Hence we are dealing with an eigensystem of two equations given by: where, C 1 ∈ R 6×6 extracting the elements located in the first 6th row and 6th column of original matrix C. In Equation (20), P 22 would be nonsingular in case of the relevant sampling points being not coplanar (since groups of X data are derived from a 3-dimensional ellipsoid), then it can be verified that Equation (20) is solvable, which yields: Substituting Equation (21) into Equation (19), we obtain the generalized eigenvalue system equation [25], it follows that: Sensors 2018, 18, 3284 6 of 14 As stated above, q is no less than 3. When q = 4, 4J − I 2 > 0, then the quadrique indicated by Equation (5) must be an ellipsoid, we say that is the sufficient condition for the present ellipsoid. In this case, we need to determine a bigger integer value q, so as to obtain a wider range of ellipsoid quadrique and satisfy qJ − I 2 > 0.
A rule of thumb, satisfactory in simple cases, is to designate q big enough (like 50), then we locate q by means of bisecting, viz., iteratively calculating it in the intervals between q and q/10.
The algorithm flow chart for the geomagnetic measurement error correction is shown in Figure 1. As indicated, the implementing volume has been divided into two parts, and the coefficients estimation appears to be merely forward, by means of which, we say the coupling effects' elimination experienced by hard and soft magnetic field interferences is fairly achievable. For detailed experiments and result analyses, please refer to Section 4.

Experiments
Experimental observations consist of numerical simulations and laboratory tests. We are generally considering the simulation environment as the ideal geomagnetic field [26], since the pure geomagnetic field never exists under normal conditions of sequent tests.

Numerical Simualtions and Analyses
The geomagnetic field of the Jilin area (Jilin City, China) is designated as our simulation environment and the geomagnetic field distribution information is derived from the International Geomagnetic Reference Field  Table 1. Similarly, the standard deviation of the white noise  (μT) is set to be 0.1 and 0.01, respectively.
The simulated magnetic flux density in μTesla is derived from a Matlab simulation platform, and the preprocessing work focuses on smoothing the white noise involved. On the basis of the de-noising data, the orthogonal tri-axis magnetic field distribution before and after correction is illustrated by the dots in Figure 2, it is actually easier to indicate that the shape of dots changes from an irregular ellipsoid to an approximate normal sphere.

Experiments
Experimental observations consist of numerical simulations and laboratory tests. We are generally considering the simulation environment as the ideal geomagnetic field [26], since the pure geomagnetic field never exists under normal conditions of sequent tests.

Numerical Simualtions and Analyses
The geomagnetic field of the Jilin area (Jilin City, China) is designated as our simulation environment and the geomagnetic field distribution information is derived from the International Geomagnetic Reference Field (IGRF 12) by reference, with altitude 183.4 m in the urban area, latitude 43 • 52 N, longitude 126 • 33 E, and date June 2018 [27]. Two sets of values for the soft magnetic coefficient matrix ε 3×3 and hard magnetic coefficient matrix H i are designated, as denoted by HH (benchmark value) in Table 1. Similarly, the standard deviation of the white noise σ (µT) is set to be 0.1 and 0.01, respectively. The simulated magnetic flux density in µTesla is derived from a Matlab simulation platform, and the preprocessing work focuses on smoothing the white noise involved. On the basis of the de-noising data, the orthogonal tri-axis magnetic field distribution before and after correction is illustrated by the dots in Figure 2, it is actually easier to indicate that the shape of dots changes from an irregular ellipsoid to an approximate normal sphere.

HP(μT)
[  Figure 3 shows the error of total magnetic flux density. As presented, the error with raw data is very large as predicted (see Figure 3a), and the result (0.7 μT) by the proposed two-step estimator (see Figure 3c) exhibits better than that (1 μT) by the traditional two-step estimator stated in Reference [12,13] (see Figure 3b). It should be noted that the average of the total magnetic flux density conforms with the pre-set condition reference, i.e., B=54.397 μT (see Figure 2), and the solution adopted has been proved to possess strong convergence.
More detailed simulation results are given in Table 1, the derived q is based on a typical bisecting iteration. HH1 represents the estimated hard and soft magnetic coefficients, here [a] and [b] respectively represent the results by the proposed two-step estimator and the traditional two-step estimator, HP (μT) represents the maximum error of total magnetic flux density, HM (°) represents the mean of the heading angle error and HT (s) represents the average running time for the numerical simulation under the same conditions. The simulation results show that the soft and hard magnetic coefficients derived by the L-M fitting and the Lagrange multiplier estimator are much closer to the true values. In contrast, the optimized algorithm-based scheme shortened the executing time, leading to better heading angle estimation accuracy without implementing any hardware-upgrading means or integrating any external measurement units.  Figure 3 shows the error of total magnetic flux density. As presented, the error with raw data is very large as predicted (see Figure 3a), and the result (0.7 µT) by the proposed two-step estimator (see Figure 3c) exhibits better than that (1 µT) by the traditional two-step estimator stated in Reference [12,13] (see Figure 3b). It should be noted that the average of the total magnetic flux density conforms with the pre-set condition reference, i.e., B=54.397 µT (see Figure 2), and the solution adopted has been proved to possess strong convergence.

Laboratory Tests and Evaluations
The correction is experimentally assessed to evaluate the effectiveness of the optimized solution in practice. The laboratory tests are conducted based upon the turntable experiments with Honeywell HMR3000 compass (which measurement accuracy is within 100 nT) and the real geomagnetic field reference is provided by a high-precision proton magnetometer (the measurement accuracy is within 1 nT). To begin with, the compass is mounted on a stationary base on a two-axis horizontal turntable. In sequence, in the course of collecting orthogonal 3-axis magnetic flux density data, the compass rotates at a constant speed to capture uniformly distributed samples across different directions. Three experiments are implemented indicating different locations of interference sources, as illustrated in Figure 4. Specifically, it is to ensure that the hard and soft magnetic material be distributed around the magnetic compass in concentric circles. Referring to Reference [28], the radii of three interference sources are set to be 90, 70 and 50 cm, respectively. As in the numerical simulation, the preprocessing of raw magnetic data should first be examined. Taking Y-axis measurements as an example, Figure 5a presents raw magnetic data derived from the Honeywell HMR3000 compass. Since the raw magnetic data contains noise, in order to filter the noise, the raw data was decomposed into six IMF components by the EMD method, diagrammatically represented by Figure 5b. In sequence, denoising the first three IMF components by improved WTD and fusing them with the remaining three IMF components, we achieved the final de-noised magnetic data. Figure 5c indicates the de-noised magnetic data by signal reconstruction in the frequency domain. Clearly, the de-noised data appears smoother, being better suited and favorable for the following ellipsoid fitting and relevant magnetic coefficients estimation. More detailed simulation results are given in Table 1, the derived q is based on a typical bisecting iteration. HH 1 represents the estimated hard and soft magnetic coefficients, here [a] and [b] respectively represent the results by the proposed two-step estimator and the traditional two-step estimator, HP (µT) represents the maximum error of total magnetic flux density, HM ( • ) represents the mean of the heading angle error and HT (s) represents the average running time for the numerical simulation under the same conditions. The simulation results show that the soft and hard magnetic coefficients derived by the L-M fitting and the Lagrange multiplier estimator are much closer to the true values. In contrast, the optimized algorithm-based scheme shortened the executing time, leading to better heading angle estimation accuracy without implementing any hardware-upgrading means or integrating any external measurement units.

Laboratory Tests and Evaluations
The correction is experimentally assessed to evaluate the effectiveness of the optimized solution in practice. The laboratory tests are conducted based upon the turntable experiments with Honeywell HMR3000 compass (which measurement accuracy is within 100 nT) and the real geomagnetic field reference is provided by a high-precision proton magnetometer (the measurement accuracy is within 1 nT). To begin with, the compass is mounted on a stationary base on a two-axis horizontal turntable. In sequence, in the course of collecting orthogonal 3-axis magnetic flux density data, the compass rotates at a constant speed to capture uniformly distributed samples across different directions. Three experiments are implemented indicating different locations of interference sources, as illustrated in Figure 4. Specifically, it is to ensure that the hard and soft magnetic material be distributed around the magnetic compass in concentric circles. Referring to Reference [28], the radii of three interference sources are set to be 90, 70 and 50 cm, respectively.
As in the numerical simulation, the preprocessing of raw magnetic data should first be examined. Taking Y-axis measurements as an example, Figure 5a presents raw magnetic data derived from the Honeywell HMR3000 compass. Since the raw magnetic data contains noise, in order to filter the noise, the raw data was decomposed into six IMF components by the EMD method, diagrammatically represented by Figure 5b. In sequence, denoising the first three IMF components by improved WTD and fusing them with the remaining three IMF components, we achieved the final de-noised magnetic data. Figure 5c indicates the de-noised magnetic data by signal reconstruction in the frequency domain. Clearly, the de-noised data appears smoother, being better suited and favorable for the following ellipsoid fitting and relevant magnetic coefficients estimation. magnetic flux density data, the compass rotates at a constant speed to capture uniformly distributed samples across different directions. Three experiments are implemented indicating different locations of interference sources, as illustrated in Figure 4. Specifically, it is to ensure that the hard and soft magnetic material be distributed around the magnetic compass in concentric circles. Referring to Reference [28], the radii of three interference sources are set to be 90, 70 and 50 cm, respectively. As in the numerical simulation, the preprocessing of raw magnetic data should first be examined. Taking Y-axis measurements as an example, Figure 5a presents raw magnetic data derived from the Honeywell HMR3000 compass. Since the raw magnetic data contains noise, in order to filter the noise, the raw data was decomposed into six IMF components by the EMD method, diagrammatically represented by Figure 5b. In sequence, denoising the first three IMF components by improved WTD and fusing them with the remaining three IMF components, we achieved the final de-noised magnetic data. Figure 5c indicates the de-noised magnetic data by signal reconstruction in the frequency domain. Clearly, the de-noised data appears smoother, being better suited and favorable for the following ellipsoid fitting and relevant magnetic coefficients estimation.   Figure 6 shows the fitted ellipsoids in cases of different circumstances where the orthogonal tri-axis magnetic field intensity is almost evenly distributed, evenly distributed, and partially distributed. Obviously, even the geomagnetic field distribution is more likely to form a regular ellipsoid, leading to an approximate normal sphere. In practice, however, it may well be that the turntable fails to ideally rotate at a constant speed, and the measured data consequently covers a very small portion of the ellipsoid surface.  Figure 6 shows the fitted ellipsoids in cases of different circumstances where the orthogonal tri-axis magnetic field intensity is almost evenly distributed, evenly distributed, and partially distributed. Obviously, even the geomagnetic field distribution is more likely to form a regular ellipsoid, leading to an approximate normal sphere. In practice, however, it may well be that the turntable fails to ideally rotate at a constant speed, and the measured data consequently covers a very small portion of the ellipsoid surface. Figure 5. The preprocessing of raw measuring magnetic data. Figure 6 shows the fitted ellipsoids in cases of different circumstances where the orthogonal tri-axis magnetic field intensity is almost evenly distributed, evenly distributed, and partially distributed. Obviously, even the geomagnetic field distribution is more likely to form a regular ellipsoid, leading to an approximate normal sphere. In practice, however, it may well be that the turntable fails to ideally rotate at a constant speed, and the measured data consequently covers a very small portion of the ellipsoid surface.  In this case, determining the accurate radian and radial length at the very start is of great importance, which is achieved through multiple iterations of q. In our framework, the proposed two-step optimized algorithm by the L-M fitting and the Lagrange multiplier estimator has been applied to a vastly wider range of datasets, and the fitting results are adequately satisfied.
The estimated total geomagnetic field measurement error is diagrammatically represented in Figure 7, with a-c corresponding to different interference sources nearby, referring to Figure 4, the radii of the interference sources appear to be 90 cm, 70 cm and 50 cm respectively. Clearly, the traditional two-step estimator reveals its limited capacity in more accurate error estimation, and the undesired curve wave also illustrates degrees of instability. The algorithm stability still needs to be enhanced. By comparison, the proposed two-step estimator presents the optimal error estimation irrespective of the variation of the environmental magnetic fields, whose results exhibit better convergent properties with respect to the coupling effects experienced by hard and soft iron.
The corresponding stochastic data (root mean square error, RMSE) comparison is illustrated by the bar graph in Figure 8. As shown, for the case that the hard and soft iron are very close to the magnetic compasses, the RMSE value of the estimated total magnetic flux density extracted from the proposed two-step estimator falls to 0.125 μT from the initial 2.503 μT (with raw data from Honeywell HMR3000). Specifically, the traditional two-step estimator presents a relatively large In this case, determining the accurate radian and radial length at the very start is of great importance, which is achieved through multiple iterations of q. In our framework, the proposed two-step optimized algorithm by the L-M fitting and the Lagrange multiplier estimator has been applied to a vastly wider range of datasets, and the fitting results are adequately satisfied.
The estimated total geomagnetic field measurement error is diagrammatically represented in Figure 7, with a-c corresponding to different interference sources nearby, referring to Figure 4, the radii of the interference sources appear to be 90 cm, 70 cm and 50 cm respectively. Clearly, the traditional two-step estimator reveals its limited capacity in more accurate error estimation, and the undesired curve wave also illustrates degrees of instability. The algorithm stability still needs to be enhanced. By comparison, the proposed two-step estimator presents the optimal error estimation irrespective of the variation of the environmental magnetic fields, whose results exhibit better convergent properties with respect to the coupling effects experienced by hard and soft iron.  The estimated heading angle error before and after correction is presented in Figure 9. Clearly, whose peak values even approach 20° when directly using the raw data from Honeywell HMR3000, i.e., the measured data is not corrected at all (see Figure 9a). It should be noted that, in this case, the distance of the interference object nearby is set to be 90 cm, indicating that the interferences are not too severe. Through Matlab simulative calculations, the mean values of the estimated heading angle error corresponding to Figure 9b-d (whose radii of interference sources are 90, 70 and 50 cm, respectively) are 0.486°, 0.825° and 0.918°, respectively. It is obvious to indicate that, after the magnetic correction, the mean values of the heading angle error estimates are no more than 1° regardless of the difference of locations of interference sources and radii of concentric circles, and that the optimized two-step magnetic correction strategy fairly meets the accuracy requirements of non-tactical level navigation applications. The corresponding stochastic data (root mean square error, RMSE) comparison is illustrated by the bar graph in Figure 8. As shown, for the case that the hard and soft iron are very close to the magnetic compasses, the RMSE value of the estimated total magnetic flux density extracted from the proposed two-step estimator falls to 0.125 µT from the initial 2.503 µT (with raw data from Honeywell HMR3000). Specifically, the traditional two-step estimator presents a relatively large RMSE, and this is due to the fact that in step 2, in order to calculate the relevant nine parameters algebraically, a number of multivariate equations are involved with hardly guaranteeing the independence of the equations, that is, the solutions for nine parameters would be not unique, and thus the magnetic flux density error would be large. In contrast, even though the proposed two-step estimator follows the same line as the traditional one in the two-step magnetic correction design, the optimized estimator addresses this problem by means of multiple iterations of the equations involved, and after these proactive iterations, the calculation error decreases, leading to more ideal RMSE results.  The estimated heading angle error before and after correction is presented in Figure 9. Clearly, whose peak values even approach 20° when directly using the raw data from Honeywell HMR3000, i.e., the measured data is not corrected at all (see Figure 9a). It should be noted that, in this case, the distance of the interference object nearby is set to be 90 cm, indicating that the interferences are not too severe. Through Matlab simulative calculations, the mean values of the estimated heading angle error corresponding to Figure 9b-d (whose radii of interference sources are 90, 70 and 50 cm, respectively) are 0.486°, 0.825° and 0.918°, respectively. It is obvious to indicate that, after the magnetic correction, the mean values of the heading angle error estimates are no more than 1° regardless of the difference of locations of interference sources and radii of concentric circles, and that the optimized two-step magnetic correction strategy fairly meets the accuracy The estimated heading angle error before and after correction is presented in Figure 9. Clearly, whose peak values even approach 20 • when directly using the raw data from Honeywell HMR3000, i.e., the measured data is not corrected at all (see Figure 9a). It should be noted that, in this case, the distance of the interference object nearby is set to be 90 cm, indicating that the interferences are not too severe. Through Matlab simulative calculations, the mean values of the estimated heading angle error corresponding to Figure 9b-d (whose radii of interference sources are 90, 70 and 50 cm, respectively) are 0.486 • , 0.825 • and 0.918 • , respectively. It is obvious to indicate that, after the magnetic correction, the mean values of the heading angle error estimates are no more than 1 • regardless of the difference of locations of interference sources and radii of concentric circles, and that the optimized two-step magnetic correction strategy fairly meets the accuracy requirements of non-tactical level navigation applications.

Conclusions
An optimized two-step model by the L-M fitting and the Lagrange multiplier estimator for magnetic field measurement correction was employed, after exhibiting ideal convergent properties in the numerical simulations, which was experimentally assessed with the Honeywell HMR3000 compass to eliminate the magnetic interferences caused by surrounding hard and soft iron. It demonstrated that, compared with the traditional two-step estimator, the proposed estimator effectively reduces the environmental magnetic field disturbance and its accuracy and reliability remain optimal with the variation of non-deterministic interferences. The experimental results indicate that the mean values of the heading angle error estimates are less than 1° under strong magnetic interferences. The algorithm-based solution with no extra assistant measurement units, therefore, may be considered as a reference tool for a class of magnetic field coefficients correction with environmental interferences hypothesis. Figure 9. The estimated heading angle error before and after correction by the proposed two-step estimator. (a) before correction; (b) the interference source radius is 90 cm; (c) the interference source radius is 70 cm; (d) the interference source radius is 50 cm.

Conclusions
An optimized two-step model by the L-M fitting and the Lagrange multiplier estimator for magnetic field measurement correction was employed, after exhibiting ideal convergent properties in the numerical simulations, which was experimentally assessed with the Honeywell HMR3000 compass to eliminate the magnetic interferences caused by surrounding hard and soft iron. It demonstrated that, compared with the traditional two-step estimator, the proposed estimator effectively reduces the environmental magnetic field disturbance and its accuracy and reliability remain optimal with the variation of non-deterministic interferences. The experimental results indicate that the mean values of the heading angle error estimates are less than 1 • under strong magnetic interferences. The algorithm-based solution with no extra assistant measurement units, therefore, may be considered as a reference tool for a class of magnetic field coefficients correction with environmental interferences hypothesis.