Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay

Vector Measurements
Alireza Khosravian, Jochen Trumpf, Robert Mahony, Tarek Hamel

Abstract— This paper proposes an attitude estimation communication channel from these sensors to the onboard
methodology for the case where attitude sensors provide attitude estimation system of the vehicle.
discrete-time samples of vector measurements at different Sampling and delays can negatively affect the stability
sample rates and with time delays. The proposed methodology
is based on a cascade combination of an output predictor and robustness of any observer or filter and degrade their
and an attitude observer or filter. The predictor compensates performance if they are not compensated for properly [18]–
for the effect of sampling and delays in vector measurements [24]. Typical estimator design methodologies to tackle the
and provides continuous-time predictions of outputs. These measurement sampling and delay problem are; estimator
predictions are then used in an observer or filter to estimate design with Lyapunov-Krasovskii modification, stochastic
the current attitude. The primary contribution of the paper is
to exploit the underlying symmetry of the attitude kinematics filtering with Out-Of-Sequence Measurements (OOSM), and
to design a recursive predictor that is computationally simple compound observer-predictor design. The classical approach
and generic, in the sense that it can be combined with any to tackle the sensor delay is to take an estimator that has the
asymptotically stable observer or filter. We prove that the desired performance for delay free measurements, and mod-
predictor is able to reproduce the continuous time delay-free ify its innovation term such that it compares each delayed
vector measurements. In a simulation example, we demonstrate
good performance of the combined predictor-observer even in measurement with its corresponding backward time-shifted
presence of measurement noise and delay uncertainties. estimate. If the delay-free estimator has a Lyapunov stability
proof, the stability analysis for the modified estimator can be
I. I NTRODUCTION undertaken using Lyapunov-Krasovskii functions [25], [26].
Although these modified estimators are commonly used in
Attitude sensors mounted on a vehicle measure partial
practice (see e.g. [27], [28]), they require complicated stabil-
information about its attitude in the form of vector direc-
ity analyses and careful and conservative gain tuning, lead-
tion measurements. The goal of an attitude estimator is to
ing to poor transient responses of the resulting estimators.
compute the orientation of the vehicle by processing those
Stochastic filtering with OOSM has been extensively studied
vector measurements. There is a large body of research
[29]–[33], albeit most of this literature focuses on target
on both stochastic attitude estimation methods (such as
tracking applications. Although OOSM filtering approaches
extended Kalman filters [1], [2], unscented filters [3], etc.) as
are flexible, easily dealing with sampled and delayed data
well as deterministic attitude observers [4]–[16]. In satellite
as well as out-of-sequence measurements, they usually have
attitude estimation applications, high accuracy sensors such
significant memory and processing requirements that are
as star trackers or earth sensors provide measurements at low
unrealistic for most embedded observer design applications,
sampling rates (0.5 to 10 Hz) [17]. In contrast, the onboard
except for linear system models where simpler OOSM filters
gyroscope can easily provide high bandwidth measurements
are available [20], [29]. For the specific problem of atti-
at kHz rates, potentially two orders of magnitude faster than
tude estimation with sampled and delayed measurements,
the direction information is obtained. The image processing
a modified extended Kalman filter with a novel real time
inside a star-tracker sensor can cause significant delays in
implementation architecture is proposed in [34]. Despite its
the order of tens of milliseconds, leading to the star-tracker
good performance in practice, this algorithm suffers from
measurement being delayed with respect to the gyroscope
major drawbacks such as unclear convergence properties and
measurements. Similar sampling and delay problems also
high computational load due to the required propagation
occur in attitude estimation for aerial robots when vision
stages associated with sensor delay compensation. Combined
based sensors such as cameras and landmarks are employed.
observer-predictor design methods for nonlinear systems on
Also, in indoor flight environments, the attitude data from
Rn have been developed in [19], [24], [35]. These methods
devices such as VICON or OptiTrack are delayed by the
take observers that have the desired stability properties for
A. Khosravian, J. Trumpf and R. Mahony are with the Research continuous delay-free measurements and combine them with
School of Engineering, Australian National University, ACT 2601, Aus- appropriate predictors that compensate for the effects of
tralia (e-mail: [email protected]; [email protected]; sensor sampling and delays, such that the combined observer-
[email protected]).
T. Hamel is with I3S UNS-CNRS, France (e-mail: [email protected]). predictor maintains the stability properties of the observer.
This work was partially supported by the Australian Research Council The authors of this paper have recently proposed a cascade
through the ARC Discovery Project DP120100316 ”Geometric Observer observer-predictor combination to handle sensor delay in
Theory for Mechanical Control Systems” and by the project ”Sensory
Control of Aerial Robot” from French Agence Nationale de la Recherche the attitude estimation problem [36]. Although the result-
through the ANR-ASTRID SCAR. ing observer-predictor combination is stable, this method
requires continuous availability of sensor outputs and is not delayed with respect to the measured physical quantities due
applicable to the sampled measurement case. To the authors’ to various reasons such as slow response rates of the physical
knowledge, there is no attitude estimation methodology with parts of the sensors, internal processing time of sensors, and
stability proof available that considers sampled and delay communication delays. In the following two subsections, we
measurements. present a general discussion about modeling sampling and
In this paper, we consider the attitude estimation prob- delays in sensors. This discussion will then be applied to the
lem when sampled and delayed vector measurements are specific case of attitude sensors and vector measurements in
available. We propose a cascade combination of a predictor Section II-C.
with an attitude observer or filter in which the predictor
compensates for the effect of sampling as well as delays A. Physically inspired modeling of sampling and delays
in vector measurements and the filter or observer processes We propose the model illustrated in Fig. 1 to include
the predicted outputs and estimates the attitude. Our design the effect of sampling and delays on the output of sensors.
is based on the exact continuous time nonlinear attitude This model is inspired by the physical process that takes
kinematics on the Lie group SO(3) without resorting to place in sensors during measuring a physical quantity. This
parameterization, linearization, or discretization. The main model consists of a zero-order-hold (ZOH) block that models
contribution of the paper is to effectively employ the sym- the effect of sampling and two delay blocks before and
metries of the attitude kinematics and vector measurement after the ZOH that, respectively, model the pre-sampling
models to design a simple generic predictor that is inde- and post-sampling delays. The pre-sampling delay on the
pendent of the choice of observer or filter. That is, our left side of Fig. 1 models ρi seconds of delay from when
proposed predictor can be combined with any observer the physical quantity yi (t) occurs to when it is observed
or filter that has asymptotically stable estimation error in by the i-th sensor. We have yiρ (t) = yi (t − ρi ) for all
ideal conditions (i.e. when it is fed with continuous time t. In practice, this delay is usually due to the physical
delay-free vector measurements) and the predictor-observer properties of the environment or the sensors. For instance,
combination maintains those stability properties in the non- a star tracker requires that its imaging sensor is exposed to
ideal conditions (i.e. sampled and delayed measurements). light from stars for a specific amount of time so that it can
The proposed predictor is recursive and requires only very produce an image of the stars. This is known as exposure
small computational power, making it ideal for embedded time and can be as large as hundreds of milliseconds [37].
implementation in real-world applications. We assume that The ZOH block in Fig. 1 takes the delayed signal and
the delay in each sensor measurement is known, that is we produces a sample at time tki . This sample is latched at
require accurate time-stamping of data, however, this is the the output of ZOH until the next sample is taken at time
only condition on the data. Given this assumption, the gain tki +1 . Hence we have ziρ (t) = yiρ (tki ) = yi (tki − ρi )
tuning process and the stability of the observer is independent for t ∈ [tki , tki +1 ). For clarity in presentation, we assume
of the size of the delay and valid even for time varying that the sequence (tki )∞ ki =1 is an ordered monotonically
delays or OOSM measurements although we do not explicitly increasing sequence, i.e. tki −1 ≤ tki ≤ tki +1 . However,
consider the latter in this paper. The proposed approach this assumption is not necessary for our proposed method
directly extends to the multi-rate measurement case without and our method is also applicable to the case where the
further modification. Via a simulation example, we show that measurements are out-of-sequence, although the necessary
our predictor-observer method performs significantly better modifications to the notation are rather cumbersome. For
than Lyapunov-Krasowskii approach. a star tracker, the sequence (tki )∞ ki =1 corresponds to the
The structure of the paper is as follows. Background and specific times when the star tracker obtains an image of stars.
problem formulation is given in section II. The proposed This sampling frequency can be as low as only 0.5 Hz up
predictor-observer approach is described in section III where to 10 Hz for practical star trackers. The post-sampling delay
the main result of the paper is given by Theorem 1. The on the right side of Fig. 1 models σi seconds of delay from
performance of our method is demonstrated via simulations when a sample of the physical variable becomes available to
in section IV. the sensor to the time when the new output zi (t) becomes
available to the user. Hence we have
II. P ROBLEM FORMULATION
zi (t) = ziρ (t − σi ) = yi (tki − ρi ), t ∈ [tki + σi , tki +1 + σi )
Attitude determination sensors aim to measure physical
(1)
quantities that are usually continuous time objects by their
nature. Examples of these physical quantities are the light In practice, the post-sampling delay models the delay due
intensity of stars or the Sun, respectively, sensed by star to the internal signal processing in the sensor or due to the
sensors or sun sensors, the Infra-Red reflection of the Earth communication delay for transmitting information from the
surface sensed by Earth sensors, or the magnetic field of the sensor to the user. For a star tracker, the post-sampling delay
Earth sensed by magnetometers, all of which are continuous is mainly due to the processing time associated with image
time objects. In practice, however, attitude sensors are only processing algorithms that analyze the images taken by the
capable of providing samples of those physical quantities at star tracker to recognize stars in the image and associate each
specific sampling rates. Moreover, these samples are usually recognized star with its corresponding star in the on-board
 Sensor Model

yi (t ) yi (t ) z i (t ) z i (t ) the combination of the delays ρi and σi of Fig. 1 and the

{tk } sampling sequence (t0ki )∞
i i i ki =1 in Fig. 1 is equivalent to the
Sensor Model sequence of times by which the user receives the outputs
Fig. 1. Modelling𝑧 the = effect
𝑦 (𝑡 of
− 𝜏sampling and delays in attitude sensors zi (t) of Fig. 2. Given a model in the form of Fig. 1, we are
1 𝑡 𝑝𝑟 )
always able to simplify that model to the form of Fig. 2 by
yi (t ) yi (t ) 𝑧2 𝑡 = 𝑦 𝑘𝑇𝑠z− i
( t𝜏)𝑝𝑟
proper choice of the delay and sampling sequence in Fig. 2.
i 𝑡 ∈ {t
[𝑘𝑇 }
ki 𝑠 , 𝑘 + 1 𝑇 𝑠 ) This in particular means that, as far as the input-to-output
characteristics of the sensors are concerned, there is no need
𝑧 𝑡 = 𝑦 𝑘𝑇𝑠 − 𝜏to 𝑝𝑟 separately know the value of the pre-sampling and post-
Fig. 2. Simple model that 𝑧1 𝑡 is=input-to-output
𝑦 (𝑡 − 𝜏𝑝𝑟 ) equivalent to Fig. 1
𝑡 ∈ [𝑘𝑇𝑠 + 𝜏𝑝𝑜 , 𝑘 + 1 sampling
𝑇𝑠 + 𝜏𝑝𝑜 ) delays. In fact, as we show in Section III, only the
𝑧2 𝑡 = 𝑦 𝑘𝑇𝑠 − 𝜏𝑝𝑟 knowledge of the total delay τi between the physical value
star catalog. The post-sampling delay can also model
𝑡 ∈ [𝑘𝑇𝑠 , 𝑘 + 1 𝑇𝑠 ) the lag yi (t) and the sensor output suffice to reproduce the physical
due to the communication delay from VICON or OptiTrack quantity yi (t) from the sampled and delayed sensor output.
𝑦: Ideal inputtotothe
systems theonboard
sensor (i.e. variable
attitude to be measured)
estimation system of flying 𝑧 𝑡 = 𝑦 𝑘𝑇𝑠 − 𝜏𝑝𝑟
C. Attitude kinematics and sensor models for vector mea-
𝑧1 : Intermediate variable denoting the output of pre-sampling delay
vehicles in indoor flight environments. It also can 𝑡 ∈ [𝑘𝑇
model𝑠 + 𝜏𝑝𝑜 ,block.
𝑘 + 1 𝑇𝑠 + 𝜏𝑝𝑜 )
surements
the measurement delay associated with internal processing
𝑧2 : Intermediate variable denoting the output of zero-order hold. Consider a rigid body with a body-fixed reference frame
in GPS modules.
𝑧: Output of sensor (i.e. available measurement) {B} and an inertial reference frame {A}. Denote the attitude
𝑦: Ideal
B. An input-to-output
input to theequivalent
sensor (i.e.modelvariable to be measured) matrix of the rigid body by R ∈ SO(3) that corresponds
Although the model
𝑧1 : Intermediate illustrated
variable denoting in Fig.the 1outputis suitable at the todelay
of pre-sampling rotation from {B} to {A}. The rigid body attitude
the block.
modeling stage to carefully describe the effect of sampling kinematics is given by
𝑧 2 : Intermediate variable denoting the
and various delays, it might not be convenient to be employed output of zero-order hold.
Ṙ(t) = R(t)Ω(t)× , R(0) = R0 (3)
𝑧: Output of sensor
for compensation of delays(i.e. and sampling
available effects at the design
measurement)
stage. The main disadvantage of this model is that even if the where Ω is the angular velocity vector of {B} with respect
user knows the value of σi , the value of tki is only available to {A} expressed in {B}. The linear operator (.)× maps any
from the time tki + σi onwards. That is, the sensors usually vector in R3 to its corresponding skew-symmetric matrix in
do not inform the user when exactly they obtain samples so(3) such that (a)× b is equal to the cross product a × b for
of measurements. Instead, they inform the user when they all a, b ∈ R3 . We assume that delay-free measurements of
finish processing the samples and the processing result (i.e. Ω(t) are available in continuous time. This is a reasonable
the sensor output) is ready to be collected by the user. This assumption since in practice Ω(t) is measured at a high
is why we discuss a simpler model here which is input-to- sampling rate using 3-axis gyros.
output equivalent to Fig. 1, but is more convenient to use for Ideal attitude sensors attached to the rigid body provide
compensation of the effect of sampling and delays. Later on partial measurements of attitude in the form of vector mea-
in this section, we discuss the condition under which these surements given by
two models are equivalent.
Assume that at time t0ki , ki = 1, 2, . . ., we receive the yi (t) = R(t)> ẙi , i = 1, 2, . . . , n (4)
0
most recent output of the i-th sensor denoted by zi (tki ). We where yi ∈ S2 denotes the measured vector in {B} and
assume that this output is delayed τi seconds with respect ẙi ∈ S2 denotes the corresponding reference vector of yi
to the measured physical quantity yi (t0ki ). This output is in {A}. One can replace yi (t) from (4) into (1) and (2)
latched until the next output arrives at t0ki +1 . This procedure to obtain attitude sensor models with sampling and delays
is equivalent to a cascade combination of a delay operator corresponding to Fig. 1 and Fig. 2, respectively.
and a ZOH, as sketched in Fig. 2. We have The problem at hand is to design an estimation methodol-
0 0
zi (t) = zi (tki ) = yi (tki − τi ), t ∈ [tki , tki +1 ). 0 0
(2) ogy that uses the continuous measurement of Ω(t) together
with the sampled and delayed vector measurements zi (t) to
Each sequence (t0ki )∞ ki =1 is ordered monotonically increasing provide continuous estimates of the attitude matrix R(t).
(again, this assumption is not necessary for our method but it
is imposed for clarity in presentation). The main difference III. P REDICTOR - OBSERVER A PPROACH
compared to the model discussed in Section II-A is that here Due to the reasons discussed in Section II-B, we opt
the sequence (t0ki ) is known to the user and can be used for to work with the simplified sensor model (2) to design an
compensating the effect of sampling and delays. It is obvious algorithm that compensates for the effects of sampling and
that the outputs of Fig. 1 and Fig. 2 can be different in both pre and post-sampling delays and estimates the attitude.
general. However, simple calculations provided in Appendix The approach that we propose here to tackle the problem
A show that both models are input-to-output equivalent (i.e. formulated in Section II is illustrated in Fig. 3. We first
the output zi (t) of both models are equal for all t when both propose a predictor that takes the sampled and delayed mea-
models measure the same physical quantity yi (t)) if and only surements zi (t) and provides continuous time predictions of
if τi = ρi + σi and (t0ki )∞ ∞
ki =1 = (tki + σi )ki =1 . That is, when yi (t) denoted by yip (t). The predictor relies on the knowledge
both models are equivalent, the delay τi in Fig. 2 represents of Ω(t) in continuous time (or practically at high frequency)
 (t ), z i (tk ), i
, i  1,..., n of the pure observer (7) is shown in [38] for general positive
i
definite gain matrices P (t) and Li , i = 1, . . . , n. These gains
are chosen to obtain the desired observer performance. For
Predictor instance, P (t) can be recursively updated using Riccati dif-
ferential equations as in extended Kalman filters [1], [2], or it
yip (t ), i  1,..., n can be obtained using modified Riccati differential equations
(t ) as in geometric approximate minimum-energy filters [10].
Choosing constant positive definite gain matrices simplifies
Observer (7) to the well-known geometric attitude observer proposed
in [4], [7], [11], [39]. In any case, assuming that the observer
Rˆ(t )
(7) fed with the ideal vector measurements yi (t) rather than
the predicted outputs yip (t) (i.e. replacing yip (t) with yi (t) in
Fig. 3. Illustration of the proposed predictor-observer approach (5)-(7)
(7)) yields stable estimation error dynamics, then Theorem
1 implies that the combined predictor-observer (5),(6), and
(7) retains those stability properties of the observer for all
and the total delay τi = ρi +σi to compensate for the effect of choices of ∆0 ∈ SO(3). In particular, if the constant gain
sampling and delay in the outputs and to predict the outputs observer of [4] is employed as (7), then the estimated attitude
such that yip (t) = yi (t) for all t ≥ t01i in noise-free conditions R̂(t) converges almost globally asymptotically and locally
(i.e. when there is no measurement noise in zi (t0ki ) or Ω(t) exponentially to the true attitude R(t) for all choices of
and the integration procedure within the predictor is also ∆0 ∈ SO(3).
exact). The predicted outputs yip (t), i = 1, . . . , n together Note that in order to implement the proposed predictor-
with the angular velocity measurement are then fed into observer methodology, it is only required to implement one
an observer to compute an estimate of attitude denoted by copy of the predictor dynamics (5) and one copy of the
R̂(t). Our proposed predictor is generic in the sense that observer dynamics (7) even though we have several vector
it is independent of the employed observer algorithm, i.e., measurements zi (t), i = 1, . . . , n with possibly different de-
the predictor can be coupled with any asymptotically stable lays τi and possibly different sampling sequences (t0ki )∞ ki =1 .
attitude observer or filter to estimate the attitude. Only a fixed duration buffer for the predictor state ∆(t) is
Our proposed predictor takes the form needed.
˙ Remark 1: Our proposed method is also applicable to
∆(t) = ∆(t)Ω(t)× , ∆(0) = ∆0 , (5) the case where the delay τi is time-varying and out-of-
yip (t) = ∆(t) >
∆(t0ki − τi )zi (t), t ∈ [t0ki , t0ki +1 ), (6) sequence measurements do potentially occur. In this case, we
should replace the notation τi with τki (forming the sequence
where ∆ ∈ SO(3) is the internal state of the predictor and
(τki )∞
ki =1 ) and each measurement delay τki should be known
∆0 ∈ SO(3) is an arbitrary initial condition. The trajectory
to the user at time t0ki . 
∆(t) of the predictor dynamics (5) needs to be stored in a
buffer for the previous t0ki +1 − t0ki + τi seconds in order to Remark 2: Although the predictor-observer idea presented
compute the prediction yip (t) (6) at each time. in this paper focuses on the attitude kinematic system on
The following theorem summarizes the properties of the SO(3), this idea can be generalized to kinematic systems
proposed predictor. on general Lie groups. For the very special case where the
underlying Lie group is Rn , the kinematic system is simply
Theorem 1: Consider the predictor (5)-(6) for the attitude
the linear integrator ẋ(t) = u(t) where x(t) ∈ Rn is the
dynamics (3) and the sensor measurements (2) with (4).
state and u(t) ∈ Rn is the input. The output is given by
The predicted output yip (t) is equal to the ideal vector
y(t) = Cx(t) ∈ Rm where C ∈ Rm×n , and the sensors
measurement yi (t) for all t > t1i and all choices of ∆0 ∈
provide the delayed measurement z(t) = y(t − τ ). It is easy
SO(3). 
to adapt the predictor proposed in this paper and obtain the
Proof of Theorem 1 is given in Appendix B.
following simple predictor
Even though the proposed predictor is independent of the
choice of observer, for the sake of concreteness, here we
couple the predictor with the following geometric attitude δ̇(t) = u(t), (8)
p
observer [10]. y (t) = C(δ(t) − δ(t − τ )) + z(t). (9)
n
˙
 X 
Li (ŷi (t) − yip (t)) × ŷi (t) ,


R̂(t) = R̂(t) Ω(t) − P (t) In this case, the predictor (8)-(9) corresponds to the well-
×
i=1 known Smith predictor [40] originally designed for output
(7) feedback control of linear systems with delayed measure-
ments. Note, however, that in the context of observers this
with R̂(0) = R̂0 ∈ SO(3) and t ≥ max t1i , where R̂(t)
i=1,...,n predictor does not seem to suffer from the Smith predictor’s
is the estimate of R(t), ŷi (t) := R̂(t)> ẙi , and P (t) and well documented stability issues in the presence of delay
Li , i = 1, . . . , n are positive definite gain matrices. Stability uncertainty (see section IV). 
 IV. S IMULATION RESULTS
In this section, we provide a set of simulations to il-
lustrate the performance of our proposed predictor-observer
methodology. To generate the trajectory of R(t), we im-
plement (3) with Ω(t) = [0; 0; 8] (deg/s) and the initial
attitude R0 corresponding to the initial roll 14 (deg), pitch
0 (deg), and yaw 0 (deg). We suppose that the attitude
sensors provide the vector measurements corresponding to Fig. 4. Attitude estimation error of combined predictor-observer (5)-(7).
the reference directions ẙ1 = [1 0 0]> and ẙ1 = [0 1 0]> . The red plot is the enlarged steady state estimation error.
Although in practice the number of vector measurements
can be high and their directions are not necessarily pairwise
perpendicular (e.g. for star trackers), here we consider only
two vector measurements with perpendicular directions to
avoid unnecessary discussions on gain tuning and focus only
on the sampling and delay effects. To model z1 (t) and z2 (t),
the ideal vector measurements y1 (t) and y2 (t) are obtained
by (4) and then fed to the block diagram of Fig. 1 with
pre and post-sampling delays of ρ1 = ρ2 = 0.1 (s) and
σ1 = σ2 = 0.3 (s), respectively, yielding a total delay of Fig. 5. Attitude estimation error of the ad-hoc observer. The red plot is
τ1 = τ2 = 0.4 (s), and a sampling rate of 5 (Hz). Zero the enlarged steady state estimation error.
mean Gaussian noises with a standard deviation of 0.01
are added to each axis of the vector measurements z1 (t)
innovation term is zero which simplifies the observer to a
and z2 (t) which approximately add perturbations with the
forward integration of attitude kinematics. This innovation
standard deviation of 1 (deg) to the directions of z1 (t) and
term is mathematically formulated as follows.
z2 (t). The angular velocity Ω(t) is sampled at 100 (Hz) and  ¯
perturbed by an additive noise of 0.05 (deg/s) in each axis.  l1 z1 (t0k1 )× R̂ad (t0k1 − τ1 )> ẙ1 , t = t0k1 6= t0k2
 ¯

0 0 >
For the simulation, we combine the predictor (5)-(6) with l z (t ) R̂ (t − τ ) ẙ2 , t = t0k2 6= t0k1
α(t) = P2 2 2 ¯k2 × 0 ad k2 0 2
the geometric observer of [4]. This observer corresponds to > 0 0
i=1 li zi (tki )× R̂ad (tki − τi ) ẙi , t = tk1 = tk2


choosing scalar constant observer gains in (7) yielding 
0 0
t 6= tki
˙
R̂(t) = R̂(t) Ω(t) + l1 y1p (t)× ŷ1 (t) + l2 y2p (t)× ŷ2 (t) ×


This ad-hoc method adaptation of observers is commonly
(10) used in engineering applications to handle sensor sampling
and delay effects (see e.g. [27], [28] for an EKF example).
with ŷi (t) := R̂(t)> ẙi (t) and li > 0, i = 1, 2. We compare The initial conditions of the combined predictor-observer
the performance of this combined predictor-observer with (i.e. R̂(0.4) and ∆(0)) and the initial condition of the
an ad-hoc adaptation of the constant gain observer of [4] ad-hoc observer (i.e. R̂ad (t), t ∈ [0, 0.4]) are set to the
to the case of sampled and delayed vector measurements. identity matrix. The attitude estimation error of the combined
˙ predictor-observer is illustrated in Fig. 4, where the observer
The dynamics of the ad-hoc observer is given by R̂ad (t) =
R̂ad (t) Ω(t) + α(t))× where R̂ad (t) is the estimate of R(t) gains are chosen as l1 = l2 = 0.5. In this figure, the error
and α(t) is the innovation term. When the attitude sensor θ̃ is the angle of rotation in the angle-axis representation
provides the measured sample z1 (t0k1 ) at time t = t0k1 , the of the attitude estimate error R̂(t)R(t)> and is given by
>
innovation term of the ad-hoc observer is inspired by the θ̃(t) = 180
π arccos(1 − 0.5tr(I − R̂(t)R(t) )). Note that the
constant gain observer as α(t0k1 ) = ¯l1 z1 (t0k1 )× R̂ad (t0k1 − observer trajectories are available after the first sample of
τ1 )> ẙ1 with ¯l1 > 0. This innovation term compares the vector measurements have been provided by the attitude
the newly received measurement z1 (t0k1 ) with its estimate sensors. The red plot shows the steady state estimation error
R̂ad (t0k1 − τ1 )> ẙ1 in which the effect of the measurement which illustrates the good performance of our proposed
delay τ1 is considered1 . Similarly, at time t = t0k2 when method even with high sensor delay, low sampling rate,
the measurement z2 (t0k2 ) is delivered by attitude sensors, the and high noise. Fig. 5 shows the estimation error θ̃ad (t) =
180 >
innovation term is α(t0k2 ) = ¯l2 z2 (t0k2 )× R̂ad (t0k2 − τ2 )> ẙ2 π arccos(1 − 0.5tr(I − R̂ad (t)R(t) )) of the ad-hoc ob-
with ¯l2 > 0. If t0k2 happens to be equal to t0k1 for some server when its gains are chosen as ¯l1 = ¯l2 = 42.5 such that
pair (k1 , k2 ), then the innovation term is simply the sum the error trajectory of this observer has approximately the
¯l1 z1 (t0 )× R̂ad (t0 − τ1 )> ẙ1 + ¯l2 z2 (t0 )× R̂ad (t0 − τ2 )> ẙ2 . same transient convergence rate as Fig. 4. Comparing Fig. 4
k1 k1 k2 k2
For the times where no sample of any vector measurement and Fig. 5, the steady state error of our predictor-observer
is available (i.e. for all t ∈ / (tk1 )∞ ∞ is almost an order of magnitude less than the steady state
ki =1 ∪ (tk2 )ki =1 ), the
error of the ad-hoc observer. Next, we increase the sensor
1 Due to the consideration of the effect of delay in the innovation term, delays to ρ1 = ρ2 = 0.5 (s) and σ1 = σ2 = 1.5 (s)
it can be thought of as a Lyapunov-Krasovskii term [25], [26], [35]. yielding a total sensor delay of 2 (s). With the same gains
Fig. 6. Attitude estimation error of combined predictor-observer (5)-(7)
with large sensor delay. The red plot is the enlarged steady state estimation Fig. 8. Attitude estimation error of combined predictor-observer (5)-(7)
error. with delay uncertainty. The small plots are the steady state estimation errors.

symmetries of the attitude kinematics and the system output

maps, our proposed predictor is capable of reconstructing
continuous-time delay free predictions of the vector mea-
surements. The proposed predictor is generic and can be
combined with arbitrary observers or filters. When combined
with a geometric attitude observer, our proposed predictor-
observer approach shows improved performance in simula-
Fig. 7. Attitude estimation error of the ad-hoc observer with large sensor tion compared to Lyapunov-Krasovskii methods.
delay.
VI. ACKNOWLEDGMENT
The authors would like to thank Prof. Randal W. Beard
and initial conditions as in the previous simulation, the error
for his helpful comments on OOSM filtering methods.
trajectories of the predictor-observer and the ad-hoc observer
are illustrated in Fig. 6 and Fig. 7, respectively. These plots A PPENDIX
show the convergence of the estimation error of our proposed
predictor-observer while the estimation error of the ad-hoc A. Equivalency of sensor models:
observer diverges. The small degradation of the steady state Setting zi (t) = wi (t) for all t in (1) and (2) yields
estimation error of Fig. 6 compared to Fig. 4 is due to the yi (tki ) = yi (t0ki −τi ) and [tki +σi , tki +1 +σi ) = [t0ki , t0ki +1 ).
fact that the predictor relies on noisy gyro measurements to These equalities hold if and only if τi = ρi + σi and
compensate for the delay in vector measurements. Hence, a (t0ki )∞ ∞
ki =1 = (tki + σi )ki =1 . 
larger delay means longer integration of gyro noise which
increases the estimation error. Nevertheless, the steady state B. Proof of Theorem 1:
estimation error of Fig. 6 is less than twice the corresponding The proof is based on application of the following Lemma
error in Fig. 4 even though the sensor delay is increased by that allows time-shifting of the attitude trajectory using the
a factor of five. trajectory of the predictor.
Next, consider the same condition as the first simulation Lemma 1: The trajectory R(t) of the attitude kinematics
scenario, but, assume that there is uncertainty in knowledge (3) and the trajectory ∆(t) of the predictor dynamics (5)
of the amount of delay. To this end, we consider the sensor satisfy R(t2 ) = R(t1 )∆(t1 )> ∆(t2 ) for all t1 , t2 > 0 and all
model of Fig. 1 with the same parameters as the first R0 , ∆0 ∈ SO(3). Moreover, assuming (4), the trajectory of
simulation but we consider two examples where the amount yi (t) satisfies yi (t2 ) = ∆(t2 )> ∆(t1 )yi (t1 ) for all t1 , t2 > 0
of the total delay that is used in the predictor (5)-(6) is either and all R0 , ∆0 ∈ SO(3). 
10 or 50 percent more than the total delay in the simulated Proof of Lemma 1: Using (3) and (5) we have
sensor model (i.e. τ1 = τ2 = 0.44 (s) or τ1 = τ2 = 0.6 d > > > >
dt (R(t)∆(t) ) = R(t)Ω(t)× ∆(t) +R(t)Ω(t)× ∆(t) = 0
(s), respectively). Fig. 8 shows that the estimation error is for all t ≥ 0. This means that R(t)∆(t) is constant >
practically stable in both cases although the steady state for all t ≥ 0 which in particular implies R(t1 )∆(t1 )> =
estimation error is increased comparing to Fig 4. The steady R(t2 )∆(t2 )> for all t1 , t2 ≥ 0 and proves the first
state estimation errors are less than 0.5 (deg) and 1.8 (deg) claim of Lemma 1. Using (6) we have yi (t2 ) =
respectively for 10% and 50% delay uncertainties which still R(t2 )> ẙi = ∆(t2 )> ∆(t1 )R(t1 )> ẙi = ∆(t2 )> ∆(t1 )yi (t1 )
demonstrate a very good performance considering the high for all t1 , t2 ≥ t1i . This completes the proof. 
values of noise and delay uncertainties. Choosing t1 = t0ki − τi and t2 = t and invoking Lemma
1 we have yi (t) = ∆(t)> ∆(t0ki − τi )yi (t0ki − τi ). Now,
V. C ONCLUSION
choosing t ∈ [t0ki , t0ki +1 ) and recalling (2) we have yi (t) =
We propose a combined predictor-observer methodology ∆(t)> ∆(t0ki − τi )zi (t) for all t ∈ [t0ki , t0ki +1 ) which together
for the attitude estimation problem in the presence of with (6) implies yip (t) = yi (t) for all t ≥ t1i . This completes
sampled and delayed vector measurements. Exploiting the the proof of Theorem 1.
 R EFERENCES [24] I. Karafyllis and C. Kravaris, “From continuous-time design to
sampled-data design of observers,” IEEE Trans. Automatic Control,
[1] F. L. Markley, “Attitude error representations for kalman filtering,” vol. 54, no. 9, pp. 2169–2174, 2009.
Journal of guidance, control, and dynamics, vol. 26, no. 2, pp. 311– [25] T. Ahmed-Ali, E. Cherrier, and F. Lamnabhi-Lagarrigue, “Cascade
317, 2003. high gain predictors for a class of nonlinear systems,” IEEE Trans.
[2] E. J. Lefferts, F. L. Markley, and M. D. Shuster, “Kalman filtering Automatic Control, vol. 57, no. 1, pp. 221–226, 2012.
for spacecraft attitude estimation,” Journal of Guidance, Control, and [26] S. Bahrami and M. Namvar, “Delay compensation in global estimation
Dynamics, vol. 5, no. 5, pp. 417–429, 1982. of rigid-body attitude under biased velocity measurement,” in Proc.
[3] J. L. Crassidis and F. L. Markley, “Unscented filtering for spacecraft IEEE Conf. on Decision and Control, December 2014.
attitude estimation,” Journal of guidance, control, and dynamics, [27] P. Riseborough, (visited on September 2014). [Online]. Available:
vol. 26, no. 4, pp. 536–542, 2003. https://github.com/priseborough/InertialNav
[28] “APM: Navigation extended Kalman filter overview,”
[4] R. Mahony, T. Hamel, and J.M. Pflimlin, “Nonlinear complementary
(visited on September 2014). [Online]. Available:
filters on the special orthogonal group,” IEEE Trans. Autom. Control,
http://copter.ardupilot.com/wiki/common-apm-navigation-extended-
vol. 53, no. 5, pp. 1203–1218, 2008.
kalman-filter-overview/
[5] H. F. Grip, T. I. Fossen, T. A. Johansen, and A. Saberi, “Globally [29] Y. Bar-Shalom, H. Chen, and M. Mallick, “One-step solution for the
exponentially stable attitude and gyro bias estimation with application multistep out-of-sequence-measurement problem in tracking,” IEEE
to GNSS/INS integration,” Automatica, vol. 51, pp. 158–166, 2015. Trans. Aerospace and Electronic Systems, vol. 40, no. 1, pp. 27–37,
[6] J. Vasconcelos, C. Silvestre, and P. Oliveira, “A nonlinear observer 2004.
for rigid body attitude estimation using vector observations,” in Proc. [30] M. Mallick, J. Krant, and Y. Bar-Shalom, “Multi-sensor multi-target
IFAC World Congr., Korea, July 2008. tracking using out-of-sequence measurements,” in Proc. IEEE Inter-
[7] S. Bonnabel, P. Martin, and P. Rouchon, “Symmetry-preserving ob- national Conf. Information Fusion, vol. 1, 2002, pp. 135–142.
servers,” IEEE Trans. Autom. Control, vol. 53, no. 11, pp. 2514–2526, [31] R. W. Beard, private communication, 2014.
2008. [32] K. Zhang, X. R. Li, and Y. Zhu, “Optimal update with out-of-sequence
[8] A. Roberts and A. Tayebi, “On the attitude estimation of accelerating measurements,” IEEE Trans. Signal Processing, vol. 53, no. 6, pp.
rigid-bodies using GPS and IMU measurements,” in IEEE Conf. 1992–2004, 2005.
Decision and Control and European Control Conf. (CDC-ECC), 2011, [33] Y. Bar-Shalom, “Update with out-of-sequence measurements in track-
pp. 8088–8093. ing: exact solution,” IEEE Trans. Aerospace and Electronic Systems,
[9] M. Izadi and A. K. Sanyal, “Rigid body attitude estimation based on vol. 38, no. 3, pp. 769–777, 2002.
the Lagrange–d’Alembert principle,” Automatica, vol. 50, no. 10, pp. [34] D. B. Kingston and R. W. Beard, “Real-time attitude and position
2570–2577, 2014. estimation for small UAVs using low-cost sensors,” in AIAA 3rd
[10] M. Zamani, J. Trumpf, and R. Mahony, “Minimum-energy filtering for Unmanned Unlimited Technical Conference, Workshop and Exhibit,
attitude estimation,” IEEE Transactions on Automatic Control, vol. 58, 2004.
pp. 2917–2921, 2013. [35] A. Germani, C. Manes, and P. Pepe, “A new approach to state
[11] H. Rehbinder and B. K. Ghosh, “Pose estimation using line-based observation of nonlinear systems with delayed output,” IEEE Trans.
dynamic vision and inertial sensors,” IEEE Trans. Automatic Control, Automatic Control, vol. 47, no. 1, pp. 96–101, 2002.
vol. 48, no. 2, pp. 186–199, 2003. [36] A. Khosravian, J. Trumpf, R. Mahony, and T. Hamel, “Velocity aided
[12] A. Khosravian and M. Namvar, “Globally exponential estimation of attitude estimation on SO(3) with sensor delay,” in Proc. IEEE Conf.
satellite attitude using a single vector measurement and gyro,” in Proc. on Decision and Control, December 2014, (accepted for publication).
49th IEEE Conf. Decision and Control, USA, Dec. 2010. [37] T. Sun, F. Xing, Z. You, and M. Wei, “Motion-blurred star acquisition
[13] A. Khosravian, J. Trumpf, R. Mahony, and C. Lageman, “Bias method of the star tracker under high dynamic conditions,” Optics
estimation for invariant systems on Lie groups with homogeneous express, vol. 21, no. 17, pp. 20 096–20 110, 2013.
outputs,” in Proc. IEEE Conf. on Decision and Control, December [38] A. Khosravian, J. Trumpf, R. Mahony, and C. Lageman, “Observers
2013. for invariant systems on lie groups with biased input measurements
[14] P. Martin and E. Salaün, “Design and implementation of a low- and homogeneous outputs,” Automatica, (to appear).
cost observer-based attitude and heading reference system,” Control [39] J. Vasconcelos, C. Silvestre, and P. Oliveira, “A nonlinear observer
Engineering Practice, vol. 18, no. 7, pp. 712–722, 2010. for rigid body attitude estimation using vector observations,” in Proc.
[15] M.-D. Hua, “Attitude estimation for accelerated vehicles using IFAC World Congr., Korea, July 2008.
GPS/INS measurements,” Control Engineering Practice, vol. 18, no. 7, [40] O. J. Smith, “A controller to overcome dead time,” ISA Journal of
pp. 723–732, 2010. Instrument Society of America, vol. 6, no. 2, pp. 28–33, 1959.
[16] M.-D. Hua, P. Martin, and T. Hamel, “Velocity-aided attitude esti-
mation for accelerated rigid bodies,” arXiv preprint arXiv:1411.3953,
2014.
[17] F. L. Markley and J. L. Crassidis, Fundamentals of Spacecraft Attitude
Determination and Control. Springer, 2014.
[18] M. Arcak and D. Nešić, “A framework for nonlinear sampled-data
observer design via approximate discrete-time models and emulation,”
Automatica, vol. 40, no. 11, pp. 1931–1938, 2004.
[19] T. Ahmed-Ali, I. Karafyllis, and F. Lamnabhi-Lagarrigue, “Global
exponential sampled-data observers for nonlinear systems with delayed
measurements,” Systems & Control Letters, vol. 62, no. 7, pp. 539–
549, 2013.
[20] B. Khaleghi, A. Khamis, F. O. Karray, and S. N. Razavi, “Multisensor
data fusion: A review of the state-of-the-art,” Information Fusion,
vol. 14, no. 1, pp. 28–44, 2013.
[21] F. Deza, E. Busvelle, J. Gauthier, and D. Rakotopara, “High gain
estimation for nonlinear systems,” Systems & control letters, vol. 18,
no. 4, pp. 295–299, 1992.
[22] W. H. Heemels, A. R. Teel, N. van de Wouw, and D. Nesic, “Net-
worked control systems with communication constraints: Tradeoffs
between transmission intervals, delays and performance,” IEEE Trans.
Automatic Control, vol. 55, no. 8, pp. 1781–1796, 2010.
[23] H. Hammouri, M. Nadri, and R. Mota, “Constant gain observer for
continuous-discrete time uniformly observable systems,” in Proc. IEEE
Conf. Decision and Control, 2006, pp. 5406–5411.