sensors Article
Complete Tri-Axis Magnetometer Calibration with a Gyro Auxiliary Deng Yang 1,2 , Zheng You 1,2, *, Bin Li 1,2 , Wenrui Duan 1,2 and Binwen Yuan 1,2 1
2
*
Department of Precision Instrument, Tsinghua University, Beijing 100084, China;
[email protected] (D.Y.);
[email protected] (B.L.);
[email protected] (W.D.);
[email protected] (B.Y.) State Key Laboratory of Precision Measurement Technology and Instruments, Tsinghua University, Beijing 100084, China Correspondence:
[email protected]; Tel.: +86-10-6278-1981; Fax: +86-10-6278-3304
Academic Editor: Gert F. Trommer Received: 30 March 2017; Accepted: 24 May 2017; Published: 26 May 2017
Abstract: Magnetometers combined with inertial sensors are widely used for orientation estimation, and calibrations are necessary to achieve high accuracy. This paper presents a complete tri-axis magnetometer calibration algorithm with a gyro auxiliary. The magnetic distortions and sensor errors, including the misalignment error between the magnetometer and assembled platform, are compensated after calibration. With the gyro auxiliary, the magnetometer linear interpolation outputs are calculated, and the error parameters are evaluated under linear operations of magnetometer interpolation outputs. The simulation and experiment are performed to illustrate the efficiency of the algorithm. After calibration, the heading errors calculated by magnetometers are reduced to 0.5◦ (1σ). This calibration algorithm can also be applied to tri-axis accelerometers whose error model is similar to tri-axis magnetometers. Keywords: tri-axis magnetometer; calibration; gyro auxiliary; interpolation; linear operation
1. Introduction Strap-down inertial measurement units (SIMU) which consist of gyroscopes and accelerometers, have been applied to orientation estimation for a long time [1]. Nowadays, magnetometers rigidly mounted to SIMU, also known as MARG (Magnetic, Angular Rate, and Gravity), are popular for achieving better performance. However, the magnetometer outputs suffer from magnetic field distortions and sensor errors, including the hard iron effect, soft iron effect, zero bias error, scale factor error, non-orthogonal error, misalignment error, and noise [2]. Magnetometer calibrations are always advisable to be done prior to application. The calibration methods which generally rely on non-magnetic turntables [3–5] and Helmholtz coils [6–9], are proposed first. The magnetometer error parameters can be precisely calibrated. However, the cumbersome processes and calibration platforms bring about the issues of inconvenience and expense. Many stand-alone magnetometer calibration algorithms based on an ellipse-fitting method have been put forward [10–15]. These algorithms are low-cost and easy to use. The aim of these ellipse-fitting based algorithms is to compensate the magnetometer output data from lying on an ellipsoid to a sphere. However, the misalignment error between magnetometers and assembled platforms, which causes a rotation of the sphere, is unable to be calibrated. For complete tri-axis magnetometer calibration, a series of calibration algorithms in combination with accelerometers [16–21] are proposed. First, the ellipsoid-fitting algorithms are performed to calibrate errors, except the misalignment error. Then, optimization problems based on the inner products of the tri-axis magnetometer outputs and accelerometer outputs are established and solved for misalignment error Sensors 2017, 17, 1223; doi:10.3390/s17061223
www.mdpi.com/journal/sensors
Sensors 2017, 17, 1223
2 of 21
estimation. The author further applies gyroscopes with magnetometers and accelerometers to run an EKF (Extended Kalman Filter) for a maximum likelihood calibration process [22,23]. The nonlinear and iterative operations as well as the reliance on initial values create the drawback of heavy computation burdens for these algorithms. Tri-axis magnetometers and gyroscopes are also simultaneously applied in [24,25] to establish an EKF and an adaptive identification algorithm for magnetometer calibration without the use of an accelerometer. The error models of a tri-axis magnetometer are simplified. Only parts of the errors are calibrated, which degrade the calibration performance. In this work, a tri-axis gyroscope is applied as an auxiliary for complete tri-axis magnetometer calibration. Firstly, the sensor set of the tri-axis magnetometer and the tri-axis gyroscope is mounted into a non-magnetic cuboid frame. Then, the frame is put on a table and rotated for at least one cycle for each side of the frame, respectively. Finally, the complete tri-axis magnetometer calibration is achieved through linear algebra operations of the magnetometer and gyroscope output data. After calibration, the heading errors calculated by magnetometers are reduced to 0.5◦ (1σ). This algorithm requires a cuboid frame to mount the sensor set, which is easy to obtain and inexpensive. The proposed method, which employs the linear algebra operations, is of low computational complexity. Furthermore, because the error model of tri-axis accelerometers has a similar form to tri-axis magnetometers, this calibration algorithm can also be applied to tri-axis accelerometer calibrations. 2. Error Model To illustrate the calibration algorithm, the error model of a tri-axis magnetometer is established. Similar models have been established in [10–16]. For convenience, the complete tri-axis error model is discussed again with misalignment errors included. The hysteresis errors, temperature-dependent errors, and time-varying errors will not be discussed in this paper. The output errors are classified into two categories: magnetic field distortions and sensor errors. 2.1. Magnetic Field Distortions The magnetic field distortions are due to the ferromagnetic materials attached to the sensor frame, including the hard iron effect and soft iron effect. The hard iron effect is a constant additional magnetic field produced by permanent magnets on the assembled platform. It is represented as a 3 × 1 vector, denoted as bH . The soft iron effect is produced by the magnetization of soft magnets. It exerts effects on the magnitude and orientation of the magnetic field according to the external field orientation, which is represented as a 3 × 3 matrix, denoted as Csi . The i-row, j-column element in the matrix indicates the influence of the external j-direction field to the i-direction field. 2.2. Sensor Errors The sensor errors of the tri-axis magnetometer include the zero bias error, scale factor error, non-orthogonal error, misalignment error, and noise. These errors are mainly due to machining and installation defects. The zero bias error makes constant offsets to each axis of the magnetometer, which is represented as a 3 × 1 vector, denoted as bzb . The scale factor error comes from the sensitivity inconsistencies of each sensor. It is represented as a 3 × 3 diagonal matrix, denoted as Csf . The elements on the diagonal represent the sensitivities of each axis sensor. The non-orthogonality between individual sensors introduces an inter-axis coupling output error, which is represented as a 3 × 3 upper triangular matrix, denoted as Cno . In general conditions of small angle errors, the diagonal elements of Cno are close to 1. The misalignment error represents the angular misalignment between the tri-axis magnetometer set and the assembled platform, which is represented as a 3 × 3 unit rotation matrix, denoted as Cm . It can be simplified into an anti-symmetric matrix, with diagonal elements equal to 1. The output noise of individual sensor is assumed as uncorrelated Gaussian white noise. The output noise is represented as a 3 × 1 vector, denoted as ε.
Sensors 2017, 17, 1223 Sensors 2017, 17, 1223 Sensors 2017, 17, 1223
3 of 21 3 of 21 3 of 21
Above all, the error model of the tri-axis magnetometer is given as: m C m m bHH )) bzbzb C Cno Csf C Csi C Cm ((m b b εε no sf si m bε Km e +bbHε) + bzb + ε m = Cno Cs f Csi CmK(m m x bbx x kk12 kk13 m k e + b + ε k11 = Km 11 12 13 x m x x m y kk22 kkm bbxyy 23 kk21 yy ε x m e k11 k12 x 13 21 22 23 y m k k z + ε = k21 k22 kk31 e 32 km 33 bbyz + k 23 31 32 33 y z z zz y m ez k31 k32 k33 m bz εz
(1) (1) (1)
In the equation, m stands for the magnetometer output vector. K stands for the total error In the the equation, equation, m m stands stands for for the the magnetometer magnetometer output output vector. vector. K K stands stands for for the the total total error error In coefficient matrix, and b stands for the total bias error vector. Note that in this paper, ~ stands for coefficient matrix, and b stands for the total bias error vector. Note that in this paper, ~ stands for coefficient matrix,^ and b stands for the totalquantities, bias error vector. Note that in this paper, ~stands for ideal ideal quantities, stands for interpolation and ͡ stands for compensated quantities. ideal quantities, ^ stands for interpolation quantities, and ͡ stands for compensated quantities. quantities, ˆ stands for interpolation quantities, and _ stands for compensated quantities. 3. Calibration Algorithm Development 3. Calibration Calibration Algorithm Algorithm Development Development 3. The calibration algorithm is developed for evaluating the error coefficient matrix and the bias The calibration calibration algorithm algorithm is is developed developed for for evaluating evaluating the the error error coefficient coefficient matrix matrix KK K and and the the bias bias The error vector b. In this calibration algorithm, the sensor set is firstly mounted into a non-magnetic error vector b. In this calibration algorithm, the sensor set is firstly mounted into a non-magnetic error vector b. In this calibration algorithm, the sensor set is firstly mounted into a non-magnetic cuboid frame. six sides of the cuboid are numbered from 11 to 6. Side 11 is to 2. cuboid frame. frame. The The numbered from to is 1opposite opposite to side side 2. Side Side cuboid Thesix sixsides sidesofofthe thecuboid cuboidare are numbered from 1 6. to Side 6. Side is opposite to side 2. 33 is opposite to side 4, and side 5 is opposite to side 6. The sensor set is aligned with the frame. The is opposite to side and is opposite to side The6.sensor set is aligned with the frame. The zz Side 3 is opposite to 4, side 4, side and 5side 5 is opposite to 6. side The sensor set is aligned with the frame. axis the set is to side The yy axis of the set is axis zof ofaxis the sensor sensor is perpendicular perpendicular to side side 11 and and side12. 2.and Theside axis the ysensor sensor setthe is perpendicular perpendicular The of the set sensor set is perpendicular to side 2. ofThe axis of sensor set is to side 3 and side 4. The x axis of the sensor set is perpendicular to side 5 and side 6, as shown in to side 3 and side 4. The x axis of the sensor set is perpendicular to side 5 and side 6, shown perpendicular to side 3 and side 4. The x axis of the sensor set is perpendicular to side 5asand side in 6, Figure 1. The calibration process is divided into two steps. In the first step, the tri-axis magnetometer Figure 1. The calibration process is divided into two steps. In the first step, the tri-axis magnetometer as shown in Figure 1. The calibration process is divided into two steps. In the first step, the tri-axis interpolation at spin angular intervals are In second the error interpolation outputs outputs at specified specified spin are calculated. calculated. In the the second step, step, error magnetometer interpolation outputs at angular specifiedintervals spin angular intervals are calculated. In thethe second coefficient matrix and bias error vector are calculated through linear algebra operations of coefficient matrix and matrix bias error vector calculated through linearlinear algebra operations of step, the error coefficient and bias errorare vector are calculated through algebra operations magnetometer interpolation outputs. magnetometer interpolation outputs. of magnetometer interpolation outputs.
Figure Figure 1. Definition of cuboid sides and sensor set coordinates. Figure 1. 1. Definition Definition of of cuboid cuboid sides sides and and sensor sensor set setcoordinates. coordinates.
3.1. Magnetometer Interpolation Output Calculation 3.1. 3.1. Magnetometer Magnetometer Interpolation Interpolation Output Output Calculation Calculation The cuboid frame is put on table and rotated over one cycle by hand. This is repeated for each The The cuboid cuboid frame frame is is put put on on aaa table table and and rotated rotated over over one one cycle cycle by byhand. hand. This This is is repeated repeated for for each each k k numbered side facing upwards. Then the tri-axis magnetometer output data m and gyroscope numbered side facing upwards. Then the tri-axis magnetometer output data m and gyroscope output k numbered side facing upwards. Then the tri-axis magnetometer output data m and gyroscope k kk == 1–6, stands for number of upward The output ω data ωk data are obtained, k = 1–6, where stands kkfor the number the upward The side. process shown are obtained, obtained, 1–6,k where where stands for the the of number of the theside. upward side. Theisprocess process output data ωk are is shown in Figure 3. Here are some points to note: (1) the sequence of which side is facing up can in are3.some to note: (1)tothe sequence which side is facing be up arbitrary; is Figure shown 2. inHere Figure Here points are some points note: (1) the of sequence of which sideup is can facing can be be arbitrary; (2) the orientation of the flank when putting the frame on the table can be arbitrary (finding (2) the orientation of the flank when putting the frame on the table can be arbitrary (finding north is arbitrary; (2) the orientation of the flank when putting the frame on the table can be arbitrary (finding north is not required); (3) both clockwise and anticlockwise rotations are accepted; (4) constant not required); (3) both clockwise and anticlockwise rotations are accepted; (4) constant rotation speed north is not required); (3) both clockwise and anticlockwise rotations are accepted; (4) constant rotation speed not required; and (5) the turns must is not required; and the number must beof rotation speed is is not(5) required; and of (5)turns the number number oflarger turnsthan must1.be be larger larger than than 1. 1. 1 1
2 2
3 3
4 4
5 5
6 6
Figure 2. Cuboid frame rotation process. Figure 2. 2. Cuboid Cuboid frame frame rotation rotation process. process. Figure
The The tri-axis tri-axis magnetometer magnetometer interpolation interpolation outputs outputs at at specified specified spin spin angular angular intervals intervals are are k and ωk. The interpolation flowchart is shown in Figure 3. Firstly, the number of calculated from m The tri-axis magnetometer interpolation outputs at specified spin angular intervals are calculated calculated from mk and ωk. The interpolation flowchart is shown in Figure 3. Firstly, the number of k and k .angular turns n, the interval Δθ are set. The value of 360/Δθ must be an integer. The upward side from m and ω The interpolation flowchart is shown in Figure 3. Firstly, the number of turns n, and turns n, and the angular interval Δθ are set. The value of 360/Δθ must be an integer. The upward side k number number kk is is to to be be verified. verified. Next, Next, by by integrating integrating the the output output data data ω ω⊥k of of the the gyroscope gyroscope whose whose sensitive sensitive ⊥
Sensors 2017, 17, 1223
4 of 21
∆θ are set. The value of 360/∆θ must be an integer. The upward side number 4 of 21 k of the gyroscope whose sensitive axis k is to be verified. Next, by integrating the output data ω⊥ axis is perpendicular the table, the rotation angles θk ofsample each sample points are derived. After is perpendicular to thetotable, the rotation angles θ k of each points are derived. After which, k ˆ k are derived the interpolation outputs outputs m mk and sideupward k is changed which, the interpolation mk arefrom derived fromθ k .mThen, andthe θk upward . Then, the side to k isa k are obtained, k ˆ different number, and the program is repeated. Finally, the interpolation outputs m changed to a different number, and the program is repeated. Finally, the interpolation i outputs mi where i = 1, 2,where . . . , 360n/∆θ, k =360n/Δθ, 1, 2, . . . ,k6. are obtained, i = 1, 2,…, = 1, 2,…, 6. the angular Sensors 2017, 17,interval 1223
Figure Figure 3. 3. Flowchart Flowchart for for magnetometer magnetometer output output interpolation. interpolation.
3.2. 3.2. Error Error Coefficient Coefficient Calculation Calculation The stable. It The geomagnetic geomagnetic field field is is stable. It can can hardly hardly change change within within days days and and is is assumed assumed to to be be constant constant in the calibration process. The geomagnetic field component perpendicular to the table is denoted in the calibration process. The geomagnetic field component perpendicular to the table is denoted as as 1 h⊥ . When side 1 faces upwards, m ∼z1 is equal to h⊥ , and Equation (1) can be rewritten as: h⊥ . When side 1 faces upwards, mz is equal to h⊥ , and Equation (1) can be rewritten as: m bx 1x ,i 1x ,1 bx ˆ1x ,i m 1 m 1 ˆ x,i m e x,i 1 bx e1x,1 bx z 1m 1 m ˆ m K m b KC m b (2) y ,i y m y ,i 1 i y ,1 1 y 1 ey,1 + e y,i + = K Ceiz m (2) by ˆ y,i =K 1 m by ˆ h h bz mz ,i h⊥ h bzbz b ˆ 1z,i m z ⊥ z
where Czi is the ideal rotation matrix around z axis of sensor set, which is expanded as: e is the ideal rotation matrix around z axis of sensor set, which is expanded as: where C i cos i sin i 0 C i ,1 cos i∆θ )i sincos (i∆θi) 0 0 C Cei,1 z C i (sin ie,2 z e = − sin(i∆θ ) cos(i∆θ ) 0 C = Ci,2 i Cie,3 0 0 1 0 0 1 C i,3
(3) (3)
Considering that:
cos i 180 sin i 180 0 C i ,1 z Ci 180 / sin i 180 cos i 180 0 C i ,2 0 0 1 C i ,3 by substituting Equation (4) into Equation (2), there is:
(4)
Sensors 2017, 17, 1223
5 of 21
Considering that:
ez C i +180◦ /∆θ
cos(i∆θ + 180◦ ) = − sin(i∆θ + 180◦ ) 0
sin(i∆θ + 180◦ ) cos(i∆θ + 180◦ ) 0
−Cei,1 0 ei,2 0 = −C ei,3 1 C
(4)
by substituting Equation (4) into Equation (2), there is: ˆ 1x,i+180◦ /∆θ m e 1x,i e 1x,i m −m bx bx 1 m 1 z e y,i + by = K −m e 1y,i + by ˆ y,i+180◦ /∆θ = KCi+180◦ /∆θ m bz bz ˆ 1z,i+180◦ /∆θ m h⊥ h⊥
(5)
∼2
Similarly, when side 2 faces upwards, mz is equals to −h⊥ . There are: ˆ 2x,i m e 2x,i m bx m 2 e 2y,i + by ˆ y,i = K m bz ˆ 2z,i m − h⊥
(6)
ˆ 2x,i+180◦ /∆θ m e 2x,i −m bx m 2 e 2x,i + by ˆ y,i+180◦ /∆θ = K −m bz − h⊥ ˆ 2z,i+180◦ /∆θ m
(7)
∼3
When side 3 faces upwards, my is equals to h⊥ . There are: ˆ 3x,i m e 3x,i m bx m 3 ˆ y,i = K h⊥ + by e 3z,i bz m ˆ 3z,i m
(8)
ˆ 3x,i+180◦ /∆θ m e 3x,i −m bx m 3 ˆ y,i+180◦ /∆θ = K h⊥ + by e 3z,i bz −m ˆ 3z,i+180◦ /∆θ m
(9)
∼4
When side 4 faces upwards, my is equals to −h⊥ . There are: ˆ 4x,i m m 4 ˆ y,i = K ˆ 4z,i m
e 4x,i m bx − h ⊥ + by 4 e z,i bz m
ˆ 4x,i+180◦ /∆θ m e 4x,i −m bx m 4 ˆ y,i+180◦ /∆θ = K −h⊥ + by e 4z,i −m bz ˆ 4z,i+180◦ /∆θ m
(10)
(11)
∼5
When side 5 faces upwards, m x is equals to h⊥ . There are: ˆ 5x,i m h⊥ bx m e5 5 ˆ y,i = K m y,i + by bz e 5z,i m ˆ 5z,i m
(12)
Sensors 2017, 17, 1223
6 of 21
ˆ 5x,i+180◦ /∆θ m h⊥ bx m 5 e 5y,i + by ˆ y,i+180◦ /∆θ = K −m bz e 5z,i −m ˆ 5z,i+180◦ /∆θ m
(13)
∼6
When side 6 faces upwards, m x is equals to −h⊥ . There are: ˆ 6x,i m − h⊥ bx e6 m 6 ˆ y,i = K m y,i + by bz e 6z,i m ˆ 6z,i m
(14)
ˆ 6x,i+180◦ /∆θ m − h⊥ bx m 6 e 6y,i + by ˆ y,i+180◦ /∆θ = K −m bz e 6z,i −m ˆ 6z,i+180◦ /∆θ m
(15)
By Equations (2)–(15), the error coefficient matrix K and bias error vector b are solved through linear operations as follows: 1.
By linear operation of Equations (2), (5)–(15), b is solved: ˆ kx,i m bx 2160n m k ˆ y,i = by ∆θ bz ˆ kz,i m 6 360n/∆θ
∑ ∑
k =1
2.
i =1
(16)
By linear operation of Equations (2), (5)–(7), h⊥ K3 is solved: 2 ˆ 1x,i ˆ x,i m m 0 k13 360n/∆θ 720n 720n m ˆ2 1 K 0 = h⊥ k23 ˆ y,i − ∑ m y,i = ∆θ ∆θ i =1 h⊥ k33 ˆ 1z,i ˆ 2z,i m m 360n/∆θ
∑
i =1
3.
(17)
By linear operation of Equations (8)–(11), h⊥ K2 is solved: 4 3 ˆ ˆ m m 0 k 12 x,i x,i 360n/∆θ 360n/∆θ 720n 720n ∑ mˆ 3y,i − ∑ mˆ 4y,i = ∆θ K h⊥ = ∆θ h⊥ k22 i =1 i =1 0 k32 ˆ 4z,i ˆ 3z,i m m
4.
(18)
By linear operation of Equations (12)–(15), h⊥ K1 is solved: 6 ˆ 5x,i ˆ x,i m m h⊥ k11 360n/∆θ 720n 720n ˆ6 m 5 K 0 = h⊥ k21 ˆ y,i − ∑ m y,i = ∆θ ∆θ i =1 k31 0 ˆ 5z,i ˆ 6z,i m m 360n/∆θ
∑
i =1
(19)
From Equations (16)–(19), h⊥ K and b are derived. h⊥ is a constant, which makes a scale for the tri-axis outputs. This scale has no effect on the orientation estimations. In many applications, the magnetometer outputs are normalized. Therefore, the tri-axis outputs can be compensated as: _ ( h K ) −1 (m − b ) m = ⊥ −1 ( h ⊥ K ) (m − b)
(20)
Sensors 2017, 17, 1223
7 of 21
However, in some applications, such as magnetic field measurements, h⊥ needs to be determined. h⊥ can be obtained by geomagnetic field models, or by a single-axis calibrated magnetometer. In general cases, the non-diagonal elements of K are close to zero, so h⊥ can also be evaluated as: h⊥ =
∆θ 360n/∆θ 1 ˆ 6x,i ˆ 5x,i − m ˆ 4y,i + m ˆ 3y,i − m ˆ 2z,i + m ˆ z,i − m m ∑ 2160n i=1
(21)
Finally, the tri-axis magnetometer outputs are compensated as follows: _
m = h ⊥ ( h ⊥ K ) −1 (m − b )
(22)
The program code of the calibration algorithm is presented in the Appendix A. 4. Calibration Error Analysis The gyro errors, cuboid frame errors, and rotation operation errors may degrade the calibration performance. The calibration errors introduced by the gyro bias, gyro misalignment, non-orthogonality of the cuboid frame, and rotation deviation are analyzed, respectively. 4.1. Gyro Bias Gyro bias is a main error of gyroscopes. Most of the bias error can be estimated during stationary time periods, and the residual tri-axis gyroscope bias error is assumed to be: bg =
h
bgx
bgy
bgz
iT
(23)
When side 1 faces upwards, the magnetometer interpolation outputs are: ˆ 1x,i m e 1x,1 m bx m 1 z e1 ˆ y,i = KCi m y,1 + by bz e 1z,1 ˆ 1z,i m m
The actual rotation matrix around the z axis is: cos i∆θ − ibgz ∆t Ciz = − sin i∆θ − ibgz ∆t 0
sin i∆θ − ibgz ∆t cos i∆θ − ibgz ∆t 0
(24)
0 0 1
(25)
∆t is the time interval between two adjacent interpolation points. The angle measurements by integrating the gyroscope output data are of good accuracy in the short-term, and the angle errors are of small value. The total time during one direction rotation is assumed to be less than 10 s, and by testing the gyroscope in the experiment, the bias error is less than 0.5◦ /s, which means the angle error estimated by the gyroscope is less than 5◦ . Under the condition of small angle errors, Equation (25) is rearranged as follows: ez + ∆C z Ciz = C i i
(26)
where ∆Ciz is the rotation matrix error caused by the gyro bias at i-th interpolation point, given as:
sin(i∆θ ) ∆Ciz = ibgz ∆t cos(i∆θ ) 0
− cos(i∆θ ) 0 sin(i∆θ ) 0 0 0
(27)
Sensors 2017, 17, 1223
8 of 21
Λ1 is defined as the sum of the rotation matrix errors caused by the gyro bias when side 1 faces upwards, derived as:
Λ1 =
360n/∆θ
∑
i =1
0 1 0
0 0 0
∆θ 2
∆θ 2
∆θ 2
1 ∆θ 180 bgz n∆t cot ∆Ciz = − 0 ∆θ 2 0
(28)
Similarly, there is: ∆θ Λ1 = − 180 ∆θ b gz n∆t cot 2 ∆θ Λ3 = − 180 ∆θ b gy n∆t cot 2 ∆θ Λ5 = − 180 ∆θ b gx n∆t cot 2
1 0 0 1 0 0 0 0 0
0 1 0 0 0 0 0 1 0
0 0 0 0 0 1 0 0 1
, Λ2 =
180 ∆θ b gz n∆t
cot
, Λ4 =
180 ∆θ b gy n∆t
180 ∆θ b gx n∆t
cot
, Λ6 =
cot
1 0 0 1 0 0 0 0 0
0 1 0 0 0 0 0 1 0
0 0 0 0 0 1 0 0 1
(29)
Thus, the calibration error of b caused by gyro bias is derived by applying Equation (29) into Equation (16), as shown in Equation (30): e 1x,1 − m e 2x,1 0 m e 3x,1 − m e 4x,1 m ∆t ∆θ 1 e5 − m e 6y,1 e y,1 − m e 2y,1 + bgy = cot 0 + bgx m bgz m y,1 12 2 e 3z,1 − m e 4z,1 m e 5z,1 − m e 6z,1 m 0
∆bgb
(30)
If the initial orientations of the flank when side k and side k + 1 (k = 1, 3, 5) face upwards are the same, the gyro bias error would cause little calibration error to b, and if the initial orientation of the flank are opposite, the gyro bias error would cause maximal calibration error to b. The calibration error of K caused by the gyro bias is derived by applying Equation (29) into Equations (17)–(19), as shown in Equation (31): ∆K gb
0
∆t ∆θ e 5y,1 + m e 6y,1 = cot bgx m 4h⊥ 2 e 5z,1 + m e 6z,1 bgx m
e 3x,1 + m e 4x,1 bgy m 0
e 3z,1 bgy m
e 4z,1 +m
e 1x,1 + m e 2x,1 bgz m e 1y,1 + m e 2y,1 bgz m
0
(31)
If the initial orientations of the flank when side k and side k + 1 (k = 1, 3, 5) face upwards are opposite, the gyro bias error would cause little calibration error to K, and if the initial orientation of the flank are the same, the gyro bias error would cause maximal calibration error to K. ∆bgb and ∆Kgb satisfy: ∆b gb ≤ ∆K ≤ gb
h iT bg F cos( I ) 1 1 1 0 1 1 ∆t ∆θ 2 cot( I ) cot 2 b g 1 0 1 1 1 0
∆t 6
cot
∆θ 2
(32)
where F is the total geomagnetic field intensity, and I is the geomagnetic inclination angle. Considering I = 59◦ , ∆t = 0.02 s, ∆θ = 1◦ , bg = 0.5◦ /s, and that the magnetometer outputs are normalized, even under the worst initial orientation conditions, the elements of ∆bgb and ∆Kgb are less than 0.0034 and 0.0060, respectively.
Sensors 2017, 17, 1223
9 of 21
4.2. Gyro Misalignment When installing the gyroscope into the cuboid frame, the misalignment between gyro axes9 and Sensors 2017, 17, 1223 of 21 cuboid faces could be introduced. This would affect the gyro outputs, and cause estimation errors of the rotation angle. The gyro misalignment is presented as a rotation matrix of the small angle: 1 o 1 C gm 1 + 1− ψo 1 θ (33) o (1) Cgm = ψ 1 + o(1) 1 −oφ 1 (33) −θ φ 1 + o (1) o(∙) denotes the infinitesimal. The gyro output errors are: o(·) denotes the infinitesimal. The gyro output errors are: g 0 1 ∆g1 g 2 C gm I 3 00 θω (34) = C − I = (34) ∆g 0 − φω g o 3 2 gm ∆g⊥ ω o (ω ) Δg1 , Δg2 are the gyro output errors whose axes are parallel to the table, and Δg⊥ is the gyro ∆g1error , ∆g2whose are theaxis gyro output errors whose axes are to the table, ∆g⊥ is the gyro output is perpendicular to the table. Theparallel misalignment angle and causes little error on output error whose axis is perpendicular to the table. The misalignment angle causes little error on g⊥ , g⊥ , which is applied to rotation estimation. whichThus, is applied to rotation estimation. the gyro misalignment error introduces slight calibration errors, which can be ignored. Thus, the gyro misalignment error introduces slight calibration errors, which can be ignored. 4.3. Non-Orthogonality of Cuboid Frame 4.3. Non-Orthogonality of Cuboid Frame Non-orthogonality of the cuboid frame is indicated in Figure 4. The α, β and γ are the deviation Non-orthogonality of the cuboid frame is indicated in Figure 4. The α, β and γ are the deviation angles, which are of small value. angles, which are of small value.
Figure 4. Non-orthogonality of the cuboid frame. Figure 4. Non-orthogonality of the cuboid frame.
When When sides sides 1–6 1–6 are are perpendicular perpendicular to to the the table, table, respectively, respectively, the the output output errors errors of of the the gyroscope gyroscope whose axis is perpendicular to the table are derived, respectively: whose axis is perpendicular to the table are derived, respectively: 1,2 1,2 1,21,2 ω gno= (cos ⊥ ⊥ cosβ −11) ==oo(β) ∆gno, , 3,4 3 ,4 3,43 ,4 (35) ∆gno, ω gno= (cos ⊥ ⊥ cosα − 1)1= o(α) (35) , 5,6 5 ,6 5,65 ,6 ∆gno, ω gno= (cos ⊥ oo(γ) cosα coscosγ −11)= oo(α) + , ⊥ It It can can be be deduced deduced from from Equation Equation (35) (35) that that the the non-orthogonality non-orthogonality of of the the cuboid cuboid frame frame has has little little effect effect on on the the vertical vertical gyro gyro outputs. outputs. The The magnetometer magnetometer output error, error, caused caused by by the the non-orthogonality non-orthogonality of of the the cuboid cuboid frame, frame, is is represented represented when when side side 11 faces faces upwards: upwards: 1 0 10 0 1 0 0 1 1 0 1 0 = m 0 no ,i 1 β 0 1 0 − β 01 γ 1 0
∆m1no,i
−0γ 01 10
1 m m h −γh⊥ ex1x,i ,i 1 0 I m h βh⊥ ey 1y,i − I3 m 3 , i = 1 1 1−β m 1 hh m e e γ m 1 m x,i y , i y,i ⊥ x ,i
In the same way, the magnetometer output errors with each side upwards are derived as:
(36) (36)
Sensors 2017, 17, 1223
10 of 21
In the same way, the magnetometer output errors with each side upwards are derived as: h iT h 1 − βm 1 1 2 e e − γh βh γ m γh⊥ − βh⊥ ∆m = , ∆m = ⊥ ⊥ x,i y,i no,i no,i h iT h iT e 3x,i 0 e 4x,i 0 ∆m3no,i = αh⊥ −αm , ∆m4no,i = −αh⊥ −αm h iT h iT ∆m5 = −γm e 5z,i 0 γh⊥ e 6z,i 0 −γh⊥ , ∆m6no,i = −γm no,i
e 2x,i − βm e 2y,i γm
iT (37)
Thus, by applying Equations (37) into Equation (16), the calibration error of b caused by the non-orthogonality of the cuboid frame is calculated: ∆bno =
h ∆θ 360n/∆θ 6 5 4 3 2 1 + ∆m + ∆m + ∆m + ∆m + ∆m ∆m 0 no,i no,i no,i no,i = no,i no,i 2160n i∑ =1
0
0
iT
(38)
By applying Equation (37) into Equations (17)–(19), the calibration error of K is calculated:
∆Kno =
∆θ 720nh⊥
360n/∆θ h
∑
i =1
∆m5no,i − ∆m6no,i
∆m3no,i + ∆m4no,i
∆m1no,i − ∆m2no,i
i
0 = 0 γ
α 0 0
−γ β 0
(39)
The non-orthogonality error of the cuboid frame introduces calibration errors to four elements of K, and does not affect the other elements of K and b. The errors are equal to the deviation angles. Assuming the machining verticality is 0.02, which is easy to be satisfied, and the side length of the cuboid frame is 100 mm, the errors to the four elements of K are less than 2 × 10−4 , which could be ignored. 4.4. Rotation Deviation While rotating the frame, the orientation of the rotations may change slightly due to the table being uneven and vibrations caused by friction during the rotation. This can result in unwanted effects on the gyro outputs and magnetometer outputs. The orientation deviation is presented as:
Crd
1 + o (1) = 0 ε1
0 1 0
−ε 1 1 0 0 1 + o (1) o (1) −ε 1 0 ε2 0 1 + o (1) ε2 0 1 + o (1) = (40) 1 + o (1) 0 −ε 2 1 + o (1) ε1 −ε 2 1 + o (1)
The gyro output error caused by the rotation deviation is:
0 ∆grd,1 o (ω ) o (ω ) − ε 1 ω ∆grd,2 = (Crd − I3 ) 0 + o (ω ) = o (ω ) + ε 2 ω ∆grd,⊥ ω 0 o (ω )
(41)
g⊥ is slightly affected by the rotation deviation. When side 1 faces upwards, the magnetometer output errors caused by the rotation deviation is: −ε11,i h⊥ e 1x,i m 1 ε12,i h⊥ e y,i = = (Crd,i − I3 ) m e 1x,i − ε12,i m e 1y,i ε11,i m h⊥
∆m1rd,i
(42)
Sensors 2017, 17, 1223
11 of 21
In the same way, the magnetometer output errors when the other sides face upwards are obtained: h iT h iT 1 h 1 h 1 m 1 − ε1 m 1 2 h 2 h 2 m 2 − ε2 m 2 1 = 2 = e e e e − ε ε ε ε − ε ε ∆m , ∆m ⊥ ⊥ ⊥ ⊥ 1,i 2,i 1,i x,i 2,i y,i 1,i 2,i 1,i x,i 2,i y,i rd,i rd,i h iT h iT e 3x,i − ε32,i m e 3z,i ε31,i h⊥ e 4x,i − ε42,i m e 4z,i −ε41,i h⊥ ∆m3rd,i = −ε32,i h⊥ ε31,i m , ∆m4rd,i = ε42,i h⊥ ε41,i m h iT h iT 5 − ε5 m 5 5 h 5 h 6 m 6 − ε6 m 6 6 h 6 h 6 = ∆m5 = ε5 m e e e e ε − ε ε − ε ε , ∆m ⊥ ⊥ ⊥ ⊥ 1,i x,i 2,i y,i 1,i 2,i 1,i x,i 2,i y,i 1,i 2,i rd,i rd,i
(43)
By applying Equation (43) into Equation (16), the calibration error of b caused by the rotation deviation is calculated: ∆brd =
∆θ 360n/∆θ 1 2 3 4 5 6 ∆m + ∆m + ∆m + ∆m + ∆m + ∆m rd,i rd,i rd,i rd,i rd,i rd,i 2160n i∑ =1
(44)
By applying Equation (43) into Equations (17)–(19), the calibration error of K is calculated: ∆Krd =
∆θ 360n/∆θ h ∆m5rd,i − ∆m6rd,i 720nh⊥ i∑ =1
∆m3rd,i + ∆m4rd,i
∆m1rd,i − ∆m2rd,i
i
(45)
The deviation angles εkj,i are assumed to be normally distributed with zero mean and variance of Qε , and the εkj,i and mkj,i are uncorrelated. The variance of the calibration error caused by the rotation deviation satisfies: h iT F2 ∆θQ < Var ∆b ( ) 1 1 1 rd 3240n 2 cot2 ( I ) 1 1 (46) ∆θQ Var (∆Krd ) < 720n 1 2 cot2 ( I ) 1 1 1 2 cot2 ( I ) The magnetometer outputs are considered as being normalized, such that the norm of the magnetic √ field F is set as 1. If Q = 1◦ , I = 59◦ , ∆θ = 1◦ , and n = 2, the standard deviations of the elements in ∆brd are less than 1.2 × 10−3 , and the standard deviations of diagonal elements and off-diagonal elements in ∆Krd are less than 2.2 × 10−3 and 2.6 × 10−3 , respectively. Above all, the calibration errors introduced by the gyro bias, gyro misalignment, non-orthogonality of the cuboid frame, and rotation deviation are slight and acceptable under normal conditions. 5. Simulation To illustrate the calibration accuracy and computational requirements of the proposed method, the ellipsoid-fitting calibration method in [13] is performed for comparison. Hereafter, the ellipsoid-fitting calibration method in [13] is referred to as the EM, to distinguish from the proposed calibration method is referred to as the PM. The local magnetic field is acquired from the world magnetic model [26]. As an example, the north, east, and down components of the Beijing area are 27,959.6 nT, −3296.3 nT, and 46,697.7 nT, respectively. Considering the sensor set used in the following experiment, the magnetometer output noise is set to 200 nT (1σ). The gyroscope output noise is 0.05◦ /s (1σ) and the zero drift is 0.5◦ /s for each individual sensor. The sample frequency is 100 Hz. The number of turns n is set to 2, and the interpolation angular interval ∆θ is set to 1◦ . Two error conditions of the tri-axis magnetometer are considered for further comparison. In the error condition 1, all of the tri-axis magnetometer errors are included. In the error condition 2, the misalignment error, soft iron error, and non-orthogonality error of the magnetometer, which cause rotations of the output data and degrade the calibration performance of the EM, are removed.
Sensors 2017, 17, 1223
12 of 21
5.1. Error Condition 1 The magnetometer errors are set and shown in Table 1. Table 1. Tri-axis magnetometer errors. Error
Value
Error
1 −0.01 0.03 0.01 1 0.02 −0.03 0.02 1 1.08 0 0 0 0.95 0 0 0 1.10 6000 2000 −3000 (nT )
Cm
Cs f bH
Value
1 0 0
Cno
−0.04 1 0
0.02 0.01 1
0.99 −0.10 −0.20 0.07 0.96 −0.15 0.22 0.13 1.02 500 −1000 800 (nT )
Csi bzb
Then the ideal total error coefficient matrix and total bias error vector are derived: 1.0780 −0.1465 −0.1529 7133.44 e e= K = 1668.75 0.0845 0.9149 −0.1107 , b 0.2112 0.1655 1.1250 −976.57 By applying the PM, the total error coefficient matrix KPM and total bias error vector bPM are calculated, as shown below. The average element estimation errors of KPM and bPM are 0.0008 and 24 nT, respectively.
K PM
1.0778 = 0.0838 0.2134
7108.58 −0.1460 −0.1511 0.9148 −0.1121 , bPM = 1657.84 −1013.90 0.1651 1.1251
By applying the EM, the total error coefficient matrix KEM and total bias error vector bEM are calculated as below. The average element estimation errors of K2 and b2 are 0.1010, and 94 nT, respectively.
KEM
1.0903 = −0.0066 0.0071
−0.0066 0.0071 7167.78 0.9175 0.0108 , bEM = 1770.43 0.0108 1.1473 −1121.99
The tri-axis magnetometer output data during the calibration process are plotted in Figure 5, where RD refers to raw data, and ID refers to ideal data. After calibration using the PM, the calibrated outputs are consistent with the ideal outputs. The distorted ellipses are rectified into circles, centered at [0, 0], and parallel to the sides of the cuboid, which means the hard iron effect, soft iron effect, zero bias error, scale factor error, non-orthogonal error, and misalignment error are compensated. After calibration using the EM, as shown in Figure 6, the distorted ellipses are rectified into circles, and the calibrated outputs are distributed on the surface of a sphere centered at [0, 0, 0], which means that the total scale error and total zero bias error, introduced by the scale factor error, zero bias error, soft iron error, and non-orthogonality error, are compensated. However, the calibrated outputs have a rotation error from the ideal outputs. The rotation error has the same matrix form as the misalignment error, which is considered as a total misalignment error introduced by the misalignment error, soft iron error, and non-orthogonality error [12].
Sensors 2017, 17, 1223 Sensors 2017, 17, 1223
13 of 21 13 of 21
Figure output data (simulation under condition 1): (a) view;view; (b) z-axis view; Figure5.5.Magnetometer Magnetometer output data (simulation under condition 1): Isometric (a) Isometric (b) z-axis (c)view; y-axis x-axis (c)view; y-axis(d) view; (d)view. x-axis view.
Usingthe thearctangent arctangentfunction, function,the the heading angles calculated from output data of twoUsing heading angles areare calculated from thethe output data of two-axis axis magnetometers parallel the table. faces upwards, the heading is calculated magnetometers parallel to the to table. WhenWhen side 1side faces1 upwards, the heading angle angle is calculated by the by the xand y-axis magnetometer outputs. When side 3 faces upwards, the heading angle is x- and y-axis magnetometer outputs. When side 3 faces upwards, the heading angle is calculated by calculated by themagnetometer x- and z-axis magnetometer outputs. Whenupwards, side 5 faces the heading angle the x- and z-axis outputs. When side 5 faces theupwards, heading angle is calculated is calculated by the yand z-axis magnetometer outputs. By comparing the raw and calibrated by the y- and z-axis magnetometer outputs. By comparing the raw and calibrated angle errors angle under errors under the conditions side side 1, side 3, andupwards, side 5 facing upwards,the respectively, calibration the conditions of side 1, side of 3, and 5 facing respectively, calibrationthe performances of the and PM are illustrated, as shown ofperformances the EM and PM areEM illustrated, as shown in Figure 7. in Figure 7.
Sensors 2017, 17, 1223
14 of 21
Sensors 2017, 2017, 17, 17, 1223 1223 Sensors
14 of of 21 21 14
Figure 6. Magnetometer output data and fitting sphere (simulation under condition 1). Figure6. 6.Magnetometer Magnetometeroutput outputdata dataand andfitting fitting sphere sphere (simulation (simulation under under condition condition 1). 1). Figure
Figure 7. Angle error (simulation under condition 1). Figure 7. Angle error (simulation under condition 1). Figure 7. Angle error (simulation under condition 1).
After calibration using the EM, the angle errors are not reduced, but such errors become more After calibration using sides the EM, the angle errors are not reduced, but such errors become more consistent when different face After using PM, the angle errors After calibration using the EM, theupwards. angle errors arecalibration not reduced, butthe such errors become moreare consistent when different sides face upwards. After calibration using the PM, the angle errors are limited towhen 0.5° (1σ), with sides the maximum less than 1.5°, as shownusing in Table consistent different face upwards. After calibration the2.PM, the angle errors are limited to 0.5◦ (1σ), with the maximum less than 1.5◦ , as shown in Table 2. limited to 0.5° (1σ), with the maximum less than 1.5°, as shown in Table 2.
Sensors 2017, 17, 1223 Sensors 2017, 17, 1223
15 of 21 15 of 21
Table 2. Angle error (simulation under condition 1).
Side 1 Upwards (°) Side 3 Upwards (°) Side 5 Upwards (°)
Table 2. Angle error (simulation under condition 1).
Calibration Method CalibrationRaw Method
EM
Raw EM PM PM
5.2. Error Condition 2
E σ max E σ max E σ max Side 3 Upwards (◦ ) Side 5 Upwards (◦ ) −6.61 6.32 17.60 −9.38 8.71 23.44 −7.70 15.20 24.95 E σ max E σ max E σ max −6.46 13.84 27.00 −9.31 13.04 28.80 −7.76 13.84 28.38 −6.61 6.32 17.60 −9.38 8.71 23.44 −7.70 15.20 24.95 −0.0313.040.37 28.801.13 −7.76 0.05 13.84 0.40 28.38 1.29 −6.46 −0.01 13.84 0.4427.00 1.36−9.31 Side 1 Upwards (◦ )
−0.01
0.44
1.36
−0.03
0.37
1.13
0.05
0.40
1.29
5.2. Error Condition Under error 2condition 2, the misalignment error Cm, soft iron error Csi, and non-orthogonality error Cno oferror the magnetometer are removed anderror the other parameters keep The simulation Under condition 2, the misalignment Cm , soft iron error Csi , the andsame. non-orthogonality program is repeated under this condition. The angle errors are eliminated effectively both using the error Cno of the magnetometer are removed and the other parameters keep the same. The simulation EM and the PM, as shown in Figure 8 and Table 3. When there is no misalignment error, softthe iron program is repeated under this condition. The angle errors are eliminated effectively both using error, non-orthogonality error, thisTable shows that the EMisperforms efficiently. The PM performs EM and and the PM, as shown in Figure 8 and 3. When there no misalignment error, soft iron error, efficiently under both error conditions. and non-orthogonality error, this shows that the EM performs efficiently. The PM performs efficiently under both error conditions.
Figure 8. Angle errors (simulation under condition 2). Figure 8. Angle errors (simulation under condition 2). Table 3. Angle error (simulation under condition 2). Table 3. Angle error (simulation under condition 2).
Side 1 Upwards (°) Side 3 Upwards (°) Side 5 Upwards (°) Calibration MethodSide 1 Upwards (◦ ) Side 3 Upwards (◦ ) Side 5 Upwards (◦ )
Calibration Method
Raw
Raw EM EM PM PM
E
E
σ
−0.03
σ
max
10.08
max
16.18
E
E
−0.03
σ
σ
9.92
max
max
14.90
E
E
−0.03
σ
σ
4.71
max
max
9.44
−0.03 10.08 16.18 −0.03 9.92 14.90 −0.03 4.71 9.44 −0.02 0.41 1.37 −0.03 0.38 1.08 −0.03 0.37 1.19 −0.02 0.41 1.37 −0.03 0.38 1.08 −0.03 0.37 1.19 −0.03 0.41 1.34 −0.03 0.38 1.07 −0.01 0.39 1.36 −0.03 0.41 1.34 −0.03 0.38 1.07 −0.01 0.39 1.36
The amounts of computation of the EM and PM are compared. Both the EM and PM are divided The amounts of computation of the EM and PM are compared. Both the EM and PM are divided into two steps. In the EM, step 1 is to perform ellipsoid-fitting using least squares technique, and step into two steps. In the EM, step 1 is to perform ellipsoid-fitting using least squares technique, and step 2 2 is to extract the error coefficients from the ellipsoid parameters. In the PM, step 1 is to calculate the is to extract the error coefficients from the ellipsoid parameters. In the PM, step 1 is to calculate the magnetometer interpolation outputs using the linear interpolation method, and step 2 is to calculate magnetometer interpolation outputs using the linear interpolation method, and step 2 is to calculate the error coefficients from the interpolation outputs by linear operations. The number of data for the error coefficients from the interpolation outputs by linear operations. The number of data for ellipsoid fitting is set to be equal to the number of interpolation outputs, which is equal to 360n/Δθ.
Sensors 2017, 17, 1223
16 of 21
Sensors 2017, 17, 1223
16 of 21 ellipsoid fitting is set to be equal to the number of interpolation outputs, which is equal to 360n/∆θ. The simulation program is repeated ten times under both of the error conditions using Matlab, and the The simulation program is repeated ten times under both of the error conditions using Matlab, and average computation time of these two algorithms is shown in Table 4. The step 1 in the EM requires the average computation time of these two algorithms is shown in Table 4. The step 1 in the EM large amounts of computation. The linear operations in the PM requires small computational amounts. requires large amounts of computation. The linear operations in the PM requires small computational The EM requires computations amounting to more than 10 times that of the PM. amounts. The EM requires computations amounting to more than 10 times that of the PM.
Table 4. Computation time (simulation, CPU: Intel i7 @3.6 GHz, RAM: 8 GB). Table 4. Computation time (simulation, CPU: Intel i7 @3.6 GHz, RAM: 8 GB). Calibration MethodMethod StepStep 1 Calibration EM PM
EM PM
1
Step 22 Step
Total Total
0.3549s 0.3549s 0.0161s
0.0012s 0.0012s 0.0014s 0.0014s
0.3561s 0.3561s 0.0175s 0.0175s
0.0161s
In summary, summary, three three conclusions conclusionsare aredrawn: drawn:(1)(1) using proposed calibration method, allthe of In using thethe proposed calibration method, all of the magnetometer errors are compensated, including the hard iron effect, softeffect, iron effect, zeroerror, bias magnetometer errors are compensated, including the hard iron effect, soft iron zero bias error, scale factor error, non-orthogonal error, and misalignment error; (2) using the standalone scale factor error, non-orthogonal error, and misalignment error; (2) using the standalone ellipsoidellipsoid-fitting thefactor totalerror scaleand factor errorzero andbias theerror totalare zero bias error fitting calibrationcalibration algorithm, algorithm, the total scale the total compensated, are compensated, but the total error is uncompensated; and (3)requirements the computational but the total misalignment errormisalignment is uncompensated; and (3) the computational of the requirements of the proposed method is much lower than the computational requirements of the proposed method is much lower than the computational requirements of the ellipsoid-fitting method. ellipsoid-fitting method. 6. Experiment 6. Experiment In this experiment, an SBG IG-500N MARG unit is used. As specified in the manual, the norm this experiment, an SBG IG-500N MARG unit is used. As specified in the theunit. norm of of theIntri-axis magnetometer output vector is approximately equal to 1 with anmanual, arbitrary The the tri-axis magnetometer vector is and approximately to 1 with arbitrary unit. Theoutput noise noise characteristics of theoutput magnetometer gyroscope equal are tested first.an The magnetometer characteristics of the magnetometer and gyroscope are tested first. The magnetometer output noise is noise is about 0.006 (1σ). The gyroscope output noise and bias are about 0.07°/s (1σ) and 0.5°/s, ◦ ◦ about 0.006 (1σ). outputare noise bias are about 0.07 /s (1σ) and 0.5 frequency /s, respectively, respectively, andThe the gyroscope gyro bias errors not and compensated in advance. The sample is set and the gyro bias errors are not compensated in advance. The sample frequency is set to be 100 Hz. to be 100 Hz. The unit unit is is mounted mounted to to aa cuboid cuboid frame frame as as shown shown in in Figure Figure 9. 9. The The number number of of turns turns nn is is set set to to 2, 2, The ◦ and the the interpolation interpolation angular angular interval interval Δθ ∆θ is The frame frame is is put put on on aa flat and and is set set to to 11°.. The flat wooden wooden table, table, and rotated by hand over n cycles for each side of the frame. rotated by hand over n cycles for each side of the frame.
Figure 9. Cuboid frame with the mounted sensor set. Figure 9. Cuboid frame with the mounted sensor set.
By applying the PM, the total error coefficient matrix and total bias error vector are calculated By applying the PM, the total error coefficient matrix and total bias error vector are calculated as follows: as follows: 0.0214 1.0757−0.0343 −0.03430.0214 −0.0427 1.0757 −0.0427 K PM =KPM−= = 0.0426 0.1020 0.9534 0.0426 0.0426 = , b, bPM PM −0.04260.9534 0.1020 0.0199 0.0747 0.0314 0.0199 0.0314 0.0747 0.9709 0.9709 By applying the EM, the total error coefficient matrix and total bias error vector are calculated as follows:
Sensors 2017, 17, 1223 Sensors 2017, 17, 1223
17 of 21 17 of 21
By applying the EM, the total error coefficient matrix and total bias error vector are calculated as follows: 1.0923 −0.02030.0110 −0.0396 0.0110 1.0923 −0.0203 −0.0396 ,b = = 0.96300.0299 0.0299 KEM = K EM −0.0203 0.0973 ,bEM EM = −0.02030.9630 0.0973 0.01100.0299 0.0253 0.02990.9863 0.9863 0.0110 0.0253 The process are are Theraw rawand andcalibrated calibratedtri-axis tri-axismagnetometer magnetometer output output data data during during the the calibration calibration process plotted in Figure 10. After calibration using the PM, the data lie on the circles parallel to the sides of plotted in Figure 10. After calibration using the PM, the data lie on the circles parallel to the sides of the atat (0,(0, 0).0).After calibration using the EM, the data lielie onon thethe circles unparalleled to thecuboid, cuboid,centered centered After calibration using the EM, the data circles unparalleled the of cuboid, which which means means that thethat misalignment error stillerror exists.still Theexists. resultsThe of the experiment to sides the sides of cuboid, the misalignment results of the are consistentare with the results of the the results simulation. experiment consistent with of the simulation.
Figure z-axis view; view; (c) (c) y-axis y-axisview; view; Figure10. 10.Magnetometer Magnetometeroutput outputdata data(experiment): (experiment):(a) (a) Isometric Isometric view; view; (b) (b) z-axis (d)(d) x-axis x-axisview. view.
Forfurther furtherverification verificationofofthe thequality qualityof ofthe thecalibrations, calibrations,the the frame frame is is installed installed on on aa turntable. For turntable. By By choosing the installation location, the magnetic disturbance from the turntable is reduced to a low choosing the installation location, the magnetic disturbance from the turntable is reduced to a low level. The turntable is rotated over 1 cycle. Assuming the output angle from turntable as the ideal heading angle, the angle errors after calibration using the EM and PM are derived under the conditions of side 1 upwards, side 3 upwards and side 5 upwards, respectively, as shown in Figure 11.
Sensors 2017, 17, 1223
18 of 21
level. The turntable is rotated over 1 cycle. Assuming the output angle from turntable as the ideal heading angle, Sensors 2017, 17, the 1223angle errors after calibration using the EM and PM are derived under the conditions 18 of 21 of side 1 upwards, side 3 upwards and side 5 upwards, respectively, as shown in Figure 11.
Figure 11. Angle errors (experiment). Figure 11. Angle errors (experiment).
As shown in Table 5, by applying the EM, the angle errors are narrowed to 2.5° (1σ) with the ◦ As shown Table4.2°. 5, by maximum lessinthan Byapplying applyingthe theEM, PM,the theangle angleerrors errorsare arenarrowed narrowedtoto2.50.5°(1σ) (1σ)with withthe the ◦ ◦ maximum less than 4.2 . By applying the PM, the angle errors are narrowed to 0.5 (1σ) with the maximum less than 1.7°. maximum less than 1.7◦ . Table 5. Angle errors (experiment). Table 5. Angle errors (experiment).
Side 1 Upwards (°) Side 3 Upwards (°) Calibration Method Side 1 Upwards (◦ ) Side 3 Upwards (◦ ) E σ max E σ max Calibration Method Raw Raw EM EM PM PM
E
σ
max
E
σ
−2.14 5.00 12.03 1.42 2.83 −2.14 5.00 12.03 1.42 2.83 0.18 0.97 2.58 0.53 2.12 0.18 0.97 2.58 0.53 2.12 0.49 −0.11 −0.11 0.45 0.451.63 1.63 0.320.32 0.49
max
6.17 6.17 4.14 4.14 1.66 1.66
Side 5 Upwards (°) Side 5 Upwards (◦ )
E
E
3.82 3.82 0.26 0.26 −0.10 − 0.10
σ
σ
max
max
11.04 16.01 11.04 16.01 2.32 3.68 2.32 3.68 0.34 0.34 1.17 1.17
The average computation time is shown in Table 6. The EM requires computations amounting The average computation time is shown in Table 6. The EM requires computations amounting to to more than 10 times that of the PM, which is consistent with the simulation results. more than 10 times that of the PM, which is consistent with the simulation results. Table Computation time (experiment, CPU: Intel @3.6 GHz, RAM: 8 GB). Table 6. 6. Computation time (experiment, CPU: Intel i7 i7 @3.6 GHz, RAM: 8 GB).
Calibration Method
Calibration Method EM PM
EM PM
Step 1 0.2177s 0.2177s 0.0163s Step 1
0.0163s
Step 2 0.0011s 0.0011s 0.0011s Step 2
0.0011s
Total Total 0.2188s 0.2188s 0.0174s 0.0174s
7. Conclusions 7. Conclusions This article proposed a complete tri-axis magnetometer calibration algorithm with a gyroscope This article proposed a complete tri-axis magnetometer calibration algorithm with a gyroscope auxiliary. A non-magnetic cuboid frame was required to mount the sensor set, and was rotated by auxiliary. A non-magnetic cuboid frame was required to mount the sensor set, and was rotated by hand during the calibration, which was inexpensive and easy to operate. hand during the calibration, which was inexpensive and easy to operate. The calibration errors introduced by the gyro bias error, gyro misalignment error, cuboid frame non-orthogonality error, and rotation deviation error were analyzed, respectively. The results of the analyses indicated that the calibration errors were slight and acceptable.
Sensors 2017, 17, 1223
19 of 21
The calibration errors introduced by the gyro bias error, gyro misalignment error, cuboid frame non-orthogonality error, and rotation deviation error were analyzed, respectively. The results of the analyses indicated that the calibration errors were slight and acceptable. The computation time of the proposed method was compared with the ellipsoid-fitting calibration method, which indicated that the linear operations provided this algorithm with low computational complexity. The calibration results of the ellipsoid-fitting method and the proposed method were compared using simulations and experiments. The tri-axis magnetometer output errors were calibrated using the proposed method, including the hard iron effect, soft iron effect, zero bias error, scale factor error, non-orthogonal error, and misalignment error, while the total misalignment error, introduced by the misalignment error, soft iron error, and non-orthogonality error, was unable to be calibrated under the stand-alone ellipsoid fitting calibration algorithms. The results from the simulations and experiments were consistent. After calibration using the proposed method, the heading error was reduced to 0.5◦ (1σ). Acknowledgments: The calibration experiments were performed at the State Key Laboratory of Precision Measurement Technology and Instruments at Tsinghua University, which are gratefully acknowledged. We would like to thank Marcus Thang, Kai Shi and Shuai Shi for language polishing. We would also like to thank the reviewers for their beneficial comments and suggestions. Author Contributions: Deng Yang proposed the idea of the calibration algorithm; Zheng You, Bin Li, and Wenrui Duan conceived and designed the experiments; Deng Yang and Binwen Yuan performed the experiments; Deng Yang and Wenrui Duan analyzed the data; Deng Yang prepared the final draft, and Zheng You guaranteed the critical reading. Conflicts of Interest: The authors declare no conflict of interest.
Appendix A The code of the proposed method is written for Matlab. Related data are posted online (https://www.researchgate.net/profile/Deng_Yang3/publication/316968794_Data_ of_magnetometer_ang_gyro_for_Complete_Tri-Axis_Magnetometer_Calibration_with_Gyro_ Auxiliary/data/591ae41aaca272fa34dbac01/MGData-YD.mat) Algorithm Complete Tri-Axis Magnetometer Calibration with a Gyro Auxiliary function [K,b,Mint]=MagCal_YD(MGdata,n,dA,dt,ht) % K: total error coefficient matrix % b: total bias error vector % Mint: tri-axis magnetometer interpolation outputs % MGdata: j x l x k matrix of tri-axis gyro and magnetometer outputs % j is the j-th sample point % l=1~6, represent the x-, y-, z-axis gyro outputs with unit of degree/s, and x-, y-, z-axis magnetometer outputs, respectively. % k=1~6, represent the sensor direction of z-axis upward, z-axis downward, y-axis upward, y-axis downward, x-axis upward, x-axis downward, respectively. % n: number of rotations, n>=1. % dA: angular interval with unit of degree. 360/dA must be an integer. % dt: sampling period, with unit of s. % ht: the geomagnetic field component perpendicular to the table. If ht is unknow, set ht=0. %% step 1: Magnetometer Interpolation Output Calculation
Sensors 2017, 17, 1223
20 of 21
Algorithm Cont. AR=360*n; ni= AR/dA; Mint=zeros(AR/dA,3,6); num=length(MGdata(:,1,1)); angle=zeros(3*num,6); for k=1:6 i=1;j=1;l=1; while abs(angle(j,k))0 angle(1:j,k)=angle(1:j,k); else angle(1:j,k)=-angle(1:j,k); end Mint(1,:,k)=MGdata(1,4:6,k); while i
