Second Order Nonlinear Uncertainty Modeling inStrapdown Integration Using MEMS IMUs

Miao Zhang, Jeroen D. Hol, Laurens Slot, Henk LuingeXsens Technologies B.V.

Enschede, the NetherlandsEmail: miao.zhang@xsens.com

Abstract—This paper applies the second order Taylor approx-imation to model the nonlinear uncertainty of acceleration rota-tion for MEMS based strapdown integration. Filtering solutionsfor tracking comprising inertial sensors require a good statisticalmodeling of the inertial measurements. The nonlinearity resultsin an umbrella-shape probability density distribution, and causesnet downward vertical acceleration bias, which can not beestimated by traditional methods using only the first orderapproximation. This in turn leads to increasing vertical velocityand position errors after double integration which is significantfor MEMS grade inertial sensors. Moreover, the analytical secondorder nonlinear term is applied in an extended Kalman filter(EKF) framework and compared with a normal EKF. Thebenefits of the method are demonstrated using a 3D MEMS IMU.

Keywords: nonlinear filtering, state estimation, strapdownintegration, MEMS inertial sensor.

I. INTRODUCTION

In recent years, micro electro mechanical system (MEMS)inertial sensors have been broadly used in navigation andtracking applications. MEMS inertial sensors usually need tobe combined with other navigation systems to provide reliablelong-term position estimates, e.g., Global Positioning System(GPS) for vehicle navigation [1], ultra wideband (UWB) forindoor positioning [2], and barometer for height estimate [3].

However, measurement noises and modeling errors willcause uncertainty in the calculation of orientation and ac-celeration. To solve this, a good modeling of the strapdowninertial integration needs to be given. The core of the strap-down inertial integration is acceleration rotation from bodyframe to navigation frame, which is a nonlinear mapping.The nonlinearity always brings a downward bias to verticalacceleration so that the integrals of that, i.e., vertical velocityand position will have increasing downward biases over time.The most traditional nonlinear filter in Bayesian framework isthe extended Kalman filter (EKF), which applies the 1st orderTaylor expansion to linearize nonlinear functions at a nominaltrajectory. More advanced nonlinear filters are 2nd order EKF,unscented filter and particle filter [4]–[6], and so on. In order tounderstand the effect of the nonlinearity in strapdown inertialintegration, here we derive the analytical 2nd order term inTaylor expansion and apply it in an EKF framework, calledEKF2 (EKF1 to refer to a normal EKF). To focus on thenonlinear problem in inertial sensors, we run the predictionpart of the filter within this work, which is in fact dead

reckoning, to demonstrate the effect of different linearizationways, i.e., EKF1 and EKF2. It is expected that having a betterlinearization model for inertial sensors will benefit the overallestimation when integrated with other sensors.

The proposed approach is tested using an MEMS inertialmeasurement unit (IMU) consisting of a 3D rate gyroscopeand a 3D accelerometer, as shown in Fig. 1. The resultsof dead reckoning using EKF1 and EKF2 show that theEKF2 provides better mean and covariance estimates of theacceleration rotation compared to that from EKF1. Especiallythe covariance estimate from EKF2 is more realistic thanEKF1, i.e., consistent to the state estimate.

Figure 1: Xsens MEMS inertial sensor.

The rest of the paper is organized as follows: Section IIintroduces the strapdown inertial integration and the measure-ment model of MEMS inertial sensors. Section III addressesthe acceleration rotation problem and the Taylor expansion isderived for this nonlinear mapping. Section IV explains theEKF2 algorithm and how it works for our problem. In sec-tion V, experimental results are analysed and demonstrate thebenefits of EKF2. Finally conclusions are given in section VI.

II. STRAPDOWN INERTIAL INTEGRATION

Strapdown inertial integration or dead reckoning [7], [8]calculates the current position from an initial position usingmeasurements of angular velocity and specific force obtainedby the inertial sensors, as shown in Fig. 2.

We first introduce the coordinate frames used in this paper:• Navigation frame (n): This is a local geographic frame in

which we want to navigate. That is, we want to know the

∫

rotateremovegravity

∫ ∫

qnb0

ωbnb qnb

f b fn an

vn0

vn

pn0

pn

Figure 2: Strapdown inertial integration. Meaning of the sym-bols is explained in the text.

position and orientation of the sensor frame with respectto this frame. For most applications it is stationary withrespect to the earth.

• Body frame (b): This is the coordinate frame of themoving IMU. Its origin is located in the center of theaccelerometer axes, and it is aligned to the casing. Allthe inertial measurements are resolved in this coordinatesystem.

The measurements of inertial sensors and quantities in nav-igation are described with respect to these coordinates. Ininertial sensors, accelerometers measure external specific forcein body frame, denoted as f b. Gyroscopes measure angularvelocity from body frame to inertial frame and expressed inthe body frame. In order to keep the discussion simple, weapply correction terms which allow us to write the gyroscopemeasurement using the angular velocity from body frame tonavigation frame and resolved in the body frame, ωb

nb.

yGyr = ωbnb + bb

ω + ebω (1)

yAcc = f b + bba + eb

a = Rbn(an − gn) + bba + eb

a (2)

where Rbn denotes the rotation matrix from navigation frameto body frame. bb

ω is the gyroscope bias and bba is the

accelerometer bias. They are slowly time-varying biases. ebω

is the gyroscope measurement noise and eba is the accelerom-

eter measurement noise. They are assumed to be zero meanindependent identically distributed (i.i.d.) Gaussian noises

ebω ∼ N (0, σ2eω ) (3)

eba ∼ N (0, σ2ea) (4)

It should be noticed that accelerometers do not measureacceleration directly, rather measure the so-called externalspecific force to which the acceleration an as well as theearth’s gravitational field gn contribute, as shown in Eq. (2).Therefore, the strapdown inertial integration as illustrated inFig. 2 has the following procedure: The orientation qnb iscalculated by integrating the angular velocity, ωb

nb, measuredby gyroscopes. Then the global acceleration is obtained byrotating the specific force measured from accelerometers, f b,and correcting the gravity. Finally the velocity and positionrelative to an initial point, vn and pn, are determined by theintegration of the acceleration, an.

III. NONLINEAR PROBLEM OF ACCELERATION ROTATION

The core of the strapdown integration is the acceleration ro-tation, rotating the specific force measured by accelerometersto the navigation frame according to the orientation, as shownin Fig. 2. It can be described by

ank = qnb

k � f bk � qnb,ck + gn (5)

The orientation is represented by quaternion [9], e.g., qnb rep-resents the rotation from body frame to the navigation frame.The vector rotation is done by quaternion multiplication, �,and the superscript c is the quaternion conjugation operation.

Equation (5) is a nonlinear function. It is problematicin the strapdown integration since it always results in adownward bias in the vertical acceleration. The probabilitydensity function of the rotated acceleration is not a Gaussianshape, but rather like an umbrella. Hence, we name thisnonlinear problem ’umbrella problem’. In Fig. 3, the arrowis an acceleration vector to be estimated, but the rotatedacceleration (the dots in the figure) by an orientation error,which is Gaussian distributed, are like an umbrella. The meanof the rotated acceleration in vertical direction is alwaysdownward.

Rotated Acceleration (X−Z View)

ax [m/s

2]

az [

m/s

2]

Figure 3: Umbrella problem illustration (X-Z view): arrow is atrue acceleration vector, dots are the rotated acceleration withuncertain orientation errors.

Figure 4 illustrates this downward bias for one sample.Assume that g is the estimate of vector g after rotating, ithas errors because there is an orientation uncertainty θ. Thevertical error e can be roughly calculated by Taylor expansionso that e = ||g|| (1 − cos(θ)) ≈ 1

2 ||g|| θ2, and this error is

always downward. This bias caused by the nonlinearity ofacceleration rotation will be integrated to the velocity andposition in vertical direction and grow fast with time.

The most common nonlinear filtering approach is to lin-earize the nonlinear functions using the 1st order Taylor expan-sion and then to apply Kalman filter. However, for the problemwe point out in this paper, the 1st order Taylor expansioncannot model the downward bias due to the nonlinearityand we need a higher order nonlinear term to solve it. For

gg

e

θ

Figure 4: Simplified explanation of ’umbrella problem’. Withinthe strapdown integration, the acceleration is computed byadding the gravity to the specific force (Eq. 5). Any orientationerror will therefore result in a downward acceleration error.

simplicity, in the following of this section, we remove thetime subscript k.

The orientation qnb can be split into a nominal orientationqnb and an orientation error θb

qnb , qnb � exp(− 12θ

b) (6)

where qnb is the linearization point for the orientation andθb denotes orientation error in helical axis. exp(·) denotesquaternion exponential, which is defined as

exp(0,v) ,(

cos ||v|| , v||v|| sin ||v||

)(7)

where v represents a vector. The orientation error is selectedfor the purpose of having a local Euclidean description of theorientation without constraints. Then the acceleration rotation,Eq. (5), can be formulated as a nonlinear function of orienta-tion error θb

an , f(θb) (8)= qnb � exp(− 1

2θb)� f b � exp(− 1

2θb)c � qnb,c + gn

(9)= Rnb · exp(− 1

2θb)� f b � exp(− 1

2θb)c + gn (10)

The Taylor expansion is

f(x) =f(x) + [Dxf ]x (x− x)

+ 12 (I⊗ (x− x)T ) [Hxxf ]x (x− x) + hot. (11)

where [Dxf ]x denotes the Jacobian matrix of the nonlinearfunctions f(x) with respect to x and [Hxxf ]x denotes theHessian matrix. They are evaluated at a linearization point x.The symbol ⊗ is the Kronecker product. The mathematics ofmatrix derivatives can be referred to in [10]. Therefore, the 1st

order Taylor linearization of the acceleration rotation, f(θb),can be derived at a linearization point θb = 0

f1st(θb) = Rnbf b + gn︸ ︷︷ ︸f(θb)

+ [Dθf ]θb · θb︸ ︷︷ ︸1st order term

(12)

and the 2nd order linearization will be

f2nd(θb) = Rnbf b + gn + [Dθf ]θb · θb

+ 12 (I⊗ (θb)T ) [Hθθf ]θb θb︸ ︷︷ ︸

2nd order term

(13)

Assume that θb is a random variable of i.i.d. Gaussian, θb ∼N (µθb ,Σθb), and the linearization point is chosen at θb =µθb = 0. Since f b is the measurement from accelerometers,it is assumed to be white Gaussian, f b ∼ N (µfb ,Σfb). Thenthe mean and covariance of the 1st order Taylor linearization,Eq. (12), are

E{f1st} = Rnbµfb + gn (14)

Cov{f1st} = [Dθf ]θb Σθb [Dθf ]Tθb + RnbΣfbR

nb,T (15)

Similarly, the mean and covariance for the 2nd order Taylorlinearization, Eq. (13), are

E{f2nd} = Rnbµfb + gn

+ 12

[tr([Hθθf ]θb,i Σθb)

]i

(16)

Cov{f2nd} = [Dθf ]θb Σθb [Dθf ]Tθb + RnbΣfbR

nb,T

+ 12

[tr([Hθθf ]θb,i Σθb [Hθθf ]θb,j Σθb)

]ij

(17)

where tr(·) is the matrix trace operation. [xi]i denotes a vectorwhose ith element is xi, and [xij ]ij denotes a matrix whoseijth element is xij . Comparing the mean and covariance of2nd order linearization with those of the 1st order, an extraterm is added and it represents the downward bias causedby the nonlinearity of acceleration rotation. In addition, thisterm in the mean as well as in the covariance depends on theuncertainty of the orientation, Σθb .

IV. NONLINEARITY MODELING IN EKFA. EKF2 Algorithm

After deriving the 1st and 2nd order Taylor linearizationexpansion for the highly nonlinear mapping of accelerationrotation, a natural choice for filtering is EKF. In real appli-cations, the 1st order EKF is the most general approach andin fact it works well when the model is almost linear or thesignal noise ratio (SNR) is high [4]. For our problem, it meansthat the higher order nonlinear term can be neglected when theuncertainty of the orientation is sufficiently small. However, inMEMS inertial sensors, this uncertainty cannot be neglectedand it grows with respect to the dead reckoning time. In thefollowing first we discuss a general EKF algorithm.

A discrete-time nonlinear state space model can be writtenas

xk+1 = f(xk,wk) (18)yk = h(xk,vk) (19)

where

wk ∼ N (0,Qk) (20)vk ∼ N (0,Rk) (21)

Then an 2nd order EKF (EKF2) [4], [11] based on the 2nd

order Taylor expansion is given byInitialization:

x+0 = E

{x0

}(22)

P+0 = Cov

{x0

}(23)

Prediction:

x−k = f(x+k−1) + 1

2

[tr(f ′′i,x(x+

k−1)P+k−1

)]i

(24)

P−k = f ′x(x+k−1)P+

k−1f′x(x+

k−1) + Qk

+ 12

[tr(f ′′i,x(x+

k−1)P+k−1f

′′j,x(x+

k−1)P+k−1

)]ij

(25)

Correction:

Sk = h′x(x−k )P−k(h′x(x−k )

)T+ Rk

+ 12

[tr(h′′i,x(x−k )P−k h′′j,x(x−k )P−k

)]ij

(26)

Kk = P−k(h′x(x−k )

)T(Sk)−1 (27)

rk = yk − h(x−k )− 12

[tr(h′′i,x(x−k )P−k

)]i

(28)

x+k = x−k + Kkrk (29)

P+k = P−k −P−k

(h′x(x−k )

)T(Sk)−1h′x(x−k )P−k (30)

where f ′x,h′x denote the Jacobian matrix and f ′′x ,h

′′x represent

the Hessian matrix. Comparing the prediction part of EKF2algorithm with Eqs. (16) and (17), the EKF2 just estimatesthe mean and covariance of the 2nd order Taylor expansion.When only the 1st order Taylor linearization is used, i.e., anormal EKF (EKF1) is applied, the Hessians f ′′x ,h

′′x are set

to zero. The additional term of EKF2 as compared to EKF1models the nonlinearity. When the 2nd order nonlinear term isnot trivial, EKF1 will fail.

B. Nonlinear Estimation Based on Simulation

As is well known, Kalman filters estimate only the meanand the covariance of the states instead of the entire probabilitydensity function. Figure 5 illustrates the performance of EKF1,EKF2 and particle filter for the addressed nonlinear problembased on a simulation. In the prediction part, the orientationevolves with respect to time and the acceleration rotates at eachtime, as shown in Fig. 2. For simplicity, the correction part ofthe filter is built by a known acceleration measurement withnoise. The specific force and orientation error are assumedzero mean white Gaussian distributed.

In Fig. 5, the red star represents a real acceleration. In eachtime sample, zero mean white Gaussian noises are added tothe orientation error and specific force. These uncertaintieswill cause the umbrella effect, as shown in Fig. 5a. The bluedots are the predicted acceleration of EKF1 and the greencircles are those from EKF2. Each dot represents one sample.The black asterisks are the mean outputs of a particle filter.The blue, green and black stars and the corresponding ellipsesare the estimates and 99.5% confidence ellipses for differentnonlinear filters in a specific sample. It is observed that thepredicted estimates of the EKF2 (green circles) are lower com-pared with those of the EKF1 (blue dots) and are comparativewith those of the particle filter. From the first thought we

(a) Predicted Acceleration

(b) Corrected Acceleration

Figure 5: Nonlinear filter performance: the red star is the realacceleration, blue dots are the estimates from EKF1, greencircles are the estimates from EKF2, black asterisks are themean outputs from a particle filter. The blue, green and blackstars are the estimates of different filters at one sample andthe corresponding ellipses represents the corresponding 99.5%confidence ellipses at this sample.

expected that the estimate of EKF2 could compensate thebias and be more close to the real acceleration. However,the EKF only gives the mean and the covariance estimateof the real distribution, i.e., a single point (blue or green)represents the mean from EKF1 or EKF2 in that time sample.Hence, EKF2 estimates the downward bias of the umbrellaproblem more correctly as compared to EKF1. Especially

when we combine the mean and covariance estimate, theellipses, the real acceleration is within the confidence areaof EKF2 whereas this is not the case for EKF1. Therefore,the predicted estimate of EKF2 provides a more correct meanas well as covariance estimate. Moreover, the performance ofEKF2 is close to that of the particle filter, which means that theEKF2 can model the nonlinearity very well. Figure 5b showsthe performance after correction. It is demonstrated that theEKF1 estimate has always a bias in height whereas the EKF2provides a much better estimate with a reasonable covarianceestimate.

Usually, a navigation or tracking solution combines inertialsensors and other systems. For example, in [2], the inertialsystem serves as a process model and a UWB system is appliedin the correction part of the filter. In order to focus on thenonlinear problem of strapdown integration, in the followingexperiment we only consider the prediction part of the Kalmanfilter, i.e., only the nonlinearity of the acceleration rotation isconsidered. In practice, the dead reckoning is very importantin the fusion since the inertial sensors have high frequencydata and other aiding sources usually provide relatively lowerfrequency data. So that a good dead reckoning output withrealistic covariance is necessary.

V. EXPERIMENTAL RESULTS

A. Inertial Measurements

The addressed nonlinearity problem is tested using anMEMS inertial sensor including 3D gyroscopes and 3D ac-celerometers, as shown in Fig. 1. The sensor unit is mountedon a stationary table, and 3 hours stationary data are measuredat a frequency of 200 Hz. Therefore, we have ground truth forevaluating the performance of the filters.

B. Process Model

As given in Eq. (1) and (2), the inertial measurementsinclude slowly time-varying biases. They should also be inthe state vector to be estimated. Therefore, a discrete-timenonlinear state-space model can be built and the state vec-tor consists of orientations, gyroscope biases, accelerometerbiases, 3D velocities and 3D positions

qnbk+1 = qnb

k � exp(T2ω

bnb,k) (31)

bbω,k = bb

ω,k−1 + wbω (32)

bba,k = bb

a,k−1 + wba (33)vnk = vn

k−1 + T · ank−1 + wv (34)

pnk = pn

k−1 + T · vnk−1 + T 2

2 · ank−1 + wp (35)

where x =[qnb,T bb,T

ω bb,Ta vn,T pn,T

]T, and T in

the process model is the sampling interval. ank results from

the highly nonlinear mapping, the rotation of acceleration, ascalculated in Eq. (8). To focus on this nonlinear problem, hereonly the process model is given. As introduced earlier, themeasurement part for a navigation and tracking scheme canbe different aiding sensors, e.g., barometers, which is not inthe scope of this paper.

C. Dead Reckoning Test

To examine the different linearization approaches for theaddressed nonlinear problem, we test EKF1 and EKF2 usingthe above process model. The strapdown integration starts withinitial orientation uncertainty of 1 degree, and other initialstates are assumed to be known exactly. 200 iterations are run,i.e., 1 second dead reckoning. Then this procedure is repeated5000 times for different datasets so that the statistics of thedead reckoning can be obtained.

Figure 6 shows the height and covariance estimate fromEKF1 and EKF2, respectively. To show the results clearly, here100 dead reckoning samples are shown in the plot. It should benoted that the acceleration in navigation frame is

[0 0 0

]Tsince we have stationary data. The true mean of the rotatedacceleration is not zero in vertical direction. From the plot itis observed that both outputs from EKF1 and EKF2 are lowerthan zero. The EKF2 provides even lower estimate in verticaldirection compared to the EKF1. This result is consistent withthe simulation in Fig. 5a.

Figure 6: Height estimate and covariance.

D. Evaluating the Covariance Estimate

We are interested in the covariance estimate since a goodestimator should provide both the correct mean and the correctcovariance from the prediction, by which the correction partof the filter can work properly when we combine inertialmeasurements with other sensors. In the following, we choosea normalized error as the criteria to evaluate the estimate.Assume the estimate after dead reckoning is defined as ex,which also represents the estimated error since it is a stationarytrial, and the covariance output is defined as P. We can define anormalized error (NE) and a normalized error squared (NES)similar to the standard quantities for evaluating the perfor-mance of a filter, normalized residual (NR) and normalizedinnovation squared (NIS). For a good estimator, NE shouldbe a multiple-dimensional normal Gaussian distribution, and

NES should be a chi-square distribution.

NE , eTx ·P

− 12 ∼ N (0,1) (36)

NES , eTx ·P−1 · ex ∼ χ2

k (37)

where k is the freedom of the chi-square distribution and itis the dimension of the state vector. Here NE and NES areevaluated in the state elements of orientation, velocity andposition, so that NES should be a chi-square distribution with9 degrees of freedom.

In Fig. 7, the normalized histogram of the orientationelements in NE is plotted with a normal Gaussian distribution.Since the same 1st order linearization model for the orientationintegration is used in both of the EKF1 and EKF2, thesame performance for the orientation elements is obtainedfor both filters. So that Fig. 7 represents both the resultsfrom EKF1 and EKF2. The histogram is plotted based on the5000 dead reckoning trials, which uses different real datasets.It is observed that each of the orientation elements in NEcomplies with the normal Gaussian distribution. This resultdemonstrates that the EKF1 model for the orientation evolutionwith time using gyroscope measurements is a good model.

−6 −4 −2 0 2 4 60

0.2

0.4

0.6

Normalized Histogram of NE for Orientation Elements

−6 −4 −2 0 2 4 60

0.2

0.4

0.6

−6 −4 −2 0 2 4 60

0.2

0.4

0.6

Figure 7: Normalized histogram of NE for the orientationelements (same for EKF1 and EKF2): red plots are normalGaussian distributions, blue plots are normalized histogram ofthe orientation elements in NE.

Figure 8 shows the normalized histogram of the positionelements in the NE from EKF1 and EKF2, respectively. Thereare minor differences between the two filters at the positionx and y elements. For the position z element, it is observedthat EKF1 has a biased error, whereas the EKF2 provides abetter estimate, which is more close to the normal Gaussiandistribution. Recall Fig. 4, the downward bias in the verticalacceleration will cause the bias in the vertical position. Fig-ure 8 proves that EKF1 does not solve this problem, whereasthe EKF2 can fix it by providing a better mean estimate andcorresponding covariance estimate. It can be expected thatby modeling the nonlinearity of acceleration rotation, correctheight estimate can be obtained by EKF2.

For more clear about the benefit of EKF2, the NES quantityis further checked as shown in Fig. 9. By the comparison ofEKF1 and EKF2, the NES from EKF2 looks more like a chi-square distribution than the NES from EKF1. It shows thatthe covariance estimate from EKF1 does not match the meanestimate error, i.e., the EKF1 can not model the downwardbias due to the nonlinearity correctly, whereas the EKF2 hasa better estimate for the nonlinear problem of accelerationrotation.

Moreover, to evaluate the covariance estimate, noncredibil-ity Indices (NIC) [12] can be calculated as

NCI ,10

M

M∑i=1

log10

[eTxP−1ex

eTxΣ−1ex

]i

(38)

=10

M

M∑i=1

log10

[eTxP−1ex

]i− 10

M

M∑i=1

log10

[eTxΣ−1ex

]i

(39)

where Σ is the true covariance of the estimate error, which isapproximated by the 5000 samples due to the known stationarytrial. M is the number of samples. As shown in Eq. (39), theNCI should be approximated to zero when the NES of thefilter is close to the true error, which means the estimatoris perfectly credible. The NCI for EKF1 is equal to 2.3 andthe NCI for EKF2 is 0.3, which demonstrates EKF2 has asignificant improvement compared to the EKF1.

VI. CONCLUSIONS

In this paper, we address the nonlinear problem of acceler-ation rotation in strapdown inertial integration. The estimateusing the traditional 1st order linearization does not modelthe downward bias in the vertical acceleration caused by thisnonlinearity. This brings fast-growing biases in the verticalvelocity and position after integration. It is very important tomodel this nonlinearity to have a realistic model of inertialsensors to be able to perform accurate height tracking. We pro-pose to use the 2nd order Taylor expansion to approximate thisnonlinearity and apply an EKF2 filter. Moreover, traditionalKalman filter solutions such as in EKF1 the error propagationis separated from the state propagation. Whereas in EKF2,the state propagation depends on the error covariances, whichmakes the error modeling more important. A dead reckoningtest using stationary measurements from an MEMS inertialsensor demonstrates that EKF2 can better model this nonlinearmapping.

In the future, more advanced nonlinear filters can be con-sidered, e.g., particle filter, and the nonlinearity modelingproposed in this paper can be applied in different fusionapplications to provide a good position estimate.

ACKNOWLEDGEMENT

The research leading to these results has received fundingfrom the EU’s Seventh Framework Programme under grantagreement no. 238710. The research has been carried out inthe MC IMPULSE project: https://mcimpulse.isy.liu.se.

−6 −4 −2 0 2 4 60

0.2

0.4

0.6

Normalized Histogram of NE for Position Elements

−6 −4 −2 0 2 4 60

0.2

0.4

0.6

−6 −4 −2 0 2 4 60

0.2

0.4

0.6

(a) EKF1

−6 −4 −2 0 2 4 60

0.2

0.4

0.6

Normalized Histogram of NE for Position Elements

−6 −4 −2 0 2 4 60

0.2

0.4

0.6

−6 −4 −2 0 2 4 60

0.2

0.4

0.6

(b) EKF2

Figure 8: Normalized histogram of NE for the position elements: red plots are normal Gaussian distributions, blue plots arenormalized histogram of the position elements in NE.

−20 0 20 40 60 80 100 1200

0.02

0.04

0.06

0.08

0.1

0.12Normalized Histogram of NES

(a) EKF1

−20 0 20 40 60 80 100 1200

0.02

0.04

0.06

0.08

0.1

0.12Normalized Histogram of NES

(b) EKF2

Figure 9: Normalized histogram of NES: red plots are a chi-square distribution with 9 dimension of freedom, blue plots arenormalized histograms of NES including orientation, velocity and position.

REFERENCES

[1] A. Vydhyanathan, M. Kok, and K. L. Johnson, “A low-cost navigationsystem: high dynamic flight test results,” in Proc. of The EuropeanNavigation Conference on Global Navigation Satellite Systems, Braun-schweig, Germany, Oct. 2010.

[2] J. D. Hol, F. Dijkstra, H. Luinge, and T. B. Schon, “Tightly coupledUWB/IMU pose estimation,” in Proc. of IEEE International Conferenceon Ultra-Wideband 2009, Vancouver, Canada, Sep. 2009, pp. 688–692.

[3] M. Tanigawa, H. Luinge, L. Schipper, and P. Slycke, “Drift-free dynamicheight sensor using MEMS IMU aided by MEMS pressure sensor,” inProc. of 5th Workshop on Positioning, Navigation and Communication,2008, Hannover, Germany, Mar. 2008, pp. 191–196.

[4] F. Gustafsson, Statistical Sensor Fusion. Studentlitteratur, 2010.[5] D. Simon, Optimal State Estimation: Kalman HInfinity, and Nonlinear

Approaches. John Wiley & Sons, 2006.[6] B. Ristic, S. Arulampalam, and N. Gordon, Beyond Kalman Filter:

Particle Filters for Tracking Applications. Artech House Radar Library,2004.

[7] D. H. Titterton and J. L. Weston, Strapdown Inertial Navigation Tech-nology. Peter Peregrinus Ltd., 1997, IEE radar, sonar, navigation andavionics series.

[8] J. E. Bortz, “A new mathematical formulation for strapdown inertialnavigation,” IEEE Transactions on Aerospace and Electronic Systems,vol. 7, no. 1, pp. 61–66, 1971.

[9] M. D. Shuster, “A survey of attitude representations,” The journal of theAstronautical Sciences, vol. 41, no. 4, pp. 439–517, Oct. 1993.

[10] J. R. Magnus and H. Neudecker, Matrix Differential Calculus withApplications in Statistics and Econometrics. John Wiley & Sons, 1999.

[11] P. S. Maybeck, Stochastic Models, Estimation, and Control. AcademicPress, 1979, vol. 2.

[12] X. Li and Z. Zhao, “Measuring estimator’s credibility: Noncredibilityindex,” in Proc. of The 9th International Conference on InformationFusion, Florence, Italy, Jul. 2006.