Research Article A DCM Based Attitude Estimation Algorithm ...downloads.hindawi.com/archive/2015/503814.pdf · combination with other sensors, such as global navigation satellite

Research ArticleA DCM Based Attitude Estimation Algorithm for Low-CostMEMS IMUs

Heikki Hyyti and Arto Visala

Autonomous Systems Research Group Department of Electrical Engineering and Automation School of Electrical EngineeringAalto University PO Box 15500 00076 Aalto Finland

Correspondence should be addressed to Heikki Hyyti heikkihyytiaaltofi

Received 16 July 2015 Revised 29 October 2015 Accepted 4 November 2015

Academic Editor Aleksandar Dogandzic

Copyright copy 2015 H Hyyti and A Visala This is an open access article distributed under the Creative Commons AttributionLicense which permits unrestricted use distribution and reproduction in any medium provided the original work is properlycited

An attitude estimation algorithm is developed using an adaptive extended Kalman filter for low-cost microelectromechanical-system (MEMS) triaxial accelerometers and gyroscopes that is inertial measurement units (IMUs) Although theseMEMS sensorsare relatively cheap they give more inaccurate measurements than conventional high-quality gyroscopes and accelerometers Tobe able to use these low-cost MEMS sensors with precision in all situations a novel attitude estimation algorithm is proposedfor fusing triaxial gyroscope and accelerometer measurements An extended Kalman filter is implemented to estimate attitude indirection cosine matrix (DCM) formation and to calibrate gyroscope biases online We use a variable measurement covariance foracceleration measurements to ensure robustness against temporary nongravitational accelerations which usually induce errorswhen estimating attitude with ordinary algorithms The proposed algorithm enables accurate gyroscope online calibration byusing only a triaxial gyroscope and accelerometer It outperforms comparable state-of-the-art algorithms in those cases whenthere are either biases in the gyroscope measurements or large temporary nongravitational accelerations present A low-costtemperature-based calibrationmethod is also discussed for initially calibrating gyroscope and acceleration sensors An open sourceimplementation of the algorithm is also available

1 Introduction

Inertial measurement units (IMUs) are widely used in atti-tude estimation in mobile robotics aeronautics and navi-gation An IMU consists of a triaxial accelerometer and atriaxial gyroscope and it is used for measuring accelerationsand angular velocities in three orthogonal directions Theattitude which is a 3D orientation of the IMUwith respect tothe Earth coordinate system can be estimated by combiningintegrated angular velocities and acceleration measurementsMicroelectromechanical-system (MEMS) IMUs are smalllight and low-cost solutions for attitude estimation Theyare widely used in mobile robotics such as unmannedaerial vehicles (UAVs) [1] MEMS IMUs are also used incombination with other sensors such as global navigationsatellite systems (GNSS) [2 3] light detection and ranging(LIDAR) sensors or cameras in various applications Inaddition MEMS IMUs are commonly included in modernmobile phones [4]

Unfortunately the use of low-cost MEMS IMUs intro-duces several challenges compared to high-precision mea-surement devices Low-cost MEMSs are noisy and their mea-surements usually include various errorsThese errors consistof an unknown zero level that is bias error and unknownscale factor that is gain error [5] Moreover the gain and thebias tend to drift over time and are affected by temperaturechange Therefore sensor calibration has become one of themost challenging issues in inertial navigation [6]

Other problems with IMUs are related to the standarddesign of the sensor fusion algorithms Usually the roll andpitch angles are estimated using the measured angle of theEarthrsquos gravitation force as reference to the integrated angleobtained from angular velocity measurements This worksperfectly if no other forces than gravity exist in the systemUnfortunately this is rarely the case In practical use whenan IMU is attached to a moving platform or object theseaccelerations are unavoidable In various cases as in theestimation of either a human body [7] or a mobile phone

Hindawi Publishing CorporationInternational Journal of Navigation and ObservationVolume 2015 Article ID 503814 18 pageshttpdxdoiorg1011552015503814

2 International Journal of Navigation and Observation

[4] position or in a legged robot [8] these accelerations canbecome significantly large Therefore their presence shouldbe taken into account

Another problem with standard IMU implementationsconcerns heading angle estimation The Earthrsquos gravitationfield gives no information about this Therefore the integra-tion of triaxial accelerometers and gyroscopes cannot providean absolute heading angleThis is commonly overcome usingan extra sensor usually a triaxial magnetometer [2 9ndash16] orsatellite navigation [3 5 17ndash20] Triaxial magnetometers offera good solution if the magnetic field measurement can betrusted In practice this is usually not the case at least inrobotics since robots are usually made of magnetic metalhave high current electronics and motor drives and maytravel within locations that are surrounded by power linesmagnets and magnetic metals Moreover because the mag-netic sensor observes the sum field caused by all magnets andelectrically inducedmagnetic fields the Earthrsquosmagnetic fieldcannot be easily separated Furthermore satellite navigationcan be of little help as it does not work well inside buildingsor caves or under dense forest foliage

The ability to reliably estimate attitude with a minimalnumber of sensors would also increase the robustness ofthe system in two ways Firstly as fewer sensors would beneeded there would be less risk of sensor failures Secondlyit would be beneficial to run an IMU algorithm on thebackground as a backup method even when other mea-surements are available In addition these results could becompared between the different algorithms to detect failuresand possibly compensate for those faults

Our proposed method solves presented challenges byfirstly formulating a calibration method as a function ofmeasured temperature for gains and biases and secondlyusing an extended Kalman filter to estimate the bias ingyroscope measurements online We have purposely omittedmagnetometer and other possible measurements from ourfilter and we estimate only absolute pitch and roll anglesthus keeping themain focus on the gyroscope bias estimationAlthough the heading estimate (yaw angle) can be computedfrom the results of the proposed filter it is not an absoluteheading Instead it is an integrated bias-corrected angularvelocity around 119911-axis The absolute yaw could be estimatedin a separate filter using any extra measurements By doingthis we can increase the robustness of the proposed sensorfusion algorithm and enhance gyroscope bias estimation Ifthe magnetometer or other sensors would fail catastrophi-cally only the heading estimate would fail leaving pitch androll estimates unaffectedThis can offer significant advantagesfor robotic applications such as flying UAVs or other robots

In contrast to other proposed solutions [21ndash24] intro-duced in more detail in the next chapter we implement anextended Kalman filter to tune the bias estimates when suchinformation is available Furthermore we do not rely onconstant gains for updating bias Instead we use an extendedKalman filter to employ available information in system andmeasurement covariances in order to tune the gains forupdating bias and attitude estimates Later in Experimentsand Results we also show in practice that even in the worstcase scenario the filter remains stable

In addition to the calibration and robust estimation ofgyroscope biases we adapt our measurement covariances toincrease the quality of the filter against changing dynamicconditions IMU algorithms usually use the direction ofgravity (through measured accelerations) to reduce accumu-lating errors in integrated angular velocities during attitudeestimation This causes unwanted errors in the attitude esti-mate when nongravitational accelerations or contact forcesare present We also use a variable-measurement-covariancemethod to reduce errors in the attitude estimation causedby rapid and temporary nongravitational accelerations It isaffordable to do as in the DCM representation of rotationthe bottom row of rotation matrix represents the directionof gravitational force As a result our implementation ofthe extended Kalman filter is able to use only six states forestimating the attitude gyroscope biases and the gravityvector

In Experiments and Results we show that our solution issignificantly more accurate than the compared algorithms inthose situations in which either temporary accelerations arepresent or significant gyroscope biases exist With fully cali-brated bias-free data without nongravitational accelerationsour algorithm performs as well as the compared algorithmssince our gyroscope bias estimates do not disturb attitudeestimation

Our work provides a complete solution that integrateslow-cost MEMS IMU temperature calibration online biasestimator and a robust extended Kalman filter which is ableto handle large temporary accelerations and changes in sam-pling rate We verify our algorithm using multiple differenttests and we also obtain accurate reference measurementsusing the KUKA LWR 4+ robot arm and comparisons toother freely available state-of-the-art algorithms The usedcalibration and measurement data for our tests and theproposed algorithm (in MATLAB and C++) are published asopen source at httpsgithubcomhhyytidcm-imu

2 Related Work

Numerous attitude estimation algorithms have become avail-able Most of these consist of various Kalman filter solutions[2 3 5 12 13 16 25] usually extended Kalman filters (EKF)[7 9 10 15 17 18 20 26] and some unscented Kalman filters(UKF) [14 19 27] though some non-Kalman filter solutionsalso exist [1 4 11 21 28ndash30] as well as some geometricmethods [31ndash34] In addition Chao et al [35] have carriedout a comparative study of low-cost IMU filters Existingalgorithms often rely on data obtained from military-gradeIMUs which are usually subject to export restrictions andhigh cost that limit commercial applications [29] Severalauthors have reported that cheaper commercial grade IMUscommonly include non-Gaussian noise in their gyroscopeand accelerometer measurements often leading to instabilityin connecting classical Kalman and extended Kalman filteralgorithms [29 30] The same has been noted in an extensivesurvey of nonlinear attitude estimation methods by Crassidiset al [36] They also state that EKF is not as good a solutionas other filtering schemes The survey was published a few

International Journal of Navigation and Observation 3

years prior to most of the papers presenting DCM basedmethods [3 9 12 21] and their possibilities were thereforenot considered in their review

Low-cost MEMSs are usually subject to time-dependenterrors such as drifting gyroscope biases Therefore all IMUalgorithms for low-cost sensors should have an online biasestimator Unfortunately few of the previously publishedalgorithms developed for only triaxial accelerometers andgyroscopes have included online bias estimates for gyro-scopes In many algorithms other measurements are neededin addition to gyroscopes and accelerometers in order toestimate gyroscope biases The most commonly used sensorsare triaxial magnetometers [2 9ndash16 25] and satellite naviga-tion [3 5 17ndash20] In addition to our work few filters [21ndash24 30] have been able to estimate gyroscope biases withoutextra sensors in addition to the triaxial accelerometer andgyroscope

The development of a filter that uses only accelerometerand gyroscope measurements is difficult and can lead to anobservability problem This problem arises when a singlevectormeasurement such as the gravity through accelerationmeasurements gives only information to correct estimatesof attitude angles as well as the related biases whichcould rotate that vector The single vector measurementprovides no information about the rotation around thatvector Hamel and Mahony [30] have discussed the problemof orientation and gyroscope bias estimation using passivecomplementary filter They have also proposed a solution toestimate biases even in the single vector case SubsequentlyMahony et al [21] derived a nonlinear observer termed theexplicit complementary filter which similarly requires onlyaccelerometer and gyromeasurements In another theoreticalstudy [37] they discussed observability and stability issuesthat arise especially while using single vector measurementsFinally they proved that in these single vector cases thederivation leads to asymptotically stable observers if theyassume persistent excitation of rigid-body motion

Similar ideas have been later used in the work by Khosra-vian and Namvar [38] who proposed a nonlinear observerusing a magnetometer as a single vector measurement Inaddition to their work Hua et al [23] have implemented anonlinear attitude estimator that allows the compensationof gyroscope biases of a low-cost IMU using an antiwindupintegration technique They also show valuable aspects of apractical implementation of a filter Finally the observabilityproblem for systems in which the measurement of systeminput is corrupted by an unknown constant bias is tackledin [39]

The observability problem can also be avoided Ruizenaaret al [22] solve the problem by adding a second IMU to thesystem They propose a filter that uses two sets of triaxialaccelerometers and gyroscopes attached in a predefinedorientation with respect to each other in order to over-come the observability problem After their work Wu et al[24] overcome the same problem by actively rotating theirIMU device Rather than having two separate measurementdevices or an instrumented rotating mechanism a simplercheaper solution to this problem could be obtained if wecould overcome the problem algorithmically

Currently the most commonly used (at least among hob-byists with low-cost MEMS) and freely available IMU algo-rithms are Madgwickrsquos [11] and Mahonyrsquos [21] non-Kalmanfilter methods Both of these methods are computation-ally simpler than any Kalman filter implementation Open-source implementations by Madgwick are used to comparethese two state-of-the-art implementations to our workThese implementations are freely available for C and MAT-LAB at httpwwwx-iocoukopen-source-imu-and-ahrs-algorithms The explicit complementary filter by Mahonyet al is used as a primary attitude estimation system onseveral MAV vehicles worldwide [21] However neither ofthese Madgwickrsquos implementations was able to estimatebiases using only accelerometer and gyroscope measure-ments Nevertheless we used these as other implementationswere not available at the time of writing

Mahonyrsquos and Baldwinrsquos IMU algorithm [21 29] is basedon an idea roughly similar to our solution In contrast to ourEKF solution they derive their direct and complementaryfilters using tools from differential geometry on the Lie groupSO(3) This solution is based on the Special Orthogonalgroup SO(3) which is the underlying Lie group structurefor space of rotation matrices Our solution is in principledefined similarly to their explicit complementary filter withbias correction however instead of their constant gains formeasurement update and bias estimator we use EKF to tunethese gains

Madgwickrsquos implementation [11 28] is a quaternionimplementation of Mahonyrsquos observer [21] and it uses agradient descent algorithm in the orientation estimationMadgwickrsquos implementation [11] also uses simple algebraicmodifications [21] to ensure separation of the roll and pitchestimation error from the yaw estimation This additionhelps to deal with unreliable magnetic field measurementsMadgwickrsquos solution is computationally efficient because onlyone iteration of the gradient descent algorithm is performedfor eachmeasurementTherefore the filter is better suited forhighmeasurement frequencies If used sampling rate is largerthan 50Hz the remaining error is negligible [11] In our testswe used a significantly larger rate of 150Hz (themaximumwecould record using Microstrain Inertia-Link)

The effects of dynamic motion and nongravitationalaccelerations have previously been taken into account in [14ndash16 18 27] Most of this previous work compares the magni-tude of accelerometer measurements to the expected magni-tude of Earthrsquos gravitation field [15 18 27] It is not a perfectapproach as exactly the same magnitude of accelerometermeasurements can occur inmultiple configurations (eg freefall and an acceleration of 1 g in any direction) A more exactapproach to do this would be using a separate estimate fornongravitational accelerations as in [14] Although they usethree separate filter states for estimating linear accelerationcomponents this unnecessarily increases the computationalload of the filter One solutionwould bemaking an adaptationin the covariance for acceleration measurements using themagnitude of the difference between an estimated gravityvector and a measured acceleration [16] We implementedthis method for our DCM-type representation of orientationIn our representation of rotation it is efficient to use an


estimated gravity vector as it is included in our state estimatesrepresenting the rotation

If there were extra measurements for the velocity forexample from satellite navigation sensors then the nongravi-tational acceleration could be added to the filter as a state to beestimatedThis would make it possible to avoid our proposedmethod to tune the measurement covariance of accelerom-eters and to improve the accuracy of the proposed filter asit would no longer be vulnerable to nontemporary or con-stant accelerations Many velocity aided attitude estimationmethods are dependent on measurement of linear velocity inaddition to gyro and accelerometer measurements in orderto reliably compensate for nongravitational accelerations [31ndash34] Instead of using velocity measurements we use only alow-cost triaxial set of an accelerometer and a gyroscope

3 Methods

31 DCM Based Partial Attitude Estimation Our proposedpartial attitude estimation builds upon the work by Phuonget al [12] It is based on the relation between the estimateddirection of gravity and measured accelerations The direc-tion of gravity is estimated by integrating measured angularvelocities using a partial direction cosine matrix (DCM)Theresults are translated into Euler angles in the119885119884119883 convention[40] which have the following relation to the DCM

119899

119887C =

[[

[

120579119888120595119888minus120601119888120595119904+ 120601119904120579119904120595119888

120601119904120595119904+ 120601119888120579119904120595119888

120579119888120595119904

120601119888120595119888+ 120601119904120579119904120595119904

minus120601119904120595119888+ 120601119888120579119904120595119904

minus120579119904

120601119904120579119888

120601119888120579119888

]]

]

(1)

The notation ldquo119904rdquo in (1) refers to sine and ldquo119888rdquo to cosine 120593to roll 120579 to pitch and 120595 to yaw in Euler angles The directioncosine matrix 119899

119887C that is the rotationmatrix defines rotation

from the body-fixed frame (119887) to the navigation frame (119899) It isintegrated from initial DCM using an angular velocity tensor[119887

120596times] formed from triaxial gyroscope measurements in thebody-fixed frame according to [41]

119899

119887C =119899

119887C [119887120596times] (2)

where

[119887

120596times] =

[[[

[

0 minus119887

120596119911

119887

120596119910

119887

120596119911

0 minus119887

120596119909

minus119887

120596119910

119887

120596119909

0

]]]

]

(3)

Normally in DCM-type filters the whole DCM matrixwould be updated however in this partial-attitude-estimation case only the bottom row of the matrix in (1) isestimated for the proposed filterThese bottom-row elementsare collected into a row vector 119899

119887C3which can be updated

using

119899

119887C3=

[[[

[

119899

11988731

119899

11988732

119899

11988733

]]]

]

=[[

[

0 minus119899

11988711986233

119899

11988711986232

119899

11988711986233

0 minus119899

11988711986231

minus119899

11988711986232

119899

11988711986231

0

]]

]⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟

[C3times]

[[[

[

119887

120596119909

119887

120596119910

119887

120596119911

]]]

]⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟

u

(4)

which is derived from (1) (2) and (3) [12] In (4) 119887120596119894 119894 isin

119909 119910 119911 are measured angular velocities and 1198991198871198623119894 119894 isin 1 2 3

are bottom-row elements of the DCM which are used asan estimate of the partial attitude Later in this paper thesethree bottom-row elements are called DCM states whichform a DCM vector 119899

119887C3 [C3times] is defined as a rotation

operator which rotates the current DCM vector according tothe measured angular velocities

The observation model is constructed using accelerom-eter measurements which are compared to the currentestimate of the direction of gravity by [12]

119887f =[[[

[

119887

119891119909

119887

119891119910

119887

119891119911

]]]

]

=

119899

119887C119879[[

[

0

0

119892

]]

]

=[[

[

119899

11988711986231

119899

11988711986232

119899

11988711986233

]]

]

119892 (5)

where 119887119891119894 119894 isin 119909 119910 119911 are accelerometer measurements

(forming a measurement vector 119887f) in the body-fixed frame(119887) and 119892 is the magnitude of the Earthrsquos gravitation fieldThis simple model assumes that the gravity is aligned parallelto the 119911-axis and that there is no other acceleration thangravity Later in this paper this assumption is relaxed withthe application of a variable measurement covariance in theextended Kalman filter algorithm

32 Adaptive Extended Kalman Filter with Gyroscope BiasEstimation and Variable Covariances An extended Kalmanfilter (EKF) is a linearized approximation of an optimalnonlinear filter similar to the original Kalman filter [42]Usually the state and measurements are predicted withthe original nonlinear functions and the covariances arepredicted and updated with a linearized mapping In thiscase the measurement model is linear and the state updateis nonlinear Some attitude estimation algorithms basedon previously presented Kalman filters [3 5 12 13 25]use a simpler linear model however in our work we usea nonlinear state-transition model for a purely nonlinearproblem

We enhance the commonly used standard EKF algorithmthrough a few additions First the filter is adapted tochanging measurements by using a variable time-dependentstate-prediction and acceleration-dependent measurementcovariances Second the filter is simplified andmade compu-tationally more feasible by using gyroscope measurements ascontrol inputs in the EKF Third the magnitude of the DCMvector is constrained to be always exactly one Finally thefilter is formulated for a variable sampling interval to toleratejitter or changes in sampling rate

The filter principle is shown as a simplified block diagramin Figure 1 The accelerometer measurements are used as ameasurement for the EKF and gyroscope measurements areused as control inputs in the prediction subsection Lateron the EKF is used to fuse these measurements in theupdate subsection In Figure 1 x refers to the EKF state vectordefined in (6) x is an unnormalized predicted state z is ameasurement u is a control input and y

119896is a measurement

residual The colored blocks in the figure are updated orestimated online The accelerometer measurement is drawn


a

g

Update

Prediction

Measurement

H

NormalizationA

B

Accelerometer

Noise

Gyroscope

Process noise

K

u

w

x

y

x

Γ

++

++

+

minus

z

Figure 1 A simplified block diagram of the DCM IMU filter Thecovariance computation has been hidden to simplify the work flowof the EKF filter The colored blocks are adjusted online

using different subblocks for gravitational119892 and nongravita-tional accelerations 119886 because nongravitational accelerationis estimated using the predicted state in the EKF thusallowing these two to be separated

The state-transition model to update angular velocities toDCM states (4) and the measurement model to incorporateaccelerations (5) are formulated into an extended Kalmanfilter that has six states three for orientation (theDCMvector119899

119887C3) and three for gyroscope biases (the bias vector 119887b120596)

x = [119899

119887C3

119887b120596] = [119899

11988711986231

119899

11988711986232

119899

11988711986233

119887

119887120596

119909

119887

119887120596

119910

119887

119887120596

119911]

119879

(6)

The state-space model for the proposed system is thefollowing

x119896+1

= 119891119896(x119896 u119896) + Γ119896w119896

y119896= Ηx119896+ k119896

119888 (x119896) = 1 constraint

E w119896 = E k

119896 = 0

E w119896w119896

119879

= Q119896

E k119896k119896

119879

= R119896

x0= [0 0 1 0 0 0]

119879

(7)

In (7) 119891119896(x119896 u119896) is the discrete nonlinear state-transition

function at time index 119896 The state-transition function is pre-sented in (8) (see Appendix for the derivation) The functionuses the state vector x

119896in (6) and gyroscope measurements

as control input u119896 The measurement y

119896in (7) is derived

with a linear and static observation model H presented in(9) In (8) [C

3times] is a rotation operator defined in (4) and

Δ119905 is a sampling interval which can vary in this formulation

of extended Kalman filter In (9) 119892 is the magnitude of thegravity

119891119896(x119896 u119896) = [

I3

minusΔ119905 [C3times]119896

03times3

I3

] x119896

+ [

Δ119905 [C3times]119896

03times3

] u119896

(8)

H = [119892I303times3

] (9)

In (7) v119896and w

119896represent a zero mean Gaussian

white noise and Γ119896= Δ119905 is a simplified time-dependent

model for the state-prediction noise This simplified modelis derived assuming a Wiener white noise process in angularvelocity measurements (used as control inputs) which areintegrated into the DCM state and bias estimates over timein each prediction step [43] Therefore we can formulate astate-prediction model to have a linear relationship to timestep size This time-dependent modification of the processnoise covariance allows the filter to behave more robustlyto changing sampling interval When this is applied to acovariance model we simplify and assume that there is nocross-correlation between states thus yielding the followingequation for process noise covariance Q

119896minus1

Q119896= Γ[

1205902

1198623

I3

03times3

03times3

(120590120596

119887)2 I3

] Γ119879

= Δ1199052

[

1205902

1198623

I3

03times3

03times3

(120590120596

119887)2 I3

]

(10)

In (10) there are two parameters First 12059021198623

is the DCM-state-prediction variance which is mainly driven by thecontrol input u (angular velocities) and thus the value ofthe parameter can be approximated as the variance of noiseof gyroscope measurements Second (120590120596

119887)2 is the bias-state-

prediction variance In this work it is assumed that sincegyroscope biases drift very slowly setting a tiny value for theprediction variance of corresponding bias states is reasonable(see experimental parameters in Table 1)This forces the filterto trust its own bias estimate much more than its attitudeestimate and bias estimates change only slightly during eachmeasurement update Ourwork omits the optimal estimationof these experimental parameters as acceptable parameterscan be found manually

Themeasurement covarianceR119896in (7) is adjusted accord-

ing to the acceleration measurement as follows

R119896= (

10038171003817100381710038171003817

119887a119896

100381710038171003817100381710038171205902

119886+ 1205902

119891) I3 (11)

which is a robust covariance for acceleration measurementsbased on the work by Li and Wang [16] The proposedmeasurement covariance R

119896is built upon two parts first a

constant part which represents a variance of a measurementnoise for a triaxial accelerometer 1205902

119891and second a variable

part which represents a constant variance of estimatedacceleration 1205902

119886scaled using the magnitude of the estimated


Table 1 Experimental parameters for IMU algorithms

Symbol Quantity Value

119892The acceleration of gravity(around Helsinki Finland) 98189ms2

Δ119905 Sampling interval sim1150 s

1205902

1198623 0

Initial variance of the DCMstate 12 (rads)2

(1205901205961198870)2 Initial variance of bias states 012 (rads)2

1205902

1198623

DCM-state-predictionvariance 012 (rads)2

(120590120596119887)2 Bias-state-prediction variance 000012 (rads)2

1205902

119891

Variance of accelerometermeasurement 052 (ms2)2

1205902

119886

Variance of estimatedacceleration 102 (ms2)2

120573A tuning parameter ofMadgwickrsquos filter 01

119870119901A tuning parameter ofMahonyrsquos filter 05

nongravitational acceleration 119887a119896 which is the difference

between themeasured acceleration and the estimated gravityThese nongravitational accelerations are estimated using arelation between a predicted DCM vector 119899

119887C3119896

(the first partof the predicted state vector in the EKF) and the currentaccelerometer measurement 119887f

119896deriving from (5)

119887a119896=119887f119896minus 119892

119899

119887C3119896 (12)

The proposed measurement covariance adaptation ismore effective than the standard approach since it mod-ifies the algorithm to become more robust against rapidaccelerations However if measurement errors are correlated(ie there exists a long-term or constant nongravitationalacceleration) the assumptionunderlying the proposedmodelwould no longer hold Therefore this solution is only limitedto cases where nongravitational acceleration 119887a

119896in (12) can

be assumed to be temporaryFor simplicity since the sampling time of accelerometer

is assumed to be constant these parameters (measurementvariances 1205902

119891and 120590

2

119886) should be tuned to include the effect

of the used sampling time Time-dependent modificationscould be added similar to that for Q in (10) However itis not necessarily needed as measurement updates can beavoided if a measurement is lost and the physical samplingtime in practice usually remains constant although thesampling interval might change or measurements might belost For a practical implementation parameter 1205902

119886(the gain

for estimated gravitational acceleration) should be set to amuch larger value than 1205902

119891(measurement noise) This makes

the adaptation to changing acceleration values significantcompared to measurement noise For the values used seeexperimental parameters in Table 1

The constraint 119888 in (7) keeps the DCM vector 119899119887C3119896

usedin the first three states as a unit vector The constraint isdefined according to

119888 (x119896) =

10038171003817100381710038171003817

119899

119887C3119896

10038171003817100381710038171003817= 1 (13)

Many different nonlinear filters could be used to solve theproposed estimation problem for example Gaussian filterslike extended and unscented Kalman filters [44] We selectedan extended Kalman filter [43] as we wanted to minimizecomputational load of the filter We used the projectionmethod by Julier and LaViola Jr [45] to handle the constraintin (7) The derivation and practical implementation of theproposed EKF and the constraint projection are explained inAppendix

33 Computation of Euler Angles from Filter States To com-pare our results to other filters and to use the estimate theestimated attitude should be able to be translated into anEuler angle representation As the proposed attitude filteronly estimates the partial attitude corresponding to two outof the three Euler angles present in the bottom row of therotation matrix in (1) the transformation of filter states toEuler angle representation is not trivial While the yaw angleshould be integrated separately pitch 120579 and roll 120593 anglescan be estimated using the following equations that can bederived from (1)

120579119896= arcsin (minus119862

31119896)

120601119896= atan2 (119862

32119896 11986233119896

)

(14)

where atan2 is an inverse tangent function with two argu-ments to distinguish angles in all four quadrants [46] and1198623119894119896

is the 119894th element of the DCM vector at time index 119896Yaw angle 120595 can be integrated from bias-corrected

angular velocities with (1) and (2) using previously computedroll pitch and yaw angles as a starting point The yaw canbe resolved from the full DCM matrix C using the followingequation

120595119896= atan2 (119862

21119896 11986211119896

) (15)

where 119862119894119895119896

is the 119894 119895th index of the DCM matrix at timeindex 119896 Note that indices 2 1 and 1 1 are not estimated inthe proposed filter In (15) since the upper rows of the matrixare needed the whole rotation matrix needs to be computedoutside the filter if the yaw angle estimate is required

34 TemperatureCalibrationMethod MEMSgyroscopes andaccelerometers usually show at least some bias error thoughsome gain error might be present as well To be able to reducethe effect of these errors and to achieve the best performanceof the IMU it is important to calibrate the system beforefeeding the data into any attitude estimation algorithm Thegains and biases of the MEMS gyroscope and accelerometervary over time a large part of which can be explainedby temperature changes in the physical instrument Ourobservations (Figures 11 and 12) indicate that while working


near room temperatures a linear model for temperature issufficiently accurate to model most of the changes in biasand gain terms using low-cost MEMS accelerometers andgyroscopes

As our proposed IMU uses accelerometers to calibrategyroscope biases online accelerometer calibration is essentialfor reaching accuratemeasurementsTherefore temperature-dependent calibration is needed at least for the accelerom-eters in order to estimate temperature-dependent bias andgain terms for all the sensor axes if there is any possibilityof temperature changes in the environment We used a linearmodel for the gain and bias parameters to scale themagnitudeand reduce the bias from all gyroscope and accelerometermeasurements The measurement model is derived from [6]by adding the linearmodel of temperature for each parameterThe used measurement model for each sensor axis 119894 isin

119909 119910 119911 is

119887

119891meas119894 = 119901119891119894

gain (119879)119887

119891119894+ 119901119891119894

bias (119879)

119887

120596meas119894 = 119901120596119894

gain (119879)119887

120596119894+ 119901120596119894

bias (119879)

(16)

119901119891119894120596119894

gainbias (119879) = 119886119891119894120596119894

gainbias119879 + 119887119891119894120596119894

gainbias (17)

In (16) and (17)119901119891119894gainbias(119879) is a function of accelerometergainbias as a function of temperature 119879 and 119901

120596119894

gainbias(119879)

is a function of gyroscope gainbias as a function of tem-perature both separately for each axis 119894 In the equation119887

119891meas119894 is the accelerometer measurement and 119887120596meas119894 isthe gyroscope measurement for each axis 119894 All accelerationsand angular velocities are in body-fixed frame (119887) In (17)the temperature-dependent linear model is expressed for alldifferent sensors and sensor axes for bias and gain In theequation 119891

119894and 120596

119894are interchangeable similar to subscripts

gain and bias As we used the linear model for all biasesand gains as a function of temperature there are a total offour parameters for each sensor axis in the calibration modelfor the accelerometer and the gyroscope thus yielding 24unknown calibration parameters to tune

The calibration of accelerometers can be performedwithout accurate reference positions using the iterativemath-ematical calibration method by Won and Golnaraghi [47]According to the method the three-axis accelerometer isplaced in six different positions and held stationary duringeach calibration measurement Measurements from thesesix positions are then used in the algorithm iteratively tooptimize gains and biases for each axis of the accelerometersensor Gyroscope biases and gains are estimated by com-paring rotations to a reference measurement and forming alinear model of gains and biases for all axes of the sensorSimilar to Sahawneh and Jarrah [6] who use an instrumentedrotation plate to perform gyroscope calibration we use onerotation axis of the robot arm to accomplish the same taskWhereas theirmethod has 12 parameters we use 24 unknownparameters Our parameters can be acquired by using twodifferent strategies and single-temperature methods [6 47]

Figure 2 KUKA robot arm and the two independent IMU devicesfixed to the tool MicroStrain Inertia-Link is installed coaxiallybelow SparkFun 6DOF Digital IMU which is inside the topmostaluminum enclosure

The first strategy is having two different constant temper-atures and performing the calibration [6] at these two differ-ent temperatures From the resulting two sets of calibrationparameters the temperature-dependent linearmodel (16) canbe calculated by fitting a parameterized line (17) into twopoints in a 12-dimensional space The other strategy whichcould be used if constant temperatures cannot be arrangedis separately cooling the device for one orientation out ofsix needed in the presented methods and to perform thecalibration measurements as a function of temperature whilethe device is gradually heated Next a linear regression linecan be fitted into the data for each sensor axis as a function oftemperature (similarly as in Figures 11 and 12)This regressionmodel can then be used to estimate constant temperatureaverages for two different temperatures These estimatescould be used similarly to the average measurements in thefirst strategy

4 Experiments and Results

Experiments were conducted using two independent IMUdevices MicroStrain Inertia-Link [48] and a low-cost Spark-Fun 6DOF Digital IMU breakout board (combination of anADXL345 accelerometer [49] and an ITG-3200 gyroscope[50]) The reference measurement was acquired using aKUKA Lightweight Robot 4+ [51] and a Fast Researchinterface [52] for measuring the pose of the IMUs fixed to thetool of the robot arm Comparetti et al [53] measured KUKALWR 4+ accuracy to be on average 118mm and 095∘ forthe translation and rotation components respectively Bothof the IMU devices were installed coaxially to the robot arm(Figure 2) and aligned to have an axis orientation similar tothat for the end effector of the robot The built-in calibrationprocedure for MicroStrain Inertia-Link was performed priorto data collection [48]

Since the developed algorithm should be robust for dif-ferent parameters between different accelerometers and gyro-scopes only one common choice of experimental parameterswas used for both measurement devices The comparisonbetween proposed algorithm and comparison algorithms isthusmore general as parameter tuning plays a less important


role in the paper The experimental parameters for theproposed filter and comparison algorithms are shown inTable 1 These same parameters (measurement and state-prediction covariances and initial values) were used in bothIMUs for simplicity These parameters were tuned using aseparate set of measurement data and a robot trajectory asthe reference measurement prior to the tests presented laterin this work

The variance of the accelerometer was estimated usingmeasured accelerometer noise in a static situation andDCM-state-prediction variance was similarly estimated using mea-sured gyroscope noise Bias-state-prediction variance isman-ually selected as an arbitrary value that is significantlysmaller than DCM-state-prediction variance Similarly ini-tial values and the variance of estimated acceleration areinitially selected as arbitrary values that are large enoughand then tuned manually The reference measurement wascompared to the estimate of the proposed algorithm andthe arbitrarily selected parameters were manually changeduntil the accuracy became satisfactory The experimentalparameters for the comparison methods by Madgwick andMahony were selected as their default values (Table 1)

The used data sequence consists of two similar calibrationsequences to calibrate accelerometers and gyroscopes beforeand after the actual test data These calibration sequencesare used to calibrate measurements and remove any bias andgain errors from the measurements using the temperaturebased calibration method described in Section 34 exceptfor Inertia-Link measurements which were calibrated usingonly a simpler temperature-non-dependent method [6]Temperature changes could not be taken into account in theInertia-Link data as the device does not give temperaturemeasurement together with its own orientation estimateLuckily the temperature change during the test was smallthus limiting the possibility of any remaining bias and gainerrors in the Inertia-Link test data after calibration

After calibration of the test data we separated the testdata into two separate test sequences The first sequence theacceleration test is designed to test the magnitude of errorscaused by induced linear accelerations Our hypothesis is thatthe larger linear acceleration is induced the more likely it isthat there will be larger errors in the attitude estimate Thesecond test sequence the rotation test is designed to testdynamic performance of gyroscopes at different velocitiesmoving according to a preprogrammed path in 6D Therotation test is designed to be long enough to truly measuredrifts and give reliable error statistics In addition tests aredesigned to have the same path in four times at differentvelocities to cover different frequencies

In addition to comparing different algorithms with fullycalibrated data the sensitivity of the algorithms to gyroscopebiases was tested by adding artificial bias to fully calibratedtest data In the third test we added different magnitudes ofartificial bias to the test data (the same for all sensor axesfor simplicity) The root mean squared errors (RMSE) to thereference measurement for yaw pitch and roll angles werecompared at different added biases As Madgwickrsquos imple-mentations of his and Mahonyrsquos algorithms do not includean online bias estimator this bias test is not completely

fair however as there were no other freely available imple-mentations to use we performed our comparison for onlythese algorithms (see details in Section 2)The gyroscope biastolerance test is presented only withMicrostrain Inertia-Linkdata which is less noisy and has better accuracies with allalgorithms in the other tests The lower-cost lower-qualitySparkFun 6DOFDigital IMUwould have yielded very similarresults between the compared algorithms

For the first three tests the orientation measurement ofthe KUKA robot arm was used as a reference measurementwhich was compared to yaw pitch and roll angles computedusing the proposed DCM-IMU algorithm as well as Madg-wickrsquos [11] andMahonyrsquos [21] algorithms as a comparisonThebuilt-in estimate of Microstrain Inertia-Link was also usedas a comparison Errors to the reference measurement areplotted separately for yaw pitch and roll angles in Figure 3The reference measurement is plotted to the topmost subplotin the figure The figure also shows the acceleration test andthe rotation test sequences As can be noted exact differencesbetween the compared algorithms are difficult to discernfrom this plot Therefore differences between the algorithmsare later studied using a box-and-whiskers plot and rootmeansquared errors

Finally at the end of results section we present theeffects of temperature change on the used low-cost IMUdevice SparkFun 6DOF Digital IMU [49 50] This sectionis important as it demonstrates the need for our linearlytemperature-dependent sensor calibration model and theproposed extension to the previous calibration methods Wealso present our temperature-dependent calibration valuesfor our low-cost device

41 Rotation Test In the rotation test IMUs were rotatedin 6D using a preprogrammed path The same path wasdriven four times first at full speed and then by halving thetarget velocity at each iteration To use some well-knownstandard rotation formalism the quality of algorithms iscompared in Euler angles which is easily computed for allcompared algorithms To highlight the differences the errorsare visualized using a box-and-whiskers plot in Figure 4 Thestatistics in the figure are computed from the errors to thereference measurement during the rotation test sequenceThe upper subplot shows yaw errors and lower subplotshows combined roll and pitch errors As revealed in thefigure roll and pitch errors behave quite similarly as themeasured gravity helps similarly in their estimation (in the119885119884119883 convention [40]) therefore their errors are combinedinto the same statistics plot in Figure 4 The initial yaw error(caused by errors before the test sequence) is reduced fromthe yaw estimates of all the compared methods

All algorithms are computed for two separate devicesMicrostrain Inertia-Link (ldquoArdquo in Figure 4) and SparkFun6DOF Digital IMU (ldquoBrdquo in Figure 4) The label ldquoInertia-Linkrdquo refers to the built-in orientation estimate ofMicrostrainInertia-Link recorded in addition to raw triaxial accelera-tions and angular velocities As our paper focuses on partialattitude estimation (roll and pitch) the main focus of the testis given to the lower subplot in Figure 4 The yaw-error plot


The reference measurement and measurement errors

YawPitch

Roll

DCMMadgwick

MahonyInertia-Link

Acceleration test Rotation test

550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

minus200minus100

0100200

Refe

renc

e ang

les (

deg

)

minus10

0

10

Yaw

erro

rs (d

eg)

minus5

0

5

Pitc

h er

rors

(deg

)

minus10minus5

05

10

Roll

erro

rs (d

eg)

Figure 3 The three compared IMU algorithms a built-in estimateof Microstrain Inertia-Link and the reference trajectory which isperformed using the KUKA LWR 4+ robot arm An error to thereference measurement is shown for each algorithm for yaw pitchand roll angles The visible time range covers only the performedtestsThe calibration sequences before and after the tests are omittedfrom the figure

(the upper subplot in Figure 4) indicates that yaw drift is notsignificantly larger than other methods although its valueis integrated outside the proposed partial attitude estimatorThe surprisingly good result observed in the yaw angle canbe explained by the accurate gyroscope bias estimates inthe proposed filter The root mean squared errors (RMSE)are also counted for all methods in Table 2 where the mostaccurate results are highlighted for each angle In this testthe DCM method produced the most accurate algorithmwith Inertia-Link data and performed slightly worse thanMahonyrsquos method with the SparkFun data

42 Acceleration Test In the acceleration test the IMUs wererotated minimally moving the KUKA robot linearly in the

Error statistics in the rotation test

minus15

minus10

minus5

0

5

10

Yaw

erro

rs (d

eg)

minus8minus6minus4minus2

02468

10

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Com

bine

d ro

ll an

d pi

tch

erro

rs(d

eg)

Figure 4 A box-and-whiskers plot showing error statistics in therotation test The red line shows the median the blue box is drawnbetween the first and the last quadrants and whiskers are drawn tothe most distant measurements

Table 2 Root mean squared errors of the rotation test

Method Yaw RMSE (deg) Pitch RMSE (deg) Roll RMSE (deg)DCMA 157lowast 056lowast 061lowast

MadgwickA 240 098 152MahonyA 249 068 082DCMB 391 129 146MadgwickB 428 146 171MahonyB 420 122 105Inertia-Link 568 117 209Data of Microstrain Inertia-Link (A) and SparkFun 6DOF digital IMU (B)lowastThemost accurate value in the test

119909 119910 and 119911 directions separately The test sequence shownin Figure 5 consists of back and forth movement first usingmaximal velocity and then halving the target velocity at eachiterationThe reference velocitymeasured usingKUKArobothand is drawn on top of the figure The purpose of thetest is to show unintended rotations in attitude estimatesduring temporary accelerationsThese errors caused by rapidaccelerations have not usually been considered in otherpapers but these accelerations are present in practical casessuch as the estimation of a human body [7] a mobile phone[4] or a legged robot [8] orientation

The statistics for the acceleration test using fully cali-brated data are presented using a box-and-whiskers plot in


Reference measurement and accelerations during the acceleration test

x

y

z

A (Inertia-Link)B (SparkFun)

x-axis movement y-axis movement z-axis movement

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus1

minus05

0

05

1

Refe

renc

e velo

city

(ms

)

789

1011

Acc z

(ms2)

minus5

0

5

Acc y

(ms2)

minus5

0

5

Acc x

(ms2)

Figure 5 Reference velocities and accelerations during the accel-eration test at first 119909-axis then 119910-axis and last 119911-axis movementseparately

Figure 6 In addition rootmean squared errors are computedfor all methods in Table 3 where themost accurate results arehighlighted As can be seen from the results the proposedDCM method is much more robust against rapid accelera-tions than any of the compared methods for pitch and rollangles The yaw angle estimate is less accurate in the DCMmethod than in Madgwickrsquos and Mahonyrsquos methods This iscaused by the bias estimate around the yaw axis in the DCMmethod that is not working perfectly in this test as there arehardly any rotations present in the test data In addition thistest reveals that Mahonyrsquos method is much more robust thanMadgwickrsquos method which is highly prone to errors causedby rapid accelerations in pitch and roll angles

43 Gyroscope Bias Tolerance Test The bias test was per-formed in the same manner for the rotation test (results inFigure 8) and for the acceleration test (results in Figure 9)sequences To save computation time all algorithms were

Error statistics in the acceleration test

Iner

tia-L

ink


01234

Yaw

erro

rs (d

eg)

minus6

minus4

minus2

0

2

4

Com

bine

d ro

ll an

d pi

tch

erro

rs

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

(deg

)

Figure 6 A box-and-whiskers plot showing error statistics in theacceleration test The red line indicates the median the blue boxis drawn between the first and the last quadrants and whiskers aredrawn to the most distant measurements which are not consideredas outliers (red + signs)

Table 3 Root mean squared errors of the acceleration test

Method Yaw RMSE (deg) Pitch RMSE (deg) Roll RMSE (deg)DCMA 119 042 017lowast

MadgwickA 031 093 087MahonyA 031 040 032DCMB 229 026lowast 070MadgwickB 014lowast 078 079MahonyB 016 030 040Inertia-Link 261 050 345Data of Microstrain Inertia-Link (A) and SparkFun 6DOF digital IMU (B)lowastThemost accurate value in the test

cold-started at the beginning of the test sequence and biasestimates are computed during the test sequence that is thefilter is reset to the default values as the test begins This coldstart reduces the measured quality of our DCM method asthe biases are assumed to be zero at the start of each testThisfirst part of the test when bias estimates are converging addsmost of the measured errors to the DCMmethod with largervalues of induced bias

For an example of biased behavior of all the comparedalgorithms a rotation test where the same bias of 1 degs isadded to all gyroscope measurements is shown in Figure 7As can be seen from the figure for pitch and roll Madgwickrsquos


DCMMadgwickMahony

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

minus10minus5

05

1015

Roll

erro

r (de

g)

minus10

minus5

0

5

10

Pitc

h er

ror (

deg

)


010

Yaw

erro

r (de

g)

Errors to the reference measurement with a bias of 1degs

Figure 7 Errors in yaw pitch and roll as an artificial 1 degs bias isadded to all gyroscope measurements in the rotation test

method performs almost as well as the DCM method whileMahonyrsquos method shows the largest error For the yawangle our DCM is the only method that is able to copewith biased gyroscope measurements and to obtain reliablemeasurements The reason for this is the fact that since ourmethod can reliably estimate gyroscope biases for each sensoraxis the end result contains less integrated bias error Thestart transient caused by the cold start is also visible in thefigure

To test bias tolerance test sequenceswere computed usingdifferent added biases changing from zero to seven degreesper second separately for rotation and acceleration tests Forboth of the test sequences (the rotation test in Figure 8 andthe acceleration test in Figure 9) the proposed DCMmethodis able to successfully find the induced bias and estimate it forpitch and roll angles The bias around the yaw angle for theacceleration test in Figure 9 is not correctly estimated in theEKF since the test data includes minimally rotations (onlylinear accelerations) The filter cannot estimate the inducedbias around the 119911-axis due to the lack of information aboutthe bias present in this test data (see the discussion aboutthe observability problem in Section 2) In the rotation test(results in Figure 8) bias estimates work for all angles andthe effect of induced bias is minimal compared to all otherpresented algorithms As can be noted from the figures theproposed DCM method has the smallest error in nearly all

RMSEs as a function of gyroscope bias (rotation test)

1 2 3 4 5 6 70Constant gyroscope bias (degs)



DCMMadgwickMahony

100

101

102

103

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Pitc

h RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)

Figure 8 Root mean squared errors of yaw pitch and roll angles inthe rotation test as a function of added constant bias in gyroscopemeasurements

cases where there is at least some unknown gyroscope biaspresent

Convergence of the bias estimate in the acceleration testis interesting as the test is a special pathological case for biasestimate around the 119911-axis (corresponds to the yaw angleseen in the upper subplot in Figures 6 and 9 as there areno rotations) In this case the yaw angle and gyroscopebias estimates around the 119911-axis are not observable Thebehavior of the proposed filter while estimating biases andtheir corresponding variances is shown in Figure 10 whenthere is an induced bias of 1 degs in each gyroscope axis Inthe figure biases for pitch and roll converge rapidly towardsthe true value of 1 degs whereas the bias estimate aroundyaw converges very slowly This happens as the filter receivesmarginal information about the tiny rotations present inthe data Even in this pathological special case with theobservability problem the presented filter behaves correctlyas demonstrated by the absence of any increase in the varianceof the bias estimate that is the filter remains stable

44 Effects of Temperature to Bias To test the sensitivity oflow-costMEMS IMUs to temperature changes a long calibra-tion sequence over different temperatures was performed forthe SparkFun 6DOFDigital IMU First the device was cooleddown and then held stationary for over 40 minutes Duringthat time the excess heat from the electronics warmed the


RMSEs as a function of gyroscope bias (acceleration test)

DCMMadgwickMahony

Pitc

h RM

SE (d

eg)




100

10minus2

100

102

104

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)

Figure 9 Root mean squared errors of yaw pitch and roll anglesin the acceleration test as a function of added constant bias ingyroscope measurements

IMU from 15∘C to 35∘C The gyroscope and temperaturemeasurements are shown in Figure 11 A linear bias model ofthemeasured temperature is plotted over the gyroscopemea-surements in the figure Similarly stationary accelerometerreadings show a linear relationship to temperature as shownin Figure 12 These results indicate that both accelerometerand gyroscope measurements are temperature-dependentand that this relation can be modeled using a linear relation-ship if working around room temperatures (25 plusmn 10

∘C) Ascan be seen the change in gyroscope bias can be as large as05 degs

In addition to presenting linearly temperature-dependentgyroscope and accelerometer measurements we used our24 parameter calibration method to calibrate our SparkFun6DOF Digital IMU The acquired calibration parametersare presented in Table 4 using a notation presented in (17)As it can be noted all temperature-dependent calibrationparameters 119886 are small but not negligible which indicatesthe minor but existing temperature-dependent effect in theaccelerometer and the gyroscope In addition it can be notedthat bias terms are more dependent on the temperature thangain parameters

5 Discussion

Experiments and Results tested our proposed algorithm withmultiple different tests and compared the results to two

Bias estimates and 1-sigma distances of standard deviations

ReferenceEstimate1-sigma

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus4minus2

0246

zbi

as(d

egs

)

05

1

15

xbi

as(d

egs

)

05

1

15

ybi

as(d

egs

)

Figure 10 An artificial 1 degs bias is added to all gyroscopemeasurements in the acceleration test Gyroscope bias estimatorsfor bias around 119909 and 119910 converge rapidly towards the correct bias(reference the black slashed line) The corresponding standarddeviations estimated by the EKF are visualized in the same plotsusing 1-sigma distance to the reference bias

state-of-the-art algorithms Table 5 summarizes the mainresults and contributions of this paper

Results of the first test (rotation test in Figure 4 andTable 2) show that in normal operation with fully calibrateddata (if there are no gyroscope biases or large dynamicaccelerations present) the DCM method has error statisticsquite similar to those for Mahonyrsquos method and slightlysmaller errors than those obtained usingMadgwickrsquosmethodThe cheaper SparkFun 6DOF IMU is noisier (larger variance)and has larger RMSE though the maximal errors are similarto those for the data of Inertia-Link This test shows thatour proposed DCM method performs as well as the othermethods in the fully calibrated case

Results of the acceleration test (in Figure 6 and Table 3)show that when temporary nongravitational acceleration isapplied all methods other than our proposed DCM methodinduce large temporary errors to the attitude estimate espe-cially to the roll and pitch angles As can be seen from theRMSE estimates in Table 3 errors caused by these accelera-tions do not increase the mean squared errors significantlyIt should be noted that while testing for the effect of rapidaccelerations the RMSE does not fully reveal the effectscaused by these short and temporary accelerations Instead


Gyroscope and temperature measurements as a function of time

MeasurementsLinear temperature model

10

20

30

40

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

02040608

1

Gyr

o z(d

egs

)

222242628

Gyr

o x(d

egs

)

minus15

minus1

minus05

Gyr

o y(d

egs

)Te

mpe

ratu

re (∘

C)

Figure 11 Gyroscope and temperature measurements as a functionof time for calibration purposesThe SparkFun 6DOFDigital IMU isfirst cooled and then held stationary for a long period to warm up toa near steady state temperature A linear model of bias as a functionof temperature is drawn over the data

Table 4 Calibration parameters for SparkFun 6DOF Digital IMU

Parameter 119909-axis 119910-axis 119911-axis119886119891119894

gain minus000049316 000040477 000102091

119887119891119894

gain 106731340 103869310 098073082

119886119891119894

bias minus001551443 minus000457992 minus001929765

119887119891119894

bias 099740194 005868846 048880423

119886120596119894

gain 000027886 minus000122424 minus000246201

119887120596119894

gain 099130982 103580126 108040715

119886120596119894

bias minus001188330 minus000845710 000542382

119887120596119894

bias 024566657 019236960 minus021682853

error statistics in the box-and-whiskers plotted in Figure 6better uncover the differences between these algorithms Ascan be noted these filters behave quite differently in thepresence of dynamic accelerations In the literature theserare events are typically ignored as outliers However as

Accelerometer measurements as a function of temperature

20 25 30 3515Temperature (∘C)

Temperature (∘C)

Temperature (∘C)

20 25 30 3515

20 25 30 3515

9

95

10

Acc z

(ms2)

minus06minus05minus04minus03minus02minus01

Acc y

(ms2)


001

Acc x

(ms2)

Figure 12 Accelerometer measurements as a function of tempera-ture for calibration purposes The SparkFun 6DOF Digital IMU isfirst cooled and then held stationary for a long period to warm up tonear steady state temperature A linear model of data as a functionof temperature is drawn over plots

the attitude estimator usually requires robust behavior in allcases the behavior of the IMU algorithm should be tested forthese large temporary accelerations as well

Our DCM method shows the most robust behavioragainst these temporary nongravitational accelerations ascompared to the other algorithms Madgwickrsquos method ishighly vulnerable to these events with errors nearly onemagnitude larger than the DCM method Finally the built-in estimate of Microstrain Inertia-Link performs much moreunreliably than any of the compared methods This test alsoreveals the only drawback of our proposed DCM methodthe bias estimate around the yaw angle cannot be correctlyestimated if there are no rotations present in the data andgravity is accurately aligned to 119911-axis of the sensor Thusin this special case (the observability problem) with fullycalibrated data the bias estimator impairs the heading angleestimation

The results of the gyroscope bias tolerance test are themost important As low-cost MEMS gyroscopes usuallyintroduce a temperature-related bias term into the measure-ments and as angular velocities measured by the gyroscopesmust be integrated to estimate the attitude the bias error (iezero level error in angular velocity measurements) causeslarge errors in the attitude estimate The results of the thirdtest (Figures 7 8 and 9) show that our proposed DCMmethod is able to handle even large bias errors and that aslittle as 1 degs error in the bias can lead to large errors in all


Table 5 Summary of performed experiments and results

The test How it is tested The main results

Rotation (Section 41)Rotations and movement in 6D usingpreprogrammed path performed atdifferent velocities

All algorithms perform similarly well in thestandard test without any bias DCM-IMU wasslightly precise compared to other algorithms

Acceleration (Section 42) Linear movement separately to 119909 119910 and119911 directions at different velocities

DCM-IMU is robust against rapidnongravitational accelerations compared tostate-of-the-art algorithms

Biases with rotation (Section 43)Rotation test is performed with anartificially added gyroscope bias in themeasurement data

Only DCM-IMU can accurately estimategyroscope biases independent of the amount ofadded bias

Biases with linear acceleration and norotations (Section 43)

Acceleration test is performed with anartificially added gyroscope bias in themeasurement data

DCM-IMU can estimate gyroscope biases for 119909and 119910 axes The gyroscope bias around 119911-axis isunobservable in this case but the proposedfilter can handle the observability issue

Temperature (Section 44)Gyroscope and accelerometer aremeasured as a function of temperaturewhile it is changed

Gyroscope bias estimates change nearly by05 degs as the temperature is changed by20∘C Similarly accelerometer readings changeconsiderably

the other comparedmethods Mahonyrsquos method is thenmorevulnerable to added bias than Madgwickrsquos method whichis able to cope with some bias errors around pitch and rollangles In contrast ourDCMmethod can calibrate gyroscopebiases online without any extra information from a compassor other sensors

The last test and related results section demonstrate anexample of bias errors in a low-cost MEMS gyroscope andaccelerometer As can be noted in Figure 11 the values ofthe gyroscope bias estimates change nearly 05 degs as thetemperature is changed by 20∘C within less than an hourThis reveals the importance of calibrating gyroscopes andaccelerometers using a temperature-dependent calibrationmodel of stabilizing the temperature with a heater or cooleror of using an online bias estimatorThe calibration ofMEMSsensors is crucial difficult task for which the calibrationparameters may still change over time Nevertheless using atemperature-dependent calibration model and the proposedattitude estimation algorithm can enable the system toperform reliably within changing temperatures and driftinggyroscope biases

6 Conclusion

In this work we have proposed a partial attitude estimationalgorithm for low-cost MEMS IMUs using a direction cosinematrix (DCM) to represent orientationThe attitude estimateis partial as only the orientation towards the gravity vectoris estimated The sensor fusion of triaxial gyroscopes andaccelerometers was accomplished using an adaptive extendedKalman filterThe filter accurately estimates gyroscope biasesonline thus enabling the filter to perform effectively even ifthe calibration is inaccurate or some unknown slowly driftingbias exists in the gyroscope measurements The proposedDCM IMU is made more robust against temporary contact

forces by using adaptive measurement covariance in the EKFalgorithm

As indicated by our test results the proposed DCM-IMUalgorithm should be used with low-costMEMS sensors whenat least one of the following is true (a) only accelerometer andgyroscope measurements are available (b) there exist largetemporary accelerations (c) there exist unknown or driftinggyroscope biases or (d) measurements are collected usinga variable sampling rate However our proposed methodshould not be used if constant nongravitational accelerationsare present nor would it be needed if gyroscopes areaccurately calibrated and no drifting biases are present inthe measurements (ie an error-free system) Long-term orconstant nongravitational accelerations will be mixed withthe estimated gravitational force as there are no externalmeasurements to separate them

Finally ourmethod is best suited for low-costMEMS sen-sors with drifting biases and erroneous measurements It alsoeliminates the need for commonly usedmagnetometers whileestimating biases for gyroscope measurements The methodhowever is not designed to give an absolute heading insteadit is best suited for measuring absolute roll and pitch anglesand a minimally drifting relative yaw angle An open sourceimplementation of the proposed algorithm is available forMATLAB and C++ at httpsgithubcomhhyytidcm-imu

Appendix

The proposed system model presented in (7) can be derivedfrom the continuous-time dynamic model

x (119905) = Atx (119905) + Btu (119905) (A1)

where u is a control input (for angular velocities) At is astate-transition model Bt is a control-input model and x is


the state vector The continuous-time dynamic model of thesystem in At and Bt is formulated as

At = [03times3

minus [C3times] (119905)

03times3

03times3

]

Bt = [[C3times] (119905)

03times3

]

(A2)

In (A2) At and Bt are state-dependent as the DCMvector [C

3times] changes along the three first states This makes

the filter nonlinear The rotation operator [C3times] and the

control-input u are defined in (4) Thereby the dynamicmodel can be understood as a rotation caused by angularvelocity measurements and a countervice rotation causedby the estimated gyroscope biases The model is discretizedusing the Euler method and the following discrete nonlinearstate-transition function is formed

x119896|119896minus1

= 119891119896minus1

(x119896minus1

u119896minus1

)

= [

I3

minusΔ119905 [C3times]119896minus1|119896minus1

03times3

I3

] x119896minus1|119896minus1

+ [

Δ119905 [C3times]119896minus1|119896minus1

03times3

] u119896minus1

(A3)

Equation (A3) is similar to (8) however the notation ischanged according to syntax in [43] In the equation 119896 | 119896minus1denotes the predicted value at time step 119896 using the valuesof previous time step 119896 minus 1 This complex notation is used todifferentiate between prediction and measurement phases inKalman filter [43] Δ119905 is a sampling interval which can varyin this formulation of the extended Kalman filter x

119896minus1|119896minus1is

a normalized state estimate of the previous time step andx119896|119896minus1

is an unnormalized predicted (a priori) state estimate ofcurrent time step In the EKF u

119896minus1is usually a control input

of the last round however in this case we assume that ourgyroscope measurement at the current round is analogousto the control of the last round The 119896 minus 1 notation is leftto indicate the last round in order to be compatible withstandard Kalman filter notation [43] A similar modificationhas previously been used by [14]

The estimate covariance matrix P is updated in theprediction step using the following equations

P119896|119896minus1

= F119896minus1

P119896minus1|119896minus1

F119879119896minus1

+Q119896minus1

F119896minus1

= I6+ [

minusΔ119905 [Utimes]119896minus1|119896minus1


03times3

03times3

]

[Utimes]

=

[[[

[

0 minus (119887

120596119911minus119887

119887120596

119911)119887

120596119910minus119887

119887120596

119910

119887

120596119911minus119887

119887120596

1199110 minus (

119887

120596119909minus119887

119887120596

119909)

minus (119887

120596119910minus119887

119887120596

119910)119887

120596119909minus119887

119887120596

1199090

]]]

]

(A4)

In (A4) the linearized state-transition matrix F119896minus1

is theJacobian of the nonlinear state-transition function in (A3)

P119896|119896minus1

is the unnormalized predicted a priori estimate ofthe estimate covariance The angular velocity tensor [Utimes]is analogous to [119887120596

119899119887times] in (2) The only difference between

these is that in [Utimes] estimated gyroscope biases (bias states)are reduced from measured angular velocities that are onlypresent in (2) Hence [Utimes] represents a bias correctedangular velocity tensor

Measurement update and a posteriori update of statesare computed using the Kalman filter algorithm [43] in thefollowing way

y119896= z119896minusHx119896|119896minus1

S119896= HP

119896|119896minus1HT

+ R119896

K119896= P119896|119896minus1

HTSminus1119896

x119896|119896

= x119896|119896minus1

+ K119896y119896

(A5)

In (A5) z119896is a vector of accelerometer measurements

H is the observation model x119896|119896minus1

is the predicted (a priori)state estimate y

119896is a measurement residual P

119896|119896minus1is the

unnormalized predicted (a priori) estimate covariance R119896is

themeasurement noise covariance andK119896is theKalman gain

at time index 119896The estimate covariance matrix P is updated using the

Joseph form of the covariance update equation which iscomputationally robust against rounding errors and theresult is granted to be always positive definite and symmetric[43 54]

P119896|119896

= [I minus K119896H] P119896|119896minus1

[I minus K119896H]T + K

119896R119896KT119896 (A6)

In (A6) K119896is the Kalman gain H is the observation

model P119896|119896minus1

is the unnormalized predicted (a priori) esti-mate covariance R

119896is the measurement noise covariance

and P119896|119896

is the unnormalized updated (a posteriori) estimatecovariance at time index 119896

Because the DCM vector presents the bottom row of arotation matrix which is a unit vector the magnitude of thisvector must always be exactly one Therefore it is importantto implement this constraint into the proposed filter For thispurpose we used the projectionmethod by Julier and LaViolaJr [45] In our filter this is implemented by normalizing thestate vector x and dividing the DCM vector by its magnitude119889

x119896|119896

= 119892 (x119896|119896) = [

[

1

119889

I3

03times3

03times3

I3

]

]

x119896|119896

119889 = radic1199092

1+ 1199092

2+ 1199092

3

(A7)


The effects of normalization are projected into the esti-mate covariance matrix P using Jacobian matrix as a linearapproximation of the normalization function 119892(x

119896|119896)

P119896|119896

= JP119896|119896JT

J =120597119892 (x119896|119896)

120597x119896|119896

= [

J1sdotsdotsdot3

03times3

03times3

I3

]

J1sdotsdotsdot3

=

1

1198893

[[[

[

1199092

2+ 1199092

3minus11990911199092

minus11990911199093

minus11990911199092

1199092

1+ 1199092

3minus11990921199093

minus11990911199093

minus11990921199093

1199092

1+ 1199092

2

]]]

]

(A8)

In (A8) J is the analytic solution for the Jacobian of thenormalization function and 119889 is the magnitude of the DCMvector defined in (A7)

Conflict of Interests

The authors declare that there is no conflict of interestsregarding the publication of this paper

Acknowledgments

This work was carried out as part of the Data to Intelligence(D2I) Research Program funded by Tekes (the Finnish Fund-ing Agency for Innovation) and a consortium of companiesThe authors wish to thank Professor Ville Kyrki for theopportunity to use the KUKA LWR 4+ robot arm as well asfor all his comments

References

[1] M Euston P Coote R Mahony J Kim and T Hamel ldquoAcomplementary filter for attitude estimation of a fixed-wingUAVrdquo in Proceedings of the IEEERSJ International Conferenceon Intelligent Robots and Systems (IROS rsquo08) pp 340ndash345 IEEENice France September 2008

[2] V Bistrov ldquoPerformance analysis of alignment process ofMEMS IMUrdquo International Journal of Navigation and Observa-tion vol 2012 Article ID 731530 11 pages 2012

[3] Z Ercan V Sezer H Heceoglu et al ldquoMulti-sensor data fusionof DCM based orientation estimation for land vehiclesrdquo in Pro-ceedings of the IEEE International Conference on Mechatronics(ICM rsquo11) pp 672ndash677 IEEE Istanbul Turkey April 2011

[4] P ZhouM Li and G Shen ldquoUse it free instantly knowing yourphone attituderdquo in Proceedings of the 20th Annual InternationalConference on Mobile Computing and Networking (MobiComrsquo14) pp 605ndash616 Maui Hawaii USA September 2014

[5] L Lou X Xu J Cao Z Chen and Y Xu ldquoSensor fusion-based attitude estimation using low-cost MEMS-IMU formobile robot navigationrdquo in Proceedings of the 6th IEEE JointInternational Information Technology and Artificial IntelligenceConference (ITAIC rsquo11) pp 465ndash468 IEEE Chongqing ChinaAugust 2011

[6] L Sahawneh and M A Jarrah ldquoDevelopment and calibrationof low cost MEMS IMU for UAV applicationsrdquo in Proceedings

of the 5th International Symposium on Mechatronics and ItsApplications (ISMA rsquo08) pp 1ndash9 Amman Jordan May 2008

[7] H J Luinge and P H Veltink ldquoInclination measurement ofhuman movement using a 3-D accelerometer with autocalibra-tionrdquo IEEE Transactions on Neural Systems and RehabilitationEngineering vol 12 no 1 pp 112ndash121 2004

[8] M H Raibert Legged Robots That Balance MIT Press 1986[9] E Edwan J Zhang J Zhou and O Loffeld ldquoReduced DCM

based attitude estimation using low-cost IMU andmagnetome-ter triadrdquo in Proceedings of the 8th Workshop on PositioningNavigation and Communication (WPNC rsquo11) pp 1ndash6 IEEEDresden Germany April 2011

[10] E Foxlin ldquoInertial head-tracker sensor fusion by a comple-mentary separate-bias Kalman filterrdquo in Proceedings of the IEEEVirtual Reality Annual International Symposium pp 185ndash194IEEE Santa Clara Calif USA April 1996

[11] S O H Madgwick A J L Harrison and R VaidyanathanldquoEstimation of IMU and MARG orientation using a gradientdescent algorithmrdquo in Proceedings of the IEEE InternationalConference on Rehabilitation Robotics (ICORR rsquo11) pp 1ndash7 IEEEZurich Switzerland July 2011

[12] N H Q Phuong H-J Kang Y-S Suh and Y-S Ro ldquoA DCMbased orientation estimation algorithm with an inertial mea-surement unit and a magnetic compassrdquo Journal of UniversalComputer Science vol 15 no 4 pp 859ndash876 2009

[13] D JurmanM Jankovec R Kamnik andM Topic ldquoCalibrationand data fusion solution for the miniature attitude and headingreference systemrdquo Sensors andActuators A Physical vol 138 no2 pp 411ndash420 2007

[14] M Romanovas L Klingbeil M Trachtler and Y ManolildquoEfficient orientation estimation algorithm for low cost inertialandmagnetic sensor systemsrdquo inProceedings of the IEEESP 15thWorkshop on Statistical Signal Processing (SSP rsquo09) pp 586ndash589IEEE Cardiff Wales September 2009

[15] R Munguia and A Grau ldquoAttitude and heading system basedon EKF total state configurationrdquo in Proceedings of the IEEEInternational Symposium on Industrial Electronics (ISIE rsquo11) pp2147ndash2152 IEEE Gdansk Poland June 2011

[16] W Li and J Wang ldquoEffective adaptive Kalman filter for MEMS-IMUmagnetometers integrated attitude and heading referencesystemsrdquo Journal of Navigation vol 66 no 1 pp 99ndash113 2013

[17] D Gebre-Egziabher R C Hayward and J D Powell ldquoDesignof multi-sensor attitude determination systemsrdquo IEEE Transac-tions onAerospace and Electronic Systems vol 40 no 2 pp 627ndash649 2004

[18] J K Hall N B Knoebel and T W McLain ldquoQuaternionattitude estimation forminiature air vehicles using amultiplica-tive extended Kalman filterrdquo in Proceedings of the IEEEIONPosition Location and Navigation Symposium (PLANS rsquo08) pp1230ndash1237 IEEE Monterey Calif USA May 2008

[19] H G De Marina F J Pereda J M Giron-Sierra and FEspinosa ldquoUAV attitude estimation using unscented Kalmanfilter and TRIADrdquo IEEE Transactions on Industrial Electronicsvol 59 no 11 pp 4465ndash4474 2012

[20] N S Kumar and T Jann ldquoEstimation of attitudes from a low-cost miniaturized inertial platform using Kalman Filter-basedsensor fusion algorithmrdquo Sadhana vol 29 pp 217ndash235 2004

[21] R Mahony T Hamel and J-M Pflimlin ldquoNonlinear com-plementary filters on the special orthogonal grouprdquo IEEE


Transactions on Automatic Control vol 53 no 5 pp 1203ndash12182008

[22] M Ruizenaar E van der Hall and M Weiss ldquoGyro bias esti-mation using a dual instrument configurationrdquo in Proceedingsof the 2nd CEAS Specialist Conference on Guidance Navigationamp Control (EuroGNC rsquo13) Delft The Netherlands April 2013

[23] M-D Hua G Ducard T Hamel R Mahony and K RudinldquoImplementation of a nonlinear attitude estimator for aerialrobotic vehiclesrdquo IEEE Transactions on Control Systems Tech-nology vol 22 no 1 pp 201ndash213 2014

[24] Z Wu Z Sun W Zhang and Q Chen ldquoA novel approach forattitude estimation using MEMS inertial sensorsrdquo in Proceed-ings of the IEEE (SENSORS rsquo14) pp 1022ndash1025 IEEE ValenciaSpain November 2014

[25] R Zhu D Sun Z Zhou and D Wang ldquoA linear fusionalgorithm for attitude determination using low cost MEMS-based sensorsrdquoMeasurement vol 40 no 3 pp 322ndash328 2007

[26] B Barshan and H F Durrant-Whyte ldquoInertial navigationsystems for mobile robotsrdquo IEEE Transactions on Robotics andAutomation vol 11 no 3 pp 328ndash342 1995

[27] B Huyghe J Doutreloigne and J Vanfleteren ldquo3D orientationtracking based on unscented kalman filtering of accelerometerand magnetometer datardquo in Proceedings of the IEEE SensorsApplications Symposium (SAS rsquo09) pp 148ndash152 New OrleansLa USA February 2009

[28] S O Madgwick An efficient orientation filter for inertial andinertialmagnetic sensor arrays University of Bristol BristolUK 2010

[29] G Baldwin R Mahony J Trumpf T Hamel and T ChevironldquoComplementary filter design on the Special Euclidean group SE (3)rdquo in Proceedings of the European Control Conference vol 1p 2 Greece Athens July 2007

[30] T Hamel and R Mahony ldquoAttitude estimation on SO[3] basedon direct inertial measurementsrdquo in Proceedings of the IEEEInternational Conference on Robotics and Automation (ICRArsquo06) pp 2170ndash2175 IEEE Orlando Fla USA May 2006

[31] H F Grip T I Fossen T A Johansen and A Saberi ldquoGloballyexponentially stable attitude and gyro bias estimation withapplication to GNSSINS integrationrdquo Automatica vol 51 pp158ndash166 2015

[32] A Khosravian J Trumpf R Mahony and T Hamel ldquoVelocityaided attitude estimation on SO(3) with sensor delayrdquo inProceedings of the IEEE 53rd Annual Conference on Decision andControl (CDC rsquo14) pp 114ndash120 IEEE Los Angeles Calif USADecember 2014

[33] P Martin and E Salaun ldquoDesign and implementation of a low-cost observer-based attitude and heading reference systemrdquoControl Engineering Practice vol 18 no 7 pp 712ndash722 2010

[34] M-D Hua ldquoAttitude estimation for accelerated vehicles usingGPSINS measurementsrdquo Control Engineering Practice vol 18no 7 pp 723ndash732 2010

[35] H Chao C Coopmans L Di and Y Q Chen ldquoA compara-tive evaluation of low-cost IMUs for unmanned autonomoussystemsrdquo in Proceedings of the IEEE Conference on MultisensorFusion and Integration for Intelligent Systems (MFI rsquo10) pp 211ndash216 IEEE Salt Lake City Utah USA September 2010

[36] J L Crassidis F L Markley and Y Cheng ldquoSurvey of nonlinearattitude estimation methodsrdquo Journal of Guidance Control andDynamics vol 30 no 1 pp 12ndash28 2007

[37] R Mahony T Hamel J Trumpf and C Lageman ldquoNonlinearattitude observers on SO(3) for complementary and compatiblemeasurements a theoretical studyrdquo in Proceedings of the 48thIEEE Conference on Decision and Control Held Jointly with the28th Chinese Control Conference (CDCCCC rsquo09) pp 6407ndash6412 Shanghai China December 2009

[38] A Khosravian and M Namvar ldquoGlobally exponential estima-tion of satellite attitude using a single vector measurement andgyrordquo in Proceedings of the 49th IEEE Conference on Decisionand Control (CDC rsquo10) pp 364ndash369 IEEE Atlanta Ga USADecember 2010

[39] A Khosravian J Trumpf R Mahony and C LagemanldquoObservers for invariant systems on Lie groups with biasedinput measurements and homogeneous outputsrdquo Automaticavol 55 pp 19ndash26 2015

[40] B Siciliano and O Khatib Springer Handbook of RoboticsSpringer 2008

[41] E Nebot and H Durrant-Whyte ldquoInitial calibration and align-ment of low-cost inertial navigation units for land vehicleapplicationsrdquo Journal of Robotic Systems vol 16 no 2 pp 81ndash92 1999

[42] R E Kalman ldquoA new approach to linear filtering and predictionproblemsrdquo Journal of Basic Engineering vol 82 no 1 pp 35ndash451960

[43] Y Bar-Shalom X R Li and T Kirubarajan Estimation withApplications to Tracking and Navigation Theory Algorithms andSoftware John Wiley amp Sons New York NY USA 2001

[44] S Thrun W Burgard and D Fox Probabilistic Robotics MITPress Cambridge Mass USA 2005

[45] S J Julier and J J LaViola Jr ldquoOn Kalman filtering withnonlinear equality constraintsrdquo IEEE Transactions on SignalProcessing vol 55 no 6 pp 2774ndash2784 2007

[46] R S Jones ldquoMathematical functionsrdquo in The C ProgrammerrsquosCompanion ANSI C Library Functions Silicon Press SummitNJ USA 1st edition 1991

[47] S-H P Won and F Golnaraghi ldquoA triaxial accelerometercalibration method using a mathematical modelrdquo IEEE Trans-actions on Instrumentation and Measurement vol 59 no 8 pp2144ndash2153 2010

[48] MicroStrain ldquoInertia-link inertial measurement unit and ver-tical gyrordquo 2007 httpwwwmicrostraincominertialInertia-Link

[49] Analog Devices Digital Accelerometer ADXL345 AnalogDevices Norwood Mass USA 2009

[50] InvenSense ITG-3200 Product Specification InvenSense 2011

[51] R Bischoff J Kurth G Schreiber et al ldquoThe KUKA-DLRlightweight robot armmdasha new reference platform for roboticsresearch and manufacturingrdquo in Proceedings of the 41st Interna-tional Symposium on Robotics and the 6th German Conferenceon Robotics (ISR-ROBOTIK rsquo10) pp 1ndash8 VDE Munich Ger-many June 2010

[52] G Schreiber A Stemmer and R Bischoff ldquoThe fast researchinterface for the kuka lightweight robotrdquo in Proceedings ofthe IEEE Workshop on Innovative Robot Control Architecturesfor Demanding (Research) Applications How to Modify andEnhance Commercial Controllers (ICRA 2010) AnchorageAlaska USA May 2010


[53] M D Comparetti E De Momi T Beyl M Kunze JRaczkowsky and G Ferrigno ldquoConvergence analysis of aniterative targetingmethod for keyhole robotic surgeryrdquo Interna-tional Journal of Advanced Robotic Systems vol 11 no 1 article60 2014

[54] D Simon Optimal State Estimation Kalman H Infinity andNonlinear Approaches John Wiley amp Sons 2006

International Journal of

AerospaceEngineeringHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

RoboticsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014


Active and Passive Electronic Components

Control Scienceand Engineering

Journal of



RotatingMachinery


Hindawi Publishing Corporation httpwwwhindawicom

Journal ofEngineeringVolume 2014

Submit your manuscripts athttpwwwhindawicom

VLSI Design



Shock and Vibration


Civil EngineeringAdvances in

Acoustics and VibrationAdvances in



Electrical and Computer Engineering

Journal of

Advances inOptoElectronics


Volume 2014

The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014

SensorsJournal of


Modelling amp Simulation in EngineeringHindawi Publishing Corporation httpwwwhindawicom Volume 2014


Chemical EngineeringInternational Journal of Antennas and

Propagation




Navigation and Observation



DistributedSensor Networks



[4] position or in a legged robot [8] these accelerations canbecome significantly large Therefore their presence shouldbe taken into account

Another problem with standard IMU implementationsconcerns heading angle estimation The Earthrsquos gravitationfield gives no information about this Therefore the integra-tion of triaxial accelerometers and gyroscopes cannot providean absolute heading angleThis is commonly overcome usingan extra sensor usually a triaxial magnetometer [2 9ndash16] orsatellite navigation [3 5 17ndash20] Triaxial magnetometers offera good solution if the magnetic field measurement can betrusted In practice this is usually not the case at least inrobotics since robots are usually made of magnetic metalhave high current electronics and motor drives and maytravel within locations that are surrounded by power linesmagnets and magnetic metals Moreover because the mag-netic sensor observes the sum field caused by all magnets andelectrically inducedmagnetic fields the Earthrsquosmagnetic fieldcannot be easily separated Furthermore satellite navigationcan be of little help as it does not work well inside buildingsor caves or under dense forest foliage

The ability to reliably estimate attitude with a minimalnumber of sensors would also increase the robustness ofthe system in two ways Firstly as fewer sensors would beneeded there would be less risk of sensor failures Secondlyit would be beneficial to run an IMU algorithm on thebackground as a backup method even when other mea-surements are available In addition these results could becompared between the different algorithms to detect failuresand possibly compensate for those faults

Our proposed method solves presented challenges byfirstly formulating a calibration method as a function ofmeasured temperature for gains and biases and secondlyusing an extended Kalman filter to estimate the bias ingyroscope measurements online We have purposely omittedmagnetometer and other possible measurements from ourfilter and we estimate only absolute pitch and roll anglesthus keeping themain focus on the gyroscope bias estimationAlthough the heading estimate (yaw angle) can be computedfrom the results of the proposed filter it is not an absoluteheading Instead it is an integrated bias-corrected angularvelocity around 119911-axis The absolute yaw could be estimatedin a separate filter using any extra measurements By doingthis we can increase the robustness of the proposed sensorfusion algorithm and enhance gyroscope bias estimation Ifthe magnetometer or other sensors would fail catastrophi-cally only the heading estimate would fail leaving pitch androll estimates unaffectedThis can offer significant advantagesfor robotic applications such as flying UAVs or other robots

In contrast to other proposed solutions [21ndash24] intro-duced in more detail in the next chapter we implement anextended Kalman filter to tune the bias estimates when suchinformation is available Furthermore we do not rely onconstant gains for updating bias Instead we use an extendedKalman filter to employ available information in system andmeasurement covariances in order to tune the gains forupdating bias and attitude estimates Later in Experimentsand Results we also show in practice that even in the worstcase scenario the filter remains stable

In addition to the calibration and robust estimation ofgyroscope biases we adapt our measurement covariances toincrease the quality of the filter against changing dynamicconditions IMU algorithms usually use the direction ofgravity (through measured accelerations) to reduce accumu-lating errors in integrated angular velocities during attitudeestimation This causes unwanted errors in the attitude esti-mate when nongravitational accelerations or contact forcesare present We also use a variable-measurement-covariancemethod to reduce errors in the attitude estimation causedby rapid and temporary nongravitational accelerations It isaffordable to do as in the DCM representation of rotationthe bottom row of rotation matrix represents the directionof gravitational force As a result our implementation ofthe extended Kalman filter is able to use only six states forestimating the attitude gyroscope biases and the gravityvector

In Experiments and Results we show that our solution issignificantly more accurate than the compared algorithms inthose situations in which either temporary accelerations arepresent or significant gyroscope biases exist With fully cali-brated bias-free data without nongravitational accelerationsour algorithm performs as well as the compared algorithmssince our gyroscope bias estimates do not disturb attitudeestimation

Our work provides a complete solution that integrateslow-cost MEMS IMU temperature calibration online biasestimator and a robust extended Kalman filter which is ableto handle large temporary accelerations and changes in sam-pling rate We verify our algorithm using multiple differenttests and we also obtain accurate reference measurementsusing the KUKA LWR 4+ robot arm and comparisons toother freely available state-of-the-art algorithms The usedcalibration and measurement data for our tests and theproposed algorithm (in MATLAB and C++) are published asopen source at httpsgithubcomhhyytidcm-imu

2 Related Work

Numerous attitude estimation algorithms have become avail-able Most of these consist of various Kalman filter solutions[2 3 5 12 13 16 25] usually extended Kalman filters (EKF)[7 9 10 15 17 18 20 26] and some unscented Kalman filters(UKF) [14 19 27] though some non-Kalman filter solutionsalso exist [1 4 11 21 28ndash30] as well as some geometricmethods [31ndash34] In addition Chao et al [35] have carriedout a comparative study of low-cost IMU filters Existingalgorithms often rely on data obtained from military-gradeIMUs which are usually subject to export restrictions andhigh cost that limit commercial applications [29] Severalauthors have reported that cheaper commercial grade IMUscommonly include non-Gaussian noise in their gyroscopeand accelerometer measurements often leading to instabilityin connecting classical Kalman and extended Kalman filteralgorithms [29 30] The same has been noted in an extensivesurvey of nonlinear attitude estimation methods by Crassidiset al [36] They also state that EKF is not as good a solutionas other filtering schemes The survey was published a few














3 Methods


119899

119887C =

[[

[

120579119888120595119888minus120601119888120595119904+ 120601119904120579119904120595119888

120601119904120595119904+ 120601119888120579119904120595119888

120579119888120595119904

120601119888120595119888+ 120601119904120579119904120595119904

minus120601119904120595119888+ 120601119888120579119904120595119904

minus120579119904

120601119904120579119888

120601119888120579119888

]]

]

(1)





119899

119887C =119899

119887C [119887120596times] (2)

where

[119887

120596times] =

[[[

[

0 minus119887

120596119911

119887

120596119910

119887

120596119911

0 minus119887

120596119909

minus119887

120596119910

119887

120596119909

0

]]]

]

(3)



using

119899

119887C3=

[[[

[

119899

11988731

119899

11988732

119899

11988733

]]]

]

=[[

[

0 minus119899

11988711986233

119899

11988711986232

119899

11988711986233

0 minus119899

11988711986231

minus119899

11988711986232

119899

11988711986231

0

]]

]⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟

[C3times]

[[[

[

119887

120596119909

119887

120596119910

119887

120596119911

]]]

]⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟

u

(4)







119887f =[[[

[

119887

119891119909

119887

119891119910

119887

119891119911

]]]

]

=

119899

119887C119879[[

[

0

0

119892

]]

]

=[[

[

119899

11988711986231

119899

11988711986232

119899

11988711986233

]]

]

119892 (5)









a

g

Update

Prediction

Measurement

H

NormalizationA

B

Accelerometer

Noise

Gyroscope

Process noise

K

u

w

x

y

x

Γ

++

++

+

minus

z





x = [119899

119887C3

119887b120596] = [119899

11988711986231

119899

11988711986232

119899

11988711986233

119887

119887120596

119909

119887

119887120596

119910

119887

119887120596

119911]

119879

(6)


x119896+1

= 119891119896(x119896 u119896) + Γ119896w119896

y119896= Ηx119896+ k119896

119888 (x119896) = 1 constraint

E w119896 = E k

119896 = 0

E w119896w119896

119879

= Q119896

E k119896k119896

119879

= R119896

x0= [0 0 1 0 0 0]

119879

(7)










119891119896(x119896 u119896) = [

I3


03times3

I3

] x119896

+ [

Δ119905 [C3times]119896

03times3

] u119896

(8)

H = [119892I303times3

] (9)

In (7) v119896and w




119896minus1

Q119896= Γ[

1205902

1198623

I3

03times3

03times3

(120590120596

119887)2 I3

] Γ119879

= Δ1199052

[

1205902

1198623

I3

03times3

03times3

(120590120596

119887)2 I3

]

(10)







R119896= (

10038171003817100381710038171003817

119887a119896

100381710038171003817100381710038171205902

119886+ 1205902

119891) I3 (11)












1205902

1198623 0



1205902

1198623



1205902

119891


1205902

119886






119887C3119896



119887a119896=119887f119896minus 119892

119899

119887C3119896 (12)


119896in (12) can



119891and 120590

2



119886(the gain






119888 (x119896) =

10038171003817100381710038171003817

119899

119887C3119896

10038171003817100381710038171003817= 1 (13)



120579119896= arcsin (minus119862

31119896)

120601119896= atan2 (119862

32119896 11986233119896

)

(14)




120595119896= atan2 (119862

21119896 11986211119896

) (15)

where 119862119894119895119896






119909 119910 119911 is

119887

119891meas119894 = 119901119891119894

gain (119879)119887

119891119894+ 119901119891119894

bias (119879)

119887

120596meas119894 = 119901120596119894

gain (119879)119887

120596119894+ 119901120596119894

bias (119879)

(16)

119901119891119894120596119894

gainbias (119879) = 119886119891119894120596119894

gainbias119879 + 119887119891119894120596119894

gainbias (17)


120596119894

gainbias(119879)



119894and 120596






















YawPitch

Roll

DCMMadgwick

MahonyInertia-Link


550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

minus200minus100

0100200

Refe

renc

e ang

les (

deg

)

minus10

0

10

Yaw

erro

rs (d

eg)

minus5

0

5

Pitc

h er

rors

(deg

)

minus10minus5

05

10

Roll

erro

rs (d

eg)





minus15

minus10

minus5

0

5

10

Yaw

erro

rs (d

eg)


02468

10

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Com

bine

d ro

ll an

d pi

tch

erro

rs(d

eg)









x

y

z



480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus1

minus05

0

05

1

Refe

renc

e velo

city

(ms

)

789

1011

Acc z

(ms2)

minus5

0

5

Acc y

(ms2)

minus5

0

5

Acc x

(ms2)





Iner

tia-L

ink


01234

Yaw

erro

rs (d

eg)

minus6

minus4

minus2

0

2

4

Com

bine

d ro

ll an

d pi

tch

erro

rs

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

(deg

)








DCMMadgwickMahony

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

minus10minus5

05

1015

Roll

erro

r (de

g)

minus10

minus5

0

5

10

Pitc

h er

ror (

deg

)


010

Yaw

erro

r (de

g)









DCMMadgwickMahony

100

101

102

103

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Pitc

h RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)







DCMMadgwickMahony

Pitc

h RM

SE (d

eg)




100

10minus2

100

102

104

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)





5 Discussion




480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus4minus2

0246

zbi

as(d

egs

)

05

1

15

xbi

as(d

egs

)

05

1

15

ybi

as(d

egs

)








10

20

30

40

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

02040608

1

Gyr

o z(d

egs

)

222242628

Gyr

o x(d

egs

)

minus15

minus1

minus05

Gyr

o y(d

egs

)Te

mpe

ratu

re (∘

C)




gain minus000049316 000040477 000102091

119887119891119894

gain 106731340 103869310 098073082

119886119891119894


119887119891119894

bias 099740194 005868846 048880423

119886120596119894

gain 000027886 minus000122424 minus000246201

119887120596119894

gain 099130982 103580126 108040715

119886120596119894

bias minus001188330 minus000845710 000542382

119887120596119894

bias 024566657 019236960 minus021682853




Temperature (∘C)

Temperature (∘C)

20 25 30 3515

20 25 30 3515

9

95

10

Acc z

(ms2)


Acc y

(ms2)


001

Acc x

(ms2)





















6 Conclusion





Appendix


x (119905) = Atx (119905) + Btu (119905) (A1)




At = [03times3


03times3

03times3

]

Bt = [[C3times] (119905)

03times3

]

(A2)





x119896|119896minus1

= 119891119896minus1

(x119896minus1

u119896minus1

)

= [

I3


03times3

I3

] x119896minus1|119896minus1

+ [


03times3

] u119896minus1

(A3)








P119896|119896minus1

= F119896minus1


F119879119896minus1

+Q119896minus1

F119896minus1

= I6+ [



03times3

03times3

]

[Utimes]

=

[[[

[

0 minus (119887

120596119911minus119887

119887120596

119911)119887

120596119910minus119887

119887120596

119910

119887

120596119911minus119887

119887120596

1199110 minus (

119887

120596119909minus119887

119887120596

119909)

minus (119887

120596119910minus119887

119887120596

119910)119887

120596119909minus119887

119887120596

1199090

]]]

]

(A4)



P119896|119896minus1





y119896= z119896minusHx119896|119896minus1

S119896= HP

119896|119896minus1HT

+ R119896

K119896= P119896|119896minus1

HTSminus1119896

x119896|119896

= x119896|119896minus1

+ K119896y119896

(A5)










P119896|119896

= [I minus K119896H] P119896|119896minus1


119896R119896KT119896 (A6)





and P119896|119896



x119896|119896

= 119892 (x119896|119896) = [

[

1

119889

I3

03times3

03times3

I3

]

]

x119896|119896

119889 = radic1199092

1+ 1199092

2+ 1199092

3

(A7)



119896|119896)

P119896|119896

= JP119896|119896JT

J =120597119892 (x119896|119896)

120597x119896|119896

= [

J1sdotsdotsdot3

03times3

03times3

I3

]

J1sdotsdotsdot3

=

1

1198893

[[[

[

1199092

2+ 1199092

3minus11990911199092

minus11990911199093

minus11990911199092

1199092

1+ 1199092

3minus11990921199093

minus11990911199093

minus11990921199093

1199092

1+ 1199092

2

]]]

]

(A8)




Acknowledgments


References





























































RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation






















3 Methods


119899

119887C =

[[

[

120579119888120595119888minus120601119888120595119904+ 120601119904120579119904120595119888

120601119904120595119904+ 120601119888120579119904120595119888

120579119888120595119904

120601119888120595119888+ 120601119904120579119904120595119904

minus120601119904120595119888+ 120601119888120579119904120595119904

minus120579119904

120601119904120579119888

120601119888120579119888

]]

]

(1)





119899

119887C =119899

119887C [119887120596times] (2)

where

[119887

120596times] =

[[[

[

0 minus119887

120596119911

119887

120596119910

119887

120596119911

0 minus119887

120596119909

minus119887

120596119910

119887

120596119909

0

]]]

]

(3)



using

119899

119887C3=

[[[

[

119899

11988731

119899

11988732

119899

11988733

]]]

]

=[[

[

0 minus119899

11988711986233

119899

11988711986232

119899

11988711986233

0 minus119899

11988711986231

minus119899

11988711986232

119899

11988711986231

0

]]

]⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟

[C3times]

[[[

[

119887

120596119909

119887

120596119910

119887

120596119911

]]]

]⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟

u

(4)







119887f =[[[

[

119887

119891119909

119887

119891119910

119887

119891119911

]]]

]

=

119899

119887C119879[[

[

0

0

119892

]]

]

=[[

[

119899

11988711986231

119899

11988711986232

119899

11988711986233

]]

]

119892 (5)









a

g

Update

Prediction

Measurement

H

NormalizationA

B

Accelerometer

Noise

Gyroscope

Process noise

K

u

w

x

y

x

Γ

++

++

+

minus

z





x = [119899

119887C3

119887b120596] = [119899

11988711986231

119899

11988711986232

119899

11988711986233

119887

119887120596

119909

119887

119887120596

119910

119887

119887120596

119911]

119879

(6)


x119896+1

= 119891119896(x119896 u119896) + Γ119896w119896

y119896= Ηx119896+ k119896

119888 (x119896) = 1 constraint

E w119896 = E k

119896 = 0

E w119896w119896

119879

= Q119896

E k119896k119896

119879

= R119896

x0= [0 0 1 0 0 0]

119879

(7)










119891119896(x119896 u119896) = [

I3


03times3

I3

] x119896

+ [

Δ119905 [C3times]119896

03times3

] u119896

(8)

H = [119892I303times3

] (9)

In (7) v119896and w




119896minus1

Q119896= Γ[

1205902

1198623

I3

03times3

03times3

(120590120596

119887)2 I3

] Γ119879

= Δ1199052

[

1205902

1198623

I3

03times3

03times3

(120590120596

119887)2 I3

]

(10)







R119896= (

10038171003817100381710038171003817

119887a119896

100381710038171003817100381710038171205902

119886+ 1205902

119891) I3 (11)












1205902

1198623 0



1205902

1198623



1205902

119891


1205902

119886






119887C3119896



119887a119896=119887f119896minus 119892

119899

119887C3119896 (12)


119896in (12) can



119891and 120590

2



119886(the gain






119888 (x119896) =

10038171003817100381710038171003817

119899

119887C3119896

10038171003817100381710038171003817= 1 (13)



120579119896= arcsin (minus119862

31119896)

120601119896= atan2 (119862

32119896 11986233119896

)

(14)




120595119896= atan2 (119862

21119896 11986211119896

) (15)

where 119862119894119895119896






119909 119910 119911 is

119887

119891meas119894 = 119901119891119894

gain (119879)119887

119891119894+ 119901119891119894

bias (119879)

119887

120596meas119894 = 119901120596119894

gain (119879)119887

120596119894+ 119901120596119894

bias (119879)

(16)

119901119891119894120596119894

gainbias (119879) = 119886119891119894120596119894

gainbias119879 + 119887119891119894120596119894

gainbias (17)


120596119894

gainbias(119879)



119894and 120596






















YawPitch

Roll

DCMMadgwick

MahonyInertia-Link


550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

minus200minus100

0100200

Refe

renc

e ang

les (

deg

)

minus10

0

10

Yaw

erro

rs (d

eg)

minus5

0

5

Pitc

h er

rors

(deg

)

minus10minus5

05

10

Roll

erro

rs (d

eg)





minus15

minus10

minus5

0

5

10

Yaw

erro

rs (d

eg)


02468

10

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Com

bine

d ro

ll an

d pi

tch

erro

rs(d

eg)









x

y

z



480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus1

minus05

0

05

1

Refe

renc

e velo

city

(ms

)

789

1011

Acc z

(ms2)

minus5

0

5

Acc y

(ms2)

minus5

0

5

Acc x

(ms2)





Iner

tia-L

ink


01234

Yaw

erro

rs (d

eg)

minus6

minus4

minus2

0

2

4

Com

bine

d ro

ll an

d pi

tch

erro

rs

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

(deg

)








DCMMadgwickMahony

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

minus10minus5

05

1015

Roll

erro

r (de

g)

minus10

minus5

0

5

10

Pitc

h er

ror (

deg

)


010

Yaw

erro

r (de

g)









DCMMadgwickMahony

100

101

102

103

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Pitc

h RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)







DCMMadgwickMahony

Pitc

h RM

SE (d

eg)




100

10minus2

100

102

104

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)





5 Discussion




480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus4minus2

0246

zbi

as(d

egs

)

05

1

15

xbi

as(d

egs

)

05

1

15

ybi

as(d

egs

)








10

20

30

40

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

02040608

1

Gyr

o z(d

egs

)

222242628

Gyr

o x(d

egs

)

minus15

minus1

minus05

Gyr

o y(d

egs

)Te

mpe

ratu

re (∘

C)




gain minus000049316 000040477 000102091

119887119891119894

gain 106731340 103869310 098073082

119886119891119894


119887119891119894

bias 099740194 005868846 048880423

119886120596119894

gain 000027886 minus000122424 minus000246201

119887120596119894

gain 099130982 103580126 108040715

119886120596119894

bias minus001188330 minus000845710 000542382

119887120596119894

bias 024566657 019236960 minus021682853




Temperature (∘C)

Temperature (∘C)

20 25 30 3515

20 25 30 3515

9

95

10

Acc z

(ms2)


Acc y

(ms2)


001

Acc x

(ms2)





















6 Conclusion





Appendix


x (119905) = Atx (119905) + Btu (119905) (A1)




At = [03times3


03times3

03times3

]

Bt = [[C3times] (119905)

03times3

]

(A2)





x119896|119896minus1

= 119891119896minus1

(x119896minus1

u119896minus1

)

= [

I3


03times3

I3

] x119896minus1|119896minus1

+ [


03times3

] u119896minus1

(A3)








P119896|119896minus1

= F119896minus1


F119879119896minus1

+Q119896minus1

F119896minus1

= I6+ [



03times3

03times3

]

[Utimes]

=

[[[

[

0 minus (119887

120596119911minus119887

119887120596

119911)119887

120596119910minus119887

119887120596

119910

119887

120596119911minus119887

119887120596

1199110 minus (

119887

120596119909minus119887

119887120596

119909)

minus (119887

120596119910minus119887

119887120596

119910)119887

120596119909minus119887

119887120596

1199090

]]]

]

(A4)



P119896|119896minus1





y119896= z119896minusHx119896|119896minus1

S119896= HP

119896|119896minus1HT

+ R119896

K119896= P119896|119896minus1

HTSminus1119896

x119896|119896

= x119896|119896minus1

+ K119896y119896

(A5)










P119896|119896

= [I minus K119896H] P119896|119896minus1


119896R119896KT119896 (A6)





and P119896|119896



x119896|119896

= 119892 (x119896|119896) = [

[

1

119889

I3

03times3

03times3

I3

]

]

x119896|119896

119889 = radic1199092

1+ 1199092

2+ 1199092

3

(A7)



119896|119896)

P119896|119896

= JP119896|119896JT

J =120597119892 (x119896|119896)

120597x119896|119896

= [

J1sdotsdotsdot3

03times3

03times3

I3

]

J1sdotsdotsdot3

=

1

1198893

[[[

[

1199092

2+ 1199092

3minus11990911199092

minus11990911199093

minus11990911199092

1199092

1+ 1199092

3minus11990921199093

minus11990911199093

minus11990921199093

1199092

1+ 1199092

2

]]]

]

(A8)




Acknowledgments


References





























































RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation












3 Methods


119899

119887C =

[[

[

120579119888120595119888minus120601119888120595119904+ 120601119904120579119904120595119888

120601119904120595119904+ 120601119888120579119904120595119888

120579119888120595119904

120601119888120595119888+ 120601119904120579119904120595119904

minus120601119904120595119888+ 120601119888120579119904120595119904

minus120579119904

120601119904120579119888

120601119888120579119888

]]

]

(1)





119899

119887C =119899

119887C [119887120596times] (2)

where

[119887

120596times] =

[[[

[

0 minus119887

120596119911

119887

120596119910

119887

120596119911

0 minus119887

120596119909

minus119887

120596119910

119887

120596119909

0

]]]

]

(3)



using

119899

119887C3=

[[[

[

119899

11988731

119899

11988732

119899

11988733

]]]

]

=[[

[

0 minus119899

11988711986233

119899

11988711986232

119899

11988711986233

0 minus119899

11988711986231

minus119899

11988711986232

119899

11988711986231

0

]]

]⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟

[C3times]

[[[

[

119887

120596119909

119887

120596119910

119887

120596119911

]]]

]⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟

u

(4)







119887f =[[[

[

119887

119891119909

119887

119891119910

119887

119891119911

]]]

]

=

119899

119887C119879[[

[

0

0

119892

]]

]

=[[

[

119899

11988711986231

119899

11988711986232

119899

11988711986233

]]

]

119892 (5)









a

g

Update

Prediction

Measurement

H

NormalizationA

B

Accelerometer

Noise

Gyroscope

Process noise

K

u

w

x

y

x

Γ

++

++

+

minus

z





x = [119899

119887C3

119887b120596] = [119899

11988711986231

119899

11988711986232

119899

11988711986233

119887

119887120596

119909

119887

119887120596

119910

119887

119887120596

119911]

119879

(6)


x119896+1

= 119891119896(x119896 u119896) + Γ119896w119896

y119896= Ηx119896+ k119896

119888 (x119896) = 1 constraint

E w119896 = E k

119896 = 0

E w119896w119896

119879

= Q119896

E k119896k119896

119879

= R119896

x0= [0 0 1 0 0 0]

119879

(7)










119891119896(x119896 u119896) = [

I3


03times3

I3

] x119896

+ [

Δ119905 [C3times]119896

03times3

] u119896

(8)

H = [119892I303times3

] (9)

In (7) v119896and w




119896minus1

Q119896= Γ[

1205902

1198623

I3

03times3

03times3

(120590120596

119887)2 I3

] Γ119879

= Δ1199052

[

1205902

1198623

I3

03times3

03times3

(120590120596

119887)2 I3

]

(10)







R119896= (

10038171003817100381710038171003817

119887a119896

100381710038171003817100381710038171205902

119886+ 1205902

119891) I3 (11)












1205902

1198623 0



1205902

1198623



1205902

119891


1205902

119886






119887C3119896



119887a119896=119887f119896minus 119892

119899

119887C3119896 (12)


119896in (12) can



119891and 120590

2



119886(the gain






119888 (x119896) =

10038171003817100381710038171003817

119899

119887C3119896

10038171003817100381710038171003817= 1 (13)



120579119896= arcsin (minus119862

31119896)

120601119896= atan2 (119862

32119896 11986233119896

)

(14)




120595119896= atan2 (119862

21119896 11986211119896

) (15)

where 119862119894119895119896






119909 119910 119911 is

119887

119891meas119894 = 119901119891119894

gain (119879)119887

119891119894+ 119901119891119894

bias (119879)

119887

120596meas119894 = 119901120596119894

gain (119879)119887

120596119894+ 119901120596119894

bias (119879)

(16)

119901119891119894120596119894

gainbias (119879) = 119886119891119894120596119894

gainbias119879 + 119887119891119894120596119894

gainbias (17)


120596119894

gainbias(119879)



119894and 120596






















YawPitch

Roll

DCMMadgwick

MahonyInertia-Link


550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

minus200minus100

0100200

Refe

renc

e ang

les (

deg

)

minus10

0

10

Yaw

erro

rs (d

eg)

minus5

0

5

Pitc

h er

rors

(deg

)

minus10minus5

05

10

Roll

erro

rs (d

eg)





minus15

minus10

minus5

0

5

10

Yaw

erro

rs (d

eg)


02468

10

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Com

bine

d ro

ll an

d pi

tch

erro

rs(d

eg)









x

y

z



480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus1

minus05

0

05

1

Refe

renc

e velo

city

(ms

)

789

1011

Acc z

(ms2)

minus5

0

5

Acc y

(ms2)

minus5

0

5

Acc x

(ms2)





Iner

tia-L

ink


01234

Yaw

erro

rs (d

eg)

minus6

minus4

minus2

0

2

4

Com

bine

d ro

ll an

d pi

tch

erro

rs

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

(deg

)








DCMMadgwickMahony

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

minus10minus5

05

1015

Roll

erro

r (de

g)

minus10

minus5

0

5

10

Pitc

h er

ror (

deg

)


010

Yaw

erro

r (de

g)









DCMMadgwickMahony

100

101

102

103

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Pitc

h RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)







DCMMadgwickMahony

Pitc

h RM

SE (d

eg)




100

10minus2

100

102

104

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)





5 Discussion




480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus4minus2

0246

zbi

as(d

egs

)

05

1

15

xbi

as(d

egs

)

05

1

15

ybi

as(d

egs

)








10

20

30

40

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

02040608

1

Gyr

o z(d

egs

)

222242628

Gyr

o x(d

egs

)

minus15

minus1

minus05

Gyr

o y(d

egs

)Te

mpe

ratu

re (∘

C)




gain minus000049316 000040477 000102091

119887119891119894

gain 106731340 103869310 098073082

119886119891119894


119887119891119894

bias 099740194 005868846 048880423

119886120596119894

gain 000027886 minus000122424 minus000246201

119887120596119894

gain 099130982 103580126 108040715

119886120596119894

bias minus001188330 minus000845710 000542382

119887120596119894

bias 024566657 019236960 minus021682853




Temperature (∘C)

Temperature (∘C)

20 25 30 3515

20 25 30 3515

9

95

10

Acc z

(ms2)


Acc y

(ms2)


001

Acc x

(ms2)





















6 Conclusion





Appendix


x (119905) = Atx (119905) + Btu (119905) (A1)




At = [03times3


03times3

03times3

]

Bt = [[C3times] (119905)

03times3

]

(A2)





x119896|119896minus1

= 119891119896minus1

(x119896minus1

u119896minus1

)

= [

I3


03times3

I3

] x119896minus1|119896minus1

+ [


03times3

] u119896minus1

(A3)








P119896|119896minus1

= F119896minus1


F119879119896minus1

+Q119896minus1

F119896minus1

= I6+ [



03times3

03times3

]

[Utimes]

=

[[[

[

0 minus (119887

120596119911minus119887

119887120596

119911)119887

120596119910minus119887

119887120596

119910

119887

120596119911minus119887

119887120596

1199110 minus (

119887

120596119909minus119887

119887120596

119909)

minus (119887

120596119910minus119887

119887120596

119910)119887

120596119909minus119887

119887120596

1199090

]]]

]

(A4)



P119896|119896minus1





y119896= z119896minusHx119896|119896minus1

S119896= HP

119896|119896minus1HT

+ R119896

K119896= P119896|119896minus1

HTSminus1119896

x119896|119896

= x119896|119896minus1

+ K119896y119896

(A5)










P119896|119896

= [I minus K119896H] P119896|119896minus1


119896R119896KT119896 (A6)





and P119896|119896



x119896|119896

= 119892 (x119896|119896) = [

[

1

119889

I3

03times3

03times3

I3

]

]

x119896|119896

119889 = radic1199092

1+ 1199092

2+ 1199092

3

(A7)



119896|119896)

P119896|119896

= JP119896|119896JT

J =120597119892 (x119896|119896)

120597x119896|119896

= [

J1sdotsdotsdot3

03times3

03times3

I3

]

J1sdotsdotsdot3

=

1

1198893

[[[

[

1199092

2+ 1199092

3minus11990911199092

minus11990911199093

minus11990911199092

1199092

1+ 1199092

3minus11990921199093

minus11990911199093

minus11990921199093

1199092

1+ 1199092

2

]]]

]

(A8)




Acknowledgments


References





























































RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation










a

g

Update

Prediction

Measurement

H

NormalizationA

B

Accelerometer

Noise

Gyroscope

Process noise

K

u

w

x

y

x

Γ

++

++

+

minus

z





x = [119899

119887C3

119887b120596] = [119899

11988711986231

119899

11988711986232

119899

11988711986233

119887

119887120596

119909

119887

119887120596

119910

119887

119887120596

119911]

119879

(6)


x119896+1

= 119891119896(x119896 u119896) + Γ119896w119896

y119896= Ηx119896+ k119896

119888 (x119896) = 1 constraint

E w119896 = E k

119896 = 0

E w119896w119896

119879

= Q119896

E k119896k119896

119879

= R119896

x0= [0 0 1 0 0 0]

119879

(7)










119891119896(x119896 u119896) = [

I3


03times3

I3

] x119896

+ [

Δ119905 [C3times]119896

03times3

] u119896

(8)

H = [119892I303times3

] (9)

In (7) v119896and w




119896minus1

Q119896= Γ[

1205902

1198623

I3

03times3

03times3

(120590120596

119887)2 I3

] Γ119879

= Δ1199052

[

1205902

1198623

I3

03times3

03times3

(120590120596

119887)2 I3

]

(10)







R119896= (

10038171003817100381710038171003817

119887a119896

100381710038171003817100381710038171205902

119886+ 1205902

119891) I3 (11)












1205902

1198623 0



1205902

1198623



1205902

119891


1205902

119886






119887C3119896



119887a119896=119887f119896minus 119892

119899

119887C3119896 (12)


119896in (12) can



119891and 120590

2



119886(the gain






119888 (x119896) =

10038171003817100381710038171003817

119899

119887C3119896

10038171003817100381710038171003817= 1 (13)



120579119896= arcsin (minus119862

31119896)

120601119896= atan2 (119862

32119896 11986233119896

)

(14)




120595119896= atan2 (119862

21119896 11986211119896

) (15)

where 119862119894119895119896






119909 119910 119911 is

119887

119891meas119894 = 119901119891119894

gain (119879)119887

119891119894+ 119901119891119894

bias (119879)

119887

120596meas119894 = 119901120596119894

gain (119879)119887

120596119894+ 119901120596119894

bias (119879)

(16)

119901119891119894120596119894

gainbias (119879) = 119886119891119894120596119894

gainbias119879 + 119887119891119894120596119894

gainbias (17)


120596119894

gainbias(119879)



119894and 120596






















YawPitch

Roll

DCMMadgwick

MahonyInertia-Link


550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

minus200minus100

0100200

Refe

renc

e ang

les (

deg

)

minus10

0

10

Yaw

erro

rs (d

eg)

minus5

0

5

Pitc

h er

rors

(deg

)

minus10minus5

05

10

Roll

erro

rs (d

eg)





minus15

minus10

minus5

0

5

10

Yaw

erro

rs (d

eg)


02468

10

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Com

bine

d ro

ll an

d pi

tch

erro

rs(d

eg)









x

y

z



480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus1

minus05

0

05

1

Refe

renc

e velo

city

(ms

)

789

1011

Acc z

(ms2)

minus5

0

5

Acc y

(ms2)

minus5

0

5

Acc x

(ms2)





Iner

tia-L

ink


01234

Yaw

erro

rs (d

eg)

minus6

minus4

minus2

0

2

4

Com

bine

d ro

ll an

d pi

tch

erro

rs

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

(deg

)








DCMMadgwickMahony

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

minus10minus5

05

1015

Roll

erro

r (de

g)

minus10

minus5

0

5

10

Pitc

h er

ror (

deg

)


010

Yaw

erro

r (de

g)









DCMMadgwickMahony

100

101

102

103

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Pitc

h RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)







DCMMadgwickMahony

Pitc

h RM

SE (d

eg)




100

10minus2

100

102

104

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)





5 Discussion




480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus4minus2

0246

zbi

as(d

egs

)

05

1

15

xbi

as(d

egs

)

05

1

15

ybi

as(d

egs

)








10

20

30

40

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

02040608

1

Gyr

o z(d

egs

)

222242628

Gyr

o x(d

egs

)

minus15

minus1

minus05

Gyr

o y(d

egs

)Te

mpe

ratu

re (∘

C)




gain minus000049316 000040477 000102091

119887119891119894

gain 106731340 103869310 098073082

119886119891119894


119887119891119894

bias 099740194 005868846 048880423

119886120596119894

gain 000027886 minus000122424 minus000246201

119887120596119894

gain 099130982 103580126 108040715

119886120596119894

bias minus001188330 minus000845710 000542382

119887120596119894

bias 024566657 019236960 minus021682853




Temperature (∘C)

Temperature (∘C)

20 25 30 3515

20 25 30 3515

9

95

10

Acc z

(ms2)


Acc y

(ms2)


001

Acc x

(ms2)





















6 Conclusion





Appendix


x (119905) = Atx (119905) + Btu (119905) (A1)




At = [03times3


03times3

03times3

]

Bt = [[C3times] (119905)

03times3

]

(A2)





x119896|119896minus1

= 119891119896minus1

(x119896minus1

u119896minus1

)

= [

I3


03times3

I3

] x119896minus1|119896minus1

+ [


03times3

] u119896minus1

(A3)








P119896|119896minus1

= F119896minus1


F119879119896minus1

+Q119896minus1

F119896minus1

= I6+ [



03times3

03times3

]

[Utimes]

=

[[[

[

0 minus (119887

120596119911minus119887

119887120596

119911)119887

120596119910minus119887

119887120596

119910

119887

120596119911minus119887

119887120596

1199110 minus (

119887

120596119909minus119887

119887120596

119909)

minus (119887

120596119910minus119887

119887120596

119910)119887

120596119909minus119887

119887120596

1199090

]]]

]

(A4)



P119896|119896minus1





y119896= z119896minusHx119896|119896minus1

S119896= HP

119896|119896minus1HT

+ R119896

K119896= P119896|119896minus1

HTSminus1119896

x119896|119896

= x119896|119896minus1

+ K119896y119896

(A5)










P119896|119896

= [I minus K119896H] P119896|119896minus1


119896R119896KT119896 (A6)





and P119896|119896



x119896|119896

= 119892 (x119896|119896) = [

[

1

119889

I3

03times3

03times3

I3

]

]

x119896|119896

119889 = radic1199092

1+ 1199092

2+ 1199092

3

(A7)



119896|119896)

P119896|119896

= JP119896|119896JT

J =120597119892 (x119896|119896)

120597x119896|119896

= [

J1sdotsdotsdot3

03times3

03times3

I3

]

J1sdotsdotsdot3

=

1

1198893

[[[

[

1199092

2+ 1199092

3minus11990911199092

minus11990911199093

minus11990911199092

1199092

1+ 1199092

3minus11990921199093

minus11990911199093

minus11990921199093

1199092

1+ 1199092

2

]]]

]

(A8)




Acknowledgments


References





























































RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation














1205902

1198623 0



1205902

1198623



1205902

119891


1205902

119886






119887C3119896



119887a119896=119887f119896minus 119892

119899

119887C3119896 (12)


119896in (12) can



119891and 120590

2



119886(the gain






119888 (x119896) =

10038171003817100381710038171003817

119899

119887C3119896

10038171003817100381710038171003817= 1 (13)



120579119896= arcsin (minus119862

31119896)

120601119896= atan2 (119862

32119896 11986233119896

)

(14)




120595119896= atan2 (119862

21119896 11986211119896

) (15)

where 119862119894119895119896






119909 119910 119911 is

119887

119891meas119894 = 119901119891119894

gain (119879)119887

119891119894+ 119901119891119894

bias (119879)

119887

120596meas119894 = 119901120596119894

gain (119879)119887

120596119894+ 119901120596119894

bias (119879)

(16)

119901119891119894120596119894

gainbias (119879) = 119886119891119894120596119894

gainbias119879 + 119887119891119894120596119894

gainbias (17)


120596119894

gainbias(119879)



119894and 120596






















YawPitch

Roll

DCMMadgwick

MahonyInertia-Link


550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

minus200minus100

0100200

Refe

renc

e ang

les (

deg

)

minus10

0

10

Yaw

erro

rs (d

eg)

minus5

0

5

Pitc

h er

rors

(deg

)

minus10minus5

05

10

Roll

erro

rs (d

eg)





minus15

minus10

minus5

0

5

10

Yaw

erro

rs (d

eg)


02468

10

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Com

bine

d ro

ll an

d pi

tch

erro

rs(d

eg)









x

y

z



480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus1

minus05

0

05

1

Refe

renc

e velo

city

(ms

)

789

1011

Acc z

(ms2)

minus5

0

5

Acc y

(ms2)

minus5

0

5

Acc x

(ms2)





Iner

tia-L

ink


01234

Yaw

erro

rs (d

eg)

minus6

minus4

minus2

0

2

4

Com

bine

d ro

ll an

d pi

tch

erro

rs

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

(deg

)








DCMMadgwickMahony

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

minus10minus5

05

1015

Roll

erro

r (de

g)

minus10

minus5

0

5

10

Pitc

h er

ror (

deg

)


010

Yaw

erro

r (de

g)









DCMMadgwickMahony

100

101

102

103

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Pitc

h RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)







DCMMadgwickMahony

Pitc

h RM

SE (d

eg)




100

10minus2

100

102

104

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)





5 Discussion




480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus4minus2

0246

zbi

as(d

egs

)

05

1

15

xbi

as(d

egs

)

05

1

15

ybi

as(d

egs

)








10

20

30

40

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

02040608

1

Gyr

o z(d

egs

)

222242628

Gyr

o x(d

egs

)

minus15

minus1

minus05

Gyr

o y(d

egs

)Te

mpe

ratu

re (∘

C)




gain minus000049316 000040477 000102091

119887119891119894

gain 106731340 103869310 098073082

119886119891119894


119887119891119894

bias 099740194 005868846 048880423

119886120596119894

gain 000027886 minus000122424 minus000246201

119887120596119894

gain 099130982 103580126 108040715

119886120596119894

bias minus001188330 minus000845710 000542382

119887120596119894

bias 024566657 019236960 minus021682853




Temperature (∘C)

Temperature (∘C)

20 25 30 3515

20 25 30 3515

9

95

10

Acc z

(ms2)


Acc y

(ms2)


001

Acc x

(ms2)





















6 Conclusion





Appendix


x (119905) = Atx (119905) + Btu (119905) (A1)




At = [03times3


03times3

03times3

]

Bt = [[C3times] (119905)

03times3

]

(A2)





x119896|119896minus1

= 119891119896minus1

(x119896minus1

u119896minus1

)

= [

I3


03times3

I3

] x119896minus1|119896minus1

+ [


03times3

] u119896minus1

(A3)








P119896|119896minus1

= F119896minus1


F119879119896minus1

+Q119896minus1

F119896minus1

= I6+ [



03times3

03times3

]

[Utimes]

=

[[[

[

0 minus (119887

120596119911minus119887

119887120596

119911)119887

120596119910minus119887

119887120596

119910

119887

120596119911minus119887

119887120596

1199110 minus (

119887

120596119909minus119887

119887120596

119909)

minus (119887

120596119910minus119887

119887120596

119910)119887

120596119909minus119887

119887120596

1199090

]]]

]

(A4)



P119896|119896minus1





y119896= z119896minusHx119896|119896minus1

S119896= HP

119896|119896minus1HT

+ R119896

K119896= P119896|119896minus1

HTSminus1119896

x119896|119896

= x119896|119896minus1

+ K119896y119896

(A5)










P119896|119896

= [I minus K119896H] P119896|119896minus1


119896R119896KT119896 (A6)





and P119896|119896



x119896|119896

= 119892 (x119896|119896) = [

[

1

119889

I3

03times3

03times3

I3

]

]

x119896|119896

119889 = radic1199092

1+ 1199092

2+ 1199092

3

(A7)



119896|119896)

P119896|119896

= JP119896|119896JT

J =120597119892 (x119896|119896)

120597x119896|119896

= [

J1sdotsdotsdot3

03times3

03times3

I3

]

J1sdotsdotsdot3

=

1

1198893

[[[

[

1199092

2+ 1199092

3minus11990911199092

minus11990911199093

minus11990911199092

1199092

1+ 1199092

3minus11990921199093

minus11990911199093

minus11990921199093

1199092

1+ 1199092

2

]]]

]

(A8)




Acknowledgments


References





























































RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation












119909 119910 119911 is

119887

119891meas119894 = 119901119891119894

gain (119879)119887

119891119894+ 119901119891119894

bias (119879)

119887

120596meas119894 = 119901120596119894

gain (119879)119887

120596119894+ 119901120596119894

bias (119879)

(16)

119901119891119894120596119894

gainbias (119879) = 119886119891119894120596119894

gainbias119879 + 119887119891119894120596119894

gainbias (17)


120596119894

gainbias(119879)



119894and 120596






















YawPitch

Roll

DCMMadgwick

MahonyInertia-Link


550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

minus200minus100

0100200

Refe

renc

e ang

les (

deg

)

minus10

0

10

Yaw

erro

rs (d

eg)

minus5

0

5

Pitc

h er

rors

(deg

)

minus10minus5

05

10

Roll

erro

rs (d

eg)





minus15

minus10

minus5

0

5

10

Yaw

erro

rs (d

eg)


02468

10

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Com

bine

d ro

ll an

d pi

tch

erro

rs(d

eg)









x

y

z



480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus1

minus05

0

05

1

Refe

renc

e velo

city

(ms

)

789

1011

Acc z

(ms2)

minus5

0

5

Acc y

(ms2)

minus5

0

5

Acc x

(ms2)





Iner

tia-L

ink


01234

Yaw

erro

rs (d

eg)

minus6

minus4

minus2

0

2

4

Com

bine

d ro

ll an

d pi

tch

erro

rs

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

(deg

)








DCMMadgwickMahony

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

minus10minus5

05

1015

Roll

erro

r (de

g)

minus10

minus5

0

5

10

Pitc

h er

ror (

deg

)


010

Yaw

erro

r (de

g)









DCMMadgwickMahony

100

101

102

103

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Pitc

h RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)







DCMMadgwickMahony

Pitc

h RM

SE (d

eg)




100

10minus2

100

102

104

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)





5 Discussion




480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus4minus2

0246

zbi

as(d

egs

)

05

1

15

xbi

as(d

egs

)

05

1

15

ybi

as(d

egs

)








10

20

30

40

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

02040608

1

Gyr

o z(d

egs

)

222242628

Gyr

o x(d

egs

)

minus15

minus1

minus05

Gyr

o y(d

egs

)Te

mpe

ratu

re (∘

C)




gain minus000049316 000040477 000102091

119887119891119894

gain 106731340 103869310 098073082

119886119891119894


119887119891119894

bias 099740194 005868846 048880423

119886120596119894

gain 000027886 minus000122424 minus000246201

119887120596119894

gain 099130982 103580126 108040715

119886120596119894

bias minus001188330 minus000845710 000542382

119887120596119894

bias 024566657 019236960 minus021682853




Temperature (∘C)

Temperature (∘C)

20 25 30 3515

20 25 30 3515

9

95

10

Acc z

(ms2)


Acc y

(ms2)


001

Acc x

(ms2)





















6 Conclusion





Appendix


x (119905) = Atx (119905) + Btu (119905) (A1)




At = [03times3


03times3

03times3

]

Bt = [[C3times] (119905)

03times3

]

(A2)





x119896|119896minus1

= 119891119896minus1

(x119896minus1

u119896minus1

)

= [

I3


03times3

I3

] x119896minus1|119896minus1

+ [


03times3

] u119896minus1

(A3)








P119896|119896minus1

= F119896minus1


F119879119896minus1

+Q119896minus1

F119896minus1

= I6+ [



03times3

03times3

]

[Utimes]

=

[[[

[

0 minus (119887

120596119911minus119887

119887120596

119911)119887

120596119910minus119887

119887120596

119910

119887

120596119911minus119887

119887120596

1199110 minus (

119887

120596119909minus119887

119887120596

119909)

minus (119887

120596119910minus119887

119887120596

119910)119887

120596119909minus119887

119887120596

1199090

]]]

]

(A4)



P119896|119896minus1





y119896= z119896minusHx119896|119896minus1

S119896= HP

119896|119896minus1HT

+ R119896

K119896= P119896|119896minus1

HTSminus1119896

x119896|119896

= x119896|119896minus1

+ K119896y119896

(A5)










P119896|119896

= [I minus K119896H] P119896|119896minus1


119896R119896KT119896 (A6)





and P119896|119896



x119896|119896

= 119892 (x119896|119896) = [

[

1

119889

I3

03times3

03times3

I3

]

]

x119896|119896

119889 = radic1199092

1+ 1199092

2+ 1199092

3

(A7)



119896|119896)

P119896|119896

= JP119896|119896JT

J =120597119892 (x119896|119896)

120597x119896|119896

= [

J1sdotsdotsdot3

03times3

03times3

I3

]

J1sdotsdotsdot3

=

1

1198893

[[[

[

1199092

2+ 1199092

3minus11990911199092

minus11990911199093

minus11990911199092

1199092

1+ 1199092

3minus11990921199093

minus11990911199093

minus11990921199093

1199092

1+ 1199092

2

]]]

]

(A8)




Acknowledgments


References





























































RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation






















YawPitch

Roll

DCMMadgwick

MahonyInertia-Link


550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

minus200minus100

0100200

Refe

renc

e ang

les (

deg

)

minus10

0

10

Yaw

erro

rs (d

eg)

minus5

0

5

Pitc

h er

rors

(deg

)

minus10minus5

05

10

Roll

erro

rs (d

eg)





minus15

minus10

minus5

0

5

10

Yaw

erro

rs (d

eg)


02468

10

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Com

bine

d ro

ll an

d pi

tch

erro

rs(d

eg)









x

y

z



480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus1

minus05

0

05

1

Refe

renc

e velo

city

(ms

)

789

1011

Acc z

(ms2)

minus5

0

5

Acc y

(ms2)

minus5

0

5

Acc x

(ms2)





Iner

tia-L

ink


01234

Yaw

erro

rs (d

eg)

minus6

minus4

minus2

0

2

4

Com

bine

d ro

ll an

d pi

tch

erro

rs

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

(deg

)








DCMMadgwickMahony

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

minus10minus5

05

1015

Roll

erro

r (de

g)

minus10

minus5

0

5

10

Pitc

h er

ror (

deg

)


010

Yaw

erro

r (de

g)









DCMMadgwickMahony

100

101

102

103

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Pitc

h RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)







DCMMadgwickMahony

Pitc

h RM

SE (d

eg)




100

10minus2

100

102

104

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)





5 Discussion




480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus4minus2

0246

zbi

as(d

egs

)

05

1

15

xbi

as(d

egs

)

05

1

15

ybi

as(d

egs

)








10

20

30

40

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

02040608

1

Gyr

o z(d

egs

)

222242628

Gyr

o x(d

egs

)

minus15

minus1

minus05

Gyr

o y(d

egs

)Te

mpe

ratu

re (∘

C)




gain minus000049316 000040477 000102091

119887119891119894

gain 106731340 103869310 098073082

119886119891119894


119887119891119894

bias 099740194 005868846 048880423

119886120596119894

gain 000027886 minus000122424 minus000246201

119887120596119894

gain 099130982 103580126 108040715

119886120596119894

bias minus001188330 minus000845710 000542382

119887120596119894

bias 024566657 019236960 minus021682853




Temperature (∘C)

Temperature (∘C)

20 25 30 3515

20 25 30 3515

9

95

10

Acc z

(ms2)


Acc y

(ms2)


001

Acc x

(ms2)





















6 Conclusion





Appendix


x (119905) = Atx (119905) + Btu (119905) (A1)




At = [03times3


03times3

03times3

]

Bt = [[C3times] (119905)

03times3

]

(A2)





x119896|119896minus1

= 119891119896minus1

(x119896minus1

u119896minus1

)

= [

I3


03times3

I3

] x119896minus1|119896minus1

+ [


03times3

] u119896minus1

(A3)








P119896|119896minus1

= F119896minus1


F119879119896minus1

+Q119896minus1

F119896minus1

= I6+ [



03times3

03times3

]

[Utimes]

=

[[[

[

0 minus (119887

120596119911minus119887

119887120596

119911)119887

120596119910minus119887

119887120596

119910

119887

120596119911minus119887

119887120596

1199110 minus (

119887

120596119909minus119887

119887120596

119909)

minus (119887

120596119910minus119887

119887120596

119910)119887

120596119909minus119887

119887120596

1199090

]]]

]

(A4)



P119896|119896minus1





y119896= z119896minusHx119896|119896minus1

S119896= HP

119896|119896minus1HT

+ R119896

K119896= P119896|119896minus1

HTSminus1119896

x119896|119896

= x119896|119896minus1

+ K119896y119896

(A5)










P119896|119896

= [I minus K119896H] P119896|119896minus1


119896R119896KT119896 (A6)





and P119896|119896



x119896|119896

= 119892 (x119896|119896) = [

[

1

119889

I3

03times3

03times3

I3

]

]

x119896|119896

119889 = radic1199092

1+ 1199092

2+ 1199092

3

(A7)



119896|119896)

P119896|119896

= JP119896|119896JT

J =120597119892 (x119896|119896)

120597x119896|119896

= [

J1sdotsdotsdot3

03times3

03times3

I3

]

J1sdotsdotsdot3

=

1

1198893

[[[

[

1199092

2+ 1199092

3minus11990911199092

minus11990911199093

minus11990911199092

1199092

1+ 1199092

3minus11990921199093

minus11990911199093

minus11990921199093

1199092

1+ 1199092

2

]]]

]

(A8)




Acknowledgments


References





























































RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation











YawPitch

Roll

DCMMadgwick

MahonyInertia-Link


550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

550 600 650 700 750500Time (s)

minus200minus100

0100200

Refe

renc

e ang

les (

deg

)

minus10

0

10

Yaw

erro

rs (d

eg)

minus5

0

5

Pitc

h er

rors

(deg

)

minus10minus5

05

10

Roll

erro

rs (d

eg)





minus15

minus10

minus5

0

5

10

Yaw

erro

rs (d

eg)


02468

10

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Com

bine

d ro

ll an

d pi

tch

erro

rs(d

eg)









x

y

z



480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus1

minus05

0

05

1

Refe

renc

e velo

city

(ms

)

789

1011

Acc z

(ms2)

minus5

0

5

Acc y

(ms2)

minus5

0

5

Acc x

(ms2)





Iner

tia-L

ink


01234

Yaw

erro

rs (d

eg)

minus6

minus4

minus2

0

2

4

Com

bine

d ro

ll an

d pi

tch

erro

rs

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

(deg

)








DCMMadgwickMahony

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

minus10minus5

05

1015

Roll

erro

r (de

g)

minus10

minus5

0

5

10

Pitc

h er

ror (

deg

)


010

Yaw

erro

r (de

g)









DCMMadgwickMahony

100

101

102

103

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Pitc

h RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)







DCMMadgwickMahony

Pitc

h RM

SE (d

eg)




100

10minus2

100

102

104

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)





5 Discussion




480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus4minus2

0246

zbi

as(d

egs

)

05

1

15

xbi

as(d

egs

)

05

1

15

ybi

as(d

egs

)








10

20

30

40

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

02040608

1

Gyr

o z(d

egs

)

222242628

Gyr

o x(d

egs

)

minus15

minus1

minus05

Gyr

o y(d

egs

)Te

mpe

ratu

re (∘

C)




gain minus000049316 000040477 000102091

119887119891119894

gain 106731340 103869310 098073082

119886119891119894


119887119891119894

bias 099740194 005868846 048880423

119886120596119894

gain 000027886 minus000122424 minus000246201

119887120596119894

gain 099130982 103580126 108040715

119886120596119894

bias minus001188330 minus000845710 000542382

119887120596119894

bias 024566657 019236960 minus021682853




Temperature (∘C)

Temperature (∘C)

20 25 30 3515

20 25 30 3515

9

95

10

Acc z

(ms2)


Acc y

(ms2)


001

Acc x

(ms2)





















6 Conclusion





Appendix


x (119905) = Atx (119905) + Btu (119905) (A1)




At = [03times3


03times3

03times3

]

Bt = [[C3times] (119905)

03times3

]

(A2)





x119896|119896minus1

= 119891119896minus1

(x119896minus1

u119896minus1

)

= [

I3


03times3

I3

] x119896minus1|119896minus1

+ [


03times3

] u119896minus1

(A3)








P119896|119896minus1

= F119896minus1


F119879119896minus1

+Q119896minus1

F119896minus1

= I6+ [



03times3

03times3

]

[Utimes]

=

[[[

[

0 minus (119887

120596119911minus119887

119887120596

119911)119887

120596119910minus119887

119887120596

119910

119887

120596119911minus119887

119887120596

1199110 minus (

119887

120596119909minus119887

119887120596

119909)

minus (119887

120596119910minus119887

119887120596

119910)119887

120596119909minus119887

119887120596

1199090

]]]

]

(A4)



P119896|119896minus1





y119896= z119896minusHx119896|119896minus1

S119896= HP

119896|119896minus1HT

+ R119896

K119896= P119896|119896minus1

HTSminus1119896

x119896|119896

= x119896|119896minus1

+ K119896y119896

(A5)










P119896|119896

= [I minus K119896H] P119896|119896minus1


119896R119896KT119896 (A6)





and P119896|119896



x119896|119896

= 119892 (x119896|119896) = [

[

1

119889

I3

03times3

03times3

I3

]

]

x119896|119896

119889 = radic1199092

1+ 1199092

2+ 1199092

3

(A7)



119896|119896)

P119896|119896

= JP119896|119896JT

J =120597119892 (x119896|119896)

120597x119896|119896

= [

J1sdotsdotsdot3

03times3

03times3

I3

]

J1sdotsdotsdot3

=

1

1198893

[[[

[

1199092

2+ 1199092

3minus11990911199092

minus11990911199093

minus11990911199092

1199092

1+ 1199092

3minus11990921199093

minus11990911199093

minus11990921199093

1199092

1+ 1199092

2

]]]

]

(A8)




Acknowledgments


References





























































RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation











x

y

z



480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus1

minus05

0

05

1

Refe

renc

e velo

city

(ms

)

789

1011

Acc z

(ms2)

minus5

0

5

Acc y

(ms2)

minus5

0

5

Acc x

(ms2)





Iner

tia-L

ink


01234

Yaw

erro

rs (d

eg)

minus6

minus4

minus2

0

2

4

Com

bine

d ro

ll an

d pi

tch

erro

rs

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

Iner

tia-L

ink

DCM

A

Mad

gwic

k A

Mah

ony A

DCM

B

Mad

gwic

k B

Mah

ony B

(deg

)








DCMMadgwickMahony

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

minus10minus5

05

1015

Roll

erro

r (de

g)

minus10

minus5

0

5

10

Pitc

h er

ror (

deg

)


010

Yaw

erro

r (de

g)









DCMMadgwickMahony

100

101

102

103

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Pitc

h RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)







DCMMadgwickMahony

Pitc

h RM

SE (d

eg)




100

10minus2

100

102

104

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)





5 Discussion




480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus4minus2

0246

zbi

as(d

egs

)

05

1

15

xbi

as(d

egs

)

05

1

15

ybi

as(d

egs

)








10

20

30

40

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

02040608

1

Gyr

o z(d

egs

)

222242628

Gyr

o x(d

egs

)

minus15

minus1

minus05

Gyr

o y(d

egs

)Te

mpe

ratu

re (∘

C)




gain minus000049316 000040477 000102091

119887119891119894

gain 106731340 103869310 098073082

119886119891119894


119887119891119894

bias 099740194 005868846 048880423

119886120596119894

gain 000027886 minus000122424 minus000246201

119887120596119894

gain 099130982 103580126 108040715

119886120596119894

bias minus001188330 minus000845710 000542382

119887120596119894

bias 024566657 019236960 minus021682853




Temperature (∘C)

Temperature (∘C)

20 25 30 3515

20 25 30 3515

9

95

10

Acc z

(ms2)


Acc y

(ms2)


001

Acc x

(ms2)





















6 Conclusion





Appendix


x (119905) = Atx (119905) + Btu (119905) (A1)




At = [03times3


03times3

03times3

]

Bt = [[C3times] (119905)

03times3

]

(A2)





x119896|119896minus1

= 119891119896minus1

(x119896minus1

u119896minus1

)

= [

I3


03times3

I3

] x119896minus1|119896minus1

+ [


03times3

] u119896minus1

(A3)








P119896|119896minus1

= F119896minus1


F119879119896minus1

+Q119896minus1

F119896minus1

= I6+ [



03times3

03times3

]

[Utimes]

=

[[[

[

0 minus (119887

120596119911minus119887

119887120596

119911)119887

120596119910minus119887

119887120596

119910

119887

120596119911minus119887

119887120596

1199110 minus (

119887

120596119909minus119887

119887120596

119909)

minus (119887

120596119910minus119887

119887120596

119910)119887

120596119909minus119887

119887120596

1199090

]]]

]

(A4)



P119896|119896minus1





y119896= z119896minusHx119896|119896minus1

S119896= HP

119896|119896minus1HT

+ R119896

K119896= P119896|119896minus1

HTSminus1119896

x119896|119896

= x119896|119896minus1

+ K119896y119896

(A5)










P119896|119896

= [I minus K119896H] P119896|119896minus1


119896R119896KT119896 (A6)





and P119896|119896



x119896|119896

= 119892 (x119896|119896) = [

[

1

119889

I3

03times3

03times3

I3

]

]

x119896|119896

119889 = radic1199092

1+ 1199092

2+ 1199092

3

(A7)



119896|119896)

P119896|119896

= JP119896|119896JT

J =120597119892 (x119896|119896)

120597x119896|119896

= [

J1sdotsdotsdot3

03times3

03times3

I3

]

J1sdotsdotsdot3

=

1

1198893

[[[

[

1199092

2+ 1199092

3minus11990911199092

minus11990911199093

minus11990911199092

1199092

1+ 1199092

3minus11990921199093

minus11990911199093

minus11990921199093

1199092

1+ 1199092

2

]]]

]

(A8)




Acknowledgments


References





























































RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation










DCMMadgwickMahony

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

600 620 640 660 680 700 720 740 760580Time (s)

minus10minus5

05

1015

Roll

erro

r (de

g)

minus10

minus5

0

5

10

Pitc

h er

ror (

deg

)


010

Yaw

erro

r (de

g)









DCMMadgwickMahony

100

101

102

103

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Pitc

h RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)







DCMMadgwickMahony

Pitc

h RM

SE (d

eg)




100

10minus2

100

102

104

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)





5 Discussion




480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus4minus2

0246

zbi

as(d

egs

)

05

1

15

xbi

as(d

egs

)

05

1

15

ybi

as(d

egs

)








10

20

30

40

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

02040608

1

Gyr

o z(d

egs

)

222242628

Gyr

o x(d

egs

)

minus15

minus1

minus05

Gyr

o y(d

egs

)Te

mpe

ratu

re (∘

C)




gain minus000049316 000040477 000102091

119887119891119894

gain 106731340 103869310 098073082

119886119891119894


119887119891119894

bias 099740194 005868846 048880423

119886120596119894

gain 000027886 minus000122424 minus000246201

119887120596119894

gain 099130982 103580126 108040715

119886120596119894

bias minus001188330 minus000845710 000542382

119887120596119894

bias 024566657 019236960 minus021682853




Temperature (∘C)

Temperature (∘C)

20 25 30 3515

20 25 30 3515

9

95

10

Acc z

(ms2)


Acc y

(ms2)


001

Acc x

(ms2)





















6 Conclusion





Appendix


x (119905) = Atx (119905) + Btu (119905) (A1)




At = [03times3


03times3

03times3

]

Bt = [[C3times] (119905)

03times3

]

(A2)





x119896|119896minus1

= 119891119896minus1

(x119896minus1

u119896minus1

)

= [

I3


03times3

I3

] x119896minus1|119896minus1

+ [


03times3

] u119896minus1

(A3)








P119896|119896minus1

= F119896minus1


F119879119896minus1

+Q119896minus1

F119896minus1

= I6+ [



03times3

03times3

]

[Utimes]

=

[[[

[

0 minus (119887

120596119911minus119887

119887120596

119911)119887

120596119910minus119887

119887120596

119910

119887

120596119911minus119887

119887120596

1199110 minus (

119887

120596119909minus119887

119887120596

119909)

minus (119887

120596119910minus119887

119887120596

119910)119887

120596119909minus119887

119887120596

1199090

]]]

]

(A4)



P119896|119896minus1





y119896= z119896minusHx119896|119896minus1

S119896= HP

119896|119896minus1HT

+ R119896

K119896= P119896|119896minus1

HTSminus1119896

x119896|119896

= x119896|119896minus1

+ K119896y119896

(A5)










P119896|119896

= [I minus K119896H] P119896|119896minus1


119896R119896KT119896 (A6)





and P119896|119896



x119896|119896

= 119892 (x119896|119896) = [

[

1

119889

I3

03times3

03times3

I3

]

]

x119896|119896

119889 = radic1199092

1+ 1199092

2+ 1199092

3

(A7)



119896|119896)

P119896|119896

= JP119896|119896JT

J =120597119892 (x119896|119896)

120597x119896|119896

= [

J1sdotsdotsdot3

03times3

03times3

I3

]

J1sdotsdotsdot3

=

1

1198893

[[[

[

1199092

2+ 1199092

3minus11990911199092

minus11990911199093

minus11990911199092

1199092

1+ 1199092

3minus11990921199093

minus11990911199093

minus11990921199093

1199092

1+ 1199092

2

]]]

]

(A8)




Acknowledgments


References





























































RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation











DCMMadgwickMahony

Pitc

h RM

SE (d

eg)




100

10minus2

100

102

104

Yaw

RM

SE (d

eg)

10minus1

100

101

102

Roll

RMSE

(deg

)





5 Discussion




480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

480 490 500 510 520 530 540 550470Time (s)

minus4minus2

0246

zbi

as(d

egs

)

05

1

15

xbi

as(d

egs

)

05

1

15

ybi

as(d

egs

)








10

20

30

40

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

02040608

1

Gyr

o z(d

egs

)

222242628

Gyr

o x(d

egs

)

minus15

minus1

minus05

Gyr

o y(d

egs

)Te

mpe

ratu

re (∘

C)




gain minus000049316 000040477 000102091

119887119891119894

gain 106731340 103869310 098073082

119886119891119894


119887119891119894

bias 099740194 005868846 048880423

119886120596119894

gain 000027886 minus000122424 minus000246201

119887120596119894

gain 099130982 103580126 108040715

119886120596119894

bias minus001188330 minus000845710 000542382

119887120596119894

bias 024566657 019236960 minus021682853




Temperature (∘C)

Temperature (∘C)

20 25 30 3515

20 25 30 3515

9

95

10

Acc z

(ms2)


Acc y

(ms2)


001

Acc x

(ms2)





















6 Conclusion





Appendix


x (119905) = Atx (119905) + Btu (119905) (A1)




At = [03times3


03times3

03times3

]

Bt = [[C3times] (119905)

03times3

]

(A2)





x119896|119896minus1

= 119891119896minus1

(x119896minus1

u119896minus1

)

= [

I3


03times3

I3

] x119896minus1|119896minus1

+ [


03times3

] u119896minus1

(A3)








P119896|119896minus1

= F119896minus1


F119879119896minus1

+Q119896minus1

F119896minus1

= I6+ [



03times3

03times3

]

[Utimes]

=

[[[

[

0 minus (119887

120596119911minus119887

119887120596

119911)119887

120596119910minus119887

119887120596

119910

119887

120596119911minus119887

119887120596

1199110 minus (

119887

120596119909minus119887

119887120596

119909)

minus (119887

120596119910minus119887

119887120596

119910)119887

120596119909minus119887

119887120596

1199090

]]]

]

(A4)



P119896|119896minus1





y119896= z119896minusHx119896|119896minus1

S119896= HP

119896|119896minus1HT

+ R119896

K119896= P119896|119896minus1

HTSminus1119896

x119896|119896

= x119896|119896minus1

+ K119896y119896

(A5)










P119896|119896

= [I minus K119896H] P119896|119896minus1


119896R119896KT119896 (A6)





and P119896|119896



x119896|119896

= 119892 (x119896|119896) = [

[

1

119889

I3

03times3

03times3

I3

]

]

x119896|119896

119889 = radic1199092

1+ 1199092

2+ 1199092

3

(A7)



119896|119896)

P119896|119896

= JP119896|119896JT

J =120597119892 (x119896|119896)

120597x119896|119896

= [

J1sdotsdotsdot3

03times3

03times3

I3

]

J1sdotsdotsdot3

=

1

1198893

[[[

[

1199092

2+ 1199092

3minus11990911199092

minus11990911199093

minus11990911199092

1199092

1+ 1199092

3minus11990921199093

minus11990911199093

minus11990921199093

1199092

1+ 1199092

2

]]]

]

(A8)




Acknowledgments


References





























































RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation












10

20

30

40

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

500 1000 1500 2000 25000Time (s)

02040608

1

Gyr

o z(d

egs

)

222242628

Gyr

o x(d

egs

)

minus15

minus1

minus05

Gyr

o y(d

egs

)Te

mpe

ratu

re (∘

C)




gain minus000049316 000040477 000102091

119887119891119894

gain 106731340 103869310 098073082

119886119891119894


119887119891119894

bias 099740194 005868846 048880423

119886120596119894

gain 000027886 minus000122424 minus000246201

119887120596119894

gain 099130982 103580126 108040715

119886120596119894

bias minus001188330 minus000845710 000542382

119887120596119894

bias 024566657 019236960 minus021682853




Temperature (∘C)

Temperature (∘C)

20 25 30 3515

20 25 30 3515

9

95

10

Acc z

(ms2)


Acc y

(ms2)


001

Acc x

(ms2)





















6 Conclusion





Appendix


x (119905) = Atx (119905) + Btu (119905) (A1)




At = [03times3


03times3

03times3

]

Bt = [[C3times] (119905)

03times3

]

(A2)





x119896|119896minus1

= 119891119896minus1

(x119896minus1

u119896minus1

)

= [

I3


03times3

I3

] x119896minus1|119896minus1

+ [


03times3

] u119896minus1

(A3)








P119896|119896minus1

= F119896minus1


F119879119896minus1

+Q119896minus1

F119896minus1

= I6+ [



03times3

03times3

]

[Utimes]

=

[[[

[

0 minus (119887

120596119911minus119887

119887120596

119911)119887

120596119910minus119887

119887120596

119910

119887

120596119911minus119887

119887120596

1199110 minus (

119887

120596119909minus119887

119887120596

119909)

minus (119887

120596119910minus119887

119887120596

119910)119887

120596119909minus119887

119887120596

1199090

]]]

]

(A4)



P119896|119896minus1





y119896= z119896minusHx119896|119896minus1

S119896= HP

119896|119896minus1HT

+ R119896

K119896= P119896|119896minus1

HTSminus1119896

x119896|119896

= x119896|119896minus1

+ K119896y119896

(A5)










P119896|119896

= [I minus K119896H] P119896|119896minus1


119896R119896KT119896 (A6)





and P119896|119896



x119896|119896

= 119892 (x119896|119896) = [

[

1

119889

I3

03times3

03times3

I3

]

]

x119896|119896

119889 = radic1199092

1+ 1199092

2+ 1199092

3

(A7)



119896|119896)

P119896|119896

= JP119896|119896JT

J =120597119892 (x119896|119896)

120597x119896|119896

= [

J1sdotsdotsdot3

03times3

03times3

I3

]

J1sdotsdotsdot3

=

1

1198893

[[[

[

1199092

2+ 1199092

3minus11990911199092

minus11990911199093

minus11990911199092

1199092

1+ 1199092

3minus11990921199093

minus11990911199093

minus11990921199093

1199092

1+ 1199092

2

]]]

]

(A8)




Acknowledgments


References





























































RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation

























6 Conclusion





Appendix


x (119905) = Atx (119905) + Btu (119905) (A1)




At = [03times3


03times3

03times3

]

Bt = [[C3times] (119905)

03times3

]

(A2)





x119896|119896minus1

= 119891119896minus1

(x119896minus1

u119896minus1

)

= [

I3


03times3

I3

] x119896minus1|119896minus1

+ [


03times3

] u119896minus1

(A3)








P119896|119896minus1

= F119896minus1


F119879119896minus1

+Q119896minus1

F119896minus1

= I6+ [



03times3

03times3

]

[Utimes]

=

[[[

[

0 minus (119887

120596119911minus119887

119887120596

119911)119887

120596119910minus119887

119887120596

119910

119887

120596119911minus119887

119887120596

1199110 minus (

119887

120596119909minus119887

119887120596

119909)

minus (119887

120596119910minus119887

119887120596

119910)119887

120596119909minus119887

119887120596

1199090

]]]

]

(A4)



P119896|119896minus1





y119896= z119896minusHx119896|119896minus1

S119896= HP

119896|119896minus1HT

+ R119896

K119896= P119896|119896minus1

HTSminus1119896

x119896|119896

= x119896|119896minus1

+ K119896y119896

(A5)










P119896|119896

= [I minus K119896H] P119896|119896minus1


119896R119896KT119896 (A6)





and P119896|119896



x119896|119896

= 119892 (x119896|119896) = [

[

1

119889

I3

03times3

03times3

I3

]

]

x119896|119896

119889 = radic1199092

1+ 1199092

2+ 1199092

3

(A7)



119896|119896)

P119896|119896

= JP119896|119896JT

J =120597119892 (x119896|119896)

120597x119896|119896

= [

J1sdotsdotsdot3

03times3

03times3

I3

]

J1sdotsdotsdot3

=

1

1198893

[[[

[

1199092

2+ 1199092

3minus11990911199092

minus11990911199093

minus11990911199092

1199092

1+ 1199092

3minus11990921199093

minus11990911199093

minus11990921199093

1199092

1+ 1199092

2

]]]

]

(A8)




Acknowledgments


References





























































RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation











At = [03times3


03times3

03times3

]

Bt = [[C3times] (119905)

03times3

]

(A2)





x119896|119896minus1

= 119891119896minus1

(x119896minus1

u119896minus1

)

= [

I3


03times3

I3

] x119896minus1|119896minus1

+ [


03times3

] u119896minus1

(A3)








P119896|119896minus1

= F119896minus1


F119879119896minus1

+Q119896minus1

F119896minus1

= I6+ [



03times3

03times3

]

[Utimes]

=

[[[

[

0 minus (119887

120596119911minus119887

119887120596

119911)119887

120596119910minus119887

119887120596

119910

119887

120596119911minus119887

119887120596

1199110 minus (

119887

120596119909minus119887

119887120596

119909)

minus (119887

120596119910minus119887

119887120596

119910)119887

120596119909minus119887

119887120596

1199090

]]]

]

(A4)



P119896|119896minus1





y119896= z119896minusHx119896|119896minus1

S119896= HP

119896|119896minus1HT

+ R119896

K119896= P119896|119896minus1

HTSminus1119896

x119896|119896

= x119896|119896minus1

+ K119896y119896

(A5)










P119896|119896

= [I minus K119896H] P119896|119896minus1


119896R119896KT119896 (A6)





and P119896|119896



x119896|119896

= 119892 (x119896|119896) = [

[

1

119889

I3

03times3

03times3

I3

]

]

x119896|119896

119889 = radic1199092

1+ 1199092

2+ 1199092

3

(A7)



119896|119896)

P119896|119896

= JP119896|119896JT

J =120597119892 (x119896|119896)

120597x119896|119896

= [

J1sdotsdotsdot3

03times3

03times3

I3

]

J1sdotsdotsdot3

=

1

1198893

[[[

[

1199092

2+ 1199092

3minus11990911199092

minus11990911199093

minus11990911199092

1199092

1+ 1199092

3minus11990921199093

minus11990911199093

minus11990921199093

1199092

1+ 1199092

2

]]]

]

(A8)




Acknowledgments


References





























































RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation











119896|119896)

P119896|119896

= JP119896|119896JT

J =120597119892 (x119896|119896)

120597x119896|119896

= [

J1sdotsdotsdot3

03times3

03times3

I3

]

J1sdotsdotsdot3

=

1

1198893

[[[

[

1199092

2+ 1199092

3minus11990911199092

minus11990911199093

minus11990911199092

1199092

1+ 1199092

3minus11990921199093

minus11990911199093

minus11990921199093

1199092

1+ 1199092

2

]]]

]

(A8)




Acknowledgments


References





























































RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation















































RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation














RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation











RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation









Documents

Research Article A DCM Based Attitude Estimation Algorithm ...downloads.hindawi.com/archive/2015/503814.pdf · combination with other sensors, such as global navigation satellite