1  Yuanxin Wu,   Senior Member, IEEE , Danping Zou, Peilin Liu and Wenxian Yu  Abstract —   Magnetometer and inertial sensors are widely used for orientation estimation. Magnetometer usage is often troublesome, as it is prone to be interfered by onboard or ambient magnetic disturbance. The onboard soft-iron material distorts not only the magnetic field, but the magnetometer sensor frame coordinate and the cross-sensor misalignment relative to inertial sensors. It is desirable to conveniently put magnetic and inertial sensors information in a common frame. Existing methods either split   the   problem   into   successive   intrinsic   and   cross-sensor calibrations, or rely on stationary accelerometer measurements which is infeasible in dynamic conditions. This paper formulates the magnetometer calibration and alignment to inertial sensors as a   state   estimation   problem,   and   collectively   solves   the magnetometer intrinsic and cross-sensor calibrations, as well as the gyroscope bias estimation. Sufficient conditions are derived for the problem to be globally observable, even when no accelerometer information is used at all. An extended Kalman filter is designed to implement the state estimation and comprehensive test data results show the superior performance of the proposed approach. It is immune to acceleration disturbance and applicable potentially in any dynamic conditions.  Index   Terms — Magnetometer   calibration,   gyroscope,   cross- sensor misalignment, observability  I.   I NTRODUCTION  Low-cost modules or chips, consisting of three triads of gyroscopes,   accelerometers   and   magnetometers,   become ubiquitous nowadays in consumer devices. They are used in unmanned aerial vehicles for flight stabilization, in mobile phones for personal navigation, in virtual reality helmets for attitude tracking and in wearable units for human body motion tracking. Gyroscopes, accelerometers and magnetometer are three different kinds of sensors with distinctive features, of which the former two are known as inertial sensors. Gyroscopes sense the body angular rate with respective to the inertial frame and the computed rotation by integration subjects to drift due to the presence of the gyroscope bias and noise. Accelerometers measure the non-gravitational acceleration that cannot be used to   derive   the   inclination   until   they   are   roughly   at   rest. Magnetometers sense the local magnetic field that may be interfered   by the   nearby ferromagnetic   material   or   strong electric currents. Specifically, the soft-iron material distorts not only   the   ambient   magnetic   field   but   the   magnetometer coordinate   frame.   As   a   result,   the   cross-sensor   frame misalignment   between   magnetometers   and gyroscopes/accelerometers are altered as well, so it is advised that both intrinsic and cross-sensor calibrations be carried out prior to magnetometer usage [1]. In comparison, the coordinate  This work was supported in part by National Natural Science Foundation of China   (61422311,   61673263)   and   Hunan   Provincial   Natural   Science Foundation of China (2015JJ1021).  frame   of   and   the   mutual   frame   between   gyroscopes   and accelerometers change little under normal working conditions and is usually calibrated once for all. As the magnetometer frame is potentially affected by other sensors   or   the   attached   platform itself,   the   magnetometer calibration should not be performed until all sensors are rigidly fixed   to   the   platform.   The   calibration   process   commonly consists of two steps: (1) intrinsic calibration and (2) cross- sensor calibration. The attitude-independent method [2-10] is the most popular magnetometer intrinsic calibration approach, which exploits the fact that the magnetometer measurement at the local position or in a homogeneous magnetic field   is constant in magnitude regardless of the orientation. The cross- sensor misalignment of accelerometers and magnetometers is estimated in [11] using the invariant angle formed by the local gravity vector and the local magnetic field vector. The cross- sensor misalignment of magnetometers relative to gyroscopes is determined in [4] by using the gyroscope-derived incremental rotation as a reference, and in [1] by exploiting the fact that in  a homogenous magnetic field the magnetometer’s measurement  variation is exclusively induced by orientation change. The incremental rotation of the magnetometer is assumed known in [12], which restricts its usage in controlled environments with attitude reference only. In [7, 13, 14], the intrinsic calibration and cross-sensor calibration are collectively addressed as a maximum   likelihood   estimation   using   magnetometer   and accelerometer measurements. Almost all previous works, e.g. [4, 7, 11-14], rely on the local gravity information for the cross- sensor   calibration,   thus   requiring   to   collect   accelerometer measurements   at   stationary   poses,   and   any   disturbed acceleration would inevitably decay the calibration quality. An exception   is   our   work   [1]   that   solves   the   cross-sensor misalignment   by   the   recursive   optimization   using   only magnetometer and gyroscope measurements, which is immune to any acceleration disturbance. As the intrinsic calibration has to be carried out a prior using the whole dataset, however, the recursive attribute of the proposed cross-sensor calibration solution [1] is significantly compromised. Compared with all previous works, the main contribution of this paper is a novel state estimation approach collectively solving   the   magnetometer   intrinsic   and   cross-sensor calibrations in a homogeneous magnetic field. With respect to our work [1], the proposed approach is truly recursive in time and immune to acceleration disturbance. The paper is organized as follows. Section II describes the sensor models of gyroscope, accelerometer   and   magnetometer.   Magnetometer  Authors’ address:   Shanghai Key Laboratory of Navigation and Location- based Services, School of Electronic Information and Electrical Engineering, Shanghai   Jiao   Tong   University,   Shanghai,   China,   200240,   E-mail: (yuanx_wu@hotmail.com).  Dynamic   Magnetometer   Calibration and Alignment  to Inertial Sensors by Kalman Filtering
2  calibration/alignment   is   formulated   as   a   state   estimation problem and two sufficient conditions of global observability are derived. Section III uses extended Kalman filtering to solve the problem and reports the field test results. The conclusions are drawn in Section V. II.   S ENSOR MODELS AND   M AGNETOMETER   C ALIBRATION  P ROBLEM  A.   Measurement Models: Magnetometer, Accelerometer and Gyroscope  Assuming the magnetic disturbance is time-invariant and taking sensor imperfection into account, the magnetometer measurement can be modelled by [2, 4] b   e m   e   m        y   SC m   h   n  (1) where e  m   is the local magnetic vector in the Earth frame (e- frame)   and m n   is   i.i.d   zero-mean   Gaussian   noise   with covariance 2 3 m     I   . In a homogeneous magnetic field, e  m   is constant and assumed to have unity norm without loss of generality. The body frame (b-frame) refers to the coordinate frame   defined   by   the   gyroscope/accelerometer   triads.   The attitude or orientation matrix b e C   transforms the magnetic field vector from e-frame to b-frame. The purpose of the magnetometer calibration is to determine the matrix S   and the vector h   , which collectively encode the magnetometer sensor triad imperfection, magnetic disturbance, and the misalignment with respect to b-frame. The calibration matrix S   can be decomposed   as 1   b m      S   C R   by   the   orthogonal-triangular decomposition, where b m C   is the cross-sensor misalignment between the magnetometer frame (m-frame) and the body frame and R   (with   positive   diagonal   entries)   belongs   to   the magnetometer intrinsic parameters. As shown in [1], m-frame is typically distinctive from the physical coordinate frame of the magnetic sensor triad in the presence of soft-iron materials. Equation (1) is a general magnetometer calibration model that combines together the intrinsic calibration and the cross-sensor calibration. For example, the attitude-independent intrinsic calibration method [2-10] aims to seek the parameters R   and h  that satisfy       1 m   b   e m   m   b   e           R y   h   n   C C m   . The accelerometer measurement is related to the local gravity vector by [15, 16]     2 b   e   e   e   e   b   e a   e   ie   a   e   a                        y   C   g   v   ω   v   n   C g   n  (2) where e  g   is the true local constant gravity vector in e-frame, 2 e   e   e ie     v   ω   v  is the sum of linear and Coriolis accelerations and aw n   is i.i.d Gaussian noise. e  v   is the velocity relative to the Earth and e ie ω   is   the Earth’s rotation rate . When the acceleration   sum   term   was   restrained   to   be   small,   the accelerometer measurement a y   would roughly reflect the local gravity and for this situation a n   could be modelled as zero-mean noise with covariance 2 3 a     I   . As shown in (2), the accelerometer   is   usually   considered   as   ideal   for   low-cost applications as its error has much less impact on orientation than the gyroscope. Equations   (1)-(2)   indicate that the magnetometer and accelerometer   measurements   are   both related   to   the   body attitude matrix b e C   which projects the local magnetic vector and the local gravity vector to b-frame. By the chain rule of the orientation matri                   0 0   0 b t   e b   b e   e b   e t  t    C   C   C   C  (3) in   which        0  b t b  C   and       0 e e t  C   respectively   describe   the orientation change of b-frame and e-frame over the considered time period. Their time rates are related to the corresponding angular velocities by [15]                                       0   0 0   0  b   b   b ib   g b t   b t e   e   e ie e t   e t                 C   C   ω   ε   n C   C   ω  (4) where b ib ω   denotes   the   gyroscope   measurement, ε   the gyroscope bias and g n   i.i.d zero-mean Gaussian noise with covariance 2 3 g     I   . The skew symmetric matrix        is defined so that the cross product satisfies            x   y   x   y   for arbitrary two vectors. For any constant vector e  r   ,                       0   0 0   0  0 b t   e   b t   b b   e   b   e e   e b   e t   b      C r   C   C   C   r   C   r  (5) Using   (4),   the   magnitude   of   the   vector   rate                   0   0  0  b   e b   e   e   e e   ie e t          r   C   C   ω   r   r  . As the magnitude of the   Earth’s rotation rate   ( 5  7.3 10          rad/s) is much smaller than low-cost gyroscope errors, it is reasonable to take     0 b  r  approximately as a constant.  B.   Magnetometer Calibration and Observability Property  Hereafter we use the initial body frame, namely     0 b   , as the   inertial   frame   (i-frame).   The   above   equations   can   be formulated in a state-space form as              0,   0 ,  i   i   b ib   g b t   b t i   i mi   gi                        C   C   ω   ε   n  ε   n S   h m   n   g   n  (6) with the magnetometer and accelerometer measurements given as      b t   i m   i   m b t   i a   i   a               y   SC   m   h   n y   C   g   n  (7) where  n   , mi n   and gi n   are i.i.d zero-mean Gaussian noises with covariance 2 3      I   , 2 3 mi     I   and 2 3 gi     I   , respectively. The augmented system state x   comprises the inertial attitude   b t i C  , the gyroscope bias ε   , the magnetometer calibration parameters S   and h   , and the approximately constant vectors i  m  and i  g   .  Definition of State Observability   [17]: A system is said to be (globally) observable if for any unknown initial state     0 x   , there exists a finite 0 t      such that the knowledge of the input and the output over     0,   t   suffices to determine uniquely the initial state     0 x   . Otherwise, the system is said to be (globally)
3  unobservable. This is a concept of deterministic observability taking no account of noises. Whatever estimation techniques are to be used, observability analysis is necessary that tells the inherent estimability of the system state [17, 18].  Theorem 2.1 : If the matrix 0  t   T   dt    W W   is nonsingular, the matrix *   * 0  t   T   dt    Y   Y   has one and only one zero eigenvalue and     0 b i C   is restricted to be an identical matrix, then the system state is globally observable. ( *  Y   and W   are defined as in (9) and (12) below) Proof. Making use of the unity-norm property of i  m   and the orthogonal-triangular   decomposition 1   b m      S   C R   ,   the magnetometer measurement in (7) yields               1   i   i   b m   m   m b t               m   C   C R y   h   R y   h  (8) Expanding the above equation and expressing it in a linear equation form [7, 8]      2   1   0 1  T T   T   T   T m   m   m T   T  vec                                             R R y   y   y   R Rh   Yz h R Rh  (9) The operator    denotes the Kronecker product. As T  R R   is symmetric,     T  vec   R R   is formed by stacking the columns of T  R R  but excluding the lower triangular entries. The columns of Y   corresponding to the three lower triangular entries are merged to those columns corresponding to their symmetric counterparts, so are the columns of z   . To avoid notation confusion, we denote the modification by superscript asterisk, namely, (9) becomes *   *   0  Y z   . Left multiplying * T  Y   and integrating   over   the   interested   time   interval,   it   gives *   *   * 0   0  t   T   dt       Y   Y   z  . If the matrix *   * 0  t   T   dt    Y   Y   has only one zero eigenvalue, then the solution of *  z   is the corresponding eigenvector,   from   which R   and h   can   be   uniquely determined. Denote     *  m   m     y   R y   h   .   The   magnetometer measurement in (7) becomes   *   b t m   i m   b   i  y   C C   m  (10) Taking time derivative and using (4),                         *   * b t m   b   i   m   b m   b   ib   i   m   b   ib                  y   C   ω   ε   C   m   y   C   ω   ε  (11) or equivalently,                 *   *   *  m b bT m   ib   m   m   m b  vec                                          C y   ω   y   y   W η  C   ε  (12) If the matrix 0  t   T   dt    W W   is nonsingular, then η   can be solved   as       1   * 0   0  =   t   t T   T m dt   dt        η   W W   W y   ,   from   which   the misalignment m b C   and   the   gyroscope   bias ε   will   be determined. Note that this nonsingular condition is sufficient but not necessary, as it does not consider the constraint among the entities of an orientation matrix. However, the initial values for   b t i C   and i  m   cannot be uniquely   determined,   as   for   any   attitude   matrix Q   the following   equation   is   always   valid   for   the   magnetometer measurement in (7)            b t   T   i m   i     y   S C   Q   Q m   h  (13) which means that the initial values of   b t i C   and i  m   both have infinite feasible solutions. Therefore, we restrict    0  b t i   t     C  to be its physical value, namely an identical matrix, to make the formulation (6)-(7) fully observable. Then the inertial attitude   b t i C  will be available with (4) and the determined gyroscope bias through integration. Then the constant magnetic vector is computed as         1 i   i m b t        m   C   S   y   h   and the constant gravity vector is computed as    i   i a b t    g   C   y   from the accelerometer measurement equation in (7).  ■  The   observability   analysis   in   Theorem   2.1 overwhelmingly   depends   on   the   magnetometer/gyroscope measurement information, and the subsequent result provides another   set   of   sufficient   conditions   for   the   system   to   be observable   by   additionally   exploiting   the   accelerometer information.  Theorem 2.2 : If the matrix 0  t   T a   a   dt    y y   is nonsingular, the matrix 0  t   T   dt    M M   has one and only one zero eigenvalue and     0 b i C  is restricted to be an identical matrix, then the system state is globally observable. ( M   is defined as in (17) below) Proof.   Taking   the   time   derivative   of   the   accelerometer measurement in (7) and using (4),                   b t b   i   b a   ib   i   ib   a                  y   ω   ε   C   g   ω   ε   y  (14) from which the gyroscope bias, if the matrix 0  t   T a   a   dt    y y   is nonsingular,   can   be   uniquely   solved   as            1 0   0  t   t T   b a   a   a   ib   a dt   dt                ε   y y   y   ω   y  .   Similarly   the   initial values for   b t i C   and i  g   have infinite feasible solutions, because for any attitude matrix Q   the following equation is always valid            b t   T   i a   i   y   C   Q   Q g  (15) By restricting     0 b i C   to be an identical matrix, the inertial attitude   b t i C   will be available with (4) and the determined gyroscope bias. With the known inertial attitude   b t i C   , the constant gravity vector is computed as    i   i a b t    g   C   y   and the magnetometer   measurement   equation   in   (7)   can   be   re- organized as        1   1   0 i   i   i m b t   b t              C   S   y   C   S h   m  (16) or alternatively in the linear equation form
4            1 1 3   0 T   i   i m   b t   b t i  vec                                                    S y   C   C   I   S   h   M κ  m  (17) If the matrix 0  t   T   dt    M M   has only one zero eigenvalue, then the solution of κ   is the corresponding eigenvector, from which   the   magnetometer   parameters S   and h   and   the constant magnetic vector i  m   can be uniquely determined.  ■  III.   E STIMATION   A LGORITHM AND   T EST   R ESULTS  A.   Estimation Algorithm  The discrete-time form of (6) is straightforward, as all sub-states but the inertial attitude are just copied from the last epoch 1 k t      to the current epoch k t   and the inertial attitude is propagated to the first order as such [15, 16]                                    1 1 1   3   1   1  k k   k   k k  b t i   i b t   b t   b t i   b k   k   ib   k b t   t   t                                   C   C   C C   I   ω   ε  (18) where 1 k    ε   is the gyroscope bias estimate at the last epoch. The inertial attitude starts from an identical matrix with zero covariance, namely implementing the requirement of     0 b i C   to be   an   identical   matrix   in   Theorems   2.1-2.2.   The   matrix parameter S   starts from an identical matrix as well, and the initial   gyroscope   bias ε   and   the   initial   magnetometer parameter h   are both set to zeros. The initial values of i  m  and i  g   are   respectively   set   to   the   magnetometer’s   and accelerometer’s   first measurements. Note that because of the potentially significant soft-iron effect, the above initial value setting might not be satisfactory for S   , h   and i  m   . Some ad- hoc techniques might be needed, as discussed in next subsection. The error-state extended Kalman filter (EKF) is employed to carry out the state estimation [19]. Deriving the first order error-state   equation   of   the   formulation   (6)-(7)   is straightforward and we directly present it below. Readers may refer to [15, 16] for details. Define the error state as the estimate subtracting the true state, i.e., ˆ     δ x   x   x   . The attitude estimate is   defined   as   being   related   to   the   true   attitude   and   the corresponding attitude error ψ   by              ˆ   i   i b t   b t         C   I   ψ   C   . The error state       T T   T   T   T   iT   iT  vec          δ x   ψ   δε   δ S   δ h   δ m   δ g   , whose dynamic equation in the state-space matrix form is given by m   m   m a   a   a                                                                     δ x   F δ x   Gw  δ y   H   n  δ y   δ x  δ y   H   n  (19) where the dynamic noise T T   T   T   T g   mi   gi             w   n   n   n   n   and the matrices are            3 3   3 6 3 3   3 18   3 3   3   3 6 21 3   21 3   21 18   12 3   12 3   12 6 6 3   6 3   6 3 3   3   3   3 3 3 3   3 9   3 3   3 3  ,   , ,  i b i b T b   i   b   i   b m   i   i   i b   i   b a   i   i                                                                                                                                             C   0   0 0   C   0   0   I   0 F   G 0   0   0   0   0   0 0   0   I H   SC   m   0   C m   I   I   SC   0 H   C g   0   0   0   0   C  (20) In   a   homogeneous   magnetic   field,   the   magnetometer measurement can always be used to update the state estimate, but the accelerometer measurement ’ s usefulness depends. We adopt   a   simple   thresholding   scheme   that   only   those accelerometer   measurements   approximately   equal   to   the gravity vector in magnitude are accepted for the EKF update. Specifically,   the   valid   accelerometer   measurements should satisfy a   md g   T     y   , where md T   is a prescribed threshold of   magnitude   discrepancy   and g   is   the   local   gravity magnitude [20, 21]. Note that the state of the constant magnetic vector i  m  should have unity magnitude. We have tried the equality- constrained EKF techniques proposed in [22, 23] but they performed   unsatisfactory   in   this   problem.   The   following technique is found to quite effective. In fact, if the unit- magnitude constraint of the constant magnetic vector was not considered in the EKF, (7) indicates that there would only exist a scalar ambiguity between the magnetometer matrix S   and the magnetic vector i  m   , namely  S   and 1   i        m   for any nonzero scalar    . Therefore, in this paper we disregard the norm constraint in the EKF, and after the EKF run re-scale the obtained   estimates   of   the   magnetometer   matrix   and   the magnetic vector by i rs    S   m   S   and i   i   i rs    m   m   m   . Note that this has no effect on the observability analysis, as the  Figure   1 .   Test   unit   (Xsens   MTi - G - 700)   and   placement   as   indicated   by  rectangular (upper - right: a coin attached at unit bottom   in   Tests # 3 - 4 )
5  nonzero scalar can always be cancelled from both sides of (8).  B.   Test Results  This subsection is devoted to verifying the above analysis and algorithm by real tests. We used the four test datasets in [1] that were collected using the Xsens MTi-G-700 unit. Each test starts and ends at an exactly same location on a wood bench, as shown by the rectangular in Fig. 1. Two datasets (#1 and #2) were collected using the MTi-G-700 unit alone, while the other two datasets (#3 and #4) were collected with a RMB coin taped onto the unit bottom plate. The coin is made of soft-iron magnetic material (Fig. 1, upper-right). The unit outputs are sampled at 100Hz. The tests were performed at latitude 28.247 deg and longitude 113.017 deg, in June 2015. The algorithm was run on a personal computer. The magnetometer, gyroscope and accelerometer outputs in Test #1 are plotted in Fig. 2 as an example. The unit stays stationary on the bench for about 75 seconds, and is picked up and taken several meters away from the data-collecting desktop. After being held still for a moment, the unit starts to be tumbled by hand for about 170 seconds. Then the test is finalized by holding the unit still for a moment again and putting it back to the placement on the bench. The other datasets follow the same motion sequence. The estimation starts at 110s instead of at some earlier time, because the unit is stationary at about 90s- 100s when the problem is obviously unobservable. Figure 3 gives the ratio of the smallest eigenvalue over the second smallest   eigenvalue   for   the   matrix *   * 0  t   T   dt    Y   Y   in   the logarithm scale. The eigenvalue ratio reaching almost zero clearly indicates the uniqueness of zero eigenvalue. Note that the ratio has two sharp reductions at 120s and 140s which respectively correspond to the starting time of the rotations along the y-axis and z-axis in the zoomed-in window of gyroscope   measurements in   Figure 2. This observation is common for all datasets. Table I lists the gyroscope biases obtained by averaging the stationary data before motion in all tests. The relative attitude in Test #1 computed by integrating the gyroscope measurements after subtracting the calculated bias is plotted in Figure 4. As the unit is put at the same placement before and after the test, the end angles should ideally be zeros and the offsets (less than 2 degrees in about five minutes) reflect the accumulating errors mainly due to gyroscope noise and the neglected Earth rotation. The   noise   standard   variances   in   (19)   are   set   by specifications or by examining the stationary data statistics as = 0.01deg g   s    , 4   3  =10   deg   s         , = mi        , 5  =9.8 gi   m   s       , =0.005 m     and =3 a   md T     . The magnetic noise standard variances mi     and m     are unit-less as the true magnetometer measurement is assumed having unit magnitude, and mi     and gi     are set according to the analysis below (5). The magnitude discrepancy threshold 2  0.03 md T   m s    , and the accelerometer measurement noise standard variance is set to three times the threshold md T   to account for the un-modeled acceleration disturbance. The initial state standard variance setting   uses   5   deg/s   for   the   gyroscope   bias,   0.1   for   the magnetometer matrix, 1 for the magnetometer bias, 0.5 for the magnetic vector and 1 m/s 2   for the gravity vector. The EKF algorithm is applied to the hand-tumbling data in Test   #1   (110s-250s).   Figure   5   gives   the   magnetometer measurement innovation and the EKF-computed three times standard variance. The average normalized innovation squared (ANIS)   across   all   time   [24],   namely   the   average   of                  1 |   1   |   1   |   1  T   T k k   k k   k k                 y   y   HP   H   R   y   y  , is employed as a  Figure   2 .   Magnetometer,   gyroscope  and accelerometer outputs in Test #1.  ( motion   sequence: still at placement,  picked up, short still, tumbl ed , short  still, put back, still at same placement) 0   50   100   150   200   250   300  -2  0  2  Mag. outputs  0   50   100   150   200   250   300  -500  0  500  Gyro outputs (deg/s)  0   50   100   150   200   250   300  -20  0  20  Acc. outputs (m/s 2 )  Time (s)  x  y  z  Figure 3. Eigenvalue ratio (smallest  over second smallest) logarithm of *   * 0  t   T   dt    Y   Y  for Test #1  Figure 4. Attitude profile in Test #1,  computed   by   integrating   gyroscope  measurements   (still - averaging   bias  removed)  Figure   5.   Magnetometer  measurement innovation and three  times   EKF - computed   standard  variance (dashed line) for Test   #1  Figure 6. Gyroscope bias estimate for  Test #1 100   120   140   160   180   200   220   240   260  0.98  0.99  1  1.01  1.02  Diagonal Entries  100   120   140   160   180   200   220   240   260  -0.01  -0.005  0  0.005  0.01  Non-diagonal Entries  Time (s)  S 1 1  S 2 2  S 3 3  S 1 2  S 1 3  S 2 1  S 2 3  S 3 1  S 3 2  Figure 7. Scaled magnetometer  matrix estimate for Test #1  Figure 8. Magnetometer bias estimate  for Test #1  Figure 9. Gravity vector estimate  and scaled magnetic vector estimate  for Test #1 .   Gravity vector has three  no nzero coordinates due to special  selection of inertial frame 100   150   200   250  -0.5  0  0.5  1  Scaled Magnetic Vector  Time (s)  100   150   200   250  -10  -5  0  5  10  Gravity Vector (m/s 2 )  Time (s)  x  y  z 100   120   140   160   180   200   220   240   260  -0.6  -0.4  -0.2  0  0.2  0.4  Magnetometer Bias  Time (s)  x  y  z 50   100   150   200   250   300   350  -0.8  -0.6  -0.4  -0.2  0  0.2  Magnetometer Bias  Time (s)  x  y  z 100   120   140   160   180   200   220   240   260  -0.05  0  0.05  100   120   140   160   180   200   220   240   260  -0.05  0  0.05  100   120   140   160   180   200   220   240   260  -0.05  0  0.05 0   50   100   150   200   250   300  -5  0  5  Attitude profile (deg)  Time (s)  roll  yaw  pitch 100   120   140   160   180   200   220   240   260  -8  -6  -4  -2  0  Time (s)  Logarithm of Eigenvalue Ratio  100   110   120   130   140   150  -200  0  200  x  y  z
6  scalar metric to quantify the measurement innovation, where |   1 k k    y  is the predicted measurement, |   1 k k    P   is the predicted state   covariance   and H   is   the   measurement   matrix. Statistically, ANIS is chi-square distributed with the same degrees of freedom as the measurement y   . For Test #1, the magnetometer ANIS is calculated to be 3.56, close to the theoretical value 3. Figures 6-9 plot the estimates for the gyroscope bias, the magnetometer parameters and the gravity/magnetic vectors, respectively. All estimates converge within 30 seconds. The final gyroscope bias estimate computed by EKF in Test #1 is listed in Table II, as well as those for other three tests. It shows that   the   proposed   EKF   algorithm is able   to   estimate   the gyroscope bias in dynamic condition up to 0.03 deg/s (taking the value by still averaging as reference), which is over three times better than what was achieved in [1]. In order to evaluate the quality of the obtained magnetometer parameters, we use the final estimate of S   and h   to calculate the magnitude of the   hand-tumbled   magnetometer   measurement   by     1 i m        m   S   y   h  . Figure 10 gives the histogram for Test #1 of the magnitude discrepancy from 1 and the fitted normal distribution   (mean 4  8   10      ,   standard   variance   0.005), compared with that of the raw magnetometer measurements. Additionally,   we   decompose   the   magnetometer   matrix   by 1   b m      S   C R  into the cross-sensor and intrinsic parameters, as listed in Table II for all tests. Regarding the cross-sensor misalignment angle, the discrepancy between Tests #1-#2 or Tests #3-#4 is reduced to less than 0.2 degrees, in contrast to 0.4 degrees in [1]. In contrast to [1] that puts equal weights on sub-functions of the total cost function, Kalman filters are able to yield the state estimate naturally weighted by the propagated state covariance. It is the probable reason better results have been obtained herein. Note that as being affected by the attached coin, the real magnetometer parameters deviate considerably from the given initial values in Tests #3-#4, particularly the magnetometer matrix S   . Therefore we process the datasets #3-#4 by the EKF twice, in which the second EKF run uses as initial setting the estimated magnetometer matrix S   by the first EKF run while the other initial settings keep unchanged. All results of Tests #3-#4 in Tables I-II are the final estimates of the second EKF run. Figures 11-13 plot the estimates for the gyroscope bias and the magnetometer parameters in the second EKF run for Test #3. Table III compares the magnetometer ANIS metrics for all tests. From   the   standpoint   of   the   calibration   task,   the gravity/magnetic vector estimates in Fig. 8 are auxiliary states. They can be used to derive the local magnetic inclination as     1  90   cos   iT   i   i rs       m g   g  in degrees. The result is 43.01 degrees, in accordance with the value (43.14 degrees) by the World Magnetic Model at the test site. They actually serve as pillar reference vectors for the inertial attitude estimate. The Euler angle discrepancy between the inertial attitude estimate and the gyroscope-maintained   relative   attitude   (computed   by integrating the gyroscope measurements after subtracting the still-averaging bias) for Test #1 and Test #3 is given in Figure 14. The angle discrepancy is about 0.5 degrees in Test #1 and about 1 degree in Test #3. This result indicates a quite good consistency considering that the gyroscope-maintained relative attitude is only 2 degrees/5 minutes in accuracy (Fig. 4). Theorem 2.1 tells that the problem could be solved using only magnetometer/gyroscope measurement information, so we check   how   the   algorithm   behaves   if   the   accelerometer measurement is abandoned. The main results are summarized in Tables I-III and Fig. 15 plots the Euler angle discrepancy for Test   #1   and   Test   #3.   Although   the   magnetometer   ANIS  Figure 10. Histogram of magnitude  discrepancy and fitted normal  contribution ( before and after  applying magnetometer parameters by  EKF   for hand - tumbling data in Test  #1)  Figure 11. Gyroscope bias estimate  for Test #3 (2nd EKF run) 50   100   150   200   250   300   350  0.6  0.8  1  1.2  Diagonal Entries  50   100   150   200   250   300   350  -0.5  0  0.5  1  Non-diagonal Entries  Time (s)  S 1 2  S 1 3  S 2 1  S 2 3  S 3 1  S 3 2  S 1 1  S 2 2  S 3 3  Figure 12. Scaled magnetometer matrix  estimate for Test #3 (2nd EKF run)  Figure 13. Magnetometer bias  estimate for Test #3 (2nd EKF run)  Figure 14.   Euler angle discrepancy between inertial attitude   estimate and  gyroscope - maintained relative attitude for Test #1 and Test #3 (2 nd   EKF  run)  Figure 15. Euler angle discrepancy between inertial attitude estimate and  gyroscope - maintained relative attitude for Test #1 and Test #3 (2nd EKF  run) when accele rometer not used  Figure 16. Magnitude of accelerometer outputs and those accepted for  EKF update (in red dots) in Tests #1 and #3. In left - upper zoomed - in  subfigure, black line denotes nominal gravity magnitude 100   150   200   250  -2  0  2  Test #1  Time (s)  Degree 50   100   150   200   250   300  -2  0  2  Test #3  Time (s)  Degree 100   150   200   250  -2  0  2  Test #1  Time (s)  Degree 50   100   150   200   250   300  -2  0  2  Test #3  Time (s)  Degree 100   120   140   160   180   200   220   240   260  5  10  15  20  m/s 2  Time (s)  Test #1  110   111   112   113   114   115  8  10 50   100   150   200   250   300   350  0  5  10  15  20  m/s 2  Time (s)  Test #3 50   100   150   200   250   300   350  -0.8  -0.6  -0.4  -0.2  0  0.2  Magnetometer Bias  Time (s)  x  y  z 50   100   150   200   250   300   350  -1  -0.5  0  0.5  1  Gyroscope Bias (deg/s)  Time (s) -1   -0.5   0   0.5   1  0  200  400  Points  -0.02   -0.01   0   0.01   0.02  0  500  Points
7  becomes significantly larger in the first EKF run of Test #3 (Table III), the proposed algorithm yields almost equivalent result   in   the   gyroscope   bias (Table   I)   and   magnetometer parameters (Table II) and performs one time worse in attitude (Fig.   15   vs.   Fig.   14).   These   observations   show   that   the accelerometer information is of little help in magnetometer calibration and alignment to inertial sensors under motion conditions, and yet is quite helpful in mitigating attitude errors. As shown in Fig. 16, the accelerometer output taken valid by the EKF (indicated by red dots) is only about 5% of the total hand-tumbling data in Test #1 and Test #3. Enlarging the magnitude discrepancy md T   by ten times brings the percentage of valid points up to 40-50%, but little estimate improvement has been obtained. IV.   C ONCLUSIONS  Modules or chips, consisting of triads of inertial and magnetic   sensors,   are   enormously   used   in   scientific   or consumer devices. Convenient, reliable and accurate mutual calibrations of these sensors are a prerequisite for any practical use. Magnetometer calibration (and its alignment to inertial sensors)   is   usually   achieved   by   heavily   relying   on accelerometer measurements at still, and thus are infeasible in motion   conditions.   This   paper   formulates   the   problem   of magnetometer calibration and alignment to inertial sensors as a state estimation problem, collectively and recursively solving the   magnetometer   intrinsic   calibration   and   cross-sensor calibration, as well as the gyroscope bias estimation. Sufficient conditions are derived for the problem to be globally observable, even when the accelerometer information is not used at all. An extended Kalman filter is designed to implement the state estimation   and   comprehensive   test   data   results   show   the superior performance of the proposed approach. It is found that the accelerometer information, though helpful in mitigating attitude errors, is of little benefit in magnetometer calibration and alignment to inertial sensors under motion conditions. The   datasets   used   are   available   online   at https://www.researchgate.net/profile/Yuanxin_Wu/contributio ns. R EFERENCES  [1]   Y. Wu and S. Luo, "On Misalignment between Magnetometer and Inertial Sensors,"   IEEE Sensors Journal,   vol. 16, pp. 6288- 6297, 2016. [2]   J. F. Vasconcelos , et al. , "Geometric Approach to Strapdown Magnetometer Calibration in Sensor Frame,"   IEEE Trans. on Aerospace and Electronic Systems,   vol. 47, pp. 1293 – 1306, 2011. [3]   J.   C.   Springmann   and   J.   W.   Cutler,   "Attitude-Independent Magnetometer Calibration with Time-Varying Bias,"   Journal of Guidance, Control, and Dynamics,   vol. 35, pp. 1080-1088, 2012. [4]   J.   Metge ,   et   al. ,   "Calibration   of   an   inertial-magnetic measurement unit without external equipment, in the presence of dynamic magnetic disturbances,"   Measurement Science and Technology,   vol. 25, 2014. [5]   R.   Alonso and M. D. Shuster, "TWOSTEP:   a fast robust algorithm   for   attitude-independent   magnetometer-bias determination,"   The Journal of the Astronautical Sciences,   vol. 50, pp. 433-451, 2002. [6]   R.   Alonso   and   M.   D.   Shuster,   "Complete   linear   attitude- independent magnetometer,"   The Journal of the Astronautical Sciences,   vol. 50, pp. 477-490, 2002. [7]   J. Hol, "Sensor Fusion and Calibration of Inertial Sensors, Vision, Ultra-Wideband and GPS," Doctoral dissertation, Department of Electrical Engineering, Linkö ping University, 2011. [8]   Y.   Wu   and   W.   Shi,   "On   Calibration   of   Three-axis Magnetometer,"   IEEE Sensors Journal,   vol. 15, pp. 6424 - 6431, 2015. [9]   B. Grandvallet , et al. , "Real-Time Attitude-Independent Three- Axis Magnetometer Calibration for Spinning Projectiles: A Sliding Window Approach,"   IEEE Trans. on Control System and Technology,   vol. 22, pp. 255-264, 2014. [10]   J. L. Crassidis , et al. , "Real-Time Attitude-Independent Three- Axis Magnetometer Calibration,"   Journal of Guidance, Control, and Dynamics,   vol. 28, pp. 115-120, 2005. [11]   X. Li and Z. Li, "A new calibration method for tri-axial field sensors   in   strap-down   navigation   systems,"   Measurement Science & Technology,   vol. 23, p. 105105, 2012. [12]   Z. Zhang and G. Yang, "Micromagnetometer Calibration for Accurate Orientation Estimation,"   IEEE Trans. on Biomedical Engineering,   vol. 62, pp. 553-560, 2015. [13]   M. Kok and T. B. Schon, "Maximum likelihood calibration of a magnetometer using inertial sensors," in   19th World Congress of the International Federation of Automatic Control (IFAC) , Cape Town, South Africa, 2014. [14]   M. Kok and T. B. Schö n, "Magnetometer Calibration Using Inertial Sensors,"   IEEE Sensors Journal,   vol. 16, pp. 5679-5689, 2016. [15]   P. D. Groves,   Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems , 2nd ed.: Artech House, Boston and London, 2013. [16]   D. H. Titterton and J. L. Weston,   Strapdown Inertial Navigation Technology ,   2nd   ed.:   the   Institute   of   Electrical   Engineers, London, United Kingdom, 2007. [17]   C.-T. Chen,   Linear System Theory and Design , 3rd ed.: Rinehart and Winston, Inc., 1999. [18]   Y. Wu , et al. , "Observability of SINS Alignment: A Global Perspective,"   IEEE Trans. on Aerospace and Electronic Systems,  vol. 48, pp. 78-102, 2012. [19]   A. Gelb,   Applied Optimal Estimation . Cambridge, Mass.,: M.I.T. Press, 1974. [20]   J. K. Lee and E. J. Park, "A Fast Quaternion-Based Orientation Optimizer via Virtual Rotation for Human Motion Tracking,"  IEEE Trans. on Biomedical Engineering,   vol. 56, pp. 1574-1582, 2009. [21]   A. M. Sabatini, "Quaternion-based extendedKalman filter for determining orientation by inertial and magnetic sensing,"   IEEE Trans. on Biomedical Engineering,   vol. 53, pp. 1346 – 1356, 2006. [22]   S. J. Julier and J. J. LaViola, "On Kalman Filtering With Nonlinear   Equality   Constraints,"   IEEE   Trans.   on   Signal Processing,   vol. 55, pp. 2774-2784, 2007. [23]   R.   Zanetti ,   et   al. ,   "Norm-Constrained   Kalman   Filtering,"  Journal of Guidance, Control, and Dynamics,   vol. 32, pp. 1458- 1465, 2009. [24]   Y. Bar-Shalom , et al. ,   Estimation with Applications To Tracking and Navigation : John Wiley & Sons, 2001.
8  Table I. Gyroscope Bias Estimates (unit: deg/s)  By Still Averaging   By EKF   By EKF   (No Acc . )  Test #1   [ - 0.195   0.168   0.256] T   [ - 0.221   0.171   0.250] T   [ - 0.216   0.16 5   0.25 4]   T  Test   #2   [ - 0.252   0.152   0.251] T   [ - 0.228   0.158   0.235] T   [ - 0.22 9   0.15 3   0.235 ]   T  Test #3   [ - 0.228   0.147   0.246] T   [ - 0.247   0.144   0.245] T   [ - 0.25 3   0.14 4   0.2 50]   T  Test #4   [ - 0.237   0.153   0.250] T   [ - 0.227   0.146   0.245] T   [ - 0.231   0.149   0.24 3]   T  Table II. Magnetometer Parameter Estimate  Test  Intrinsic Parameter  ( , R h   )  Cross - sensor Parameter m b C  (in Euler angles, deg)  EKF   EKF  (No Acc.)   EKF   EKF  (No Acc.)  #1      1.0021   -0.0039   0.0011 0   0.9969   0.0019 0   0   1.0058 -0.5018   0.0421   0.2379   T                           1.0023   -0.0050   0.0010 0   0.9972   0.0016 0   0   1.0060 -0.5017   0.0428   0.2378   T                      0.005 0.1 0 0.014 2                       0.014 0.1 3 0.003 4                        #2      1.0019   -0.0059   0.0006 0   0.9979   0.0010 0   0   1.0047 -0.5013   0.0415   0.2374   T                           1.0020   -0.0059   0.0006 0   0.9979   0.0008 0   0   1.0047 -0.5013   0.0416   0.2372   T                      0.062 0.005 0.118                      0.071 0.005 0.117                       #3      1.2668   0.3130   -0.4191 0   1.5174   0.3419 0   0   0.8240 -0.6288   -0.1026   -0.2513   T                           1.2656   0.3134   -0.4182 0   1.5169   0.3416 0   0   0.8238 -0.6289   -0.1027   -0.2508   T                      16.272 23.944 10.069                      16.265 23.873 10.071                       #4      1.2649   0.3150   -0.4177 0   1.5204   0.3401 0   0   0.8251 -0.6307   -0.1049   -0.2446   T                           1.2651   0.3147   -0.4178 0   1.5202   0.3402 0   0   0.8252 -0.6307   -0.1049   -0.2447   T                      16.253 23.762 10.056                      16.262 23.770 10.047                       Table III. Magnetometer ANIS Metrics for All Tests  Test   Mag. ANIS   Mag. ANIS (No   Acc.)  #1   3.56   3.6 1  #2   3.74   3.7 7  #3  (1 st   run, 2 nd   run)   4.14, 4.35   42.3 8 , 4.16  #4  (1 st   run, 2 nd   run)   6.50, 4.99   5.74 , 5.0 2