Adaptive Estimation of Measurement Bias in Six Degree of Freedom InertialMeasurement Units: Theory and Preliminary Simulation Evaluation

Andrew R. Spielvogel and Louis L. Whitcomb

Abstract— Six-degree of freedom (DOF) inertial measure-ment units (IMUs) are widely used for attitude estimation.However, such systems’ accuracy is limited by the accuracyof calibration of bias, scale factors, and non-orthogonality ofsensor measurements. This paper reports a stable adaptiveestimator of measurement bias in six-DOF IMUs and prelim-inary simulation results employing a commercially availableIMU comprising a 3-axis fiber optic gyroscope (FOG) with a 3-axis micro-electro-mechanical systems (MEMS) accelerometer.A stability proof of the adaptive estimator and preliminarynumerical simulation results are reported. The simulationresults for a rotating IMU configuration are promising, andfurther experimental evaluation and extension of the algorithmfor the case of a translating IMU configuration typically foundon moving robotic vehicles are needed.

I. INTRODUCTION

Six-degree of freedom (DOF) inertial measurement units(IMUs) are frequently used in many applications includingrobotics, vehicle navigation systems, and smart-phones. Six-DOF IMUs are comprised of a 3-axis angular-rate gyroscopeand a 3-axis linear accelerometer. Angular-rate gyroscopesmeasure the angular rotation rate of the instrument, whilelinear accelerometers measure linear acceleration and areoften used to measure the device’s local level (roll and pitch)with respect to the Earth’s local gravity vector.

In addition, six-DOF IMUs are commonly used in attitudeestimation systems (eg. [5], [6], [14], [15], [3], [24], [23]).However, these systems’ accuracies are affected by biases,scale factors, and non-orthogonality of their sensor measure-ments. Thus, calibration of these parameters (angular-rateand linear acceleration sensor biases) of six-DOF IMUs iscritical for accurate attitude estimation.

A. Literature Review

Several methods for IMU measurement bias estimationhave been reported in recent years. However, majority ofthis literature focuses on magnetometer bias estimation (eg.[2], [1], [4], [7], [9], [11], [13]). In contrast, the presentpaper addresses angular-rate and linear acceleration sensorbias identification.

Metni et al. and Pflimlin et al. report nonlinear comple-mentary filters for estimating attitude and gyroscope sensorbias ( [16], [17], [19]). While these estimators identifyangular-rate sensor bias, they do not address linear accel-eration sensor bias.

We gratefully acknowledge the support of the National Science Founda-tion under NSF award OCE-1435818. The authors are with the Departmentof Mechanical Engineering, Johns Hopkins University, Baltimore, MD21218, USA. Email: {aspielv1,llw}@jhu.edu

Fig. 1. The star frame (s), Earth frame (e), and NED frame (N ). we isthe Earth’s angular velocity with a magnitude of ∼ +15◦/hr.

In [8], George and Sukkarieh report an identifier foraccelerometer and gyroscope sensor bias. However, theyutilize global positioning system (GPS) which is unsuitablefor robotic vehicles operating in GPS-denied environments(eg. submerged vehicles).

Scandaroli et al. and Scandaroli and Morin ( [22], [21])also report a sensor bias estimator for 6-DOF IMUs utilizingcomputer vision. This method though is dependent on thepresence of a vision system, which requires identificationmarkers and a camera system which may not be availablefor a robotic vehicle (eg. underwater vehicles).

B. Paper’s Contribution

The present paper reports (to the best knowledge of theauthors) the first stable adaptive algorithm for estimatingmeasurement bias of a dynamic (rotating) six-DOF IMUalong with preliminary simulation results of the identifier.

This paper is organized as follows: Section II gives anoverview of preliminaries. Section III reports the adaptivemeasurement bias identifier. Section IV presents numericalsimulations. Section V summarizes and concludes.

II. PRELIMINARIES

A. Coordinate Frames

We define the following coordinate frames:Star Frame: The star frame (s) has its origin at the center

of the Earth, with its axes fixed (non-rotating) with

respect to the stars, and with its z-axis aligned with theEarth’s axis of rotation. For the purpose of this analysisthe star frame can be considered an inertial referenceframe.

Earth Frame: The Earth Frame (e) has its origin at thecenter of the Earth, and with its z-axis aligned with theEarth’s axis of rotation. The Earth frame rotates withrespect to the star frame about their coincident z-axisat the Earth’s rotation of ∼ +15◦ per hour.

Instrument Frame: A frame (i) fixed in the IMU instru-ment.

North-East-Down (NED) Frame: The NED frame (N )has its x-axis pointing North, its y-axis pointing East,its z-axis pointing down, and its origin co-located withthat of the instrument frame.

B. Notation and Definitions

For vectors, a leading superscript indicates the frameof reference and a following subscript indicates the signalsource, thus Nwm is the measured instrument angular veloc-ity in the NED frame and iam is the measured instrumentlinear acceleration in the instrument sensor frame.

Definition: The set of 3×3 rotation matrices forms a group,known as the special orthogonal group, SO(3), defined as

SO(3) = {R : R ∈ R3×3, RTR = I3×3,det(R) = 1}. (1)

For rotation matrices a leading superscript and subscriptindicates the frames of reference. For example, N

i R is therotation from the instrument frame to the NED frame.

The elements of a vector x ∈ R3 are defined with theusual subscripts,

x =

x1x2x3

. (2)

Definition: The set of 3×3 skew-symmetric matrices is

so(3) = {S : S ∈ R3×3, ST = −S}. (3)

Definition: J is a function that maps a 3×1 vector to thecorresponding 3×3 skew-symmetric matrix, J : R3 → so(3).For any k ∈ R3

J(k) =

0 −k3 k2k3 0 −k1−k2 k1 0

. (4)

We define its inverse J−1 : so(3)→ R3, such that ∀x ∈ R3,J−1(J(x)) = x.

Definition: The Euclidean vector norm is defined as usualas: ∀x(t) ∈ R3

‖x(t)‖ =(xT (t)x(t)

)1/2. (5)

Definition [10]: A function V (x) is said to be radiallyunbounded if

V (x)→∞ as ‖x‖ → ∞. (6)

C. Sensor ModelThe sensor data is modeled as

iwm(t) = iwE(t) + iwv(t) + iwb + iηw(t) (7)iam(t) = iag(t) + iav(t) + iab + iηa(t) (8)iwe(t) = iwE(t) + iwv(t) + iwb (9)iae(t) = iag(t) + iav(t) + iab (10)

where iwm(t) is the IMU measured angular-rate, iwe(t) isthe expected angular-rate, iwE(t) is the true angular velocitydue to the rotation of the Earth, iwv(t) is the true angularvelocity due to the rotation of the instrument with respect tothe NED frame, iwb is the angular velocity sensor bias offset,iηw(t) is the zero-mean Gaussian angular velocity sensornoise, iam(t) is the IMU measured linear acceleration, iae(t)is the expected linear acceleration, iag(t) is the true linearacceleration due to gravity and the Earth’s rotation, iav(t)is the instrument’s true linear acceleration with respect toEarth, iab is the linear accelerometer sensor bias, and iηa(t)is the zero-mean Gaussian linear accelerometer sensor noise.

For this paper, we assume that the instrument is rotatingwith respect to the NED frame (iav(t) = 0). For robotic ve-hicles with small peak vehicle accelerations and a zero meanvehicle acceleration over time (eg. underwater vehicles), theacceleration due to gravity is the dominating signal in theacceleration measurement. For this reason, we are ignoringvehicle acceleration (since this formulation is applicable toslowly accelerating vehicles) but are conducting continuingresearch to extend our algorithm to the accelerating vehiclecase. With the zero vehicle acceleration assumption, thesensor data model simplifies to

iwm(t) = iwE(t) + iwv(t) + iwb + iηw(t) (11)iam(t) = iag(t) + iab + iηa(t) (12)iwe(t) = iwE(t) + iwv(t) + iwb (13)iae(t) = iag(t) + iab. (14)

III. AN ADAPTIVE MEASUREMENT BIAS IDENTIFIER

This section reports a novel adaptive bias identifier (6-DOF IMU angular rate and linear acceleration biases) basedupon the adaptive estimation of measurement bias in 3-DOFfield sensors reported by Troni and Whitcomb in [25].

A. System ModelWe consider the system model

sNR(t)Nag = s

iR(t)(iae(t)− iab

). (15)

Differentiating (15), yields,sNR(t)J

(NwE

)Nag = s

iR(t)J(iwe(t)− iwb

) (iae(t)− iab

)+s

iR(t)iae(t). (16)

Rearranging terms in (16) yields,iae(t) = N

i RT (t)J(NwE)Nag

−J(iwe(t)− iwb

) (iae(t)− iab

)= N

i RT (t)J

(NwE

)Nag − J

(iwe(t)

)iae(t)

+J(iwe(t)

)iab − J

(iae(t)

)iwb − iz (17)

0 500 1000 1500 2000 2500

Seconds

89.5

90

90.5D

egre

es

Roll

0 500 1000 1500 2000 2500

Seconds

-0.5

0

0.5

Degre

es

Roll

0 500 1000 1500 2000 2500

Seconds

-20

0

20

Degre

es

Roll

Fig. 2. Simulation dataset attitude.

where iz is the constant iz = J(iwb

)iab.

B. Adaptive Identifier

We consider the identifier system model

i ˙ae = Ni R

T (t)J(NwE

)Nag − J

(iwe(t)

)iae(t)

+J(iwe(t)

)iab(t)− J

(iae(t)

)iwb(t)

−iz(t)− k1∆a(t) (18)i ˙wb(t) = −k2J

(iae(t)

)∆a(t) (19)

i ˙ab(t) = k3J(iwe(t)

)∆a(t) (20)

i ˙z(t) = k4∆a(t) (21)

where the estimation errors are defined as

∆a(t) = iae(t)− iag(t) (22)∆wb(t) = iwb(t)− iwb (23)∆ab(t) = iab(t)− iab (24)∆z(t) = iz(t)− iz. (25)

Note that the attitude (rotation matrix Ni R(t)) is needed for

the algorithm.

C. Error System

The error system is

∆a(t) = J(iwe(t)

)∆ab(t)− J

(iae(t)

)∆wb(t)

−∆z(t)− k1∆a(t) (26)∆wb(t) = −k2J

(iae(t)

)∆a(t) (27)

∆ab(t) = k3J(iwe(t)

)∆a(t) (28)

∆z(t) = k4∆a(t). (29)

D. Stability

Consider the Lyapunov candidate function

V =1

2‖∆a(t)‖2 +

1

2k2‖∆wb(t)‖2

+1

2k3‖∆ab(t)‖2 +

1

2k4‖∆z(t)‖2. (30)

Taking the time derivative and substituting in (26) - (29)yields

V = ∆aT (t)∆a(t) +1

k2∆w

T

b (t)∆wb(t)

+1

k3∆a

T

b (t)∆ab(t) +1

k4∆z

T(t)∆z(t)

= −k1∆aT (t)∆a(t)

+(−∆aT (t)J

(iae(t)

)+ ∆aT (t)J

(iae(t)

))∆wb(t)

+(∆aT (t)J

(iwe(t)

)−∆aT (t)J

(iwe(t)

))∆ab(t)

+(∆aT (t)−∆aT (t)

)∆z(t)

= −k1‖∆a(t)‖2. (31)

Since the Lyapunov function is radially unbounded andits time derivative is negative semidefinite, the adap-tive identifier is globally stable. In addition, becauseV = −k1‖∆a(t)‖2, the state ∆a(t) converges asymp-totically to zero while the other states, ∆wb(t), ∆ab(t),and ∆z(t), are stable. Additional arguments beyond thescope of this paper and persistent excitation (PE) conditionsare needed for global asymptotic stability of the adaptiveidentifier [18], [20].

IV. NUMERICAL SIMULATIONS

The performance of the measurement bias adaptive identi-fier was evaluated with numerical simulations. Section IV-Apresents the simulation setup and Section IV-B reports thesimulation results.

A. Simulation Setup

• Two numerical simulations (TRUEATT and MAGATT)were implemented using one generated dataset.

• The dataset was sampled at 5kHz for 45 minutesand included sensor noise and sensor bias repre-sentative of the KVH 1775 IMU (KVH Industries,Inc., Middletown, RI, USA). Angular velocity sen-sor and linear accelerometer sensor noises were com-puted from the IMU manufacturer’s specifications [12],as per [26], and sensor biases are chosen to be inline with the KVH datasheet ( σw = 6.32 × 10−3

rads/s, σa = 0.0037 g, iab =[

1 2 −2]T/103, and

iwb =[−2 1 −1

]T/105 ). These noise and bias char-

acteristics are on par with ones we have observedexperimentally while using the 1775 IMU.

• The simulated IMU is mounted to a vehicle. If we definea vehicle frame to be such that the x-axis is pointingforward, the y-axis is pointing starboard, and the z-axis is pointing down, the IMU is mounted such thatits coordinate frame’s origin is co-aligned with that ofthe vehicle and rotated by 45◦ around the x-axis (roll).

0 500 1000 1500 2000 2500-3

-2

-1

0

1

2

310 -5

wx

wy

wz

Fig. 3. TRUEATT simulation wb(t) estimate.

0 500 1000 1500 2000 2500-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

110 -5

wx

wy

wz

Error

Fig. 4. TRUEATT simulation wb(t) estimates’ error.

0 500 1000 1500 2000 2500-5

-4

-3

-2

-1

0

1

2

3

4

510 -3

ax

ay

az

Fig. 5. TRUEATT simulation ab(t) estimate.

0 500 1000 1500 2000 2500-5

-4

-3

-2

-1

0

1

2

3

4

510 -4

ax

ay

az

Fig. 6. TRUEATT simulation ab(t) estimates’ error.

• During the simulation, the instrumentexperiences a sinusoidal angular velocity ofiwv(t) =

[0 − cos(t/5)/20 − cos(t/5)/20

]Twhich

results in the vehicle heading shown Figure 2.

B. Simulations

Both simulations use adaptive identifier gains of k1 = 1,k2 = 0.01, k3 = 2, and k4 = 0.000001.

1) TRUEATT Simulation: The TRUEATT simulation im-plements the bias adaptive identifier using the true attitude(roll, pitch, heading) for calculating the N

i R(t) matrix.2) MAGATT Simulation: The MAGATT implements the

bias adaptive identifier using corrupted heading measure-ments (3◦ heading bias) for calculating the N

i R(t) matrix.Accelerometers can be used to find local level (roll and pitch)to within a fraction of a degree, so for this study we focusedon the affect of heading error from magnetometers, whichis commonly around 3◦, on the 6-DOF IMU sensor biasidentifier.

C. Results

1) TRUEATT Simulation: The angular rate bias estima-tion and errors for the TRUEATT simulation are shown inFigures 3 and 4, respectively, while the linear accelerationbias estimation and errors for the TRUEATT simulation areshown in Figures 5 and 6, respectively. The simulation resultsshow that with an accurate knowledge of the N

i R(t) matrixand the IMU experiencing PE, the bias estimates convergeto their true values within the 45 minutes of the simulation.

2) MAGATT Simulation: The angular rate bias estimationand errors for the MAGATT simulation are shown in Figures7 and 8, respectively, while the linear acceleration biasestimation and errors for the MAGATT simulation are shownin Figures 9 and 10, respectively. The simulation resultsshow that with a knowledge of the N

i R(t) matrix and theIMU experiencing PE, the linear acceleration bias estimateconverges to its true values within the 45 minutes of thesimulation, while the angular rate bias estimate converges toa value close to the true sensor bias but with a small offset

0 500 1000 1500 2000 2500-3

-2

-1

0

1

2

310 -5

wx

wy

wz

Fig. 7. MAGATT simulation wb(t) estimate.

0 500 1000 1500 2000 2500-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

110 -5

wx

wy

wz

Error

Fig. 8. MAGATT simulation wb(t) estimates’ error.

0 500 1000 1500 2000 2500-5

-4

-3

-2

-1

0

1

2

3

4

510 -3

ax

ay

az

Fig. 9. MAGATT simulation ab(t) estimate.

0 500 1000 1500 2000 2500-5

-4

-3

-2

-1

0

1

2

3

4

510 -4

ax

ay

az

Fig. 10. MAGATT simulation ab(t) estimates’ error.

due to the inaccurate heading measurement (3◦ error).

V. CONCLUSION

This paper reports a novel stable adaptive 6-DOF IMUmeasurement bias estimator and preliminary simulation eval-uations based on the measurement noise model of thecommercially available KVH 1775 IMU data-sheet. Thesimulation results of a rotating system employing a fiberoptic gyroscope (FOG) IMU indicates that algorithm cansuccessfully estimate measurement bias if the attitude (roll,pitch, heading) is known to create the N

i R(t) matrix. Overall,these data suggest the convergence of the adaptive identifier’sbias estimates to their true values for the case of a persistentlyrotating IMU and knowledge of the instruments attitude.

In future research, we will experimentally evaluate theadaptive identifier and address the general use-case of thesimultaneously rotating and translating instrument configu-ration.

REFERENCES

[1] R. Alonso and M. D. Shuster. Complete linear attitude-independentmagnetometer calibration. Journal of the Astronautical Sciences,50(4):477–490, 2002.

[2] R. Alonso and M. D. Shuster. Twostep: A fast robust algorithm forattitude-independent magnetometer-bias determination. Journal of theAstronautical Sciences, 50(4):433–452, 2002.

[3] R. Costanzi, F. Fanelli, N. Monni, A. Ridolfi, and B. Allotta. An atti-tude estimation algorithm for mobile robots under unknown magneticdisturbances. IEEE/ASME Transactions on Mechatronics, 21(4):1900–1911, Aug 2016.

[4] J. L. Crassidis, K.-L. Lai, and R. R. Harman. Real-time attitude-independent three-axis magnetometer calibration. Journal of Guid-ance, Control, and Dynamics, 28(1):115–120, 2005.

[5] J. L. Crassidis, F. L. Markley, and Y. Cheng. Survey of nonlinear atti-tude estimation methods. Journal of guidance, control, and dynamics,30(1):12–28, 2007.

[6] M. Euston, P. Coote, R. Mahony, J. Kim, and T. Hamel. A comple-mentary filter for attitude estimation of a fixed-wing UAV. In 2008IEEE/RSJ International Conference on Intelligent Robots and Systems,pages 340–345, Sept 2008.

[7] B. Gambhir. Determination of magnetometer biases using moduleresidg. Computer Sciences Corporation, Report, (3000-32700):01,1975.

[8] M. George and S. Sukkarieh. Tightly coupled INS/GPS with biasestimation for UAV applications. In Proceedings of AustraliasianConference on Robotics and Automation (ACRA), 2005.

[9] P. Guo, H. Qiu, Y. Yang, and Z. Ren. The soft iron and hardiron calibration method using extended Kalman filter for attitudeand heading reference system. In Position, Location and NavigationSymposium, 2008 IEEE/ION, pages 1167–1174. IEEE, 2008.

[10] H. K. Khalil. Noninear Systems. Prentice-Hall, New Jersey, 1996.[11] M. Kok, J. D. Hol, T. B. Schon, F. Gustafsson, and H. Luinge.

Calibration of a magnetometer in combination with inertial sensors.In Information Fusion (FUSION), 2012 15th International Conferenceon, pages 787–793. IEEE, 2012.

[12] KVH Industries, Inc., Middletown, RI, USA. The KVH WebSite http://www.kvh.com and KVH 1775 IMU specifications sheethttp://www.kvh.com/ViewAttachment.aspx?guidID=DFB3BCDF-CFA1-422D-837A-92F4F68D9BE5, 2015.

[13] X. Li and Z. Li. A new calibration method for tri-axial field sensors instrap-down navigation systems. Measurement Science and technology,23(10):105105, 2012.

[14] R. Mahony, T. Hamel, and J. M. Pflimlin. Complementary filter designon the special orthogonal group SO(3). In Proceedings of the 44thIEEE Conference on Decision and Control, pages 1477–1484, Dec2005.

[15] R. Mahony, T. Hamel, and J. M. Pflimlin. Nonlinear complemen-tary filters on the special orthogonal group. IEEE Transactions onAutomatic Control, 53(5):1203–1218, June 2008.

[16] N. Metni, J. M. Pflimlin, T. Hamel, and P. Soueres. Attitude and gyrobias estimation for a flying UAV. In 2005 IEEE/RSJ InternationalConference on Intelligent Robots and Systems, pages 1114–1120, Aug2005.

[17] N. Metni, J.-M. Pflimlin, T. Hamel, and P. Soures. Attitude and gyro

bias estimation for a VTOL UAV. Control Engineering Practice,14(12):1511 – 1520, 2006.

[18] K. S. Narendra and A. M. Annaswamy. Stable adaptive systems.Courier Corporation, 2012.

[19] J. M. Pflimlin, T. Hamel, and P. Soures. Nonlinear attitude andgyroscope’s bias estimation for a VTOL UAV. International Journalof Systems Science, 38(3):197–210, 2007.

[20] S. Sastry and M. Bodson. Adaptive control: stability, convergence androbustness. Courier Corporation, 2011.

[21] G. G. Scandaroli and P. Morin. Nonlinear filter design for pose andIMU bias estimation. In 2011 IEEE International Conference onRobotics and Automation, pages 4524–4530, May 2011.

[22] G. G. Scandaroli, P. Morin, and G. Silveira. A nonlinear observerapproach for concurrent estimation of pose, IMU bias and camera-to-IMU rotation. In 2011 IEEE/RSJ International Conference onIntelligent Robots and Systems, pages 3335–3341, Sept 2011.

[23] A. R. Spielvogel and L. L. Whitcomb. Preliminary results with a low-cost fiber-optic gyrocompass system. In OCEANS 2015 - MTS/IEEEWashington, pages 1–5, Oct 2015.

[24] A. R. Spielvogel and L. L. Whitcomb. A Stable Adaptive Estimatoron SO(3) for True-North Seeking Gyrocompass Systems: Theory andPreliminary Simulation Evaluation. In Proceedings of the 2017 IEEEInternational Conference on Robotics and Automation, May 2017.

[25] G. Troni and L. L. Whitcomb. Adaptive estimation of measurementbias in three-dimensional field sensors with angular rate sensors:Theory and comparative experimental evaluation. In Robotics: Scienceand Systems, 2013.

[26] Woodman, Oliver J. An introduction to inertial navigation. Technicalreport, University of Cambridge, Cambridge, UK, 2007.