4300 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 67, NO.
8, AUGUST 2022
Lie Algebraic Unscented Kalman Filter for Pose Estimation
Alexander Meyer Sjøberg and Olav Egeland , Senior Member, IEEE
Abstract—An unscented Kalman filter (UKF) for matrix Lie
groups is proposed where the time propagation of the state is
formulated on the Lie algebra. This is done with the kinematic
differential equation of the logarithm, where the inverse of the right
Jacobian is used. The sigma points can then be expressed as log-
arithms in vector form, and time propagation of the sigma points
and the computation of the mean and the covariance can be done
on the Lie algebra. The resulting formulation is to a large extent
based on logarithms in vector form and is, therefore, closer to the
UKF for systems in Rn . This gives an elegant and well-structured
formulation, which provides additional insight into the problem,
and which is computationally efficient. The proposed method is
in particular formulated and investigated on the matrix Lie group
SE(3). A discussion on right and left Jacobians is included, and
a novel closed-form solution for the inverse of the right Jacobian
on SE(3) is derived, which gives a compact representation involv-
ing fewer matrix operations. The proposed method is validated in
simulations.
Index Terms—Matrix Lie group, unscented Kalman filter (UKF).
I. INTRODUCTION
The use of Lie group theory for attitude and pose estimation has
received considerable attention in the research literature. The reason
for this is that the set of rotation matrices SO(3) and the set of homo-
geneous transformation matrices SE(3) are both matrix Lie groups.
Matrix Lie groups have a number of properties that are useful in the
design of estimators and observers. In addition, unit quaternions form a
Lie group, and some of the design techniques for quaternion estimators
and observers can be related to their Lie group properties. The main
branches of methods for Lie group estimators are based on Kalman
filtering and nonlinear observer design.
Early work on nonlinear attitude estimation and control with quater-
nions is found in [19], [40], and [48], where the structural properties
of the unit quaternions were used in the design. This was used in
navigation for pose estimation in [46] and for attitude estimation in [45],
where bias estimation was included. An important development in
Kalman filtering based on quaternions was the multiplicative extended
Kalman filter (EKF) [30], where the global attitude was represented by
the four-dimensional unit quaternion, whereas the three-dimensional
(3-D) quaternion vector was estimated at each time step. This was later
generalized to alternative three-dimensional vector representations of
attitude, including the modified Rodrigues parameters [16] and the
Manuscript received 27 April 2021; accepted 17 September 2021.
Date of publication 19 October 2021; date of current version 29 July
2022. This work was supported by the Norwegian Research Council
under Grant 237896, SFI Offshore Mechatronics. Recommended by
Associate Editor G. Besancon. (Corresponding author: Alexander Meyer
Sjøberg.)
The authors are with the Department of Mechanical and Indus-
trial Engineering, Norwegian University of Science and Technol-
ogy, 7491 Trondheim, Norway (e-mail: alexander.m.sjoberg@ntnu.no;
olav.egeland@ntnu.no).
Color versions of one or more figures in this article are available at
https://doi.org/10.1109/TAC.2021.3121247.
Digital Object Identifier 10.1109/TAC.2021.3121247
0018-9286 © 2021 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission.
See https://www.ieee.org/publications/rights/index.html for more information.
Authorized licensed use limited to: Indian Institute of Technology Patna. Downloaded on April 22,2024 at 17:06:40 UTC from IEEE Xplore. Restrictions apply.
rotation vector [38], which is the vector form of the logarithm in
SO(3) [37]. An unscented Kalman filter (UKF) [26] was developed for
attitude estimation in [15], where the kinematic differential equation
for the quaternions was used for the time propagation of the sigma
points. Another example of attitude estimation on quaternions using
the UKF is found in [41]. The multiplicative EKF for quaternions has
been extended to pose estimation by introducing dual quaternions [20],
whereas in [17], a UKF was developed for pose estimation using
dual-modified Rodrigues parameters.
An important development was the nonlinear complementary fil-
ter [34], which was an attitude observer where the global attitude
was represented by a rotation matrix, and the observation error was
represented by the 3-D vector form of the antisymmetric part of the
rotation matrix. The resulting filter is robust and well suited for low-cost
sensors, and for a number of different sensor configurations. The
nonlinear complementary filter was generalized to the special linear
group in [33] and to SE(3) in [1]. A related work on pose estimation is
found in [39], where vision and inertial sensors are used. A nonlinear
observer for Lie groups based on Riemannian gradient descent was
presented in [24] and [28]. This article also addressed the invariance
properties that were addressed in the symmetry-preserving observer of
Bonnabel et al. [5] and Bonnabel et al. [6], which was further developed
to an invariant EKF that was used as a stable observer on Lie groups [4],
and for consistency in extended Kalman filtering for SLAM [9].
The concept of a concentrated Gaussian distribution on Lie groups
was introduced in [14] and [47], where a normal distribution on a Lie
group was defined in terms of a normal distribution of the logarithm in
vector form. This was further developed in [3], where this formulation
was used for fusion of multiple measurements of pose. The formulation
of Barfoot and Furgale [3] was used in [7] to formulate an EKF
for matrix Lie groups, where the covariance was calculated for the
concentrated Gaussian distribution, and the time propagation of the
state and the covariance was derived from the first-order approximation
of the Baker–Campbell–Hausdorff (BCH) formula. A similar approach
was used in [12] for an extended information filter on matrix Lie groups.
The method of Bourmaud et al. [7] and Bourmaud et al. [8] was further
developed in [42], where the time propagation of the state and the
covariance of an EKF were computed on the Lie algebra using the
kinematic differential equations of the logarithm.
In [22], a UKF was formulated for Riemannian manifolds. This was
done by generating sigma points as elements of the manifold, and then
calculating the mean by minimization on the manifold, whereas the
covariance was calculated in the tangent plane of the mean. It was
remarked that the sigma points and the mean can alternatively be
calculated in the tangent plane. In [23], a UKF framework for sensor
fusion on manifolds was presented where the sigma points were given
on the manifold. A UKF for quadrotors on SE(3) was presented in [31],
where the sigma points were computed on the manifold. In [10], the
concept of concentrated Gaussian distributions was used to formulate
a UKF for Lie groups where the sigma points are in the Lie algebra,
whereas the time propagation is formulated on the Lie group. In [36], a
systematic overview of Riemannian extensions of UKFs was presented
based on the formalism in [35]. A simulation study was included
IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 67, NO. 8, AUGUST 2022 4301

where a UKF was implemented for unit quaternions, where the sigma given by
points were computed on the manifold, and the mean was found as an
optimization problem on the manifold. This article stated that future Ẋ = [J l (ad(u))u̇]∧G X = X[J r (ad(u))u̇]∧G (3)
work should focus on computationally efficient UKFs for Riemannian where
manifolds for real-time applications. In [32], a UKF is formulated where
the sigma points are calculated in the Lie algebra, whereas the time ∞
(ad(u))k
J l (ad(u)) = (4)
propagation was in the manifold. k=0
(k + 1)!
The main contribution of this article is that the time propagation is
formulated in terms of the kinematic differential equation of the loga- is the left Jacobian and J r (ad(u)) = J l (−ad(u)) is the right Ja-
rithm, using the inverse of the right Jacobian. Then, time propagation cobian. It is noted that in view of the Cayley–Hamilton theorem, the
of the sigma points can be formulated on the Lie algebra, and moreover, Jacobian can be evaluated from a finite number of terms of the form
the mean and the covariance can be computed on the Lie algebra. (ad(u))k .
The covariance matrix is transformed between different tangent planes The kinematic differential equation for X ∈ G is given by
based on the BCH formula using the right Jacobian. The measurement
Ẋ = [v l ]∧G X = X[v r ]∧G (5)
update is based on Brossard et al. [10]. The resulting formulation is to
a large extent given in terms of logarithms in vector form, which makes where v l , v r ∈ R are the right and left invariant velocities, re-
n

the proposed UKF more similar to the original formulation for Rn . spectively. Comparison with (3) leads to the kinematic differential
This may lead to added insight and ease of implementation. Moreover, equations [11]
some of the steps of the method will be computationally efficient. In
particular, time propagation of the group element and optimization on u̇ = J −1 −1
l (ad(u))v l = J r (ad(u))v r . (6)
the manifold is avoided, and the method involves few calculations The inverse of the left and right Jacobian is given by
of exponentials and logarithms. The right Jacobian is important in
our method as it appears in the kinematic differential equation of the ∞
Bk (ad(u))k
logarithm. A new closed-form solution for the inverse right and left J −1 −1
l (ad(u)) = J r (−ad(u)) =
k=0
k!
Jacobian in SE(3) is derived in this article based on Barfoot and
Furgale [3] and Bullo and Murray [11]. where Bk are the Bernoulli numbers [3].
The rest of this article is organized as follows. Section II presents
basic theory on Lie groups and Lie algebras, including probability B. BCH Formula
distributions and the left and right Jacobians. In Section III, a new
closed-form solution for the inverse right and left Jacobian in SE(3) Consider exp(c) = exp(a) exp(b), where a, b, c ∈ g. A first-order
is derived. Then, in Section IV, a Lie algebraic UKF on SE(3) is pre- approximation in b of the BCH formula is given in vector form as
sented. Finally, the performance of the proposed UKF is demonstrated c = a + J −1
r (ad(a))b [27]. This leads to the two first-order approx-

in simulations. imations

exp([a]∧G ) exp([b]∧G ) = exp([a + J −1 ∧

r (ad(a))b]G ) (7)
II. PRELIMINARIES
exp([a + d]∧G ) = exp([a]∧G ) exp([J r (ad(a))d]∧G ) (8)
A. Matrix Lie Groups
which were used in [3], [8], and [12].
Consider the matrix Lie group G and the associatedLie algebra g
[14], [21]. The exponential of u ∈ g is X = exp u = ∞ k=0 u /k! ∈
k
C. Gaussian Distributions on Matrix Lie Groups
G. Then, u = log X is the logarithm of X locally around the identity
id ∈ G. Let a, b ∈ g be elements of the Lie algebra. Then, the adjoint It is noted that the normal distribution is of maximum entropy in
map ad(a) is given by ad(a)b = [a, b] = ab − ba, which is the Lie Rn , whereas this is not the case for Lie groups. Maximum entropy
bracket. distributions for Lie groups are possible to find based on Souriau’s
The time derivative of the exponential function is work [2]. Alternative global approaches to find a probability density
function (pdf) for SO(3) are found in [29] and [43], where a Bingham
d d distribution was used for pose estimation. In this work, our target
X(t) = (exp u(t)) = (Tu exp) u̇(t) (1)
dt dt has been to formulate a UKF, and therefore it is interesting to use a
formulation based on a Gaussian distribution.
where Tu exp : g → Texp u G is the linear mapping [18] In the following, we will restrict our discussion to the Lie groups
SO(3), SE(2), and SE(3). Consider the random variable X ∈ G
∞
(+ad(u))k
Tu exp = Tid R(exp u) ◦ and define the distance function d(X 1 , X 2 ) = (log(X −1 ∨
1 X 2 ))G 
k=0
(k + 1)! for X 1 , X 2 ∈ G. Then, the pdf pX (X) of X is said to be highly
concentrated around X̄ if d(X̄,X)< pX (X)dX ≈ 1 for  π [14,
∞
(−ad(u))k ∧
= Tid L(exp u) ◦ (2) p. 381]. In this article, we will use X = X̄ exp([u]  G ) and the pdf
k=0
(k + 1)! p(u) of the logarithm, which gives the condition u< p(u)du ≈ 1
for  π for a highly concentrated distribution of the logarithm around
where Tid R(X) and Tid L(X) are the tangent mappings at the iden- zero. This was used for SE(3) in [3], where the Gaussian pdf p(u) ∼
tity of the right and left translation by X, respectively. In the NRn (0, P ) of the logarithm was used, and the mean and covariance
case of a matrix group, Tid R(X)a = aX and Tid L(X)a = Xa. Let of the logarithm were given by
a = [a1 , . . . , an ]T ∈ Rn be the vector representation of a ∈ g. The  
notations [a]∨G = a and [a]∧G = a are used [7]. Moreover, [ad(a)b]∧G = μ= up(u)du, Σ = uuT p(u)du. (9)
ad(a)b, where [b]∧G = b. The time derivative of the exponential is then Rn Rn

Authorized licensed use limited to: Indian Institute of Technology Patna. Downloaded on April 22,2024 at 17:06:40 UTC from IEEE Xplore. Restrictions apply.
4302 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 67, NO. 8, AUGUST 2022

The pdf p(u) will be highly concentrated around zero if the largest In comparison to this, for sufficiently small i , the calculation of the
singular value of the covariance Σ is much smaller than π. Then, the mean Ȳ by calculating the average logarithm [ξ̄]∧G in (15) corresponds
pdf p(u) will be very small for u ≥ π, and the normal distribution to the minimization problem
is a valid approximation with a folding or cutting technique [3], [14]. −1 −1
Ȳ = arg min ([log(Ȳ Y i )]∨G )T J T J [log(Ȳ Y i )]]∨G (19)
Next, consider a random variable Y = exp([ξ]∧G ) ∈ G, where ξ ∼ Ȳ ∈G
NRn (ξ̄, Q) is normally distributed with nonzero mean. The zero-mean
vector δξ = ξ − ξ̄ ∼ NRn (0, Q) is introduced. Then, from (8), it is on G, where J = J r (ad(ξ̄)). It is seen that the only difference between
seen that a first-order approximation in δξ is given by the two optimization problems (18) and (19) is the weighting matrix
J T J in (19). For small ξ̄, it is seen from (4) that this weighting matrix
Y = exp([ξ̄ + δξ]∧G ) = Ȳ exp([e]∧G ) (10) will be close to the identity matrix, and it is reasonable to expect that
Ȳ will be close to μ for this distance function.
where Ȳ = exp([ξ̄]∧G ) and e = J r (ad(ξ̄))δξ ∼ NRn (0, R) is nor-
mally distributed with zero mean and covariance
F. Time Integration and Discrete-Time Model
R = J r (ad(ξ̄))QJ Tr (ad(ξ̄)). (11)
Euler’s method for the kinematic differential equation (5) gives
This result was derived in [7] for use in the update of an EKF on
X(tk+1 ) = X(tk ) exp(hv r (tk )) (20)
a matrix Lie group. A related problem was treated in [13], where a
merging algorithm for Gaussian components on G was developed. where tk+1 = tk + h, h is the time step, and v r is assumed to be
constant over the time interval. Alternatively, Euler’s method can be
D. Calculation of Mean and Covariance applied to (6), which gives
Consider a set Y i = exp([ξi ]∧G ) ∈ G of N Lie group elements with u(tk+1 ) = u(tk ) + hJ −1
r (ad(u(tk )))v r (tk ). (21)
corresponding logarithms given in vector form by ξi , where ξi is highly
From X(tk ) = exp([u(tk )]∧G ) and (7), it follows that
concentrated around zero. In [22], a UKF for Riemannian manifolds
was proposed where the elements were expressed in terms of the mean X(tk+1 ) = exp([u(tk ) + hJ −1 ∧
r (ad(u(tk )))v r (tk)]G ) (22)
μ ∈ G as
is a first-order approximation of (20). This means that the discretizations
Y i = μ exp([i ]∧G ) (12) (20) and (21) give the same result to the first order. We will use the
discretization (21) for our UKF.
where the mean was found from the minimization problem
Additional results on integration schemes based on (6) are found
μ = arg min d(Y i , μ)2 (13) in [25] and [44].
µ∈G

for some distance

function d. The empirical covariance was calculated III. CALCULATION OF INVERSE JACOBIAN
from P G = N1 N
i=1  i  T
i , where [i ]∧G = log(μ−1 Y i ). The calcula-
tion of the mean as the minimization problem (13) on G may be time In our proposed UKF for matrix Lie groups, the inverse of the right
consuming in real-time applications. Jacobian plays an important role. We will therefore take a closer look
Therefore, we propose a new formulation for a UKF on a Lie group, at expressions for the inverse of the right Jacobian, and a novel closed-
where the mean of the set Y i = exp([ξi ]∧G ) is calculated on the tangent form solution for SE(3) will be developed.
space as
A. Jacobians in SO(3)
Ȳ = exp([ξ̄]∧G ) (14)
The logarithm in SO(3) is the skew symmetric matrix θ̂ ∈ so(3)
where corresponding to the vector θ, whereas ad(θ) = θ̂. The right Jacobian
and its inverse are given in closed form as [11]
1 
N
ξ̄ = ξ. (15) 1 − cos θ θ − sin θ 2
N i=1 i Ψr (θ̂) = I − θ̂ + θ̂ (23)
θ2 θ3
It is seen from (10) that this leads to the first-order approximation
1 1 − θ2 cot θ2 2
Ψ−1
r (θ̂) = I + θ̂ + θ̂ (24)
Y i = Ȳ exp(ei ) (16) 2 θ2
where ei = J r (ad(ξ̄))δξi and δξi = (ξi − ξ̄). The empirical covari- where θ = θ. The coefficients of the Jacobian and its inverse are well
ance can then be calculated as defined for all θ, which is verified by Taylor series expansion of the
  coefficients.
1  1 
N N
P = ei eTi = J δξi δξTi J T (17)
N i=1 N i=1 B. Jacobians in SE(3)

where J = J r (ad(ξ̄)). The logarithm of T = (R, r) ∈ SE(3) is given by


∧ θ̂ ρ
E. Calculation of Mean by Optimization [ξ]SE(3) = T ∈ se(3) (25)
0 0
2
Consider the case where the distance function in (13) is d(Y i , μ) =
Ti i , which is the usual angular distance in SO(3) and a left-invariant where θ̂ ∈ so(3), ρ ∈ R3 and ξ = [θ T , ρT ]T is the vector form of the
metric in SE(3). Then, the mean calculated by minimization on the logarithm. The exponential map is [11], [37]
group will be 
∧ exp θ̂ Ψl (θ̂)ρ
μ = arg min([log(μ−1 Y i )]∨G )T [log(μ−1 Y i )]∨G . (18) T = exp([ξ]SE(3) ) = (26)
µ∈G 0T 1

Authorized licensed use limited to: Indian Institute of Technology Patna. Downloaded on April 22,2024 at 17:06:40 UTC from IEEE Xplore. Restrictions apply.
IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 67, NO. 8, AUGUST 2022 4303

3 2 2
where exp θ̂ is the exponential function in SO(3), and Ψl (θ̂) is the where s1 = ρ̂, s2 = θ̂ρ̂ + ρ̂θ̂, and s4 = θ̂ ρ̂ + θ̂ ρ̂θ̂ + θ̂ρ̂θ̂ +
left Jacobian in SO(3). The logarithm can be computed from the skew 3 2
ρ̂θ̂ . The expression for s4 is simplified by observing that θ̂ ρ̂θ̂ +
symmetric matrix ŷ = 12 (R − RT ) as [25] 2 2
θ̂ρ̂θ̂ = θ̂(ρ̂θ̂ + θ̂ρ̂)θ̂ = −2(θ T ρ)θ̂ . This in combination with
3
sin−1 y θ̂ = −θ2 θ̂ gives
θ̂ = ŷ, ρ = Ψ−1
l (θ̂)r. (27)
y 2
s4 = −θ2 s2 − 2(θ T ρ)θ̂ . (37)
The matrix form of the adjoint map in SE(3) is given by
 Insertion of this expression for s4 in (36) along with the expressions
θ̂ 0 for γ1 (θ) and γ2 (θ) gives the closed-form solution
ad(ξ) = . (28)
ρ̂ θ̂ 1 1 − α(θ)
Cr = ρ̂ + (θ̂ρ̂ + ρ̂θ̂)
The kinematic differential equation in terms of T is given by 2 θ2
α(θ) + β(θ) − 2 T 2
Ṫ = [V l ]∧SE(3) T = T [V r ]∧SE(3) (29) + 4
(θ ρ)θ̂ . (38)
θ
where [V l ]∧SE(3) ∈ se(3) is the left velocity, and [V r ]∧SE(3) ∈ se(3) This gives the desired closed-form solution for the inverse of the right
is the right velocity, which have vector forms Jacobian by inserting (24) and (38) into (33). The closed-form solution
   for the inverse of the left Jacobian is then
ωr ωl Rω r 
Vr = , Vl= = . (30) Ψ−1
vr vl Rv r + r̂Rω −1 −1 l (θ̂) 0
Φl (ad(ξ)) = Φr (−ad(ξ)) = (39)
Cl Ψ−1l (θ̂)
It follows from (6) that the kinematic differential equation in terms of
the logarithm is where C l is equal to C r except for a change of sign for the 12 ρ̂ term.
It is seen from the Taylor series expansions
ξ̇ = Φ−1 −1
l (ad(ξ))V l = Φr (ad(ξ))V r (31)
1 − α(θ) 1 θ2 θ4
where Φl is the left Jacobian and Φr is the right Jacobian. = + + + ··· (40)
θ2 12 720 30 240
The right Jacobian is given in closed form as [11]
 α(θ) + β(θ) − 2 1 θ2 θ4
4
=− − − + ··· (41)
Ψr (θ̂) 0 θ 720 15 120 403 200
Φr (ad(ξ)) = (32)
Qr Ψr (θ̂) that the coefficients in (38) are well-behaved for all θ.
where a closed-form solution for the submatrix Qr was found in [3].
Inversion of the matrix in (32) gives the expression IV. LIE ALGEBRAIC UKF ON SE(3)
 A. System Dynamics
−1 Ψ−1
r (θ̂) 0
Φr (ad(ξ)) = −1
(33) The state is given by T ∈ SE(3), and the system dynamics is the
Cr Ψr (θ̂)
kinematic differential equation
for the inverse of the right Jacobian, as presented in [11],
where the submatrix C r was unspecified. Later, the expression Ṫ = T [V + w]∧SE(3) (42)
C r = −Ψ−1 −1
r (θ̂)Qr Ψr (θ̂) was obtained in [3]. It is noted that a where V is the vector form of the right velocity, and w ∼ N (0, Q) is
closed-form solution for C r was not found, which means that a closed- a noise vector. The discrete-time model is
form solution for the inverse of the right Jacobian in SE(3) has not
been reported so far. T k+1 = T k exp([ξk+1 ]∧SE(3) ) (43)

where the global state is given by T k , and the increment from one
C. Closed Form for Inverse Jacobians in SE(3)
sample to the next is described by the logarithm ξk+1 . This is similar
In this section, we will derive a simple closed-form solution for the to the multiplicative EKF [15], where a quaternion gives the global
inverse of the right and left Jacobians in SE(3). In [11], it was shown state, and a three-dimensional vector is used for the increment. The
that the inverse right Jacobian on SE(3) can be computed as corresponding discrete-time equation for the logarithm is
1 ξk+1 = ξk + hΦ−1
r (ad(ξ k )) (V k + w k ) (44)
Φ−1
r (ad(ξ)) = I + ad(ξ) + γ1 (θ)ad(ξ)2 + γ2 (θ)ad(ξ)4 (34)
2
where wk ∼ N (0, Qk ). It is assumed that we can measure the full
with γ1 (y) = (4 − 3α(y) − β(y))/(2y 2 ), γ2 (y) = (2 − α(y) − pose, and that the measurements are given by
β(y))/(2y 4 ), α(y) = (y/2) cot(y/2), and β(y) = (y/2)2 / sin2
(y/2). This was used in [11] to derive (33). Z = T exp([ν]∧SE(3) ) ∈ SE(3) (45)
It follows from (28) that
where ν k ∼ N (0, N k ) is the measurement noise vector. In the pro-
 k
θ̂ 0 posed UKF, the kinematic differential equation (44) is used to describe
ad(ξ)k = k (35) the system dynamics, whereas previous work used the kinematic dif-
sk θ̂
ferential equation (43) on G.
k−1 k−i−1 i
where sk = i=0 θ̂ ρ̂θ̂ . From this in combination with (33),
B. Filter Dynamics
(34), and (35), the matrix C r must be of the form
1 The sigma points of the time update are given by ξak|k,i =
Cr =
2
s1 + γ1 (θ)s2 + γ2 (θ)s4 (36) [ξxk|k,iT , ξwi = 0, . . . , 2m, where m = 12. The vector ξxi ∈ R6
T T
k|k,i ] ,

Authorized licensed use limited to: Indian Institute of Technology Patna. Downloaded on April 22,2024 at 17:06:40 UTC from IEEE Xplore. Restrictions apply.
4304 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 67, NO. 8, AUGUST 2022

Algorithm 1: UKF-LieAlg Time Update on SE(3).

Input: T k|k , P k|k , Qk , V k , h, α, β
1: λ = (α2 − 1)m m = 12
λ 1
2: w0μ = λ+m , wj>0μ
= 2(λ+m)
λ 1
w0P = λ+m + 1 − α2 + β, wj>0 P
= 2(λ+m) β = 2
a
3: P k|k = diag(P k|k , Qk )
4: σ k|k = Chol((m + λ)P ak|k )
5: ξak|k,0 = 0 ∈ Rm
ξak|k,i = coli (σ k|k ) i = 1 . . . m
Fig. 1. Relation between a predicted error vector ek+1|k propagated
ξak|k,i+m = −coli (σ k|k )
6
6: ξak|k,i = [ξxk|k,i k|k,i ] ξ i , ξ i ∈ R
, ξw
T T T x w
on the tangent space at the identity, since ξ̄k|k = 0, and the correspond-
−1
ing error vector k+1|k on the tangent space at the predicted state, which 7: ξk+1|k,i = ξk|k,i + hΦr (ad(ξk|k,i ))(V k + ξw
x x x
k|k,i )
is given through the Jacobian. 2m μ x
8: ξ̄k+1|k = i=0 wi ξk+1|k,i
9: ek+1|k,i = ξxk+1|k,i − ξ̄k+1|k

10: P k+1|k = J 1 [ 2m P T T
i=0 wi (ek+1|k,i )(ek+1|k,i ) ]J 1
6
i ∈ R correspond to the pro-
is related to the state variables, and ξw  J 1 = Φr (ad(ξ̄k+1|k ))
cess noise. The sigma points are propagated by using the discretized 11: T k+1|k = T k|k exp([ξ̄k+1|k ]∧ )
dynamics in (44), which gives Output: T k+1|k , P k+1|k
ξxk+1|k,i = ξxk|k,i + hΦ−1 x w
r (ad(ξ k|k,i ))(V m,k + ξ k|k,i ) (46)

where V m,k are the measured velocities. The predicted mean of the C. Measurement Update
logarithm is computed as the weighted sum
The measurement update is based on the formulation of Brossard
2q
 et al. [10] and is computed according to Algorithm 2. The
ξ̄k+1|k = wiμ ξxk+1|k,i ∈ R6 (47) sigma points of the measurement update are given by ξak+1|k,i =
] . The logarithm [ζ k+1,i ]∧ corresponding to sigma
i=0
[ξxk+1|k,i
T
, ξνk+1|k,i
T T

where the weights wiμ are given in Step 2 in Algorithm 1. This is used
to calculate the predicted mean of the global state as
T k+1|k = T k|k exp([ξ̄k+1|k ]∧SE(3) )
and the deviation ek+1|k,i = − ξ̄k+1|k . Then, using the result
ξxk+1|k,i
(11), the covariance is computed from

2m
P k+1|k = wiP (k+1|k,i )(k+1|k,i )T
i=0
where k+1|k,i = Φr (ad(ξ̄k+1|k ))ek+1|k,i and the weights wiP are
given in Step 2 in Algorithm 1. Fig. 1 illustrates how an error vector on
the logarithm e maps to an error vector  on the tangent plane shifted
by the exponential map of the mean vector ξ̄.
The new contribution is that the sigma points are computed from
(46) with the kinematic differential equation of the logarithm. Then,
the mean of the time update is computed from (47) as the average of
vectors, whereas the covariance is calculated in terms of vectors on the
tangent plane and is transformed to the next tangent plane by a matrix
operation. This is computationally more efficient than averaging on
G, which is typically done in previous work [22], [23], [32], [36], or
computing the time propagation on G and then averaging the logarithm
and using parallel transport of the covariance as in [31]. In [22], it
was stated that the mean and the covariance can be computed in terms
of the logarithm, and that the covariance could be transformed with
parallel transport, however, details on the time propagation of the
sigma points and the parallel were not included. In [10], the mean
was not calculated from the sigma points, instead, based on Barfoot
and Furgale [3], velocity measurements were used to calculate the
mean. Moreover, we would like to point out that the proposed for-
mulation gives a UKF for Lie groups that is more similar to the original
formulation on Rn , which could facilitate ease of understanding and
implementation.
point i of the measurement is defined by
exp [ζ k+1,i ]∧ = exp [ξxk+1|k,i ]∧ exp [ξνk+1,i ]∧ . (50)
(48)

The weighted mean is found from ζ̄ k+1 = 2m μ μ
i=0 wi ζ k+1,i , where wi
is found in Step 2 in Algorithm 2. The covariance can then be computed
from Δζ k+1,i = ζ k+1,i − ζ̄ k+1 . The remaining steps are given in
Algorithm 2. It is noted that the transformation of the covariance in
(49) Step 15 of Algorithm 2 was not performed in [10], but appeared in [8]
for an EKF on Lie groups.
The calculations of the measurement update are in terms of vectors in
the tangent plane, which simplifies implementation, and can potentially
reduce computational costs. This method was used in [10], whereas the
works in [22], [23], [32], and [36] used Lie group elements, which were
transformed to the tangent plane.
V. SIMULATIONS
In this section, we present a comparison of three UKF filters: The
UKF-LG of Brossard et al. [10], our proposed Lie algebraic UKF (UKF-
LA) described in Section IV, and our method with optimization on the
manifold in the prediction step (UKF-LA-Opt) according to (18). The
parameter α = 10−3 was used in all cases. The velocity measurements
were body-fixed and obtained at a rate of 100 Hz, whereas the pose
measurements were given in the inertial frame and obtained with a
sample rate of 1 Hz. Furthermore, the measurements were elements of
SE(3), and the measurement noise was multiplicative and assumed to
be given as in (45). The angular and linear velocities in the time interval
t ∈ [0, T ] were given by ω b (t) = [0, 0, 2t/(0.001t2 + 8t + 1)]T rad/s
and v b (t) = [t/(1 + 2t), 0, 0]T m/s, and were used to describe the right
velocity as given in (30). The analytic expressions were allowed for
exact solutions, and we could therefore use an exact true trajectory for
comparisons. All simulations were initiated with the covariance P 0|0 =
0.01I 6×6 . The process noise matrix was given as Q = diag(Qω , Qv ),

Authorized licensed use limited to: Indian Institute of Technology Patna. Downloaded on April 22,2024 at 17:06:40 UTC from IEEE Xplore. Restrictions apply.
IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 67, NO. 8, AUGUST 2022 4305

Algorithm 2: UKF-LieAlg Measurement Update on SE(3).

Input: T k+1|k , P k+1|k , Z k+1 , N k ∈ R6×6
1: λ = (α2 − 1)r r = 12
λ 1
2: w0μ = λ+r μ
, wj>0 = 2(λ+r)
λ 1
w0P = λ+r + 1 − α2 + β, wj>0 P
= 2(λ+r) β = 2
a
3: P k+1|k = diag(P k+1|k , N k )
4: σ k+1|k = Chol((r + λ)P ak|k )
5: ξak+1|k,0 = 0 ∈ Rr
ξak+1|k,i = coli (σ k+1|k ) i = 1 . . . r
ξak+1|k,i+r = −coli (σ k+1|k )
T
6: ξak+1|k,i = ξxk+1|k,iT
, ξνk+1|k,i
T
ξν ∈ R6
Fig. 2. Body frame was initiated with a 45◦ angular error as well as
x ∧ ν ∧ ∨
7: ζ k+1,i = [log(exp([ξk+1|k,i ] ) exp([ξk+1,i ] ))] 0.5 m off the initial position. Before any measurement updates had been
 obtained, the predicted motion was incorrect and the body frame was
8: ζ̄ k+1 = 2m μ
i=0 wi ζ k+1,i heading away from the xy plane, which the true trajectory reside on,
9: Δζ k+1,i = ζ k+1,i − ζ̄ k+1
 but was able to converge toward the true trajectory as the pose was
10: P xz = 2m μ x
i=0 wi (ξ k+1|k,i )(Δζ k+1,i )
T measured.
2m μ
11: P zz = i=0 wi (Δζ k+1,i )(Δζ k+1,i )T
12: P − k+1|k+1 = P k+1|k − KP zz K
T
K = P xz P −1
zz
13: η k+1 = [log(T −1k+1|k Z k+1 )]
∨

14: mk+1 = Kη k+1

15: P k+1|k+1 = J 2 P − k+1|k+1 J 2
T
J 2 = Φr (ad(mk+1 ))
16: T k+1|k+1 = T k+1|k exp([mk+1 ]∧ )
Output: T k+1|k+1 , P k+1|k+1

where Qω = σω2 I 3×3 and Qv = σv2 I 3×3 , which describes the uncer-
tainty of the velocity measurements. The noise parameters were set to
σω = 0.1 rad/s and σv = 0.05 m/s. The covariance matrix describing
the measurement noise matrix was given as N = diag(N R , N p ),
2
where N R = σR I 3×3 and N p = σp2 I 3×3 . The measurement noise
π
parameters were given as σR = 180 rad and σp = 0.01 m. The angular
error at time tk was computed as θe,k = [log(RTk|k Rk )]∨SO(3) , where
Rk|k is the estimated rotation matrix between the body-fixed frame
and the inertial frame, and Rk was the true attitude of the system. Fig. 3. Estimation error due to a poorly chosen initial condition was
reduced when the pose of the body had been measured.
The positional error was found as re,k = r k|k − r k , where r k|k was
the estimated position and r k was the true position, both given in the
inertial frame.

A. Case Studies
In the first case, the trajectory was estimated over T = 40 s. The
trajectory was in the xy plane. The estimator had an initial state given
by a rotation of 45◦ about the y-axis, and by a positional offset of 0.5 m
along the y-axis. Due to the initial angular offset, estimated state left
the xy plane for the first 100 samples (see Fig. 2), until the first pose
measurement made the estimator errors converge to values close to
zero, as seen in Fig. 3.
In the second case, the three estimators had initial states given
by identity matrices, such that T 0|0 = I 4×4 before estimating the Fig. 4. Spiral trajectory initiated with zero offsets.
trajectory. It is seen in Fig. 4 that all the three estimators tracked the
trajectory with high accuracy. The estimates provided by the three filters
mean computational time was computed. The average computational
were close to indistinguishable when they are used with the same set
time spent for each estimator is provided in Table I together with the
of measurements. This is seen in Figs. 3 and 5.
difference given in terms of percentages. It is seen that the proposed
UKF-LA gave computational time which was 37.3% of the compu-
B. Computational Efficiency
tational time of UKF-LG, which was set to 100%. When the mean
In order to evaluate the computational effort of the UKF filters, 100 was obtained through optimization on the manifold, then UKF-LA
different sets of full pose measurements of 1000 samples were gener- was slower than UKF-LG. It is noted that closed-form solutions of
ated, where the time update was performed at a rate of 100 Hz, and the the exponential and logarithmic maps on SE(3), as described in (26)
measurement update was performed at 1 Hz. The computational time and (27), were used for efficiency in computation. If library functions
over each set of measurements was evaluated for each estimator, and the in MATLAB were used for computation of the exponentials and the

Authorized licensed use limited to: Indian Institute of Technology Patna. Downloaded on April 22,2024 at 17:06:40 UTC from IEEE Xplore. Restrictions apply.
4306
Fig. 5. Angular and positional error compared with the ground truth
when the system after the system had been initialized with zero offsets.
TABLE I
UKF-LA REQUIRED LESS COMPUTATIONAL EFFORT COMPARED TO
UKF-LG AND UKF-LA-OPT
logarithms maps, then UKF-LA could perform up to ten times faster
than UKF-LG. This becomes evident when studying the prediction
step presented in [10], as the exponential map must be computed
twice, the logarithmic map once, and two matrix multiplications are
required for each sigma point in each prediction step. In contrast, the
exponential map is computed once per prediction step in UKF-LA, and
no logarithms must be called unless the estimated mean is obtained
through optimization.
VI. CONCLUSION AND FUTURE WORKS
A UKF for matrix Lie groups has been proposed where the time
propagation is formulated in terms of the kinematic differential equation
of the logarithm. The proposed method is to a large extent formulated
in terms of vector operations on the Lie algebra, and the formulation is
closer to the original UKF on Rn than previous works on Lie groups.
This leads to efficient formulations and potentially to reduced com-
putational costs, in particular, in the time update. This article includes
details on how to implement the proposed UKF for SE(3). The method
was compared to the UKF-LG of Brossard et al. [10] in simulations on
SE(3), where it was found that the difference in the estimator errors
was not significant, whereas the computational cost of the proposed
method was 37% of the UKF-LG. The proposed method with averaging
of the sigma points on the Lie algebra was compared to a method with
averaging of the sigma points on the manifold. Again, there was no
significant difference in the estimation error, and the computation time
of the proposed method was 32% of the method with averaging on the
manifold. Future work may include equations of motion and different
sensor systems, including IMUs and bias modeling.
REFERENCES
[1] G. Baldwin, R. E. Mahony, J. Trumpf, T. Hamel, and T. Cheviron, “Com-
plementary filter design on the special Euclidean group SE(3),” in Proc.
Eur. Control Conf., 2007, pp. 3763–3770.
Authorized licensed use limited to: Indian Institute of Technology Patna. Downloaded on April 22,2024 at 17:06:40 UTC from IEEE Xplore. Restrictions apply.
IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 67, NO. 8, AUGUST 2022
[2] F. Barbaresco, “Lie group statistics and Lie group machine learn-
ing based on Souriau Lie groups thermodynamics & Koszul-Souriau-
Fisher metric: New entropy definition as generalized Casimir invariant
function in coadjoint representation,” Entropy, vol. 22, no. 6, 2020,
Art. no. 642.
[3] T. D. Barfoot and P. T. Furgale, “Associating uncertainty with three-
dimensional poses for use in estimation problems,” IEEE Trans. Robot.,
vol. 30, no. 3, pp. 679–693, Jun. 2014.
[4] A. Barrau and S. Bonnabel, “The invariant extended Kalman filter as a sta-
ble observer,” IEEE Trans. Autom. Control, vol. 62, no. 4, pp. 1797–1812,
Apr. 2017.
[5] S. Bonnabel, P. Martin, and P. Rouchon, “Symmetry-preserving ob-
servers,” IEEE Trans. Autom. Control, vol. 53, no. 11, pp. 2514–2526,
Dec. 2008.
[6] S. Bonnabel, P. Martin, and P. Rouchon, “Non-linear symmetry-preserving
observers on Lie groups,” IEEE Trans. Autom. Control, vol. 54, no. 7,
pp. 1709–1713, Jul. 2009.
[7] G. Bourmaud, R. Mégret, M. Arnaudon, and A. Giremus, “Continuous-
discrete extended Kalman filter on matrix Lie groups using concentrated
Gaussian distributions,” J. Math. Imag. Vis., vol. 51, no. 1, pp. 209–228,
2015.
[8] G. Bourmaud, R. Mégret, A. Giremus, and Y. Berthoumieu, “Discrete
extended Kalman filter on Lie groups,” in Proc. 21st Eur. Signal Process.
Conf., 2013, pp. 1–5.
[9] M. Brossard, A. Barrau, and S. Bonnabel, “Exploiting symmetries to
design EKFs with consistency properties for navigation and SLAM,” IEEE
Sensors J., vol. 19, no. 4, pp. 1572–1579, Feb. 2019.
[10] M. Brossard, S. Bonnabel, and J.-P. Condomines, “Unscented Kalman
filtering on Lie groups,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst.,
2017, pp. 2485–2491.
[11] F. Bullo and R. M. Murray, “Proportional derivative (PD) control on the
Euclidean group,” California Inst. Technol., Pasadena, CA, USA, CDS
Tech. Rep. 95-010, 1995.
[12] J. Ćesić, I. Marković, M. Bukal, and I. Petrović, “Extended information
filter on matrix Lie groups,” Automatica, vol. 82, pp. 226–234, 2017.
[13] J. Ćesić, I. Marković, and I. Petrović, “Mixture reduction on matrix Lie
groups,” IEEE Signal Process. Lett., vol. 24, no. 11, pp. 1719–1723,
Nov. 2017.
[14] G. S. Chirikjian, Stochastic Models, Information Theory, and Lie Groups,
vol. 2. Cambridge, MA, USA: Birkhäuser, 2012.
[15] J. Crassidis and F. L. Markley, “Unscented filtering for spacecraft attitude
estimation,” J. Guid. Control Dyn., vol. 26, no. 4, pp. 536–542, 2003.
[16] J. L. Crassidis, F. L. Markley, and Y. Cheng, “Survey of nonlinear attitude
estimation methods,” J. Guid., Control, Dyn., vol. 30, no. 1, pp. 12–28,
2007.
[17] Y. Deng, Z. Wang, and L. Liu, “Unscented Kalman filter for spacecraft
pose estimation using twistors,” J. Guid., Control, Dyn., vol. 39, no. 8,
pp. 1844–1856, 2016.
[18] J. Duistermaat and J. Kolk, Lie Groups. New York, NY, USA: Springer,
2000.
[19] O. Egeland and J.-M. Godhavn, “Passivity-based adaptive attitude con-
trol of a rigid spacecraft,” IEEE Trans. Autom. Control, vol. 39, no. 4,
pp. 842–846, Apr. 1994.
[20] N. Filipe, M. Kontitsis, and P. Tsiotras, “Extended Kalman filter for
spacecraft pose estimation using dual quaternions,” J. Guid., Control, Dyn.,
vol. 38, no. 9, pp. 1–17, 2015.
[21] B. C. Hall, Lie Groups, Lie Algebras, and Representations. An Elementary
Introduction. Graduate Texts in Mathematics. Berlin, Germany: Springer,
2003.
[22] S. Hauberg, F. Lauze, and K. Pedersen, “Unscented Kalman filtering on
Riemannian manifolds,” J. Math. Imag. Vis., vol. 46, pp. 103–120, 2013.
[23] C. Hertzberg, R. Wagner, U. Frese, and L. Schröder, “Integrating
generic sensor fusion algorithms with sound state representations through
encapsulation of manifolds,” Inf. Fusion, vol. 14, no. 1, pp. 57–77,
2013.
[24] M.-D. Hua, M. Zamani, J. Trumpf, R. E. Mahony, and T. Hamel, “Observer
design on the special Euclidean group SE(3),” in Proc. IEEE Conf. Decis.
Control Eur. Control Conf., 2011, pp. 8169–8175.
[25] A. Iserles, H. Munthe-Kaas, S. Nørsett, and A. Zanna, “Lie-group meth-
ods,” Acta Numerica, vol. 9, pp. 215–365, 2005.
[26] S. J. Julier and J. K. Uhlmann, “New extension of the Kalman filter to
nonlinear systems,” in Proc. Defense, Secur., Sens., 1997, pp. 182–193.
[27] S. Klarsfeld and J. A. Oteo, “The Baker-Campbell-Hausdorff formula and
the convergence of the Magnus expansion,” J. Phys. A: Math. General,
vol. 22, pp. 4565–4572, 1989.
IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 67, NO. 8, AUGUST 2022
[28] C. Lageman, J. Trumpf, and R. Mahony, “Gradient-like observers for
invariant dynamics on a Lie group,” IEEE Trans. Autom. Control, vol. 55,
no. 2, pp. 367–377, Feb. 2010.
[29] T. Lee, M. Leok, and N. McClamroch, “Global symplectic uncertainty
propagation on SO(3),” in Proc. IEEE Conf. Decis. Control, 2008,
pp. 61–66.
[30] E. J. Lefferts, F. L. Markley, and M. D. Shuster, “Kalman filtering for
spacecraft attitude estimation,” J. Guid., Control, Dyn., vol. 5, no. 5,
pp. 417–429, 1982.
[31] G. Loianno, M. Watterson, and V. Kumar, “Visual inertial odometry for
quadrotors on SE(3),” in Proc. IEEE Int. Conf. Robot. Autom., May 2016,
pp. 1544–1551.
[32] G. Magalhães, Y. Cáceres, J. B. do Val, and R. S. Mendes, “UKF on Lie
groups for radar tracking using polar and Doppler measurements,” in Proc.
22nd Congresso Brasileiro de Automática, Sep. 2018.
[33] R. Mahony, T. Hamel, P. Morin, and E. Malis, “Nonlinear complemen-
tary filters on the special linear group,” Int. J. Control, vol. 85, no. 10,
pp. 1557–1573, 2012.
[34] R. Mahony, T. Hamel, and J.-M. Pflimlin, “Nonlinear complementary
filters on the special orthogonal group,” IEEE Trans. Autom. Control,
vol. 53, no. 5, pp. 1203–1218, Jun. 2008.
[35] H. M. T. Menegaz, J. Y. Ishihara, G. A. Borges, and A. N. Vargas,
“A systematization of the unscented Kalman filter theory,” IEEE Trans.
Autom. Control, vol. 60, no. 10, pp. 2583–2598, Oct. 2015.
[36] H. M. T. Menegaz, J. Y. Ishihara, and H. T. M. Kussaba, “Unscented
Kalman filters for Riemannian state-space systems,” IEEE Trans. Autom.
Control, vol. 64, no. 4, pp. 1487–1502, Apr. 2019.
[37] F. C. Park, “Distance metrics on the rigid-body motions with applica-
tions to mechanism design,” J. Mech. Des., vol. 117, no. 1, pp. 48–54,
1995.
Authorized licensed use limited to: Indian Institute of Technology Patna. Downloaded on April 22,2024 at 17:06:40 UTC from IEEE Xplore. Restrictions apply.
4307
[38] M. E. Pittelkau, “Rotation vector in attitude estimation,” J. Guid., Control,
Dyn., vol. 26, no. 6, pp. 855–860, 2003.
[39] H. Rehbinder and B. K. Ghosh, “Pose estimation using line-based dynamic
vision and inertial sensors,” IEEE Trans. Autom. Control, vol. 48, no. 2,
pp. 186–199, Feb. 2003.
[40] S. Salcudean, “A globally convergent angular velocity observer for rigid
body motion,” IEEE Trans. Autom. Control, vol. 36, no. 12, pp. 1493–1497,
Dec. 1991.
[41] B. J. Sipos, “Application of the manifold-constrained unscented Kalman
filter,” in Proc. IEEE/ION Position, Location, Navig. Symp., 2008, pp. 30–
43.
[42] A. M. Sjøberg and O. Egeland, “An EKF for Lie groups with application
to crane load dynamics,” Model., Identification, Control, vol. 40, no. 2,
pp. 109–124, 2019.
[43] R. A. Srivatsan, M. Xu, N. Zevallos, and H. Choset, “Probabilistic pose
estimation using a Bingham distribution-based linear filter,” Int. J. Robot.
Res., vol. 37, pp. 13–14, 2018.
[44] A. Sveier, A. M. Sjøberg, and O. Egeland, “Applied Runge-Kutta-Munthe-
Kaas integration for the quaternion kinematics,” J. Guid., Control, Dyn.,
vol. 42, no. 12, pp. 2747–2754, 2019.
[45] J. Thienel and R. M. Sanner, “A coupled nonlinear spacecraft attitude
controller and observer with an unknown constant gyro and gyro noise,”
IEEE Trans. Autom. Control, vol. 48, no. 11, pp. 2011–2015, Nov. 2003.
[46] B. Vik and T. I. Fossen, “A nonlinear observer for GPS and INS integra-
tion,” in Proc. 40th IEEE Conf. Decis. Control, Dec. 2001, pp. 2956–2961.
[47] Y. Wang and G. S. Chirikjian, “Error propagation on the Euclidean group
with applications to manipulator kinematics,” IEEE Trans. Robot., vol. 22,
no. 4, pp. 591–602, Aug. 2006.
[48] J. T. Wen and K. Kreutz-Delgado, “The attitude control problem,” IEEE
Trans. Autom. Control, vol. 36, no. 10, pp. 1148–1162, Oct. 1991.