a conventional inertial measurement unit (imu) constrained angular motion estimation ... · 2013....

Constrained Angular MotionEstimation in a Gyro-Free IMU

EZZALDEEN EDWAN, Member, IEEESTEFAN KNEDLIK, Member, IEEEOTMAR LOFFELD, Senior Member, IEEEZESSGermany

In this paper, we present an extended Kalman filter

(EKF)-based solution for the estimation of the angular motion

using a gyro-free inertial measurement unit (GF-IMU) built of

twelve separate mono-axial accelerometers. Using such a GF-IMU

produces a vector, which we call the angular information vector

(AIV) that consists of 3D angular acceleration terms and six

quadratic terms of angular velocities. We consider the multiple

distributed orthogonal triads of accelerometers that consist

of three nonplanar distributed triads equally spaced from a

central triad as a specific case to solve. During research for the

possible filter schemes, we derived equality constraints. Hence we

incorporate the constraints in the filter to improve the accuracy

of the angular motion estimation, which in turn improves the

attitude accuracy (direction cosine matrix (DCM) or quaternion

vector).

Manuscript received July 8, 2008; revised June 5, 2009; released forpublication October 10, 2009.

IEEE Log No. T-AES/47/1/940050.

Refereeing of this contribution was handled by D. Gebre-Egziabher.

This work was partially supported by the German ResearchFoundation (DFG) under Grant KN 876/1-2.

Authors’ address: Center for Sensorsystems (ZESS), Universityof Siegen, Paul-Bonatz Str. 9-11, Siegen 57068, Germany, E-mail:([email protected]).

0018-9251/11/$26.00 c° 2011 IEEE

I. INTRODUCTION

A conventional inertial measurement unit (IMU)consists of three gyros for measuring rotationalmotion and three accelerometers for measuring linearacceleration. In contrast to a conventional IMU, agyro-free IMU (GF-IMU) uses a configuration ofaccelerometers only to measure acceleration androtational motion of a rigid body in 3D space. Inprinciple it benefits from an effect known as lever-armeffect. To have this effect, the accelerometers mustbe distributed in distance over the body. In orderto distinguish it from a conventional gyro IMU,researchers use other names interchangeably todescribe its functionality. The most commonly usednames are GF-IMU [1—3], nongyroscopic inertialmeasurement unit (NGIMU) [4, 5], accelerometerbased IMU, and all-accelerometer IMU [6—8].There exists a variety of reasons for using

distributed accelerometers to estimate the angularvelocity vector. Most of the GF-IMU publicationslist some of the justifications [1, 3, 4, 6, 8—13]. Inshort, the gyroscope typically has the disadvantageof complicated manufacturing techniques, high cost,high power consumption, large weight, large volume,and limited dynamic range [11]. In addition, low-costmicro-electro mechanical system (MEMS) gyroscopeshave more inherent physical complexities thanaccelerometers [1]. On the other hand, accelerometersare smaller, less costly, and less power consumingthan comparable gyros.Using certain configurations of twelve separate

mono-axial accelerometers produces an angularinformation vector (AIV) that consists of a 3D angularacceleration vector and six quadratic terms of angularvelocities, as is shown later. Unfortunately the angularvelocity vector is not expressed explicitly in the AIV.The motivation for this research is that little attentionhas been given to the sensor fusion of the AIV.The approach of using a double integration of theangular acceleration as described in [14] to achievethe orientation has been proven to be theoreticallyfeasible, but it has a faster error growth rate comparedwith a conventional inertial navigation system (INS).The advantage of only using the angular accelerationvector has a little value compared with the need fora double integrator. Another problem with such anapproach is the need to initialize the angular velocityto be able to integrate the angular acceleration.Continuous research efforts have been conductedto benefit from the other components in the AIV.Recently Park [1] aided the angular accelerationvector by three cross-quadratic terms of angularvelocity using an extended Kalman filter (EKF),but his scheme is not using all possible quadraticangular velocity terms as it is possible to get threemore quadratic terms. Furthermore, such a schemesuffers from an observability problem if one of the

596 IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL. 47, NO. 1 JANUARY 2011

angular velocity terms is zero. Parsa [6] obtained thenine-element AIV using twelve accelerometers. Inthat work, the complete AIV is used, but the focusis on the optimal design of the GF-IMU, avoidingthe use of tri-axial accelerometers, and not on thefusion scheme. Zappa [15] presented the rules forusing twelve separate mono-axial accelerometersto obtain the AIV without inhibiting singularityof the coefficient matrix. Ying Kun [13] used thesame configuration of twelve accelerometers thatwe use in this paper to create a GF-INS fromaccelerometers that already exist in an automobile’sonboard control and safety system. Again his focuswas not on the sensor fusion scheme; rather he useda simple numerical approach, which may result inan imaginary solution. In our new EKF scheme, weuse all of the six quadratic terms of angular velocityas suggested by Zappa [15] for the deterministiccase but this time for the stochastic case, which isthe realistic one, and we benefit from the angularacceleration vector at the same time. In other words,we aid the integrated angular acceleration vector bysix quadratic terms of angular velocities using anEKF.The choice of the EKF for the fusion of the AIV is

a trivial choice because we get measurable terms thathave the nonlinear function of the angular velocitycomponents. The recently developed modificationsof the Kalman filter for nonlinear systems like theunscented Kalman filter (UKF) are also applicable.During the formulation of the EKF, we derivednonlinear equality constraints that can improve theestimation performance of the EKF, but the statemodel needs to be expanded as we explain later.Constrained Kalman filtering has proven to be anefficient technique for improving the estimated statevector if the constraints are handled properly.This paper is organized as follows. Section II

introduces the distributed orthogonal triad ofaccelerometers. Section III describes the multipledistributed accelerometer triad set configuration thatgives the AIV and presents a calibration procedure.Section IV describes the underlying three-statemodel EKF for estimating the angular change vector.Section V shows the three-state model simulationresults. Section VI shows how to use the angularchange vector to update attitude. Section VII derivesthe constraints. Section VIII shows how to handle theconstraints. Section IX shows simulation results forthe constrained model and analysis. Section X givesconclusions and suggestions for future work.

II. DISTRIBUTED ACCELEROMETER TRIADDEFINITION

Following [16] for the derivation of the lever-armeffect, we assume that we have a rigid body with

Fig. 1. Distributed orthogonal triad of accelerometers at point pb.

its body center at point ob = [0 0 0]T as shown inFig. 1. The measurable acceleration of a distributedorthogonal triad of accelerometers fixed at a rigidpoint pb = [px py pz]

T that is aligned with the bodyframe can be expressed as

abp = abo+ g

bo¡gb

p+ _!bib£pb+!bib £!bib £pb (1)

where

abo is the central acceleration vector,pb is the displacement vector from ob to pb equalspb¡ ob,gbo¡gb

pis the local gravity difference vector,

!bibis the angular velocity vector,_!bibis the angular acceleration vector,£ is the cross-product operation defined asskew-symmetric form

!bib =

264!x!y!z

375 , !bib£=264 0 ¡!z !y!z 0 ¡!x¡!y !x 0

375 :(2)

In (1), (2), the superscript b denotes that the quantityis expressed in the body frame. The gravitationaldifference vector can be ignored for relatively smalldistribution distance, and hence (1) simplifies to

abp ¼ abo + _!bib £pb +!bib£!bib£pb: (3)Since the angular velocity vector is not expressedexplicitly in the above equation and in reality theequation is stochastic, extra effort must be takento extract the angular velocity. One approach is tointegrate the angular acceleration to get the angularvelocity and to perform another integration to getthe attitude. An example of such an approach,where a Kalman filter is used with external GPSobservations, is given in [9]. The drawback ofsuch an approach is that attitude and positionerrors grow quickly because of error growth withintegration. In the following equations, we omit thebody superscript as all equations in this paper aregiven in the body frame. We rewrite (3) in matrix

EDWAN ET AL.: CONSTRAINED ANGULAR MOTION ESTIMATION IN A GYRO-FREE IMU 597

Fig. 2. Configuration of multiple distributed tri-axialaccelerometers.

form as264apxapyapz

375=264aoxaoyaoz

375

+

264¡!2y ¡!2z !x!y ¡ _!z !x!z + _!y

!x!y + _!z ¡!2z ¡!2x !y!z ¡ _!x!x!z ¡ _!y !y!z + _!x ¡!2x ¡!2y

375264pxpypz

375 :(4)

III. CONFIGURATION OF MULTIPLE DISTRIBUTEDTRI-AXIAL ACCELEROMETERS

We focus on configurations consisting of twelvemono-axial accelerometers that follow the rules listedby Zappa [15]. Without loss of generality, we considerthe set of four tri-axial accelerometers shown in Fig. 2as an example configuration that follows Zappa’s rulesfor extracting the AIV without inhibiting singularityof the coefficient matrix.Mainly we consider this configuration because

a minimum of twelve accelerometers are needed todetermine the magnitude of the angular velocity andits direction (algebraic sign cannot be determineduniquely as explained in [15]). The greatest amountof angular motion information, which is in thenine angular terms that we show next, can beextracted from this configuration. Moreover, thisconfiguration has, according to [3], a low geometricdilution of precision (GDOP) factor for bothangular and translational acceleration without thecentral accelerometer triad. In [8] it is stated thata configuration of twelve accelerometers is theonly known minimum number to both navigateand determine gravity in a gravitational fieldwith a uniform gradient. Finally yet importantly,it is the most practical configuration becauseIMUs exist in triads of gyros and accelerometers.

This specific configuration was analyzed byAlgrain [17] where he derived the followingrelations:

_!x = (aCz ¡ aAz )=2dC +(aAy ¡ aDy )=2dD

_!y = (aDx ¡ aAx )=2dD +(aAz ¡ aBz )=2dB

_!z = (aBy ¡ aAy )=2dB +(aAx ¡ aCx )=2dC

(5)

!x!y = (aBy ¡ aAy )=2dB +(aCx ¡ aAx )=2dC

!x!z = (aBz ¡ aAz )=2dB +(aDx ¡ aAx )=2dD

!y!z = (aCz ¡ aAz )=2dC +(aDy ¡ aAy )=2dD

(6)

!2x = (aBx ¡ aAx )=2dB +(aAy ¡ aCy )=2dC +(aAz ¡ aDz )=2dD

!2y = (aCy ¡ aAy )=2dC +(aAx ¡ aBx )=2dB +(aAz ¡ aDz )=2dD

!2z = (aDz ¡ aAz )=2dD +(aAx ¡ aBx )=2dB +(aAy ¡ aCy )=2dC:

(7)

The notation atriadaxis refers to the distributedaccelerometer measurement with a superscriptreferring to the triad location and a subscript referringto the axis index. For the case of a unique distributiondistance d for every distributed triad, the AIVbecomes:

_!x = (aCz ¡ aAz ¡ aDy + aAy )=2d

_!y = (aDx ¡ aAx ¡ aBz + aAz )=2d

_!z = (aBy ¡ aAy ¡ aCx + aAx )=2d

(8)

!x!y = (aBy ¡ aAy + aCx ¡ aAx )=2d

!x!z = (aBz ¡ aAz + aDx ¡ aAx )=2d

!y!z = (aCz ¡ aAz + aDy ¡ aAy )=2d

(9)

!2x = (aBx ¡ aAx ¡ aCy + aAy ¡ aDz + aAz )=2d

!2y = (aCy ¡ aAy ¡ aBx + aAx ¡ aDz + aAz )=2d

!2z = (aDz ¡ aAz ¡ aBx + aAx ¡ aCy + aAy )=2d:

(10)

All of the previous equations are linear combinationsof accelerometers measurements.

A. GF-IMU Error Calibration

A model that describes the sensor’s outputrelated to the reference value to be measured canapproximate the behavior of most of the inertialsensors. A proper model can be established basedon the data sheets provided with the sensor andusing the lab’s calibration equipment. The calibrationissue of a GF-IMU has been addressed in theliterature before. Park [1] gives a detailed descriptionof calibrating uni-axial accelerometers in a cubeconfiguration GF-IMU. However in our configurationwe recommend calibrating each tri-axial independentlyfor the following reasons.


1) Uni-axial calibration does not take intoaccount the cross-axis sensitivities, and hence anothercalibration for misalignment must be performed.2) In our derivation, we assumed that all

accelerometers are aligned with one frame called thebody frame. Therefore, the most convenient way tocalibrate is to find the misalignment of each of thefour triads from that common body frame.

The calibration of every accelerometer triad will bedone faster because we benefit from the symmetry inthe configuration by collecting measurements for thefour-accelerometer triads in every flipping position.Accordingly we suggest calibrating each tri-axialof the accelerometers as a whole group rather thancalibrating every single accelerometer independently.There are two types of errors that result in the

measurable AIV in the GF-IMU. The first type oferror is due to the tri-axial accelerometers’ errors, andthe second type of error is due to errors in distributedtriad locations in the geometric configuration. Theerror model of a tri-axial accelerometer is composedof different sources of error that can be generallyclassified into four parts: misalignment, scale factor,bias, and noise. We can calibrate for three typesof errors: scale factor parameters, misalignment ofthe accelerometers, and three bias parameters in asingle calibration procedure of each triad. We find the9-element sensitivity matrix whose diagonal elementscontain the scale factors and whose off-diagonalelements contain misalignment parameters. Likewisewe find the three bias elements. In matrix form theaccelerometer triad output is related to the referencespecific force as264zxzy

zz

375=264mxx mxy mxzmyx myy myzmzx mzy mzz

375264axayaz

375+264baxbaybaz

375+264vaxvayvaz

375 :(11)

There are many methods to solve for m and belements [18, 19]. Following a similar approach tothe one given in [19], we rearrange (11) into thefollowing form:

z =Oμ+ va

μ =

26664m1

m2

m3

ba

37775 , m1 =264mxxmxymxz

375

m2 =

264myxmyymyz

375 , m3 =264mzxmzymzz

375va = [vax vay vaz]

T

ba = [bax bay baz]T:

(12)

The observation matrix for every measurement isgiven by

O =

264ax ay az 0 0 0 0 0 0 1 0 00 0 0 ax ay az 0 0 0 0 1 00 0 0 0 0 0 ax ay az 0 0 1

375 :(13)

We stack N observation matrices from N observationsto form the total measurement vector and the totalobservation matrix shown next:2664

z1

...

zN

3775=264O

1

...

ON

375μ+2664v1a...

vNa

3775 , zt =Otμ+ vta:(14)

To have an observable system, we need a minimum offour measurements with proper values of the specificforce values. To extract the m and b elements, we useleast square estimation

μ̂ = (OtTOt)¡1OtTzt: (15)

For separation distances calibration, a turntableis used to measure the separation distance fromevery distributed accelerometer triad to the centralaccelerometer triad. Hence we have two optionshere:

1) Use the form of equations for the AIV that hasdifferent separation distances given in (5)—(7) andcompute the corresponding error covariance to beused later in the estimation.2) Instead of using different separation distances,

we can adjust the distance manually for everydistributed triad so that each is unique and validateit experimentally. Having a unique separation distancefrom every distributed tri-axial to the central tri-axialsimplifies computations. For this reason, we considerthe equally spaced triads configuration in the rest ofthis paper.

IV. THE THREE-STATE MODEL EKF

Based on the previously derived AIV (8)—(10),we can formulate the EKF setup. The quadraticterms do not give a unique angular velocity vectorsolution. Instead we get two solutions. In our setup,we consider only a fixed accelerometers configuration.For the determination of the algebraic sign in acompletely GF-IMU, there exist solutions usingaccelerometers that vibrate in a known matter to bodyas proposed initially by Merhav [5] and recentlyby Costello [10] who gave a solution that does notrequire integration. The conventional low-cost gyroscan be used to insure a correct sign convergencein the GF-IMU as shown in [20]. Other possible


integration schemes include GPS [9, 12] and tri-axialmagnetometers [7] to have a nondrifting orientationestimate. We are interested in estimating the angularvelocity component along each body axis in 3D.In reality the continuous angular velocity vector isreplaced with the angular change vector as given in(16) because the computerized implementation isdiscrete. The angular change is the sampled angularvelocity multiplied by the sampling time and is givenas

x= [x1 x2 x3]T = [¾x ¾y ¾z]

T: (16)

A. Initialization

The initial state vector can be set as

x̂+0 = Efx0g= [¾x0 ¾y0 ¾z0 ]T: (17)

The initial estimation error covariance is given as

P+0 = Ef(x0¡ x̂+0 )(x0¡ x̂+0 )Tg: (18)

B. Prediction

In discrete time the actual output of eachaccelerometer is the velocity change; hence the outputof (8) is the angular velocity change vector ®. Theprocess model based on Euler integration is

¾xk = ¾xk¡1 +®xk¡1¢t+wxk¡1

¾yk = ¾yk¡1 +®yk¡1¢t+wyk¡1

¾zk = ¾zk¡1 +®zk¡1¢t+wzk¡1 :

(19)

We define the following vector as the processinput

u= ®¢t= [®x¢t ®y¢t ®z¢t]T: (20)

Using (20), the process given in (19) has the linearform of

xk = Fk¡1xk¡1 +Gk¡1uk¡1 +wk¡1

Fk¡1 =Gk¡1 = I3£3:(21)

We assume that the uncertainty in the processis mainly due to the uncertainty in the angularvelocity change. In this work, we consider the errorof each accelerometer as white Gaussian noise forsimplicity. For an accelerometer error accountingfor the remaining bias, we developed a solutionutilizing the dynamic models to estimate the biasparameters in the nine angular information termsgiven in [21]. In that solution, the bias parametersand the angular acceleration vector are augmentedwithin the state-space model to form a 15-state model.For that process update, we used the Wiener processor simply the nearly constant acceleration model.When using such a model, all the bias parameters inthe AIV become observable under the condition thatthe angular acceleration has a non-zero magnitude.

The three-state model has the advantage of simplecalculations because only three states need to beestimated. Moreover there is no need to make anassumption about the dynamics of the motion,and hence such a solution fits most scenarios.Each accelerometer discrete time measurementis composed of true value plus a white noisecomponent.

ameas = atrue +wacc=p¢t: (22)

The white noise wacc has the unit of g=pHz, where

g is the gravity or its equivalent derivatives. Thediscrete white noise depends on the square root of thesampling time

p¢t. The measured velocity change of

each accelerometer is expressed as

¢ºmeas =¢ºtrue +waccp¢t: (23)

The variance Ra of each accelerometer measurementof velocity change is

Ra = Ef(¢ºmeas¡¢ºtrue)2g= Efw2accg¢t: (24)All accelerometers are modeled with a commonupper bound of the noise variance, as they wouldbe in reality. The error corrupting the angularacceleration vector given in (8) is inherited from theaccelerometers’ errors, as it is a linear combinationof accelerometers. This combination results ina correlated process noise, and its covariance iscomputed as

Qk¡1 = (¢t)2μRad2

¶264 1 ¡14 ¡ 14

¡ 14 1 ¡ 14¡ 14 ¡ 14 1

375 : (25)The predicted or a priori estimation error covariance isupdated as

P¡k = Fk¡1P+k¡1F

Tk¡1 +Qk¡1: (26)

The predicted or a priori state estimate is updated as

x̂¡k = Fk¡1x̂

+k¡1 +Gk¡1uk¡1: (27)

C. Measurement Update

We plug the measured velocity changes of theaccelerometers in the six quadratic terms in to (9)and (10) and multiply the resulting sum by thesampling time to derive the measurement of thestate vector. Considering the existence of whiteGaussian noise in each accelerometer measurement,the observation inherits also a white Gaussian noisev vector

yk= hk(x,v) = [x1x2 x1x3 x2x3 x

21 x

22 x

23]Tk

+[v1 ¢ ¢ ¢v6]Tk : (28)


Fig. 3. Angular change profile.

The Jacobian of the measurement vector is computedas

Hk =@hk(x,v)@x

¯̄̄̄x=x̂¡k

=

26666666664

x2 x1 0

x3 0 x10 x3 x22x1 0 0

0 2x2 0

0 0 2x3

37777777775x=x̂¡k

:

(29)The measurement is correlated, and its covariance iscomputed as

Rk = (¢t)2μRad2

¶266666666664

1 1414 0 0 ¡ 12

14 1

14 0 ¡ 12 0

14

14 1 ¡ 12 0 0

0 0 ¡ 12 32 ¡ 12 ¡ 120 ¡ 12 0 ¡ 12 32 ¡ 12¡ 12 0 0 ¡ 12 ¡ 12 32

377777777775:

(30)

The process noise is correlated with the measurementnoise, and its cross-covariance is computed as

EfwkvTj g=Mk±k¡j+1

Mk = (¢t)2μRad2

¶(31)

£

264¡14

14 0 0 ¡ 12 12

14 0 ¡ 14 12 0 ¡ 120 ¡ 14 14 ¡ 12 12 0

375 :The Kalman gain is updated since we consider across-covariance to have a better performance as

shown in the literature covering the Kalman filtering,e.g., [22].

Kk = (P¡k H

Tk +Mk)(HkP

¡k H

Tk +HkMk +M

Tk H

Tk +Rk)

¡1:

(32)The corrected or a posteriori estimation errorcovariance is updated as

P+k = P¡k ¡Kk(HkP¡k +MTk ): (33)

The corrected or a posteriori state estimate is updatedas

x̂+k = x̂

¡k +Kk(yk ¡ hk(x̂

¡k ,0)): (34)

V. THREE-STATE MODEL SIMULATION RESULTS

A. The Trajectory Profile

In simulations we consider the trajectory ofstabilizing an object subject to parasitic vibrations inone dimension [23] combined with a constant angularrotation in the other dimension and no motion in thethird dimension. The mathematical description of thisangular motion is given as

!(t) = [!x 0 !z sin(2¼ft)]T

¾k =¢t!(tk):(35)

The sampling time ¢t is chosen to be 0.01 s, andthe frequency of vibration f is set to 0.1 Hz. Themagnitudes of angular velocity components areset to !x = 3:29 rad/s and !z = 0:658 rad/s. Thetotal simulation time is 20 s, and the separationdistance is set to 0.1 m. We seeded a white noiseerror of 50 ¹g=

pHz (velocity random walk) in each

accelerometer measurement and extracted the AIVbased on (8)—(10). Fig. 3 shows a plot of the angularchange profile.


Fig. 4. Angular change estimation errors.

Fig. 5. Angular change estimation errors with bias in accelerometers.

We plotted the estimation errors for the trajectoryas shown in Fig. 4.

B. Effect of Noncalibrated Bias

To test the effect of noncalibrated bias, we addeda random constant bias generated from a Gaussiandistribution with one standard deviation of 50 ¹gto each noisy accelerometer measurement describedpreviously. We repeated the same setup as before andcreated a plot of the estimation errors shown in Fig. 5.One important note here: at a relatively high

sampling rate (e.g., 0.01 s or more), the magnitudeof the error due to white noise is about 10 times themagnitude of error due to the remaining bias for this

example of accelerometer specifications. Consequentlyfor this sampling rate, the noise error dominates thebias error in this accelerometer, and for this scenariothe EKF model works without a big difference fromthe one without bias. Hence the use of a white noiseerror model can be justified considering that the Kalmanfilter will tolerate such a small remaining bias error.

C. Effect of Improper Initialization

Here we investigated the effect of improperinitialization on the filter performance. We simulatedthe previously described setup under initializationerrors. The initialization error is set to be equal to0.01 rad in each of three angular change components.


Fig. 6. Angular change estimation errors for improper initialization.

In general a proper sign initialization is important toavoid convergence to the opposite sign. The filterconverges to the reference angular change profilebecause of the availability of measurements ofquadratic angular change as shown in Fig. 6.

VI. ATTITUDE UPDATE USING ROTATION ANGLEVECTOR

The rotation angle vector is used to update theattitude direction cosing matrix (DCM) as advocatedby Bortz [24] or to update the quaternion vector[25]. In this section, we show how to update theDCM using a rotation angle vector; however, thesame principle applies to quaternion’s update. Theattitude DCM, denoted next as C, is the attitude ofthe body with respect to the fixed inertial frame. Thematrix Ak is another DCM that transforms from bodycoordinates at the (k+1)th computer cycle to bodycoordinates at the kth computer cycle using the anglerotation vector as shown in [24]—[26]. The continuousform of the DCM differential equation and its discretetime solution are given as

_C = C!bib£

Ck+1 = CkAk, Ak = eR tk+1tk

!bib£dt:

(36)

Next we consider the case where the direction of theangular rate vector !bib remains fixed in space, i.e.,nonconing motion. Assuming that the angular velocityvector has little change during the update interval, wecan approximate the angle rotation by the previouslyestimated angular change ¾.Z tk+1

tk

!bibdt ¼ ¾: (37)

This transformation matrix Ak is computed as

Ak = I+sin¾¾[¾£] + 1¡ cos¾

¾2[¾£]2

¾ =q¾2x +¾2y +¾2z :

(38)

Equation (38) is composed of either linear orquadratic terms of the angular rotation vector. It canbe approximated considering the first three terms asgiven next:

Ak ¼

266666641¡ (¾

2y +¾

2z )

2

¾x¾y2¡¾z

¾x¾z2+¾y

¾x¾y2

+¾z 1¡(¾2x +¾

2z )

2

¾y¾z2¡¾x

¾x¾z2¡¾y

¾y¾z2

+¾x 1¡(¾2x +¾

2y )

2

37777775 :

(39)

VII. CONSTRAINTS DERIVATION

In this section we derive the equality constraintsthat exist in the model because of orthogonalaccelerations. For a nonconing motion, the centripetalacceleration vector !bib£!bib £pb and the tangentialacceleration vector _!bib£pb in (1) are orthogonal. Thetwo vectors are represented as

ac =

264¡(!2y +!

2z )px+!x!ypy +!x!zpz

!x!ypx¡ (!2z +!2x )py +!y!zpz!x!zpx+!y!zpy ¡ (!2x +!2y )pz

375 (40)

at =

264¡ _!zpy + _!ypz_!zpx¡ _!xpz¡ _!ypx+ _!xpy

375 : (41)EDWAN ET AL.: CONSTRAINED ANGULAR MOTION ESTIMATION IN A GYRO-FREE IMU 603

We use three vectors as a basis set that spans allpossible distributed positions264pxpy

pz

375 28>:264100

375 ,264010

375 ,264001

3759>=>; : (42)

Applying the inner product of the two vectorsgiven in (40) and (41) with three different values of[px py pz]

T as given in (42) produces the followingset of equations:264¡(!

2y +!

2z )

!x!y

!x!z

375¯264 0_!z¡ _!y

375= 0 (43)264 !x!y¡(!2z +!2x )

!y!z

375¯264¡ _!z0_!x

375= 0 (44)264 !x!z!y!z¡(!2x +!2y )

375¯264 _!y¡ _!x0

375= 0: (45)After computing the inner product in the aboveequations, we get

!x!y _!z ¡!x!z _!y = 0!y!z _!x¡!x!y _!z = 0!x!z _!y ¡!y!z _!x = 0:

(46)

The last constraint is dependent on the first two.Therefore, the remaining two independent constraintsare

!x!y _!z ¡!x!z _!y = 0!y!z _!x¡!x!y _!z = 0:

(47)

VIII. HANDLING OF CONSTRAINTS WITHIN THEEKF

We derived two constraints that need to beintegrated within the EKF algorithm. The constraintequations shown in (47) contain angular accelerationcomponents. The derivation of the angularacceleration vector from the angular velocity vector bydifferentiation gives a noisy value; hence we augmentthe angular acceleration in the state vector. Moreoverthe derivation of the Jacobian of the constraints willbe much easier. Using the angular change vectoraugmented with the angular velocity change vectoras a state vector results in the following state vector:

x= [x1 ¢ ¢ ¢x6]T = [¾x ¾y ¾z ®x ®y ®z]T:(48)

A. Initialization

We initialize the state vector and the estimationerror covariance in a similar way to (17) and (18).

B. Prediction

We use a nearly constant angular velocitychange state-space model, which is also known asWiener-sequence model, for the process update. Itassumes that the angular velocity change incrementis an independent (white noise) process. Othersophisticated models for target dynamics which canbe used for the process update are surveyed in [27].As discussed previously, using this model, we canestimate the bias parameters in the AIV when they areaugmented to the state vector. This model is expressedin discrete time as

xk = F0k¡1xk¡1 +G

0k¡1wk¡1

F 0k¡1 =·I3£3 ¢tI3£303£3 I3£3

¸G0k¡1 =

·¢tI3£3I3£3

¸:

(49)

This process noise covariance is computed as

Q0k¡1 =G0k¡1cov(wk¡1)G

0k¡1

T: (50)

The a priori estimation error covariance is updated as

P¡k = F0k¡1P

+k¡1F

0k¡1

T+Q0k¡1: (51)

The a priori state estimate is updated as

x̂¡k = F

0k¡1x̂

+k¡1: (52)

C. Measurement Update

The whole AIV given in (8)—(10) is used to derivethe measurement of the new state vector. Consideringthe existence of a white Gaussian noise in eachaccelerometer measurement, the observation inheritsalso white Gaussian noise v vector

y0k= h0k(x,v)

= [x4 x5 x6 x1x2 x1x3 x2x3 x21 x

22 x

23]Tk

+[v1 ¢ ¢ ¢v9]Tk : (53)The Jacobian of the new measurement model H 0kis expressed in terms of the Jacobian of three-statemodel H, given in (29)

H 0k =@h0k(x,v)@x

¯̄̄̄x=x̂¡k

=·03£3 I3£3H 06£3

¸x=x̂¡k

: (54)

The measurement covariance R0k of the new modelis computed based on the previously derivedmeasurement covariance Rk, cross-correlatedcovariance Mk, and covariance Qk¡1 of the three-statemodel

R0k =·Qk¡1=(¢t)

2 Mk=¢t

MTk =¢t Rk

¸: (55)

The Kalman gain is updated as

Kk = P¡k H

0kT(H 0kP

¡k H

0kT+R0k)

¡1: (56)


The a posteriori estimation error covariance can beupdated as

P+k = P¡k ¡KkH 0kP¡k : (57)

The a posteriori state estimate is updated as

x̂+k = x̂

¡k +Kk(y

0k¡h0k(x̂¡k ,0)): (58)

D. Handling of the Nonlinear Equality Constraints

There are many approaches for handling equalityconstraints in the Kalman filtering literature. Commonapproaches include pseudomeasurement, projection,and model reduction approaches. Due to thenonlinearity of the constraints, model reduction is notapplicable in our system. In the pseudomeasurementapproach, also known as the pseudoobservationapproach, each nonlinear constraint is modeledas an observation with additive white Gaussiannoise to relax the rigidity of the constraints. Anexample of that approach for the use of the kinematicconstraint is described in [28] with the decreasingcovariance rationale which was introduced to relaxthe rigidity of the constraints. In the projectionapproach, the unconstrained estimate is projectedonto the constraints surface as developed by Simon[29] using its second-order nonlinear extension [30].The UKF, which has proven to be superior to theEKF in the case of significant nonlinearities, canalso be applied to incorporate nonlinear constraintsto achieve a better estimation performance eitherusing the pseudomeasurement approach or projectionfunction [31]. However, we choose an algorithm,which might be computationally less demandingcompared with other algorithms. In our simulationswe use the smoothly constrained Kalman filter(SCKF) developed by Geeter for applying nonlinearconstraints because nonlinear constraints are differentfrom linear constraints due to truncation andbase-point errors resulting from EKF linearization[31, 32]. Essentially the SCKF algorithm is classifiedas a pseudomeasurement approach combined withthe iterated EKF (IEKF), which iteratively appliesnonlinear observations to reduce truncation errors.In every time step, each constraint is linearized withthe last available estimate and applied iteratively withexponentially decreasing uncertainty. The smoothapplication of constraints is motivated by the fact thatapplying a measurement of variance ³ is equivalentto applying the same measurement k times each withvariance k³. The equality constraints function and itsJacobian matrix, considering the new state vector, arederived from (47) as

ec(x) =·x1x2x6¡ x1x3x5x2x3x4¡ x1x2x6

¸= 0

Ec =@ec(x)@x

=·x2x6¡ x3x5 x1x6 ¡x1x5 0 ¡x1x3 x1x2¡x2x6 x3x4¡ x1x6 x2x4 x2x3 0 ¡x1x2

¸: (59)

The following algorithm summarizes how toincorporate the constraints after measurement updateusing the SCKF

x̂= x̂+k , P = P+k

for c= 1,2

fi = 0, e(x) = rowc(ec(x)), E = rowc(Ec)jx̂³0 = °EPE

T, s=maxj(EjPjjEj)=EPET

while (s < sth)

f³c = ³0e¡i, E = rowc(Ec)jx̂K = (PET)(EPET+ ³c)

¡1

x̂= x̂¡Ke(x̂)P = (I6£6¡KE)Pi= i+1

s=maxj(EjPjjEj)=EPET

g end while g end forx̂+k = x̂, P

+k = P:

(60)

The function rowc finds the cth row of ec(x) andEc to apply constraints sequentially. The value ofthe fraction factor ° for the initial variance needs tobe verified by simulations. The relative strength scompares the variance in the denominator, which isa sum of terms, to one of the largest terms formingthat variance. The symbol sth is a termination criterionfor the application of constraints, which Geeter setsto be sth = 100 as a practical value. To avoid anyconvergence problems, we apply those constraintsafter sufficiently applying the observations. For therapidly changing angular velocity profile, it is difficultto estimate the angular velocity change; consequentlyin such situations the constraints are not applied.

IX. CONSTRAINED MODEL SIMULATION RESULTSAND ANALYSIS

A similar setup to the one described in Section Vis repeated here for the constrained model. In thefollowing plots, we compare the performance of theEKF constrained model with the EKF unconstrainedmodel. We choose a constant angular velocitytrajectory and we set the separation distance d to 1 m.

!(t) = ![1 1 1]T: (61)


Fig. 7. RMSE of angular change vector.

Fig. 8. RMSE of ¾x.

TABLE IAccelerometers Categories

PerformanceParameter Consumer Automotive Tactical

Noise Floor VRW(¹g=

pHz)

2000 1000 500

The angular velocity along each component is setto ! = 3:29 rad/s. Three levels of white noise errorare considered in the simulation, namely, tactical,automotive, and consumer grade, as categorized

by Gebre-Egziabher [33] with a doubling of theconsumer-grade noise level to distinguish it from theautomotive grade as shown in Table I. We evaluatethe improvement in the estimation performanceby computing the root mean square error (RMSE)over the Monte Carlo runs. Clearly there is animprovement due to the use of the dynamic modelover the three-state model.The RMSE is computed over 500 Monte Carlo

runs. Due to space limitations, we show here thesimulation results in Figs. 7—10 using the tacticalaccelerometers category only. We applied theconstraints directly after 10 s had passed to distinguish


Fig. 9. RMSE of ¾y .

Fig. 10. RMSE of ¾z .

TABLE IIComparison of Steady State RMSE for Different Accelerometers Categories

d = 1 m Tactical Grade Automotive Grade Consumer Grade

EKF Model

Term Unconstrained Constrained Unconstrained Constrained Unconstrained Constrained

¾ [rad] 4:09£ 10¡5 8:54£ 10¡6 8:44£ 10¡5 1:47£ 10¡5 1:66£ 10¡4 2:64£ 10¡5¾x [rad] 2:38£ 10¡5 4:97£ 10¡6 4:74£ 10¡5 8:17£ 10¡6 9:94£ 10¡5 1:55£ 10¡5¾y [rad] 2:29£ 10¡5 4:87£ 10¡6 4:84£ 10¡5 8:53£ 10¡6 9:32£ 10¡5 1:50£ 10¡5¾z [rad] 2:41£ 10¡5 4:95£ 10¡6 5:04£ 10¡5 8:74£ 10¡6 9:42£ 10¡5 1:52£ 10¡5

between the improvement due to use of the dynamicmodel and the improvement due to the application ofconstraints. The RMSE of the angular change

components and the angular change vector aretabulated in Table II for the three categories ofaccelerometers.


For all categories of accelerometers, there was aclear superiority of the constrained model over thethree-state model in the steady-state RMSE. Typicallywe should have a similar improvement reflected in theRMSE of each estimated angular change componentdue to the symmetry of the GF-IMU configuration,equal signal input in the 3D components, andsymmetry of the constraints. The simulation resultsare in agreement with this.

X. CONCLUSION

We proposed two EKF models for the estimationof the angular motion from a multiple distributedtri-axial accelerometers measurements in a GF-IMU.The performance of the constrained EKF wasremarkably better than the unconstrained EKF. Afuture work is to incorporate rotating accelerometersin the GF-IMU and to find the corresponding properfilter scheme.

ACKNOWLEDGMENTS

We greatly appreciate the support of the GermanAcademic Exchange Service (DAAD) for the doctoralwork of Ezzaldeen Edwan within the InternationalPostgraduate Programme (IPP) Multi Sensorics.

REFERENCES

[1] Park, S., Tan, C-W., and Park, J.A scheme for improving the performance of agyroscope-free inertial measurement unit.Sensors and Actuators A: Physical, 121 (2005), 410—420.

[2] Chen, J. H., Lee, S. C., and DeBra, D. B.Gyroscope free strapdown inertial measurement unit bysix linear accelerometers.Journal of Guidance Control and Dynamics, 17 (1994),286.

[3] Hanson, R.Using multiple MEMS IMUs to form a distributed inertialmeasurement unit.Master thesis, School of Engineering and Management,Air Force Institute of Technology, Wright-Patterson AirForce Base, 2005.

[4] Wang, Q., Ding, M., and Zhao, P.A new scheme of non-gyro inertial measurement unit forestimating angular velocity.In Proceedings of the 29th Annual Conference of theIEEE Industrial Electronics Society (IECON ’03), 2003,1564—1567.

[5] Merhav, S. J.A nongyroscopic inertial measurement unit.Journal of Guidance, Control, and Dynamics, 5 (1982),227—235.

[6] Parsa, K., Lasky, T. A., and Ravani, B.Design and implementation of a mechatronic,all-accelerometer inertial measurement unit.IEEE/ASME Transactions on Mechatronics, 12 (2007),640—650.

[7] Wei Tech, A., Khosla, P. K., and Riviere, C. N.Kalman filtering for real-time orientation tracking ofhandheld microsurgical instrument.In Proceedings of IEEE/RSJ International Conference onIntelligent Robots and Systems (IROS 2004), vol. 3, 2004,2574—2580.

[8] Zorn, A. H.A merging of system technologies: All-accelerometerinertial navigation and gravity gradiometry.In Proceedings of IEEE Position Location and NavigationSymposium (PLANS 2002), 2002, 66—73.

[9] Clark, G.Angular and linear velocity estimation for a re-entryvehicle using six distributed accelerometers: Theory,simulation and feasibility.Lawrence Livermore National Lab., CA, ReportUCRL-ID-153253, 2003.

[10] Costello, M. and Webb, C.Angular rate estimation using fixed and vibrating triaxialacceleration measurements.Journal of Spacecraft and Rockets, 42 (2005), 1133—1137.

[11] Chin-Woo, T. and Sungsu, P.Design of accelerometer-based inertial navigationsystems.IEEE Transactions on Instrumentation and Measurement,54 (2005), 2520—2530.

[12] Mostov, K. S., Soloviev, A. A., and Koo, T. K. J.Initial attitude determination and correction of gyro-freeINS angular orientation on the basis of GPS linearnavigation parameters.In Proceedings of the IEEE Conference on IntelligentTransportation System (ITSC ’97), 1997, 1034—1039.

[13] Ying Kun, P. and Golnaraghi, M. F.A vector-based gyro-free inertial navigation system byintegrating existing accelerometer network in a passengervehicle.In Proceedings of the IEEE Position Location andNavigation Symposium (PLANS 2004), 2004, 234—242.

[14] Chin-Woo, T. and Sungsu, P.Design of accelerometer-based inertial navigationsystems.IEEE Transactions on Instrumentation and Measurement,54 (2005), 2520—2530.

[15] Zappa, B., Legnani, G., van den Bogert, A. J., andAdamini, R.On the number and placement of accelerometers forangular velocity and acceleration determination.Journal of Dynamic Systems, Measurement, and Control,123 (2001), 552—554.

[16] Jekeli, C.Inertial Navigation Wystems with Geodetic Applications.New York: Walter de Gruyter, 2001.

[17] Algrain, M. C. and Saniie, J.Estimation of 3D angular motion using gyroscopes andlinear accelerometers.IEEE Transactions on Aerospace and Electronic Systems,27 (1991), 910—920.

[18] Syed, Z. F., Aggarwal, P., Goodall, C., Niu, X., andEl-Sheimy, N.A new multi-position calibration method for MEMSinertial navigation systems.Measurement Science & Technology, 18 (2007),1897—1907.

[19] Bragge, T., Hakkarainen, M., Tarvainen, M., Liikavainio, T.,Arokoski, J., and Karjalainen, P.Calibration of triaxial accelerometer by determiningsensitivity matrix and offsets simultaneously.In Proceedings of the 1st Joint ESMAC-GCMAS Meeting,Amsterdam, The Netherlands, 2006.

[20] Edwan, E., Knedlik, S., and Loffeld, O.An extended Kalman filter for improving angular motionknowledge in a multiple distributed IMU set.In Proceedings of Symposium Gyro Technology 2008,DGON, Karlsruhe, Germany, 2008.


[21] Edwan, E., Knedlik, S., Zhang, M., and Loffeld, O.Investigation of dynamic models for angular motionestimation in gyro-free IMU.In Proceedings of the 16th Saint Petersburg InternationalConference on Integrated Navigation Systems, SaintPetersburg, Russia, 2009.

[22] Simon, D.Optimal State Estimation Kalman, H [infinity] andNonlinear Approaches.Hoboken, NJ: Wiley-Interscience, 2006.

[23] Cardou, P. and Angeles, J.Estimating the angular velocity of a rigid body movingin the plane from tangential and centripetal accelerationmeasurements.Multibody System Dynamics, 19 (2008), 383—406.

[24] Bortz, J. E.A new mathematical formulation for strapdown inertialnavigation.IEEE Transactions on Aerospace and Electronic Systems,AES-7 (1971), 61—66.

[25] Titterton, D. H. and Weston, J. L.Strapdown Inertial Navigation Technology (2nd ed.)(Progress in Astronautics and Aeronautics)AIAA, Washington, D.C., 2004.

[26] Savage, P. G.Strapdown system computational elements.In NATO RTO Educational Notes, SET-064,Neuilly-sur-Seine Cedex, France, 2004.

[27] Rong Li, X. and Jilkov, V. P.Survey of maneuvering target tracking. Part I. Dynamicmodels.IEEE Transactions on Aerospace and Electronic Systems,39 (2003), 1333—1364.

Ezzaldeen Edwan (M’01) received the B.Sc. degree in electrical engineeringfrom Birzeit University, West Bank, Palestine in 1997 and the M.Sc. degree inelectrical engineering from Oklahoma State University, OK, in 2003.Since June 2006, he has been pursuing his doctoral degree in the IPP

Multi Sensorics at the Center for Sensorsystems (ZESS), University of Siegen,Germany. His main research interests are low-cost INS, gyro-free INS, andestimation theory applied in the integrated navigation systems. He has experiencein the academic field lecturing for electrical engineering courses.Mr. Edwan is a recipient of the Fulbright and the DAAD scholarships,

respectively.

[28] Alouani, A. T. and Blair, W. D.Use of a kinematic constraint in tracking constant speed,maneuvering targets.IEEE Transactions on Automatic Control, 38 (1993),1107—1111.

[29] Simon, D. and Tien Li, C.Kalman filtering with state equality constraints.IEEE Transactions on Aerospace and Electronic Systems,38 (2002), 128—136.

[30] Chun, Y. and Blasch, E.Kalman filtering with nonlinear state constraints.IEEE Transactions on Aerospace and Electronic Systems,45 (2009), 70—84.

[31] Julier, S. J. and LaViola, J. J.On Kalman filtering with nonlinear equality constraints.IEEE Transactions on Signal Processing, 55 (2007),2774—2784.

[32] De Geeter, J., Van Brussel, H., De Schutter, J., andDecreton, M.A smoothly constrained Kalman filter.IEEE Transactions on Pattern Analysis and MachineIntelligence, 19 (1997), 1171—1177.

[33] Gebre-Egziabher, D.Design and performance analysis of a low-cost aideddead reckoning navigator.Ph.D. dissertation, Dept. of Aeronautics and Astronautics,Stanford University, Stanford, CA, 2004.


Stefan Knedlik (M’04) received the Diploma degree in electrical engineering andthe Dr. Eng. degree from the University of Siegen, Siegen, Germany in 1998 and2003, respectively.Since 1998, he has been a member of the Center for Sensorsystems (ZESS),

University of Siegen, and since 2003 he has also been a researcher/assistantprofessor at the Institute of Signal Processing and Communication Theory atthe Department of Electrical Engineering and Computer Science. In severalresearch projects, e.g., in cooperation with the German Aerospace Agency (DLR),he developed state and parameter estimation strategies, often with respect tosynthetic aperture radar (SAR) interferometry. A few years ago, he founded aresearch group on navigation. He is also the executive director of the InternationalPostgraduate Program (IPP) Multi Sensorics and of the NRW Research Schoolon Multi Modal Sensor Systems for Environmental Exploration and Safety(MOSES). His current research interests include signal processing and appliedestimation theory in navigation.

Otmar Loffeld (M’05–SM’06) received the Diploma degree in electricalengineering from the Technical University of Aachen, Aachen, Germany in1982 and the Eng.Dr. and the “Habilitation” degrees in the field of digital signalprocessing and estimation theory from the University of Siegen, Siegen, Germanyin 1986 and 1989, respectively.Since 1991, he has been a Professor of Digital Signal Processing and

Estimation Theory with the University of Siegen. In 1995, he became aMember of the Center for Sensorsystems (ZESS), which is a central scientificresearch establishment at the University of Siegen, where since 2005 he hasbeen the chairman. He is author of two textbooks on estimation theory. He iscurrently PI for interferometric techniques in the German TerraSAR-X mission,and together with Prof. Ender from Forschungsgesellschaft für AngewandteNaturwissenschaften (FGAN), he is one the PIs for a bistatic spaceborne-airborneexperiment, where TerraSAR-X serves as the bistatic illuminator while FGAN’sPAMIR system mounted on a Transall airplane is used as a bistatic receiver. In2002, he founded the International Postgraduate Program “Multi Sensorics,”and based on that program, he established the “NRW Research School onMulti Modal Sensor Systems for Environmental Exploration and Safety” at theUniversity of Siegen as an upgrade of excellence in 2008. He is the speaker andcoordinator of both doctoral degree programs, hosted by ZESS. Furthermore,he is the university’s Scientific Coordinator for Multidimensional and ImagingSystems.His current research interests include multisensor data fusion, Kalman filtering

techniques for data fusion, optimal filtering and process identification, SARprocessing and simulation, SAR interferometry, phase unwrapping, and baselineestimation. A recent field of interest of his is bistatic SAR processing.Professor Loffeld is a Member of the ITG/VDE and Senior Member of

the IEEE/GRSS. He was the recipient of the Scientific Research Award ofNorthrhine-Westphalia (“Bennigsen-Foerder Preis”) for his works on applyingKalman filters to phase-estimation problems such as Doppler centroid estimationin SAR, phase, and frequency demodulation.


a conventional inertial measurement unit (imu) constrained angular motion estimation ... · 2013....

Documents