Invariant Trapezoidal Kalman Filter for Applicationto Attitude Estimation

S. Mikael Persson∗ and Inna Sharf†

McGill University, Montreal, Quebec H3A 2K6, Canada

DOI: 10.2514/1.59052

This paper incorporates formal concepts of invariance of numerical integration schemes into the design of Kalman

filters. In general terms, invariant discretizations of dynamical systems can form the basis for the derivation of the

covariance propagation during the prediction and update phases of a Kalman filter, and this paper presents a

framework to that effect. Specifically, natural invariants of angular motion are introduced, as part of a symmetry-

preserving trapezoidal integration rule, to form the basis for the derivation of a discrete-time invariant Kalman filter

for anattitude estimationproblem.Theproposed filter is realizedby expressing all zero-meanrandomvariables in the

Lie algebra, while the statemanipulations are performed in special orthogonal group 3. Simulation and experimental

results are obtained using a neutrally buoyant spherical blimp to validate the proposed method against state-of-the-

art Kalman filters in a broad range of angular speeds and sampling rates. Furthermore, the results support claims of

invariance of the estimator with respect to angular speed, as well as the preservation of the system’s symmetries

through the covariance calculations.

Nomenclature

A = matrixδx = random component associated to an uncertain value xPxy = covariance matrix obtained from cov�x; y�q = unit quaternionx = column vectorxk = state estimate at discrete time kxk�1jk = a priori state estimate at discrete time k� 1 based on

previous estimate (xk)

I. Introduction

K ALMAN filtering is a classic method to estimate the state, andthe associated uncertainty, of a system subject to Gaussian

disturbances and measurement noise. A Kalman filter proceeds intwo steps: it first predicts the state of the system based on the previousestimates and then corrects that prediction with the help of the latestmeasurements. By assuming that all sources of uncertainty areGaussian processes and representing the uncertainty on the stateestimates in the same manner (Gaussian distribution), one can obtainan optimal minimum mean-squared error estimate of the statevariables, given that the system is linear, by solving an algebraicRiccati equation for the optimal gain in the correction (also calledthe Kalman gain) [1]. Today, this method is ubiquitous to manyengineering problems in robotics, automation, and control. One ofthe most attractive features of a Kalman filter is its straightforwardparametrization in terms of thematrices that describe the dynamics ofthe system, and the covariance matrices of the measurements anddisturbances, which greatly facilitates the synthesis of aKalman filterfor a given system. However, a major drawback of Kalman filteringis the requirement that the discrete-time system be linear, which israrely the case in practice.For decades now, many researchers and engineers have relied on

the extended Kalman filter (EKF) to handle nonlinear systems by asimple linearization about the current state estimate [2]. The EKF hasproven to be successful in countless applications and invarious fields,

however, significant flaws have often manifested themselves andhave generated a vast body of scientific literature on differentvariations on the Kalman filtering scheme that attempt to mitigatesome of these problems. In this paper, contributions are made to thisbody of literature with the introduction of the trapezoidal Kalmanfilter (TKF), a discussion on the invariantization of aKalman filteringscheme, and by providing an invariant TKF for an attitude estimationproblem, the invariant momentum-tracking Kalman filter (IMKF),which follows from a previous method by the authors, published in[3].One popular alternative to the EKF is a class of methods that

employ a completely different strategy altogether by representing theGaussian belief state (state estimate and covariance) through samplepoints that are individually propagated through the nonlinear systemdynamics to form the next belief state. The sample points are eitherchosen deterministically, e.g., sigma points as in the unscentedKalman filter [4–6], or at random as in the ensemble Kalman filters[7]. These methods are in a class of their own (with some advantagesand disadvantages), and will not be discussed further in this paper,which will focus on “classic” Kalman filtering schemes.The synthesis process involved in designing an EKF for a given

estimation problem is worth reflecting upon. Usually, one starts witha nonlinear model of the continuous-time dynamics. Then the systemis discretized in time and, because the requirement for a first-orderEKF is for a linear state-transition matrix, an obvious choice for thediscretization is an explicit Euler method. Now, at every estimationpoint, one can construct the system matrices that define a locallylinear system, which is then used to compute the Kalman gain as witha linear Kalman filter. Finally, a correction is applied to the statepredictions. From these steps, two critical observations can be drawn:each of these steps can be problematic when applied to a nonlinearsystem, and none of them have to be accomplished strictly in thatmanner.In an application domain like attitude estimation, the governing

dynamics are not only nonlinear, but also operate in the specialorthogonal group 3 (SO3) and, depending on the actual application,have an inherent structure or symmetry. This renders the naïveapplication of the aforementioned steps, often referred to as anadditive extendedKalman filter (AEKF), problematic indeed as it canhardly provide estimates that are consistent with the constraints of theSO3 group and that respect the symmetries of the system, oftenleading to divergence of the estimator undermore difficult estimationconditions. For a comprehensive survey of the many attitudeestimation techniques that have been explored in the past decades, thereader is referred to Crassidis et al. [8] as well as the seminal work of

Received 22May 2012; revision received 13 September 2012; accepted forpublication 13 September 2012; published online 7March 2013. Copyright©2012 by the American Institute of Aeronautics and Astronautics, Inc. Allrights reserved. Copies of this paper may be made for personal or internal use,on condition that the copier pay the $10.00 per-copy fee to the CopyrightClearance Center, Inc., 222 Rosewood Drive, Danvers, MA 01923; includethe code 1533-3884/13 and $10.00 in correspondence with the CCC.

*Ph.D. Candidate and Vanier Canada Graduate Scholar, MechanicalEngineering Department.

†Associate Professor, Mechanical Engineering Department.

721

JOURNAL OF GUIDANCE, CONTROL, AND DYNAMICS

Vol. 36, No. 3, May–June 2013

Dow

nloa

ded

by S

TA

TE

UN

IVE

RIS

TY

OF

NE

W Y

OR

K -

on

May

18,

201

3 | h

ttp://

arc.

aiaa

.org

| D

OI:

10.

2514

/1.5

9052

Lefferts et al. [9].Wewill briefly review a few of themain approachesof the last decade.Because of the special structure of SO3, many approaches have

involved alternate strategies to represent the innovation vector(difference betweenmeasurements and predictedmeasurements) andthe correction formula. Most notably, a class of methods called themultiplicative extended Kalman filter (MEKF) [8] represent theinnovation in terms of a multiplicative difference (or composition ofSO3 rotations) and apply the correction accordingly. Lately, theinvariant extended Kalman filter (IEKF) [10] as well as its latestgeneralization as the generalized multiplicative extended Kalmanfilter [11], are state-of-the-art examples of this strategy whicheffectively revolves around the idea of operating in the Lie algebra ofSO3 as a local vector space, in which the covariance matrix can bemore consistent with the actual estimation error. Another alternativemethod, proposed by Zanetti et al. [12], attempts to apply a second-order correction to the covariance matrices to take the unit-norm ofthe quaternion into account and achieve better consistency with theactual estimation errors.Stemming from attempts to represent estimation errors on unit

quaternions directly in so-called quaternion estimators [8], a numberof estimation methods are formulated specifically for quaternionicalgebra [13]. Markley [14] presents a good summary of therepresentation of errors in attitude, which, the authors might add,have the great merit of handling unit quaternions (and relatedorientation representations) as SO3 objects (hypergeodesic) ratherthan as an R4 vector with a unit-norm constraint. This approach isvery reasonable, as quaternions are objects that ought to be handledwithin the rules of their Lie group [15,16].On another front, that of discretization of the continuous-time

dynamics, there has beenmuch attention focused in the past decade ortwo on the subject of variational integrators [17]. The extensivetreatment of the subject by Marsden and West [17] is highlyrecommended, inwhich definitions, theorems, and examples providesolid arguments for discrete mechanics. The numerical integration ofattitude dynamics per se has had some exciting developments. Lately,Saccon [18] has presented a novel midpoint-rule of variationalintegration on a Lie group, as well as a summary of several otherrelated variational integrators for attitude dynamics, notably, themomentum-conserving trapezoidal method (TRAPM) [19]. Otherinteresting variational integrators on Lie groups were also reported[20,21]. Kim [22] also presents a number of formal definitions andexamples to employ the invariance of a system (or its symmetries)in the numerical integration scheme to preserve the structures ofa system. To the authors’ knowledge, the only reported use of avariational integrator as part of a Kalman filtering scheme is thesymplectic filter [23], but it is an interesting avenue that this paperwill explore at greater lengths.The remainder of the paper is organized as follows. First, in Sec. II,

a brief summary of quaternionic algebra, unit quaternions, and rigid-body rotations is presented as a necessary basis for later examplesand derivations in the application domain of attitude dynamics andestimation. Then, in Sec. III, a number of constructive definitions arepresented pertaining to the concept of invariance and how it applies todynamic systems, numerical integration schemes, andKalman filters,which together form the theoretical framework for the proposedKalman filter. In Sec. IV, an application to attitude estimation isshown, in which the concepts of invariance are applied to a TKF,and thus, an IKMF is obtained. Finally, in Sec. V, simulation andexperimental results validate the proposed Kalman filtering schemewhen applied to free-floating rigid-body motion, and highlights itsmain qualities as compared with other filters.

II. Unit Quaternions and SO3

This section is a brief summary of quaternionic algebra, a proxy forSO3, used in subsequent sections when drawing examples fromattitude dynamics; amore comprehensive treatment of the subject canbe found in [15].Expressing quaternions as a scalar and vector part, the set Q of

quaternions is:

Q∶ fq � �q0 qTv �T jq0 ∈ R ∧ qv ∈ R3g (1)

and the set �Q of unit quaternions is:

�Q∶ fq � � cos ϕ sin ϕuT �T jϕ ∈ R ∧ u ∈ R3 ∧ kuk22 � 1g (2)

A rotation inR3 by an angle of θ ∈ R about a unit axisu ∈ R3 can beexpressed by the Euler–Rodriguez parametersq or the correspondingrotation matrix R�q� as follows:

q0 � cosθ

2and qv � sin

θ

2u (3)

R�q� � �q20 − qTvqv�I3 � 2qvqTv � 2q0�qv×� (4)

R�q�v � qvq−1 ⇒ R�q1�R�q2� � R�q1q2� (5)

where �qv� denotes the skew-symmetric matrix for vector qv (alsocalled the cross-product matrix). Note also, in Eq. (5), an abuse ofnotation that will carry through this paper, that is, no distinction ismade betweenR3 vectors and pure quaternions (i.e., quaternionswithzero scalar part).In quaternionic algebra, the composition operator over quaternions

is defined as:

�composition� qp��

q0p0 −qTvpvq0pv�p0qv�qv ×pv

�for q;p∈Q (6)

along with several fundamental operations, for p ∈ Q and unitquaternions q ∈ �Q:Inversion:

p−1 � 1

kpk22�p0 −pTv �T q−1 � �q0 −qTv �T (7)

Logarithm:

log p ��log�kpk2� pTv

kpvk2 cos−1�

p0

kpk2

��T

log q � ϕu (8)

Exponential:

exp�p� � exp�p0��cos�kpvk2� pTv

kpvk2 sin�kpvk2��T

exp�ϕu� � q

(9)

Power function:

pt � exp�t log p� qt � exp�tϕu� (10)

The noncommutativity of the composition operator is a central issuewith quaternionic algebra, but two important exceptions exist,collinearity and small-angle approximation:

qTvpv � �kqvkkpvk ⇔ eqep � eq�p (11)

kpvk ≪ 1 ⇒ eqep ≈ eq�p (12)

In Secs. III and IV, a few theorems will be needed, and thus, will bepresented here. Although simple, these theorems are not readilyavailable in major works on quaternions.Lemma 1: Given two quaternions q, p ∈ Q, the following is

always true:

qpq−1 ��

p0

qpvq−1

�(13)

Proof: Can be proven by a simple arithmetic expansion, the proofis left to the reader.

722 PERSSON AND SHARF

Dow

nloa

ded

by S

TA

TE

UN

IVE

RIS

TY

OF

NE

W Y

OR

K -

on

May

18,

201

3 | h

ttp://

arc.

aiaa

.org

| D

OI:

10.

2514

/1.5

9052

Theorem 2: Given two quaternions q, p ∈ Q, the following

identity is true:

log�qpq−1� � q log�p�q−1 (14)

Proof: Using the expression of Lemma 1 and expanding thequaternionic logarithm:

log�qpq−1� � log

��p0

qpvq−1

��

�

24 log�

�����������������������������������p20 � kqpvq−1k22

p�

qpvq−1

kqpvq−1k2cos−1

�p0�����������������������

p20�kqpvq−1k2

2

p�35 (15)

From basic quaternionic algebra, it is known that kqpvq−1k2 �kpvk2, which leads to:

log�qpq−1� �

24 log�kpk2�

qpvq−1

kpvk2 cos−1�

p0

kpk2

�35 (16)

and, again, using Lemma 1, the identity is obtained:

log�qpq−1� � q

� log�kpk2�pvkpvk2 cos

−1�

p0

kpk2

��q−1 � q log�p�q−1 (17)

completing the proof.Corollary 3: Given two quaternions q, p ∈ Q, the following

identity is true:

exp�qpq−1� � q exp�p�q−1 (18)

Proof: Applying the logarithm on Eq. (18), and using Theorem 2,the following is obtained:

qpq−1 � log�q exp�p�q−1� � q log�exp�p��q−1 � qpq−1 (19)

completing the proof.As a final note, obviously, unit quaternions being a subset of

general quaternions, Lemma 1, Theorem 2, and Corollary 3 all applyequally well to unit quaternions.

III. Invariant Discrete-Time Estimator

Invariance in the context of discrete-time state estimators such asKalman filters is defined in this section. Invariance, in general,provides a transformation group on a system for which thefundamental expressions of the system remain unchanged (at leastlocally). Particular choices of transformations, such as permanenttrajectories [11], will preserve the system’s structure throughnumerical calculations, which is of particular interest across discretetime intervals. Formal definitions of invariance for systems andnumerical integration rules need to be established, and then thesedefinitions are applied to prediction and Bayesian update phases ofthe Kalman filtering framework.

A. Invariant System

First, a formal definition of an invariant system will be adopted.This section is largely inspired by the work of Bonnabel et al. [10],however, the definitions are relaxed to include invariance underdiffeomorphisms whose domain and codomain are distinct, whichis important in applications where the algebraic structure of atranformation is different from the original algebraic structure, suchas moving between SO3 and its Lie algebras. Also note that thedefinitions apply globally if their defining properties apply globallyas well; otherwise, the arguments are only valid locally.

Consider the continuous-time differential system

_x � f�t;x;u� (20)

z � h�t;x;u� (21)

which operates on the tuple �t;x;u� ∈ T × X × U via a state-derivative function f∶ �T × X × U� ↦ TX (where TX stands for thetangent space of X) and an output function h∶ �T × X × U� ↦ Z.Definition 1 (Transformation Bundle): Let G be a Lie group

and g ∈ G parametrize the (local) transformation bundle πg≡�ϕg;ψg; ρg� where:

ϕg∶ �X;�� ↦ � ~X;��; i:e:; ~x � ϕg�x1 � x2� � ϕg�x1� � ϕg�x2�with x1;x2 ∈ X; ~x ∈ ~X (22)

ψg∶ �U;��↦ � ~U;��; i:e:; ~u� ψg�u1�u2� � ψg�u1�� ψg�u2�with u1;u2 ∈U; ~u ∈ ~U (23)

ρg∶ �Z;�� ↦ � ~Z;��; i:e:; ~z � ρg�z1 � z2� � ρg�z1� � ρg�z2�with z1; z2 ∈ Z; ~z ∈ ~Z (24)

with all transformations being (local) group diffeomorphisms,mapping the elements from a domain to a codomain while retainingconsistent composition operators (represented by � and �).If the defining functions of a system are mutable across the group

diffeomorphisms, then the concept of an invariant system follows.Definition 2 (Invariant System): Let S ≡ �f; h� be a differential

system as in Eqs. (20) and (21), and πg be a transformation bundle asof Definition 1, then the system S is said to be a (local) π-invariantsystem if the following holds for all g ∈ G (locally):

_~x � Dϕg�x� · f�t;x;u�� f�t;ϕg�x�;ψg�u�� (25)

~z � ρg�h�t;x;u��� h�t;ϕg�x�;ψg�u�� (26)

where Dϕg�x� · is the partial-derivative mapping TX ↦ T ~X (e.g., ifX and ~X are vector-spaces, then Dϕg�x� · is a Jacobian matrixmultiplication).In a π-invariant system, π is called a symmetry group (or symmetry

bundle) of the system: a group of preserved structures or permanenttrajectories of the system. Obviously, permanent trajectories are notunique, and that is exactly where opportunity lies. Choosingtransformation bundles in accordance with a numerical integrationscheme can preserve important structures across the time intervals,i.e., enforcing the symmetries at the integration bounds.

B. Invariant Numerical Schemes

Invariance of a numerical scheme to a given transformation bundlewill now be defined. This section is largely based on Kim [22], butagain, broadens the definitions to group diffeomorphisms as inSec. III.A. A numerical scheme for integrating a system in the form ofEq. (20) can, in general, be cast as an equation of the form:

N�s� � 0 (27)

where s is the sequence of sample points involved in one integrationstep. For example, in a single-step integration scheme, it is defined ass � f�tk;xk;uk�; �tk�1;xk�1;uk�1�g where xk�1 becomes theunknown to be computed from Eq. (27). Kim [22] states thatN�s� �0 is a numerical approximation to a given differential system ifN�s�is approximately 0whenever s is drawn from the exact solution of the

PERSSON AND SHARF 723

Dow

nloa

ded

by S

TA

TE

UN

IVE

RIS

TY

OF

NE

W Y

OR

K -

on

May

18,

201

3 | h

ttp://

arc.

aiaa

.org

| D

OI:

10.

2514

/1.5

9052

given differential system. Formally, s is an element of the set ofsample-point sequences:

S ≡ ff�tk�1−i;xk�1−i;uk�1−i�gji � 0 : : : mg (28)

for a numerical integrator that involvesm integration steps excludingthe unknown sample point at tk�1. In this new context, thetransformation bundle definition in Definition 1, is amended with anadditional notational convention for s ∈ S:

πg�s� � f�tk�1−m;ϕg�xk�1−m�;ψg�uk�1−m��; · · · ;�tk�1;ϕg�xk�1�;ψg�uk�1��g (29)

which allows for the definition of a π-invariant numerical scheme.Definition 3 (Invariant Numerical Scheme): Let N�s� � 0 be a

numerical integration scheme for a given system and over the set ofsample-point sequences S, and πg be a transformation bundle as ofDefinition 1, then the numerical scheme N is said to be a (local) π-invariant numerical scheme if for all g ∈ G (locally) and for all s ∈ Ssuch that N�s� � 0, it is also true that N�πg�s�� � 0.An additional property of an invariant numerical scheme is

infinitesimal invariance:

Dϕg · N�s� � N�πg�s�� ∀ s ∈ N �s⋆� (30)

where Dϕg is the Jacobian of the vector-space diffeomorphism ϕg,and N �s⋆� denotes some infinitesimal neighborhood of s⋆ (withN�s⋆� � 0).The relationship between the symmetries of the continuous-time

system and those of the numerical scheme depend on the choice ofsymmetries and the numerical integration scheme, but the system’ssymmetries are prime candidates for the invariantization of thenumerical scheme [22].Example 1: Consider a 3-D rigid body undergoing a pure rotation

with a constant angular momentum, and thus, governed by thefollowing differential system:

_q � 1

2qω (31)

_ω � −J−1ω × Jω (32)

where J is the inertia tensor expressed in body-fixed coordinates.Applying an implicit trapezoidal rule in the interval �tk; tk�1�, thefollowing numerical scheme is obtained:

qk�1

�1 −

h

4ωk�1

�� qk

�1� h

4ωk

�(33)

�I� h

2�ωk�1×�

�Jωk�1 �

�I −

h

2�ωk×�

�Jωk (34)

Observing this integration rule, one can recognize expressions forinfinitesimal rotations in terms of h

2ωk and

h2ωk�1, as a quaternion

[Eq. (33)] and as a rotationmatrix [Eq. (34)]. The integration rule canthus be rewritten as:

qk�1e−h4 ωk�1 � qke

h4ωk (35)

R�eh4ωk�1�Jωk�1 � R�e−h4ωk�Jωk (36)

which is known in literature as the momentum-conserving

trapezoidal method [19]. If the state is expressed in terms of arotating frame defined by an initial time t0, an initial orientation q0,and a constant angular velocity ω0, then the numerical scheme isexpected to remain unaffected (cf., Galilean Invariance). Indeed,given the mapping

ϕt0;q0;ω0∶ �t − t0;q;ω� ↦ �t; e

−�t−t0 �2

w0q−10 q;ω − q−1q0ω0q

−10 q�

(37)

the substitution of the transformed state into Eq. (35) yields thefollowing series of simplifications:

e−tk�1

2ω0q−1

0 qk�1e−h4 ωk�1�h4q−1

k�1q0ω0q−10qk�1

� e−tk2 ω0q−1

0 qkeh4ωk−h4q

−1kq0ω0q

−10qk e

−h4q−10qk�1ωk�1q−1

k�1q0�h4ω0q−10 qk�1

� eh2ω0eh4q−10qkωkq−1

kq0−h4ω0q−1

0 qk e−h4 q

−10qk�1ωk�1q−1

k�1q0q−10 qk�1

� eh4q−10qkωkq−1

kq0q−1

0 qk qk�1e−h4ωk�1 � qke

h4ωk (38)

which is the original numerical scheme and angular momentum isconserved (by Noether’s theorem).

C. Invariant Kalman Filter

In this section, the definition of invariance is extended to a Kalmanfiltering scheme. Obviously, a Kalman filter is a numerical scheme,and as such, could fall under Definition 3 with a transformationbundle on the belief state. However, given the particular structure ofthe Gaussian belief state and the underlying dynamical system, itseems relevant to provide a more specific definition in terms of atransformation bundle on the underlying state input–output space.Although they can be inferred from the vast body of literature, the

definitions presented in this section are novel. It is important tosuccinctly formalize those concepts that are otherwise colloquiallydiscussed in literature. Additionally, these definitions enlighten thepath toward formulating specialized Kalman filters, in particular,invariant Kalman filtering schemes.A Kalman filter can be regarded as a numerical scheme composed

of a prediction and an update phase, as expressed by Np and Nu,respectively, as so:

Np�tk; bx;k; bu;k; tk�1; bx;k�1jk; bu;k�1� � 0 (39)

Nu�tk�1; bx;k�1jk; bu;k�1; bz;k�1; bx;k�1� � 0 (40)

where, for example, bx is the belief on x, that is, bx ≡ �x;Pxx� withmean x and covariancePxx, and similarly for all other subscripted b srepresenting beliefs on the state x, actual input u, and measurementsz. AKalman filter should have the following symmetry, for all beliefsinvolved:

Pxx � cov�x� (41)

where x represents any vector towhichPxx is associated (input, state,or measurements).Definition 4 (Gaussian Lift): Let f∶ X ↦ ~X, the Gaussian lift of f

is denoted by fG and is defined as:

fG∶ �x; cov�x�� ↦ �f�x�; cov�f�x��� (42)

or, infinitesimally:

� ~x; ~Pxx� � fG��x;Pxx�� � �f�x�;Df�x�PxxDf�x�T� (43)

The symmetry of Eq. (41) is easily preserved by defining thetransformation bundle of the beliefs in terms of a Gaussian lift of atransformation bundle in the state input–output space, as so:

πGg ≡ �ϕGg ;ψGg ; ρGg � (44)

where the tuple �ϕg;ψg; ρg� is that from Definition 1.At this point it is pertinent to give a formal definition of what is

meant, for the purpose of this article, by Kalman filtering scheme,given the term’s liberal use in literature in general.

724 PERSSON AND SHARF

Dow

nloa

ded

by S

TA

TE

UN

IVE

RIS

TY

OF

NE

W Y

OR

K -

on

May

18,

201

3 | h

ttp://

arc.

aiaa

.org

| D

OI:

10.

2514

/1.5

9052

Definition 5 (Kalman Filtering Scheme): Let S � �f; h� be adifferential input–output system as in Eqs. (20) and (21). A Kalmanfiltering scheme is a numerical scheme from which a sequence ofGaussian beliefs on the estimated state of the system can be obtainedthat minimizes the covariance in each posterior belief state.Concretely, this numerical scheme involves two phases, predictionand (Bayesian) update, which, under the Gaussian assumption, yieldthe following schemes:Prediction: Given a sequence of estimated mean values

s ≡ f�tk−i; xk−i; uk−i�; · · · ; �tk; xk; uk�; �tk�1; xk�1jk; uk�1�g (45)

and the numerical scheme N�s� � 0, which is a numericalapproximation of the differential system given by _x � f�t; x; u�,solve the numerical scheme to obtain the a priori state estimate xk�1jk.Then, solve for the prediction covariance Pxx;k�1jk ≡ cov�xk�1jk�from the expression

cov�N�s�� � 0 (46)

where 0 here is a zero matrix of appropriate dimensions (the sameas Pxx;k�1jk). Finally, the a priori belief state can be formed asbx;k�1jk � �xk�1jk;Pxx;k�1jk�.Update: Given an innovation function y, which can be used to

obtain an innovation vector by

yk�1 ≡ y�tk�1; zk�1; h�tk�1; xk�1jk; uk�1�� (47)

where zk�1 is the last measurement obtained, and given a correctionscheme

C�tk�1; xk�1jk; xk�1;Kk�1yk�1� � 0 (48)

whereKk�1 is the Kalman gain matrix, the Kalman gain is obtainedby minimizing the a posteriori covariance term in the covariance ofthe correction scheme:

Kk�1 ≡ argminKk�1

cov�xk�1� (49)

with C�tk�1; xk�1jk; xk�1;Kk�1yk�1� � 0 (50)

cov�C�tk�1; xk�1jk; xk�1;Kk�1yk�1�� � 0 (51)

where the posterior belief bx;k�1 � �xk�1;Pxx;k�1� is obtained fromEqs. (50) and (51).The reader will notice in Definition 5 that the usual assumption of

linearity of the discrete system is not made, because the linearityassumption can be seen not as a defining aspect of Kalman filtering,but rather as a necessary assumption when looking for exact andclosed-form solutions in the scheme [notably Eqs. (46) and (49)]. Thereader can convince himself that Definition 5 does indeed encompassall discreteKalman filters used in practice. Definition 5 lacks a formalstructure constraining the choices of prediction, innovation, andcorrection functions; the intent is to allow for functions other than thestandard ones, in particular ones that preserve symmetries.Definition 6 (Invariant Kalman Filter): Given a Kalman filtering

schemewhose prediction and update phases are cast as two numericalschemes �Np;Nu� as of Eqs. (39) and (40), and given a transforma-tion bundle πg for some g ∈ G, then the scheme is called a (local)π-invariant Kalman filter if both numerical schemes Np and Nu are(locally) invariant with respect to the Gaussian lift of thetransformation bundle, that is πGg .

IV. Invariant Momentum-Tracking Kalman Filter

In this section, a novel formulation of a Kalman filter for theproblem of attitude estimation is presented. In this context, attitudeestimationmeans the problem of obtaining, at discrete sample points,estimates of the orientation and angular velocity of a free-floating

rigid body with known input torque and full orientation measure-ment, which are both subject to Gaussian noise (or disturbance). Thisis a rather simple scenario, but with valid applications (e.g., ananosatellite with low-cost measurement devices, uncooperativerendezvous, or unmanned aerial vehicles).This Kalman filter is built as an invariantization of a trapezoidal

Kalman filter using the invariants of free-floating rigid-body motion.

A. Trapezoidal Kalman Filter

First, a general formulation of a trapezoidal Kalman filter isconstructed. Although this filter is a simple application of Kalmanfiltering to a nonlinear system through a trapezoidal integration rule,it is worth exposing here, as it is the basis for the presented Kalmanfilter.Assuming a nonlinear system defined by the following differential

equation:

_x�t� � f�t;x�t�;u�t�� (52)

where x�t� is the state vector and u�t� is a vector of inputs (e.g.,forces). For the purposes of state estimation with a Kalman filteringscheme, a discrete-time state-transition matrix must be extractedabout the current state estimates. A poor choice of discretization willlead to divergence of the estimator [4] through broken symmetriesand systematic accumulation of errors [24].In some cases, symmetries can be preserved by implicit integration

rules, such as the implicit trapezoidal rule [25], and structurepreservation on state predictions carries over to the uncertaintypredictions, if they are consistent. The system of Eq. (52) can beintegrated implicitly with the following trapezoidal rule:

xk�1jk −h

2f�tk�1; xk�1jk; uk�1� � xk �

h

2f�tk; xk; uk� (53)

where xk and uk are the state and input estimates at time tk, and h isthe time step. Using “input estimates”means that the true input vectoruk is a composition of known inputs (e.g., control inputs) and somedisturbances δuk ∈ N �0;Puu;k�, which form, together, a belief (orinput belief) in the probabilistic sense, and idem for measurements.Note that this does not imply that the inputs are part of the estimatedquantities.If the implicit trapezoidal rule is stable for the system (possibly,

within a bounded regime), then the state predictions are unbiased andthus, estimation errors have zero mean:

δxk ≡ xk − xk ∈ N �0;Pxx;k� and

δxk�1jk ≡ xk�1 − xk�1jk ∈ N �0;Pxx;k�1jk� (54)

where the normal distributions are adopted by assumption andrepresented by the covariance matrices Pxx;k and Pxx;k�1jk.From Eq. (53), one obtains the following first-order Taylor

expansion of the estimation errors:

δxk�1jk � δxk �h

2

�∂f∂x�tk; xk; uk�δxk �

∂f∂u�tk; xk; uk�δuk

�� �O�h2; kδxk22; kδuk22�

� h2

�∂f∂x�tk�1; xk�1jk; uk�1jk�δxk�1jk

� ∂f∂u�tk�1; xk�1jk; uk�1jk�δuk�1jk

�(55)

where �O�h2; kδxk22; kδuk22� is a compound second-order error termthat bounds both the error in the numerical integrator (O�h2�) and thatof the Taylor expansion (O�kδxk22; kδuk22�).To simplify the notation, Jacobian matrices (or partial derivatives)

are introduced:

Jx;k ≡∂f∂x�tk; xk; uk� Jx;k�1jk ≡

∂f∂x�tk�1; xk�1jk; uk�1� (56)

PERSSON AND SHARF 725

Dow

nloa

ded

by S

TA

TE

UN

IVE

RIS

TY

OF

NE

W Y

OR

K -

on

May

18,

201

3 | h

ttp://

arc.

aiaa

.org

| D

OI:

10.

2514

/1.5

9052

Ju;k ≡∂f∂u�tk; xk; uk� Ju;k�1jk ≡

∂f∂u�tk�1; xk�1jk; uk�1� (57)

Applying the covariance operator on both sides of Eq. (55), thefollowing expression for the prediction covariance can be obtained:

�I −

h

2Jx;k�1jk

�Pxx;k�1jk

�I −

h

2Jx;k�1jk

�T

��I� h

2Jx;k

�Pxx;k

�I� h

2Jx;k

�T

� h2

4�Ju;kPuu;kJTu;k � Ju;k�1jkPuu;k�1J

Tu;k�1jk� (58)

which can be cast to a standard form for the prediction covariance asfollows:

Pk�1jk � FkPkFTk �Qk where Pk�1jk ≡ Pxx;k�1jk

Pk ≡ Pxx;k Fk ≡�I −

h

2Jx;k�1jk

�−1�I� h

2Jx;k

�

Qk ≡h2

4

�I −

h

2Jx;k�1jk

�−1�Ju;kPuu;kJTu;k

� Ju;k�1jkPuu;k�1JTu;k�1jk�

�I −

h

2Jx;k�1jk

�−T(59)

One can observe that, even though the integration method is implicit,the resulting prediction covariance is an explicit expression given thetwo state vectors and input vectors. However, an inversion of the term�I − h

2Jx;k�1jk� is required, but it is a square, nonsingular, and well-

conditioned matrix. The main attractive feature of such implicitintegration rules is their symmetrywith respect to integration bounds,which make them candidates of choice for an invariantization aboutthose integration bounds �tk; tk�1�, as with the TRAPM, which formsthe basis of the Kalman filter presented next.

B. State Prediction

Estimating the rotation of a rigid bodywill now be considered. Thestate is characterized by a unit quaternion q ∈ �Q and an angularvelocity vector ω ∈ R3 expressed in body-fixed coordinates. Thecontinuous-time dynamics are as follows:

_q � 1

2qω (60)

_ω � J−1�t − �ω×�Jω� (61)

where τ is the net torque vector applied to the system (by inputs anddisturbances) and J is the constant inertia tensor, both expressed inbody-fixed coordinates.As shown Example 1, the TRAPM is a good candidate for

integrating the system just detailed, both because of its invariance tothe choice ofmoving reference frames and its very good performancein practice. The following numerical scheme is obtained as thediscrete-time approximation of the continuous-time dynamics:

qk�1e−h4ωk�1 � qke

h4ωk (62)

qk�1

�Jωk�1 −

h

2τk�1

�q−1k�1 � qk

�Jωk �

h

2τk

�q−1k (63)

which can be solved by fixed-point iterationswith rapid convergence.The a priori state estimates �qk�1jk; ωk�1jk� can be obtained from thelast state estimates �qk; ωk� and the estimate of the input torque atboth times tk and tk�1 using the same schemewith estimates in placeof actual values.

At this point, random quantities are introduced to represent theuncertainty

δqk ≡ q−1k qk and δqk�1jk ≡ q−1

k�1jkqk�1 (64)

δωk ≡ δqkωkδq−1k − ωk and

δωk�1jk ≡ δqk�1jkωk�1δq−1k�1jk − ωk�1jk (65)

δτk ≡ δqkτkδq−1k − τk and

δτk�1jk ≡ δqk�1jkτk�1δq−1k�1jk − τk�1jk (66)

where the error terms of the angular velocity and torque vectors areobtained by rotating the actual vectors �ωk; τk� into the estimatedframe by the orientation error δqk. This rotation is necessary becausethe actual vectors are expressed relative to the actual body-fixedcoordinate frame (described by qk), while the estimated vectors areexpressed relative to the estimated body-fixed coordinate frame(described by qk), and the discrete-time dynamics equations wouldnot be consistent if the actual vectors are expressed in the estimatedbody-fixed coordinates, or vice versa.

C. Prediction Covariance

As per Eq. (46) in Definition 5, the covariance of the numericalprediction scheme defined by Eqs. (62) and (63) must now beinvestigated. First, one must assume a Gaussian distribution for theerror terms, which, under the presumption of unbiased predictions,have zero mean and covariance given by:

Pk ��Pqq;k Pqω;kPωq;k Pωω;k

�≡ cov

��2 log�δqk�

δωk

��(67)

Pττ;k ≡ cov�δτk� (68)

which, evidently, yields dimensions for the covariance matricesof Pk ∈ R6×6, Pqq;k ∈ R3×3, Pωω;k ∈ R3×3, and Pττ;k ∈ R3×3. Thecritical aspect in Eqs. (67) and (68) is that the random part of thequaternion estimate is taken as small variations in the Lie algebra ofthe estimated quaternion.It was shown in Example 1 that the TRAPM is invariant to a

moving frame transformation, as of Eq. (37). That property will beexploited to express the true states of the system with respect to themoving estimated frame (moving from the previous state to thepredicted state).If Eq. (62) is first expressed in terms of the previous estimates, that

is, with ϕ�tk;qk;wk�, the following is obtained:

e−h2 ωk q−1

k qk�1e−h4�ωk�1−q−1

k�1qkωkq−1kqk�1� � q−1

k qkeh4�ωk−q−1

kqkωkq−1

kqk�

(69)

whose right-hand side can be simplified by substituting for thedefinition of the error terms and using Corollary 3, which yields:

e−h2ωk q−1

k qk�1e−h4�ωk�1−q−1

k�1qkωkq−1kqk�1� � eh4δωkδqk (70)

Similarly, casting Eq. (62) in the predicted frame, withϕ�tk�1;qk�1jk;wk�1jk�, yields the following:

e−h4δωk�1jkδqk�1jk � e

h2ωk�1jk q−1

k�1jkqkeh4�ωk−q−1

kqk�1jkωk�1jkq−1

k�1jkqk� (71)

Both sides of Eqs. (70) and (71) represent the deviation of the truequaternion from the estimated moving frame at the midpoint, but areexpressed in the previous and predicted frames, respectively.Expressing them in the common inertial frame gives

qk�1jke−h4δωk�1jkδqk�1jkq

−1k�1jk � qke

h4δωkδqkq

−1k (72)

726 PERSSON AND SHARF

Dow

nloa

ded

by S

TA

TE

UN

IVE

RIS

TY

OF

NE

W Y

OR

K -

on

May

18,

201

3 | h

ttp://

arc.

aiaa

.org

| D

OI:

10.

2514

/1.5

9052

Equation (72) represents the random variations, and thus, castingthem to the Lie algebra and taking the covariance of both sides yieldsthe following result:

�I3×3

−h2I3×3

�Pk�1jk

"I3×3

−h2I3×3

#

��R�q−1

k�1jkqk� h2R�q−1

k�1jkqk��Pk

"R�q−1

k�1jkqk�T

h2R�q−1

k�1jkqk�T

#(73)

To complete the formulation, the covariance of the second TRAPMequation is investigated. In this case, a simple difference between thenumerical approximation [Eq. (63)] with actual and estimated valueswill yield:

R�qk�1��Jωk�1 −

h

2τk�1

�−R�qk�1jk�

�Jωk�1jk −

h

2τk�1jk

�

� R�qk��Jωk −

h

2τk

�−R�qk�

�Jωk −

h

2τk

�(74)

which can be expressed in terms of the error terms as follows:

R�δqk�1jk�JR�δqk�1jk�T�ωk�1jk � δωk�1jk�� R�q−1

k�1jkqk�R�δqk�JR�δqk�T�ωk � δωk�

� Jωk�1jk −R�q−1k�1jkqk�Jωk �

h

2δτk�1jk �

h

2R�q−1

k�1jkqk�δτk(75)

If the covariance of both sides of Eq. (75) is taken, then it will yieldterms involving the expectation value of �R�δq�JR�δq�T − J�,which is an odd function of δq and will thus cancel out when theexpectation operator is applied given that the expected value ofthe orientation error is nil. Therefore, the only terms remaining in thecovariance calculation are the angular velocity and input torqueestimation errors. The covariance of Eq. (75) becomes:

� 03×3 J �Pk�1jk�03×3

J

�� � 03×3 R�q−1

k�1jkqk�J �

Pk

� 03×3

JR�q−1k�1jkqk�T

�� h

2

4R�q−1

k�1jkqk�Pττ;kR�q−1k�1jkqk�T

� h2

4Pττ;k�1jk (76)

Or, alternatively, if it is assumed that the input disturbance covarianceis a constant scalar matrix Pττ, then Eq. (76) becomes:

� 03×3 J �Pk�1jk�03×3

J

�

� � 03×3 R�q−1k�1jkqk�J �Pk

� 03×3

JR�q−1k�1jkqk�T

�� h2Pττ (77)

where the apparent order of magnitude increase on the weight of theinput covariance is explained by the fact that Pττ would span theentire time-step, which is half the interval spanned by both Pττ;k andPττ;k�1jk.Now, Eqs. (73) and (76) can be combined to obtain this prediction

covariance formulation:

Fk�1jkPk�1jkFTk�1jk � FkPkF

Tk

�G�Rk�1jkPττ;kRTk�1jk � Pττ;k�1jk�GT (78)

Fk�1jk ≡�I3×3

−h2I3×3

03×3 J

�Fk ≡

�Rk�1jk

h2Rk�1jk

03×3 Rk�1jkJ

�(79)

G ≡�

03×3h2I3×3

�Rk�1jk ≡R�q−1

k�1jkqk� (80)

which can be further simplified if the Fk�1jk matrix is inverted, asfollows:

F−1k�1jk �

�I3×3

h2J−1

03×3 J−1

�(81)

leading to the following explicit expression for the predictioncovariance:

Pk�1jk � ~FPk ~FT � ~G�Rk�1jkPττ;kR

Tk�1jk � Pττ;k�1jk� ~GT (82)

~F ≡�Rk�1jk

h2�Rk�1jk � J−1Rk�1jkJ�

03×3 J−1Rk�1jkJ

�~G ≡

�h2

4J−1

h2J−1

�(83)

If one further assumes the input disturbance covariance matrixPττ �γI with γ ∈ R, a simpler relation results:

Pk�1jk � ~FPk ~FT �

�h2

2J−1

hJ−1

�Pττ

�h2

2J−1 hJ−1

�(84)

At this point, it is worth contrasting the prediction covarianceexpression obtained with the TKF, in Sec. IV.A, with the expressionsobtained here. The rotation matrices appearing in these expressionsare, in fact, composed of the covariance parts of the Gaussian lift, seeEq. (43), associated to the invariance transformations used in thederivation, i.e., ϕ�tk;qk;ωk� and ϕ�tk�1;qk�1jk;ωk�1jk�. These differentialmappings naturally arise from basing the formulation on an invariantintegration scheme, although they could also be applied only at theend of a derivation based on a normal integration scheme, i.e.,invariantizing the covariance calculation. Clearly, these rotations arethe essential elements of the state transitionmatrices that preserve thesymmetry during the covariance calculations.

D. Measurement Update

Given a state prediction and a new measurement, it is time toperform a measurement update of the state prediction to obtain the aposteriori state estimate. This phase will follow the tradition ofmultiplicative extended Kalman filters. However, only the randomquantities are cast in the Lie algebra of SO3, while the innovation andthe correction (or update) follow the geodesic topology of SO3,which yields a novel result, different from the previous MEKFupdates found in literature.The innovation vector and its estimate are defined as follows:

yk�1 ≡ 2 log�q−1k�1jkqk�1� yk�1 ≡ 2 log�q−1

k�1jkqz;k�1� (85)

where qz;k�1 is the quaternion measurement and yk�1, yk�1 ∈ R3 arethe innovation vectors (actual and estimated). For the purpose ofexposition, the actual innovation and its estimate are differentiated;the actual innovation is expected to be zero, because qk�1jk isexpected to be equal to qk�1 if the numerical approximation of thesystem is correct (i.e., unbiased). However, using this vector inthe derivations to follow is revealing of the nature of the uncertainty inthe measurement system. By multiplying the innovation by a gainmatrix Kk�1 ∈ R6×3, the update vector is obtained:�vk�1wk�1

�≡Kk�1yk�1 �

�Kq;k�1Kω;k�1

�yk�1

�vk�1wk�1

�≡Kk�1yk�1

(86)

PERSSON AND SHARF 727

Dow

nloa

ded

by S

TA

TE

UN

IVE

RIS

TY

OF

NE

W Y

OR

K -

on

May

18,

201

3 | h

ttp://

arc.

aiaa

.org

| D

OI:

10.

2514

/1.5

9052

where vk�1 ∈ R3 is an axis-angle update, andwk�1 ∈ R3 is an updateto the angular velocity. These update vectors can then be applied tothe predicted states qk�1jk and ωk�1jk as follows:

qk�1 � qk�1jke12vk�1 (87)

ωk�1 � R�q−1k�1jkqk�1�T�ωk�1jk � wk�1� (88)

where Eq. (87) is the composition of the predicted quaternion and itsupdate vector, and Eq. (88) first updates the angular velocity vectorexpressed in the predicted frame, and then rotates it to the updatedcoordinate frame usingR�q−1

k�1jkqk�1�T . One should notice here thatthe quaternion update vector vk�1 is an SO3 arc stemming from theestimated predicted frame.With the aim of taking the covariance of this measurement update

formula, the error terms involved are first defined in a similar fashionas the prediction error terms:

δqk�1 ≡ q−1k�1qk�1 (89)

δωk�1 ≡ δqk�1ωk�1δq−1k�1 − ωk�1 (90)

δqz;k�1 ≡ q−1z;k�1qk�1 (91)

where all error terms are assumed to have a normal distribution ontheir Lie algebra, as before, that is, 2 log δqz;k�1 ∈ N �0;Pzz;k�1�.This also leads to the following definition of the random componenton the innovation:

δyk�1 ≡ 2 log�δqz;k�1δq−1k�1jk� (92)

which represents the random component associated to the rotationfrom the predicted frame to the measured frame, expressed in the Liealgebra at the predicted frame.To get to the covariance of the update scheme of Eq. (87), its

inverse is multiplied with the actual state update equation:

q−1k�1qk�1 � e

−12Kq;k�1 yk�1 q−1

k�1jkqk�1jke12Kq;k�1yk�1 (93)

in which the random component can be inserted in the predictedframe’s Lie algebra as the actual innovation is substituted for theestimate innovation

δqk�1 � e−12 Kq;k�1 yk�1e

12Kq;k�1δyk�1δqk�1jke

12Kq;k�1 yk�1 (94)

Using Eq. (87) again, e12Kq;k�1 yk�1 can be substituted for the prior and

posterior quaternion estimates, giving

δqk�1 � q−1k�1qk�1jke

12Kq;k�1δyk�1δqk�1jkq

−1k�1jkqk�1 (95)

and substituting Eq. (92) into Eq. (95) finally yields

δqk�1 � q−1k�1qk�1jke

Kq;k�1 log�δqz;k�1δq−1k�1jk�δqk�1jkq

−1k�1jkqk�1 (96)

which is an expression that can be used toward a formulation of thecovariance after the update. Because both δyk�1 and log δqk�1jk aresmall and expressed in the same Lie algebra, that of quaternionqk�1jk, the two central terms of the last equation can be merged,giving

δqk�1 � q−1k�1qk�1jke

log δqk�1jk�Kq;k�1 log�δqz;k�1δq−1k�1jk�q−1

k�1jkqk�1

(97)

As in the previous section, the quaternionic logarithm of Eq. (97) istaken, and using Corollary 3, the following is obtained:

2 log�δqk�1� � R�q−1k�1qk�1jk��2 log δqk�1jk

�Kq;k�12 log�δqz;k�1δq−1k�1jk��

(98)

on which the covariance operator can easily be applied for each sideof the equation and obtain

Pqq;k�1 � Rk�1��I3×3 −Kq;k�1�Pqq;k�1jk�I3×3 −KTq;k�1�

�Kq;k�1Pzz;k�1KTq;k�1�RT

k�1(99)

Rk�1 ≡R�q−1k�1qk�1jk� (100)

Moving the focus toward the second update equation, i.e., Eq. (88),which is much simpler, its covariance can be obtained by directlyexpressing the error on the posterior angular velocity in terms of itsupdate expression:

δωk�1 � R�δqk�1�ωk�1 − ωk�1 (101)

δωk�1 � R�q−1k�1qk�1jk��R�δqk�1jk��ωk�1 �Kω;k�1yk�1�

− ωk�1jk −Kω;k�1yk�1� (102)

which is then expressed in terms of the random components of theestimates as follows:

δωk�1 � R�q−1k�1qk�1jk��δωk�1jk �Kω;k�12 log�δqz;k�1δq−1

k�1jk��(103)

on which the covariance operator can easily be applied, yielding thefollowing expression:

Pωω;k�1 � Rk�1��−Kω;k�1 I3×3 �Pk�1jk�−KT

ω;k�1

I3×3

�

�Kω;k�1Pzz;k�1KTω;k�1�RT

k�1 (104)

Combining Eqs. (99) and (104), the overall equation for the posteriorcovariance matrix is

Pk�1 �Wk�1��I6×6 −Kk�1H�Pk�1jk�I6×6 −HTKTk�1�

�Kk�1Pzz;k�1KTk�1�WT

k�1 (105)

Wk�1 ≡�Rk�1 03×303×3 Rk�1

�(106)

H ≡ � I3×3 03×3 � (107)

which has, before the rotation, the same characteristic Riccatiequation of most Kalman filters, and for which the optimal Kalmangain matrix Kk�1 can be obtained by the following matrixexpression:

Kk�1 � Pk�1jkHT�HPk�1jkH

T � Pzz;k�1�−1 (108)

which completes the formulation of the IMKF presented here. Acomplete summary of the algorithm is presented in Table 1.

V. Results

In this section, a number of results are presented that were obtainedfrom simulation scenarios as well as experimental testing on aneutrally buoyant airship. The proposed method is validated in bothsettings.

728 PERSSON AND SHARF

Dow

nloa

ded

by S

TA

TE

UN

IVE

RIS

TY

OF

NE

W Y

OR

K -

on

May

18,

201

3 | h

ttp://

arc.

aiaa

.org

| D

OI:

10.

2514

/1.5

9052

This section is separated into three parts. First, the alternativeKalman filters that are used as a basis for comparison of the proposedKalman filter are briefly presented. Then, the simulation resultsare reported, and finally, experimental results obtained on anexperimental test bed are discussed.

A. Summary of Compared Kalman Filters

Before reporting results that show the performance of the novelKalman filter proposed in this paper, the compared approaches arefirst summarized. Three other filters will be used as a basis forcomparison, namely, the (additive) EKF, the IEKF, and the IMKF(initial version), or IMKFv1. The latter is a previous versionpresented by the authors [3], based on an invariantized explicit Eulermethod. The new version will be referred to as IMKFv2 in theexposition of the results. The choice of these filters is motivated bythe following. The EKF is a very popular and generic choice fornonlinear estimation, albeit a naïve one, which many employ due toits apparent simplicity and demonstrated success in many contexts.The IEKF represents one of the most sophisticated approaches inthe class of MEKFs.The EKF is first examined. This filter linearizes the system

dynamics around the current state estimate, treating the equationswith vector calculus (i.e., an additive discretization). Starting with an(additive) explicit Euler method, one obtains a linear approximationof the system, to which the classic Kalman filter can be applied. Thecurrent state estimate �qk; wk� at time tk produces to the linearizedsystem matrices:

FEKFk ≡

�SEKFk

h2TEKFk

03×4 I3 − hJ−1�ωk×�J

�(109)

GEKFk ≡

� −h24qTk;vJ

−1

h2

4�qk;0I3 � �qk;v×��J−1

hJ−1

�(110)

HEKF ≡ � I4×4 04×3 � (111)

where:

SEKFk ≡

24 1 − h

2�ωTk

h2�ωk I3 − h

2� �ωk×�

35

for �ωk ≡ ωk �h

2J−1�τk − �ωk×�Jωk� (112)

TEKFk ≡

�−qTk;v�I3 − h

2J−1�ωk×�J�

�qk;0I3 � �qk;v×���I3 − h2J−1�ωk×�J�

�(113)

These equations are used in the classic context of an additive Kalmanfilter as follows:

PEKFk ≡ cov

��qk − qkωk − ωk

��(114)

PEKFk�1jk � FEKF

k PEKFk �FEKF

k �T �GEKFk Pττ;k�GEKF

k �T (115)

KEKFk�1 � PEKF

k�1jk�HEKF�T�HEKFPEKFk�1jk�HEKF�T � Pzz;k�1�−1 (116)

�qk�1ωk�1

���qk�1jkωk�1jk

��KEKF

k�1�qz;k�1 − qk�1jk� (117)

PEKFk�1 � �I7×7 −KEKF

k�1HEKF�PEKF

k�1jk (118)

The EKF approach evidently has many drawbacks. Beyond the factthat a first-order approximation is used, it is also clear that the EKFdisregards the constraint on the norm of the quaternion; the ad hocsolution is to normalize the quaternion estimates after each update.Furthermore, the EKF rectifies the covariance matrices in light ofthe rotating state estimates via a first-order Taylor expansion of therotation matrix (e.g., represented by Sk in the previous term andsimilar terms elsewhere). Finally, the computational cost of the EKFis actually expected to be greater than for the IMKF and IEKF, giventhe larger matrices and cost of forming them.As for the IEKF, because the original derivation [10] of the IEKF

was in continuous time and applied to a different attitude estimationproblem (low-cost inertial navigation), a direct comparison would bemeaningless. However, using the fundamental tenants of the IEKF, adiscrete-time IEKF can be formulated as follows:

FIEKFk ≡

�I3 − h�ωk×� hI3 − h2

4J−1��ωk×�J − �Jωk×��

03×3 I3 − h2J−1��ωk×�J − �Jωk×��

�(119)

GIEKFk ≡

�h2

2J−1

hJ−1

�(120)

HIEKF ≡ � I3×3 03×3 � (121)

Table 1 The overall invariant momentum-tracking Kalman filter algorithm

Invariant momentum-tracking Kalman filter

State estimation:

qk�1jke−h4ωk�1jk � qke

h4wk

qk�1jk�Jωk�1jk − h2τk�1jk�q−1

k�1jk � qk�Jωk � h2τk�q−1

k

yk�1 � 2 log�q−1k�1jkqz;k�1�

� vTk�1 wTk�1 �T � Kk�1yk�1qk�1 � qk�1jke

12vk�1

wk�1 � R�q−1k�1qk�1jk��ωk�1jk � wk�1�

Covariance estimation:

~F ≡�R�q−1

k�1jkqk� h2�R�q−1

k�1jkqk� � J−1R�q−1k�1jkqk�J�

03x3 J−1R�q−1k�1jkqk�J

�~G ≡

�h2

4J−1

h2J−1

�Pk�1jk � ~FPk ~F

T � ~G�Rk�1jkPττ;kRTk�1jk � Pττ;k�1jk� ~GT

Kk�1 � Pk�1jkHT�HPk�1jkH

T � Pzz;k�1�−1

Wk�1 ≡ �R�q−1

k�1qk�1jk� 03x303×3 R�q−1

k�1qk�1jk�� H ≡ � I3×3 03×3 �

Pk�1 �Wk�1��I6x6 −Kk�1H�Pk�1jk�I6×6 −HTKTk�1� �Kk�1Pzz;k�1K

Tk�1�WT

k�1

PERSSON AND SHARF 729

Dow

nloa

ded

by S

TA

TE

UN

IVE

RIS

TY

OF

NE

W Y

OR

K -

on

May

18,

201

3 | h

ttp://

arc.

aiaa

.org

| D

OI:

10.

2514

/1.5

9052

The IEKF shares many features in common with the Kalman filterderived in this paper, including the definition of the covariancematrices in the Lie algebra of SO3, as well as the definition of theinnovation vector and the corresponding correction function in termsof quaternion compositions (the defining characteristic of MEKFschemes). However, the IEKF lacks a complete rotation of thecovariance matrix between estimation frames and has, instead, anapproximated rotation by the first-order term I3 − h�ωk×�. In a verypractical sense, the advantage of the proposed Kalman filter is theleveraging of the actual rotation between estimation frames instead ofrelying on an approximation of it.Finally, the original version of the proposed Kalman filter, the

IMKFv1 [3], is very similar to the IMKFv2 with the exception of thestate transition matrix FIMKFv1, which is defined as follows:

FIMKFv1k ≡

�Rk�1jk hRk�1jk03×3 Rk�1jk

�(122)

and is a result of applying an invariantized explicit Euler method inthe formulation of the state estimation covariances, where Rk�1jk isdefined as before, in Eq. (80). The IMKFv1 is the simplestformulation, and includes the rotation between estimation frames.Consequently, the effects of three main qualities of the proposedKalman filter can be observed, that is, the expression of uncertainty inthe Lie algebra of SO3 (not in the EKF), the exact rotation betweenestimation frames (not in the EKF or IEKF), and the rotation of theinertia tensor (not in the IMKFv1).To conclude this summary, it is important to remark upon the fact

that all the Kalman filters, in the implementation used to produce theresults, use the same numerical integrator to compute the statepredictions between time steps, and that method is the TRAPM aspresented earlier in this paper. This eliminates any contributions frompoor state prediction on the estimation errors. Furthermore, allimplementations are in C++ as part of the ReaK library developed bythe author.‡

B. Simulation Results

First, Monte Carlo simulations were carried out on a dynamicsmodel of a free-floating rigid body under random disturbances. Ineach simulation scenario, the rigid body starts at a nominal rotationspeed about an arbitrary axis and is then simulated over 60 s ofmotion. Disturbance torques and measurement noise are generatedartificially. For the purpose of all the simulations, the followingparameters are used:

JSIM �

26641.0 0 0

0 0.5 0

0 0 0.25

3775kgm2

QSIM � 0.1 N2m2 I3

RSIM � 0.01 rad2I3 (123)

where JSIM is the inertia tensor, QSIM is the covariance of theinput disturbances (torques, sampled at 100 Hz), and RSIM is thecovariance of the angular error in quaternion measurements.For each scenario and sampling rate, 100 trajectories of the rigid

body are generated. Then, each trajectory is used to test the fourestimators presented in the previous section, the EKF, IEKF,IMKFv1, and IMKFv2. The sampling rates tested range from 100 to0.5 Hz (plotted by sample periods from 0.01 to 2 s). The standarddeviations are calculated as the standard deviation of the angular errorof the estimates (angle of rotation between estimated quaternionsand the ground truth).Figures 1a, 1b, and 2a show the resulting standard deviations of the

different estimators when the nominal rotation speed is 0, 1, and2 rad∕s, respectively. In all cases, the error worsens toward themeasurement error level as the sampling rate is decreased, asexpected. Also, in every case, at high-measurement frequencies(100–50 Hz), all estimators tend to produce the same quality in theestimation, which is also expected due to the fact that the effectsof the nonlinearities are not felt on such short state-prediction timeintervals.However, through the figures one observes a pattern where the

IMKFv1 and IMKFv2 detach themselves from the EKF and IEKFwhen the average rotation speed is increased, especially at lowersampling rates. When the nominal rotation speed is 0, i.e., an almoststationary object, all filters produce virtually identical results, whichis expected because there are no significant rotations occurring, andangular velocity terms are negligible. If one looks closely at theresults, one observes that the detachment of the IMKF variants fromthe EKF and IEKF filters occurs as a result of the EKF and IEKFworsening as the nominal angular speed is increased, while the IMKFvariants perform consistently across different regimes. This is anexpected result of the invariance used in formulating these filters, i.e.,the formulae are invariant to any chosen moving frames, and thus,invariant to the angular speed of the estimated system. The smalldifferences that do appear as a result of increased nominal angularspeed are due to larger errors in the state predictions (integrationerrors).As of the two versions of the IMKF, one finds that in all cases they

match very closely. The only practical difference between them is therotation of the inertia tensor between predicted frames. This extracalculation seems to have no significant effect in all tests performed

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

0.02

0.04

0.06

0.08

0.1

0.12

Sample Period (s)

Std

−de

v. o

n A

ngul

ar E

rror

(ra

d)

Standard Deviation of Angular Error vs. Sample Period − SimulationNo Nominal Speed

Meas.EKFIEKFIMKFv1IMKFv2

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

0.02

0.04

0.06

0.08

0.1

0.12

Sample Period (s)

Std

−de

v. o

n A

ngul

ar E

rror

(ra

d)

Standard Deviation of Angular Error vs. Sample Period − SimulationNominal Speed of 1 rad/s

Meas.EKFIEKFIMKFv1IMKFv2

a) b)Fig. 1 MonteCarlo results as a standarddeviationof angular error versus the samplingperiod for a rotationof a) 0 rad∕s, andb) 1 rad∕s, comparing theEKF, IEKF, IMKFv1, and IMKFv2.

‡Further information available online under GPLv3 at https://github.com/mikael-s-persson/ReaK [accessed January 2013].

730 PERSSON AND SHARF

Dow

nloa

ded

by S

TA

TE

UN

IVE

RIS

TY

OF

NE

W Y

OR

K -

on

May

18,

201

3 | h

ttp://

arc.

aiaa

.org

| D

OI:

10.

2514

/1.5

9052

here. Nevertheless, one would expect the IMKFv2 to perform betterthan the IMKFv1 at all times, and indeed it does, but the margin isbarely measureable.Under much worse estimation conditions (high-speed rotations,

low sampling rates), the EKF is very susceptible to divergence,whichis not a problem observed for the IEKF or IMKF variants, unlessapproaching certain theoretical bounds (e.g., Nyquist frequency).The IMKF variants show slight divergence patterns (errors greaterthan measurement errors) when crossing the Nyquist frequency, asseen in Fig. 2b (at 2 rad∕s, the Nyquist frequency is at a sampleperiod of 157 s), while the EKFdiverges drastically, andmuch earlier,at almost half the Nyquist frequency. On the other hand, the IEKFperforms better than the IMKF variants when crossing the Nyquistfrequency. This is explained by the numerically unstable nature of thefirst-order approximation of the rotation matrix, i.e., the I3 − h�ωk×�term used in the IEKF, which, in this case, causes an overestimationof the prediction covariance, quelling the state predictions (whichbecome meaningless nearing the Nyquist frequency). Of course, inpractice, such extreme estimation scenarios are not likely to occur,certainly not by design, but these observations were made fortheoretical interests.

C. Experimental Results

In this section, the experimental results obtained with an indoorairship are presented. As part of the Aerospace MechatronicsLaboratory atMcGill University, this aerial platform is composed of a6-ft-diameter spherical bladder filled with helium, which allows it tobe neutrally buoyant and near-perfectly balanced. In addition, theairship has a number of onboard sensors and propellers (ducted fans),which allows fully actuated holonomic closed-loop control (seeFig. 3). Precise measurements of the position and orientation of theairship can be achieved through a vision-based motion capturingsystem fromVicon. This vision system is composed of six cameras inthe laboratory that emit and detect infrared light reflected off theretroreflective markers on the airship.During the experiments presented here, the controller of the airship

was inactive, and thus, the airshipwas left under free-floatingmotion,with aerodynamic disturbances from the ambient environment. Theexperiments were conducted by manually throwing the airship into aspinning motion about an arbitrary axis for a few seconds at a time,beyond which it would get too close to surrounding obstacles in theroom and would thus be stopped manually. The measurements werepreprocessed to eliminate all the transition periods and obtain only aset of records of spinning motion by a free-floating airship. Theaverage angular speeds applied during the experiments ranged from1to 2 rad∕s.From detailed CAD models of the airship and a complete mass

budget, the following inertial information were compiled:

JEXP �

26664

1.629 0 0.0183

0 1.8185 0.0101

0.0183 0.0101 1.683

37775 kgm2

mEXP � 3.7368 kg (124)

Furthermore, using records of fixed motion as well as free-floatingmotionswith no initial spin, and through careful statistical analysis ofthat data, the noise and disturbances on the system were determinedto be

QEXP � 0.027 N2m2I3 REXP � 2.25 × 10−5I4 (125)

Asone can see, themeasurement noise on the quaternion componentsis very low, as expected from the Vicon system’s specifications,which make the measurements appropriate as a “ground truth” forthe experiments. Consequently, artificial noise was added to themeasurements with the following covariance:

REXP;added � 0.01I4 (126)

After collecting a total of 44 successful test runs, the four differentKalman filters were tested on these runs. The original sampling rateof the measurement system is exactly 100 Hz, which was thendownsampled to 10 different sampling rates between 100 and 10 Hz,thus generating, in effect, 10 records for each actual test run, for a total

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

0.02

0.04

0.06

0.08

0.1

0.12

Sample Period (s)

Std

−de

v. o

n A

ngul

ar E

rror

(ra

d)

Standard Deviation of Angular Error vs. Sample Period − SimulationNominal Speed of 2 rad/s

Meas.EKFIEKFIMKFv1IMKFv2

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

2

Sample Period (s)

Std

−de

v. o

n A

ngul

ar E

rror

(ra

d)

Standard Deviation of Angular Error vs. Sample Period − SimulationNominal Speed of 2 rad/s

Meas.EKFIEKFIMKFv1IMKFv2

a) b)

Fig. 2 MonteCarlo results as standarddeviationof angular error versus the samplingperiod, up to a) 1 s, andb) 2 s, for a rotationof 2 rad∕s, comparingthe EKF, IEKF, IMKFv1, and IMKFv2.

Fig. 3 Picture of the experimental test bed, a neutrally buoyant and fullyactuatedhelium-filled airship for indoor flights, AerospaceMechatronicsLab., McGill University.

PERSSON AND SHARF 731

Dow

nloa

ded

by S

TA

TE

UN

IVE

RIS

TY

OF

NE

W Y

OR

K -

on

May

18,

201

3 | h

ttp://

arc.

aiaa

.org

| D

OI:

10.

2514

/1.5

9052

of 440 flight records. Figure 4 shows all the flight records for theKalman filters: the EKF, the IEKF, the IMKFv1, and the IMKFv2(proposed in this paper). The figure shows a scatter plot of thestandard deviation of the angular error in the estimates of the attitudegiven by the different estimation methods, against the normalizedfrequency of rotation for the given flight record (a nondimensionalnumber representing the fraction of a full rotation done by the airshipbetween two measurements). Additionally, the figure shows thescatter of standard deviations of the angular error introduced by theartificial noise, as computed from themeasurement data, which is justbelow 0.35 rad, as expected from the value of REXP;added. Finally,Fig. 4 also shows the cubic polynomial fits for the different data sets.One can observe from this figure that all methods perform very well,with no significant difference, i.e., validating the proposed filter withexperimental data.To better characterize the behaviors of the different filters, Fig. 5

shows the estimation covariance’s time history produced by theKalman filters (i.e., matrix Pk) for a representative flight record. Thefigure shows the three diagonal components of the angular errorcovariance (estimated by the filter). The main observation to make isthat the EKF rotates its covariance matrix components along with therotation of the estimation subject. Thiswill inevitably cause a rotationof the components of the Kalman gain, which prohibits any attemptsat a steady-state Kalman filter (i.e., a Luenberger observer).Furthermore, one can observe that the components of the IEKFestimation covariance do not rotate as the EKF components do,however, the two components in question are centered around thesamevalues, while the third component is higher (and in this test, thatcomponent is aligned with the rotation). This is indicative of thebiasing of the estimation covariance toward the direction of rotation.As the angular velocity increases, this bias increases as well. On the

other hand, the IMKF variants consistently show nearly equalcomponents on all axes, which is consistent with the invariance withrespect to moving frames. Additionally, this consistency of thecovariance estimates given by the IMKF variants allows for steady-state Kalman gains for steady and computationally inexpensive stateestimation.

VI. Conclusions

In this paper, theoretical concepts of invariance have beenexamined with the intent of applying them to a Kalman filteringproblem. To this end, a novel and general outline of the Kalmanfiltering process was constructed and opportunities for invariantiza-tion of the estimation process were identified and formalized.Using the natural invariance of angular motion, and a general

derivation of a trapezoidal Kalman filter (TKF), a novel formulationfor a symmetry-preserving Kalman filtering scheme for a simpleattitude estimation problem was presented in detail. This novelKalman filter, the invariant momentum-tracking Kalman filter(IMKF), incorporates two important features. First, the covariancematrices represent random quantities expressed as vectors of the Liealgebra of SO3 at specific estimated coordinate frames, which resultsin a consistent rotation of the covariances to follow the stateestimates. Second, discrete rotations of the random components andof the inertia tensor are used instead of the usual skew-symmetricterm resulting from a vector calculus approach (additive) to thediscretization problem. It was demonstrated, through extensivesimulations and experimental tests, that this proposed Kalmanfiltering scheme does indeed perform better than current state-of-the-art methods at high angular velocities, and performs equally well atgentler regimes. Additionally, the proposed method presents less

0 0.005 0.01 0.015 0.02 0.025 0.03 0.0350.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4Standard Deviation in Angular Error − Experimental Data

Normalized Frequency (Sampling Period x Angular Frequency)

Std

. Dev

. of A

ngul

ar E

rror

(ra

d)

Meas. ScatterEKF ScatterIEKF ScatterIMKFv1 ScatterIMKFv2 ScatterEKF Cubic−fitIEKF Cubic−fitIMKFv1 Cubic−fitIMKFv2 Cubic−fit

Fig. 4 Experimental results as a scatter plot of the standard deviations of the attitude estimation error (in radians) of 440 experiments, plotted by thenondimensional frequency of rotation.

0 2 4 60

0.5

1

1.5

2x 10

−3 EKF

Time (s)

Est

imat

ed C

ovar

ianc

e (r

ad2 )

0 2 4 60

0.5

1

1.5

2x 10

−3

Experimental Run 18 − Covariances for Angular ErrorsIEKF

Time (s)

Est

imat

ed C

ovar

ianc

e (r

ad2 )

0 2 4 60

0.5

1

1.5

2x 10

−3 IMKFv2

Time (s)

Est

imat

ed C

ovar

ianc

e (r

ad2 )

P

k (ang. x)

Pk (ang. y)

Pk (ang. z)

Pk (ang. x)

Pk (ang. y)

Pk (ang. z)

Pk (ang. x)

Pk (ang. y)

Pk (ang. z)

Fig. 5 Result example with experiment 18 at 100 Hz showing the estimation covariance estimated by the different filtering methods, decomposed in thethree diagonal components of attitude.

732 PERSSON AND SHARF

Dow

nloa

ded

by S

TA

TE

UN

IVE

RIS

TY

OF

NE

W Y

OR

K -

on

May

18,

201

3 | h

ttp://

arc.

aiaa

.org

| D

OI:

10.

2514

/1.5

9052

computational cost with respect to other multiplicative Kalmanfiltering approaches, and in fact, less than a typical additive Kalmanfiltering approach as well. Furthermore, results show that theproposed method lends itself better to a steady-state Kalman filterdesign, which is a direct result of the invariantization effortsincorporated into the design of this filter.Supported by the results obtained, the reader was provided with a

qualitative and quantitative assessment of the effects of the differentqualities of the proposed estimator and their contribution to theoverall success and reliability of the attitude estimation it provides.The comparison with the invariant extended Kalman filter (IEKF)and with a previous, simpler version of the IMKF, showed that theformulation of the covariance and correction formulas in the Liealgebra of SO3 already provides a major improvement (from EKF toIEKF), but that the discrete rotation of the covariance matrices alsoconstitutes a beneficial factor in formulating an accurate and reliableKalman filter for attitude estimation.

Acknowledgments

This work was made possible with the support of the VanierCanada Graduate Scholarship from the National Science andEngineering Research Council of Canada and the support of theHydro Quebec Doctoral Award as part of the McGill EngineeringDoctoral Awards.

References

[1] Kalman, R., “A New Approach to Linear Filtering and Predic-tion Problems,” Journal of Basic Engineering, Vol. 82, No. 1, 1960,pp. 35–45.doi:10.1115/1.3662552

[2] Smith, G. L., Schmidt, S. F., and McGee, L. A., “Application ofStatistical Filter Theory to the Optimal Estimation of Position andVelocity on Board a Circumlunar Vehicle,” NASATech. Rept. R-135,1962.

[3] Persson, S. M., and Sharf, I., “Invariant Momentum-Tracking KalmanFilter for Attitude Estimation,” Proceedings of the 2012 IEEE

International Conference on Robotics and Automation, Institute ofElectrical and Electronics Engineers Inc., United States, May 2012,pp. 592–598.

[4] Julier, S., Uhlmann, J., and Durrant-Whyte, H. F., “A New Method forthe Nonlinear Transformation of Means and Covariances in Filters andEstimators,” IEEE Transactions on Automatic Control, Vol. 45, No. 3,2000, pp. 477–482.doi:10.1109/9.847726

[5] Crassidis, J. L., and Markley, F. L., “Unscented Filtering for SpacecraftAttitude Estimation,” Journal of Guidance, Control, and Dynamics,Vol. 26, No. 4, 2003, pp. 536–542.doi:10.2514/2.5102

[6] VanDyke, M. C., Schwartz, J. L., and Hall, C. D., “Unscented KalmanFiltering for Spacecraft Attitude State and Parameter Estimation,”Proceedings of the AAS/AIAA Space Flight Mechanics Conference,AAS Paper 2004-115, Maui, Hawaii, 8–12 Feb. 2004.

[7] Evensen, G., “The Ensemble Kalman Filter: Theoretical Formulationand Practical Implementation,” Journal of Ocean Dynamics, Vol. 53,No. 4, 2003, pp. 343–367.doi:10.1007/s10236-003-0036-9

[8] Crassidis, J. L., Markley, F. L., and Cheng, Y., “Survey of NonlinearAttitude Estimation Methods,” Journal of Guidance, Control and

Dynamics, Vol. 30, No. 1, 2007, pp. 12–28.doi:10.2514/1.22452

[9] Lefferts, E. J., Markley, F. L., and Shuster, M. D., “Kalman Filtering forSpacecraft Attitude Estimation,” AIAA Paper 1982-0070, 1982.

[10] Bonnabel, S., Martin, P., and Salaun, E., “Invariant Extended KalmanFilter: Theory and Applications to a Velocity-aided Attitude EstimationProblem,” Proceedings of the Joint 48th IEEE Conference on Decision

and Control and 28th Chinese Control Conference, Shanghai, Instituteof Electrical and Electronics Engineers Inc., United States, 2009,pp. 1297–1304.doi:10.1109/CDC.2009.5400372

[11] Martin, P., and Salaun, E., “Generalized Multiplicative ExtendedKalman Filter for Aided Attitude and Heading Reference System,”AIAA Paper 2010-8300, 2010.

[12] Zanetti, R., Majji, M., Bishop, R. H., and Mortari, D., “Norm-Constrained Kalman Filtering,” Journal of Guidance, Control, and

Dynamics, Vol. 32, No. 5, 2009, pp. 1458–1465.doi:10.2514/1.43119

[13] Choukroun, D., Bar–Itzhack, I. Y., and Oshman, Y., “Novel QuaternionKalman Filter,” IEEE Transactions on Aerospace and Electronic

Systems, Vol. 41, No. 1, Jan. 2006, pp. 174–190.doi:10.1109/TAES.2006.1603413

[14] Markley, F. L., “Attitude Error Representation for Kalman Filtering,”Journal of Guidance, Control, and Dynamics, Vol. 26, No. 2, 2003,pp. 311–317.doi:10.2514/2.5048

[15] Girard, P. R., Quaternions, Clifford Algebras and Relativistic Physics,Springer Verlag, Berlin, 2007, pp. 19–35.

[16] Dam, E. B., Koch, M., and Lillholm, M., “Quaternions, Interpola-tions and Animation,” Dept. of Computer Science, University ofCopenhagen, Tech. Rept. DIKU-TR-98/5, Copenhagen, 1998.

[17] Marsden, J. E., and West, M., “Discrete Mechanics and VariationalIntegrators,” Acta Numerica, Vol. 10, No. 1, 2001, pp. 357–514.doi:10.1017/S096249290100006X

[18] Saccon, A., “Midpoint Rule for Variational Integrators on Lie Groups,”International Journal of Numerical Methods in Engineering, Vol. 78,No. 11, 2009, pp. 1345–1364.doi:10.1002/nme.2541

[19] Krysl, P., “Direct Time Integration of Rigid BodyMotionwith Discrete-Impulse Midpoint Approximation: Explicit Newmark Algorithms,”Communications in Numerical Methods in Engineering, Vol. 22, No. 5,2006, pp. 441–451.doi:10.1002/cnm.826

[20] Lee, T., Leok, M., and McClamroch, N. H., “Lie Group VariationalIntegrators for the Full Body Problem in Orbital Mechanics,”Celestial Mechanics and Dynamical Astronomy, Vol. 98, No. 2, 2007,pp. 121–144.doi:10.1007/s10569-007-9073-x

[21] Bou–Rabee, N., “Hamilton–Pontryagin Integrators on Lie Groups,”Ph.D. Thesis, California Inst. of Technology, Pasadena, CA, June 2007.

[22] Kim, P., “Invariantization of Numerical Schemes Using Mov-ing Frames,” Bit Numerical Mathematics, Vol. 47, No. 3, 2006,pp. 525–546.doi:10.1007/s10543-007-0138-8

[23] Valpiani, J. M., and Palmer, P. L., “Nonlinear Symplectic AttitudeEstimation for Small Satellites,” Collection of Technical Papers —

AIAA/AAS Astrodynamics Specialist Conference, Vol. 1, AIAA, UnitedStates, 2006, pp. 306–321.

[24] Dahlquist, G. G., “A Special Stability Problem for Linear MultistepMethods,” BIT Numerical Mathematics, Vol. 3, No. 1, 1963, pp. 27–43.doi:10.1007/BF01963532

[25] Rahman, Q., and Schmeisser, G., “Characterization of the Speed ofConvergence of the Trapezoidal Rule,” Numerische Mathematik,Vol. 57, No. 1, 1990, pp. 123–138.doi:10.1007/BF01386402

PERSSON AND SHARF 733

Dow

nloa

ded

by S

TA

TE

UN

IVE

RIS

TY

OF

NE

W Y

OR

K -

on

May

18,

201

3 | h

ttp://

arc.

aiaa

.org

| D

OI:

10.

2514

/1.5

9052