Dynamic Magnetometer Calibration and Alignment to Inertial Sensors by Kalman Filtering
11 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: ([email protected]).
Dynamic Magnetometer Calibration and Alignment to Inertial Sensors by Kalman Filtering 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 em 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 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 be 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 bm S C R by the orthogonal-triangular decomposition, where bm 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 m b em m b e R y h n C C m . The accelerometer measurement is related to the local gravity vector by [15, 16] b e e e e b ea 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, e e eie 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 eie ω 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 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 be C which projects the local magnetic vector and the local gravity vector to b-frame. By the chain rule of the orientation matri b t eb be eb e t t C C C C (3) in which b tb C and ee 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] b b bib gb t b te e eiee t e t C C ω ε nC C ω (4) where bib ω denotes the gyroscope measurement, ε the gyroscope bias and g n i.i.d zero-mean Gaussian noise with covariance 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 , b t e b t bb e b ee eb e t b C r C C C r C r (5) Using (4), the magnitude of the vector rate b eb e e ee iee t r C C ω r r . As the magnitude of the Earth’s rotation rate ( rad/s) is much smaller than low-cost gyroscope errors, it is reasonable to take b r approximately as a constant. B. Magnetometer Calibration and Observability Property
Hereafter we use the initial body frame, namely b , as the inertial frame (i-frame). The above equations can be formulated in a state-space form as
0, 0, i i bib gb t b ti imi gi C C ω ε nε nS hm n g n (6) with the magnetometer and accelerometer measurements given as b t im i mb t ia i a y SC m h ny C g n (7) where n , mi n and gi n are i.i.d zero-mean Gaussian noises with covariance I , mi I and gi I , respectively. The augmented system state x comprises the inertial attitude b ti 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 x , there exists a finite t such that the knowledge of the input and the output over t suffices to determine uniquely the initial state x . Otherwise, the system is said to be (globally) 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 t T dt W W is nonsingular, the matrix * *0 t T dt Y Y has one and only one zero eigenvalue and bi 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 bm S C R , the magnetometer measurement in (7) yields i i bm m mb 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] TT T T Tm m m T T vec
R Ry y y R Rh Yzh 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 * * Y z . Left multiplying * T Y and integrating over the interested time interval, it gives * * *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 tm im b i y C C m (10) Taking time derivative and using (4), * * b tm b i m bm b ib i m b ib y C ω ε C m y C ω ε (11) or equivalently, * * * mbbTm ib m m mb vec Cy ω y y WηC ε (12) If the matrix t T dt W W is nonsingular, then η can be solved as = t tT T m dt dt η W W W y , from which the misalignment mb 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 ti 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 im i y S C Q Q m h (13) which means that the initial values of b ti C and i m both have infinite feasible solutions. Therefore, we restrict b ti t C to be its physical value, namely an identical matrix, to make the formulation (6)-(7) fully observable. Then the inertial attitude b ti C will be available with (4) and the determined gyroscope bias through integration. Then the constant magnetic vector is computed as i i mb t m C S y h and the constant gravity vector is computed as i i ab 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 t Ta a dt y y is nonsingular, the matrix t T dt M M has one and only one zero eigenvalue and bi 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 tb i ba ib i ib a y ω ε C g ω ε y (14) from which the gyroscope bias, if the matrix t Ta a dt y y is nonsingular, can be uniquely solved as
10 0 t tT ba a a ib a dt dt ε y y y ω y . Similarly the initial values for b ti C and i g have infinite feasible solutions, because for any attitude matrix Q the following equation is always valid b t T ia i y C Q Q g (15) By restricting bi C to be an identical matrix, the inertial attitude b ti C will be available with (4) and the determined gyroscope bias. With the known inertial attitude b ti C , the constant gravity vector is computed as i i ab t g C y and the magnetometer measurement equation in (7) can be re-organized as i i imb t b t C S y C S h m (16) or alternatively in the linear equation form T i im b t b t i vec Sy C C I S h Mκm (17) If the matrix 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 k t to the current epoch k t and the inertial attitude is propagated to the first order as such [15, 16] kk k kk b ti ib t b t b ti bk k ib kb t t t C C CC I ω ε (18) where 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 bi 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 ib t b t C I ψ C . The error state
TT 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 ma a a δx Fδx Gwδy H nδy δxδy H n (19) where the dynamic noise
TT T T Tg mi gi w n n n n and the matrices are , , , ibib Tb i b i bm i i ib i ba i i C 0 00 C 0 0 I 0F G0 0 0 0 0 00 0 IH SC m 0 C m I I SC 0H 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 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 irs S m S and i i irs 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 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 ( * *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 = 0.01deg g s , =10 deg s , = mi , =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 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 for the gravity vector. The EKF algorithm is applied to the hand-tumbling data in Test
1| 1 | 1 | 1
T Tk 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 M ag . ou t pu t s G y r o ou t pu t s ( deg / s ) A cc . ou t pu t s ( m / s ) Time (s)xyz
Figure 3. Eigenvalue ratio (smallest over second smallest) logarithm of * *0 t T dt Y Y for Test
Figure 4. Attitude profile in Test
Figure 5. Magnetometer measurement innovation and three times EKF-computed standard variance (dashed line) for Test
Figure 6. Gyroscope bias estimate for Test
100 120 140 160 180 200 220 240 2600.980.9911.011.02 D i agona l E n t r i e s
100 120 140 160 180 200 220 240 260-0.01-0.00500.0050.01 N on - d i agona l E n t r i e s Time (s) S S S S S S S S S Figure 7. Scaled magnetometer matrix estimate for Test
Figure 8. Magnetometer bias estimate for Test
Figure 9. Gravity vector estimate and scaled magnetic vector estimate for Test
100 150 200 250-0.500.51 S c a l ed M agne t i c V e c t o r Time (s)100 150 200 250-10-50510 G r a v i t y V e c t o r ( m / s ) Time (s) xyz
100 120 140 160 180 200 220 240 260-0.6-0.4-0.200.20.4 M agne t o m e t e r B i a s Time (s) xyz
50 100 150 200 250 300 350-0.8-0.6-0.4-0.200.2 M agne t o m e t e r B i a s Time (s) xyz
100 120 140 160 180 200 220 240 260-0.0500.05100 120 140 160 180 200 220 240 260-0.0500.05100 120 140 160 180 200 220 240 260-0.0500.05 A tt i t ude p r o f il e ( deg ) Time (s) rollyawpitch
100 120 140 160 180 200 220 240 260-8-6-4-20 Time (s)
Loga r i t h m o f E i gen v a l ue R a t i o
100 110 120 130 140 150-2000200 xyz 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 S and h to calculate the magnitude of the hand-tumbled magnetometer measurement by i m m S y h . Figure 10 gives the histogram for Test , standard variance 0.005), compared with that of the raw magnetometer measurements. Additionally, we decompose the magnetometer matrix by bm 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 S . Therefore we process the datasets S by the first EKF run while the other initial settings keep unchanged. All results of Tests
90 cos iT i irs 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 Figure 10. Histogram of magnitude discrepancy and fitted normal contribution (before and after applying magnetometer parameters by EKF for hand-tumbling data in Test
Figure 11. Gyroscope bias estimate for Test
50 100 150 200 250 300 3500.60.811.2 D i agona l E n t r i e s
50 100 150 200 250 300 350-0.500.51 N on - d i agona l E n t r i e s Time (s) S S S S S S S S S Figure 12. Scaled magnetometer matrix estimate for Test
Figure 13. Magnetometer bias estimate for Test
Figure 14. Euler angle discrepancy between inertial attitude estimate and gyroscope-maintained relative attitude for Test nd EKF run)
Figure 15. Euler angle discrepancy between inertial attitude estimate and gyroscope-maintained relative attitude for Test
Figure 16. Magnitude of accelerometer outputs and those accepted for EKF update (in red dots) in Tests
100 150 200 250-202 Test D eg r ee
50 100 150 200 250 300-202 Test D eg r ee
100 150 200 250-202 Test D eg r ee
50 100 150 200 250 300-202 Test D eg r ee
100 120 140 160 180 200 220 240 2605101520 m / s Time (s)Test
50 100 150 200 250 300 35005101520 m / s Time (s)Test
50 100 150 200 250 300 350-0.8-0.6-0.4-0.200.2 M agne t o m e t e r B i a s Time (s) xyz
50 100 150 200 250 300 350-1-0.500.51 G y r o sc ope B i a s ( deg / s ) Time (s) -1 -0.5 0 0.5 10200400 P o i n t s -0.02 -0.01 0 0.01 0.020500 P o i n t s becomes significantly larger in the first EKF run of Test 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
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 , 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.
Table I. Gyroscope Bias Estimates (unit: deg/s)
By Still Averaging By EKF By EKF (No Acc.) Test [-0.195 0.168 0.256] T [-0.221 0.171 0.250] T [-0.216 0.165 0.254] T Test [-0.252 0.152 0.251] T [-0.228 0.158 0.235] T [-0.229 0.153 0.235] T Test [-0.228 0.147 0.246] T [-0.247 0.144 0.245] T [-0.253 0.144 0.250] T Test [-0.237 0.153 0.250] T [-0.227 0.146 0.245] T [-0.231 0.149 0.243] T Table II. Magnetometer Parameter Estimate
Test Intrinsic Parameter ( , R h ) Cross-sensor Parameter mb C (in Euler angles, deg) EKF EKF (No Acc.) EKF EKF (No Acc.) T T T T T T T T Table III. Magnetometer ANIS Metrics for All Tests
Test
Mag. ANIS
Mag. ANIS (No Acc.) st run, 2 nd run) 4.14, 4.35 42.38, 4.16 st run, 2 nd run) 6.50, 4.99 5.74, 5.02run) 6.50, 4.99 5.74, 5.02