Available via license: CC BY 4.0
Content may be subject to copyright.
Citation: Clemens, J.; Wellhausen, C.
The Square-Root Unscented and the
Square-Root Cubature Kalman Filters
on Manifolds. Sensors 2024,24, 6622.
https://doi.org/10.3390/s24206622
Academic Editor: Paolo Mercorelli
Received: 26 August 2024
Revised: 3 October 2024
Accepted: 5 October 2024
Published: 14 October 2024
Copyright: © 2024 by the authors.
Licensee MDPI, Basel, Switzerland.
This article is an open access article
distributed under the terms and
conditions of the Creative Commons
Attribution (CC BY) license (https://
creativecommons.org/licenses/by/
4.0/).
sensors
Article
The Square-Root Unscented and the Square-Root Cubature
Kalman Filters on Manifolds
Joachim Clemens * and Constantin Wellhausen
Cognitive Neuroinformatics Group, University of Bremen, 28359 Bremen, Germany; wellhausen@uni-bremen.de
*Correspondence: clemens@uni-bremen.de
Abstract: Estimating the state of a system by fusing sensor data is a major prerequisite in many
applications. When the state is time-variant, derivatives of the Kalman filter are a popular choice
for solving that task. Two variants are the square-root unscented Kalman filter (SRUKF) and the
square-root cubature Kalman filter (SCKF). In contrast to the unscented Kalman filter (UKF) and
the cubature Kalman filter (CKF), they do not operate on the covariance matrix but on its square
root. In this work, we modify the SRUKF and the SCKF for use on manifolds. This is particularly
relevant for many state estimation problems when, for example, an orientation is part of a state or
a measurement. In contrast to other approaches, our solution is both generic and mathematically
coherent. It has the same theoretical complexity as the UKF and CKF on manifolds, but we show that
the practical implementation can be faster. Furthermore, it gains the improved numerical properties
of the classical SRUKF and SCKF. We compare the SRUKF and the SCKF on manifolds to the UKF
and the CKF on manifolds, using the example of odometry estimation for an autonomous car. It is
demonstrated that all algorithms have the same localization performance, but our SRUKF and SCKF
have lower computational demands.
Keywords: unscented Kalman filter; square-root unscented Kalman filter; cubature Kalman filter;
square-root cubature Kalman filter; manifolds; state estimation; orientation estimation; localization;
autonomous systems
1. Introduction
State estimation is one of the most essential tasks in automated and autonomous sys-
tems. It refers to determining a usually time-variant state, which cannot be observed directly,
by fusing information from one or multiple sensors. A common example is localization,
i.e., estimating the position and altitude of a vehicle or other objects in an environment.
Due to their computational efficiency, variants of the Kalman filter [
1
] are popular
choices for solving state estimation problems. They assume that the uncertainty of all
models and the posterior is normally distributed, and they estimate the state in terms of its
mean and covariance. For non-linear systems, the extended Kalman filter (EKF) [
2
], the
unscented Kalman filter (UKF) [
3
], and the cubature Kalman filter (CKF) [
4
] are three of the
most fundamental implementations. The EKF uses a first-order Taylor series expansion
in order to linearize the models, while the latter two employ deterministic sampling
techniques. The UKF uses the so-called sigma points as samples, and the CKF uses cubature
points, which are generated in a slightly different way. In contrast to the EKF, the UKF
and the CKF are derivative-free and tend to be more accurate, but they also have a higher
computational complexity. An enhancement of the UKF and the CKF is the square-root
UKF (SRUKF) [
5
] and the square-root CKF (SCKF) [
4
], respectively, which do not track
the covariance matrix but tracks its square root. In the case of state estimation, they have
the same theoretical complexity as the corresponding non-square-root versions, but the
practical implementation can be faster. For the special case of parameter estimation, the
algorithms can be modified, resulting in an improved theoretical complexity [
5
]. Finally,
they are numerically more stable.
Sensors 2024,24, 6622. https://doi.org/10.3390/s24206622 https://www.mdpi.com/journal/sensors
Sensors 2024,24, 6622 2 of 19
A challenge in state estimation is the handling of manifold spaces. They behave like
a vector space locally but have a more complex global topology. Particularly in the case
of localization, one is almost always facing the special orthogonal group
SO(n)
, which
is the space of all rotations in
n
-dimensional space (usually
n=
2, 3) and has a circular
topological structure. In other words, whenever an orientation in 2D or 3D is part of a
state or a measurement, one has to deal with a manifold. This applies to many practically
relevant applications, including but not limited to aerospace, ground vehicles, maritime
applications, household robotics, and logistics. However, many estimation algorithms,
including the basic variants of the Kalman filter, can only operate on vector spaces and
cannot be applied here. As a result, dedicated implementations are needed, and a detailed
discussion on that can be found in [
6
,
7
]. There exist multiple approaches for the EKF [
8
–
11
],
the UKF [
7
,
12
], and the CKF [
13
]. However, to the best of our knowledge, there is no
generic and coherent implementation of the SRUKF or the SCKF on manifolds.
In this paper, we present modified versions of the SRUKF and the SCKF for use on
manifold spaces, which are based on the
⊞
-method (pronounced “boxplus-method”) [
7
].
With those algorithms, one can benefit from the improved properties of a square-root
filter in all applications where the state space or the measurement space is a manifold.
We make a one-to-one comparison of the SRUKF on manifolds to the SRUKF for vector
space and discuss all necessary changes in detail. Furthermore, we show how to convert
the SRUKF on manifolds into a SCKF on manifolds. The resulting algorithms can be
applied to almost arbitrary manifolds and are not limited to any particular ones. We use
the challenging problem of localizing an autonomous vehicle in 3D Euclidean space as an
example application in order to demonstrate the effectiveness of our approach. In particular,
the proposed SRUKF and SCKF are compared to the UKF and the CKF on a real-world
dataset. While maintaining the same localization performance, the SRUKF and the SCKF
are shown to be computationally more efficient.
The remainder of the paper is structured as follows. We first review related work
regarding, in particular, other attempts to apply the SRUKF and the SCKF to manifolds as
well as applications of the
⊞
-method. Furthermore, a brief overview of other advances in
Kalman-filtering techniques is given. In Section 3, we discuss the basics of encapsulating
manifolds using the
⊞
-method and corresponding challenges. In Section 4, we recap the
classical SRUKF and derive our SRUKF approach for use on manifolds. After that, in
Section 5, the necessary changes to turn the SRUKF on manifolds into a SCKF on manifolds
are discussed. The example application and a detailed empirical evaluation are presented
in Section 6. The paper concludes with a summary and an outlook.
2. Related Work
There are some works on using the SRUKF and the SCKF on manifold spaces. But to
the best of our knowledge, there is no solution that is both generic and coherent. In [
14
],
an SRUKF is used for visual–inertial navigation, with a quaternion being part of the state
space. The parameters of the quaternion are treated as a vector. Accordingly, the state
space has more degrees of freedom than the underlying manifold space, and thus, the
resulting constraints (unit quaternion) need to be enforced by a workaround. A quater-
nion is also used in the state vector of [
15
], where the SRUKF is applied to spacecraft
relative navigation. Here, a three-dimensional vector representing the attitude error is
used when generating the sigma points, which solves the issue of an over-parametrized
state space. While this is conceptually similar to the
⊞
-method used in our approach, the
particular solution is specific to quaternions. Furthermore, the algorithm cannot deal with
measurement spaces that are manifolds. A similar error-state formulation is employed
in [
16
] for using the SCKF in a strap-down inertial navigation system. This also applies
to [
17
], where the SCKF is used for attitude estimation. Accordingly, those approaches have
the same disadvantages. A detailed analysis on how to treat quaternions in a UKF, and a
possible solution is presented in [
18
]. The findings are applied to the SRUKF as well and a
corresponding algorithm is proposed. Both filters are able to handle not only quaternions
Sensors 2024,24, 6622 3 of 19
in the state but also quaternions as measurements. However, the treatment of the state and
the measurement space is specific to quaternions. In [
19
], a SCKF for attitude estimation is
proposed. It also allows for using quaternions in the state and as measurements. But this
approach is specific to quaternions as well and thus cannot be applied to other manifolds.
A more general approach to using the UKF and the SRUKF on constraint state spaces
is proposed in [
20
], which also has similarities to the
⊞
-method. However, the algorithm
does not apply a final transformation of the covariance matrix (or, more precisely, its
Cholesky factor), which is required to correctly represent the posterior state uncertainty.
This particular problem is discussed more in detail in the context of the UKF in [
7
] and in
Section 3of this work. Furthermore, the approach also does not support measurements
in manifold spaces. Both apply to [
21
] as well, where the SCKF is modified for use on
manifolds by employing Lie groups. It also does not perform the final transformation of the
covariance matrix, and the measurements are restricted to vector spaces. In [
22
], another
more general algorithm is presented, which utilizes Lie groups to implement an SRUKF on
manifolds. The need for a final transformation of the state uncertainty is addressed here.
The proposed solution is to apply a linearization using a first-order Taylor series expansion,
which is also discussed in Section 3(see (5) and (6)). However, this approach is undesirable
in a derivative-free filter like the SRUKF. In [
23
], the PhD thesis corresponding to [
18
], an
SRUKF on Riemannian manifolds is proposed. Since this algorithm is very general, it cannot
be embedded within a generic framework, which would allow for a convenient handling
of practically relevant manifolds (e.g.,
SO(n)
,
SE(n)
, and combinations of those with
Rn
).
Instead, it requires the definition of manifolds-specific operations in order to apply the
approach to a particular problem, which results in a significant implementation effort.
One of the most popular approaches for generic filtering on manifolds is the use of Lie
groups [
8
,
9
,
12
,
13
,
21
,
22
,
24
]. The general idea is to represent the state as a group element,
i.e., in a manifold space, while filter operations take place in the tangent space, which is a
vector space. The
⊞
-method, which is used for our approach, has been proposed in [
7
]. It is
mathematically basically equivalent to filtering on Lie groups but offers a more convenient
notation by encapsulating the group operations. The formalism has been used to implement
various estimation algorithms on manifolds. In the original publications, modified variants
of least squares and the UKF are presented. An implementation of the EKF on manifolds is
derived in [
10
] and further improved in [
11
]. The latter publication also demonstrates how
to realize a particle filter on manifolds using the
⊞
-method. Another popular application
is graph optimization [
25
], which is particularly relevant in the context of graph-based
simultaneous localization and mapping [
26
]. Most recently, [
27
] utilized the
⊞
-method for
applying the interacting multiple model filter and smoother to manifold spaces.
There are other advances in Kalman-filtering techniques beyond the SRUKF and SCKF
that are worth mentioning. However, since we are modifying existing approaches and do
not propose a fundamentally new one, we leave the detailed comparison of the different
algorithms to the corresponding papers. Furthermore, we are focusing on conceptional
developments and do not include application-specific improvements. One advancement
is the combination of interacting multi-model techniques with the EKF (IMM-EKF). Here,
the filter can switch between different models based on the development of uncertainty
in order to adapt to changing conditions. This has, for example, been applied to vehicle
localization [
28
] and multi-target state estimation [
29
]. The technique has also been applied
to the UKF, which is consequently referred to as interacting multi-model UKF (IMM-
UKF). Possible applications are, for example, sensor fault detection for unmanned aerial
vehicles [
30
] or sensor calibration for underwater vehicles [
31
]. Another development is the
multi-state constraint Kalman filter (MSCKF) [
32
], which is based on the EKF. Here, the filter
tracks multiple instances of the state at different time points, which is particularly relevant
for visual odometry. There is also a cubature-variant of the MSCKF [
33
]. Furthermore,
IMM techniques have most recently been combined with the MSCKF in order to form the
IMM-MSCKF [34].
Finally, the invariant EKF (IEKF) [
24
] is another significant advancement
in recent years. It is mainly used for filtering on Lie groups and uses an alternative
Sensors 2024,24, 6622 4 of 19
formulation of the EKF, where the filter gain becomes invariant of the estimated state.
As a result, the filter has a better convergence behavior and is more stable in cases of a
large deviation between the estimate and the true state. An invariant UKF (IUKF) [
12
] and
invariant CKF (ICKF) [
13
,
35
] have been proposed as well. In addition to the Kalman filter,
the unscented transform has been applied to the particle filter, which is referred to as the
unscented particle filter (UPF) [
36
]. There exists also a square-root variant, which is the
square-root UPF (SRUPF) [37].
3. Encapsulation of Manifolds Using the ⊞-Method
The
⊞
-method is based on two observations: First, manifolds locally behave like
a vector space. Second, many algorithms that may potentially be used on manifolds
(examples see previous section) mainly manipulate the elements of the state space using
addition and subtraction. This is exploited by encapsulating a manifold state space
S
using two operators
⊞:S ×Rn7→ S
and
⊟:S ×S 7→ Rn
, where
n
denotes the number
of degrees of freedom in
S
. The operator
⊞
adds a small perturbation given in
Rn
to
an element in
S
, which yields an element in
S
. The operator
⊟
calculates the difference
between two elements in S, which yields an element in Rn. Thus we have
y=x⊞d, (1)
d=y⊟x, (2)
with
x
,
y∈ S
and
d∈Rn
. A manifold space with both box operators is referred to as a
⊞
-
manifold. Further relevant properties, mathematical derivations, as well as the definitions
of those operators for different manifold spaces can be found in [7].
In order to extend an algorithm defined for vector spaces to work on
⊞
-manifolds,
one basically needs to replace
+/−
with
⊞/⊟
. However, many fusion algorithms rely
on probability distributions in order to represent the uncertainty of the estimate and the
sensor measurements. Most commonly, a normal distribution is used. Its definition can be
extended to ⊞-manifolds using [7]
NS(µ,Σ) = µ⊞N(0, Σ), (3)
with mean
µ∈ S
and covariance
Σ∈Rn×n
. Here,
N
is an ordinary multivariate normal
distribution defined on vector spaces, while
NS
is a normal distribution on
S
. When clear
from the context, the subscript of NSis omitted in the following.
An important implication of (3) is that the mean cannot be changed without updating
the covariance as well [
7
]. In particular, when adding a perturbation
d
to
µ
using
µ⊞d
(e.g.,
in the correction step of a Kalman filter),
Σ
is still defined with respect to the old mean.
This results in an inconsistency since
µ⊞N(d,Σ)= (µ⊞d)⊞N(0, Σ). (4)
For derivation-based algorithms, like the EKF on
⊞
-manifolds [
11
], the problem can
be solved by linearizing
µ⊞d
using a first-order Taylor series expansion developed around
µand transforming Σaccordingly. This gives us
µ′=µ⊞d, (5)
Σ′=JdΣJ⊤
d, (6)
with
Jd=∂µ⊞d
∂µ
. However, as discussed already, this approach is not desired in derivation-
free algorithms.
Consequently, in the UKF on
⊞
-manifolds [
7
], an additional sigma point transforma-
tion is performed to retrieve the updated mean and covariance. In particular, instead of
using the
⊞
-operator to apply
d
to
µ
directly, sigma points with a corresponding devia-
tion are generated, which are then used to compute the new mean and covariance. This
results in
Sensors 2024,24, 6622 5 of 19
X=µ⊞dµ⊞(d+√Σ)¯
µ⊞(d−√Σ), (7)
µ′=MEANOFSI GMA POINTS(X), (8)
Σ′=1
2
2n
∑
i=0Xi⊟µ′Xi⊟µ′⊤, (9)
where
MEANOFSI GMA POINTS
is given in Table 1. Further details on this approach can be
found in [7] and are discussed in the context of the SRUKF in the following section.
Table 1. Method for computing the mean
µ′
of a set of sigma points
Y
, where
wi
corresponds to the
weight associated with the i-th sigma point. (Algorithm adopted from [7]).
MEANOFSI GMA POINTS
Input:
Yi,i=0, . . . , 2n(10)
Determine mean µ′:
µ′
0=Y0(11)
µ′
k+1=µ′
k⊞
2n
∑
i=0
wiYi⊟µ′
k(12)
µ′=lim
k7→∞µ′
k(13)
4. Square-Root Unscented Kalman Filter on Manifolds
In this section, we first review the classical SRUKF for regular vector spaces. After
that, we introduce and discuss the necessary modifications in order to apply the SRUKF to
⊞-manifolds. Both algorithms are given in Table 2next to each other.
4.1. Classical Square-Root Unscented Kalman Filter
In the classical SRUKF [
5
], the state
xt∈Rn
, the state transition measurement
ut∈Rm
,
and the correction measurement
zt∈Rk
are regular vectors, where
t
is the current time
step. Furthermore, as for the majority of Kalman filters, the state is assumed to be normally
distributed given the set of measurements, i.e.,
xt|u1:t,z1:t∼ N (µt,Σt). (14)
The filter also estimates the mean
µt∈Rn
. But instead of determining the covariance
Σt∈Rn×n
, it tracks its square root
St=√Σt
. The square root is calculated once on
initialization using a Cholesky decomposition. After that, the filter operates on the Cholesky
factors, which are propagated and updated accordingly.
Thus, the inputs in (15) of one time step of the SRUKF are the previous estimate defined
by
µt−1
,
St−1
, the current state transition measurement
ut
, and the current correction
measurement zt. Furthermore, the uncertainty of the state transition and of the correction
measurement are given in terms of covariance matrices
Ut∈Rn×n
,
Zt∈Rk×k
of the
corresponding additive, normally distributed noise, where, again, the square-root form
is used.
Sensors 2024,24, 6622 6 of 19
Table 2. Comparison of the classical SRUKF for vector spaces and the modified SRUKF for
⊞-manifolds.
Classical SRUKF
Input:
µt−1,St−1,ut,zt,√Ut,√Zt(15)
Prediction Step:
Xt−1=(µt−1µt−1+ηSt−1µt−1−ηSt−1)(16)
¯
X∗
t=f(Xt−1,ut)(17)
¯
µt=
2n
∑
i=0
wi¯
X∗
t,i(18)
¯
St=qrn√w1¯
X∗
t,1:2n−¯
µt√Uto (19)
¯
St=cholupdate¯
St,¯
X∗
t,0 −¯
µt,w0(20)
Correction Step:
¯
Xt=(¯
µt¯
µt+η¯
St¯
µt−η¯
St)(21)
¯
Zt=h(¯
Xt)(22)
ˆ
zt=
2n
∑
i=0
wi¯
Zt,i(23)
Sz
t=qrn√w1¯
Zt,1:2n−ˆ
zt√Zto (24)
Sz
t=cholupdateSz
t,¯
Z∗
t,0 −ˆ
zt,w0(25)
Σx,z
t=
2n
∑
i=0
wi¯
Xt,i−¯
µt ¯
Zt,i−ˆ
zt⊤(26)
Kt= (Σx,z
t/Sz
t⊤)/Sz
t(27)
µt=¯
µt+Kt(zt−ˆ
zt)(28)
Vt=KtSz
t(29)
St=cholupdate¯
St,Vt,−1(30)
SRUKF on ⊞-Manifolds
aInput:
µt−1,St−1,ut,zt,√Ut,√Zt(31)
Prediction Step:
Xt−1=(µt−1µt−1⊞ηSt−1µt−1⊞−ηSt−1)(32)
¯
X∗
t=f(Xt−1,ut)(33)
¯
µt=MEANOFSIGMA POINTS(¯
X∗
t)(34)
¯
St=qrn√w1¯
X∗
t,1:2n⊟¯
µt√Uto (35)
¯
St=cholupdate¯
St,¯
X∗
t,0 ⊟¯
µt,w0(36)
Correction Step:
¯
Xt=(¯
µt¯
µt⊞η¯
St¯
µt⊞−η¯
St)(37)
¯
Zt=h(¯
Xt)(38)
ˆ
zt=MEANOFSI GMA POINTS(¯
Zt)(39)
Sz
t=qrn√w1¯
Zt,1:2n⊟ˆ
zt√Zto (40)
Sz
t=cholupdateSz
t,¯
Z∗
t,0 ⊟ˆ
zt,w0(41)
Σx,z
t=
2n
∑
i=0
wi¯
Xt,i⊟¯
µt ¯
Zt,i⊟ˆ
zt⊤(42)
Kt= (Σx,z
t/Sz
t⊤)/Sz
t(43)
δt=Kt(zt⊟ˆ
zt)(44)
Vt=KtSz
t(45)
S′
t=cholupdate¯
St,Vt,−1(46)
X′
t=¯
µt⊞δt¯
µt⊞η(δt+S′
t)¯
µt⊞η(δt−S′
t)(47)
µt=MEANOFSI GMA POINTS(X′
t)(48)
St=qr√w1X′
t,1:2n⊟µt (49)
St=cholupdateS′
t,X′
t,0 ⊟µt,w0(50)
4.1.1. Prediction Step
The mean
µt−1
is propagated in the same way as in the regular UKF by generating
a set of sigma points
Xt−1
for the previous estimate, propagating them through the state
transition function
f:Rn×Rm7→ Rn
, and retrieving the predicted mean
¯
µt
using a
weighted sum (see (16)–(18)). The only difference is that the covariance
Σt−1
is already
given in its square-root form
St−1
, and thus, no Cholesky decomposition needs to be
applied when generating the sigma points in (16). Note that each sigma point
Xt−1,i∈Rn
,
i=
0,
. . .
, 2
n
is a vector with the same dimensionality as the state. For the scaling factor
η
of the sigma points, we choose
η=√n+κ
with
κ=
3
−n
according to [
3
]. Consequently,
the weights are set to w0=κ
n+κand wi=1
2(n+κ),i=1, . . . , 2n.
The propagated Cholesky factor
¯
St
is computed via a QR decomposition followed by
a Cholesky update (or downdate). The QR decomposition in (19) is applied to the matrix
of the weighted differences between the propagated sigma points ¯
X∗
tand the propagated
Sensors 2024,24, 6622 7 of 19
mean
¯
µt
compound with the square root of the state transition noise
Rt
. We adopt the
notation of [
5
], where
qr{A}
is used as shorthand for computing the QR decomposition
A⊤=QR
and returning the transposed upper triangular matrix of
R
. The Cholesky
update in (20) is necessary because
w0
may be negative, in which case the update actually
becomes a downdate. Accordingly, the QR decomposition is only applied to the sigma
points 1,
. . .
, 2
n
, while the 0-th sigma point is incorporated using the Cholesky up- or
downdate. We again follow the notation of [
5
] and use
cholupdate{S
,
v
,
±w}
for updating
the Cholesky factor
S
of
P=AA⊤
such that it corresponds to the Cholesky factor of the
rank-1 update
P±√w vv⊤
. Furthermore, if
v
is a matrix instead of a vector,
cholupdate{·}
performs consecutive updates for each column of v.
4.1.2. Correction Step
In (21)–(26) of the correction step, the differences between the SRUKF and the UKF
are the same as in the prediction step. More precisely, generating the sigma points in (21)
does not require a Cholesky decomposition since the propagated Cholesky factor
¯
St
is
already given. Furthermore, the Cholesky factor
Sz
t
of the measurement error covariance
is computed using a QR decomposition in (24) followed by a Cholesky up- or downdate
in (25). The computation of the expected measurement
ˆ
zt
using the measurement function
h:Rn7→ Rk
in (22) and a weighted sum in (23) as well as the calculation of the cross-
covariance Σx,z
tin (26) are unchanged.
Calculating the Kalman gain
Kt
requires an expensive matrix inversion in the UKF.
In the SRUKF, it is instead computed in (27) using two nested inverse solutions of
Kt(Sz
tSz
t⊤) = Σx,z
t,
which can be implemented using efficient back-substitutions. As in [
5
],
we use
x=b/A
to denote solving
Ax =b
for
x
. The Kalman gain is used to weigh the
innovation
zt−ˆ
zt
, which is subsequently added to the propagated mean
¯
µt
in order to
obtain the posterior mean
µt
in (28). This is, in turn, equivalent to the UKF. For computing
the posterior Cholesky factor
St
in (29) and (30), we utilize a series of Cholesky downdates.
They are applied to the predicted Cholesky factor
¯
St
, while the columns of
Vt=KtSz
t
, i.e.,
the weighed Cholesky factor of the measurement error covariance, are used as arguments.
This concludes the classical SRUKF algorithm. For further details, including a compar-
ison of the complexity to the UKF, the reader is referred to the original publication [5].
4.2. Modifying the Square-Root Unscented Kalman Filter
In order to apply the SRUKF to
⊞
-manifolds, a few modifications are required. First,
the state
xt∈ S
, the state transition measurement
ut∈ U
, and the correction measurement
zt∈ Z
are allowed to be elements in arbitrary manifold spaces
S
,
U
,
Z
with corresponding
box operators. The number of degrees of freedom of those spaces are
n
,
m
,
k
, i.e., the
dimensionality of the corresponding covariance matrices and Cholesky factor remains the
same. The posterior
p(xt|u0:t
,
z0:t)
is assumed to be normally distributed as in (14), but here
we have a normal distribution on a ⊞-manifold defined according to (3). This gives us
xt|u1:t,z1:t∼ NS(µt,Σt), (51)
with
µt∈ S
being an element in the manifold space of the state. Analogously, the state
transition noise and the measurement noise are defined by normal distributions on
⊞
-
manifolds as well. The filter estimates
µt
and the corresponding Cholesky factor
St=√Σt
.
Accordingly, the input in (31) for one time step of the modified SRUKF is the same as for
the classical SRUKF, except that
µt−1
,
ut
,
zt
can be an element in manifold spaces instead of
being limited to vector spaces.
4.2.1. Prediction Step
In the first step of the prediction in (32), the sigma points
Xt−1
are generated using
the mean
µt−1
and the Cholesky factor
St−1
of the previous time step without the need
to compute a Cholesky decomposition. Since
µt−1
is a manifold, the perturbation vectors
are added using the
⊞
-operator. Note that the negative perturbation
µt−1−St−1
turns
Sensors 2024,24, 6622 8 of 19
into µt−1⊞−St−1, since the ⊟-operator is not applicable here. Each resulting sigma point
Xt−1,i∈ S
,
i=
0,
. . .
, 2
n
is an element in the manifold space
S
of the state. The sigma
points are then propagated through the state transition function
f:S × U 7→ S
in (33),
which takes elements of manifold spaces as arguments and maps them to a manifold space.
For calculating the mean of sigma points, one cannot use a weighted average because
of the potentially complex structure of the underlying manifold. Instead, an iterative
approach is employed, which is adapted from [
7
] and shown in Table 1. This algorithm is
used to retrieve the propagated mean
¯
µt
in (34). As in the classical SRUKF, the propagated
Cholesky factor
¯
St
is computed using a QR decomposition followed by a Cholesky up- or
downdate (see (35) and (36)). However, since the sigma points, as well as the propagated
mean, are elements of a manifold space, the
⊟
-operator is used to calculate the differences
between the sigma points ¯
X∗
tand the propagated mean ¯
µt.
4.2.2. Correction Step
The first part of the correction step is modified analogously to the prediction step
(see (37)–(43)). In particular, the
⊞
-operator is used to generate the sigma points in (37),
the measurement function
h:S 7→ Z
in (38) is defined on manifold spaces, and the
iterative algorithm of Table 1is applied to compute the expected measurement in (39).
Furthermore, the
⊟
-operator is utilized to determine the difference between the sigma
points and the expected measurement in the QR decomposition in (40) and in the Cholesky
up- or downdate in (41). The same applies to the calculation of the cross-covariance
in (42), where the
⊟
-operator is used as well. The equation for the Kalman gain in (43)
remains unchanged.
A major difference is the update of the mean. An ad-hoc approach would be replacing
−
with
⊟
and
+
with
⊞
in (28) for calculating the innovation and adding the weighted
innovation to the propagated mean. This would yield
µt=¯
µt⊞Kt(zt⊟ˆ
zt)
. However, as
discussed in Section 3, this would change the mean, while the covariance (or, in our case,
the Cholesky factor) is still defined with respect to the old mean (see (4)). This is also not
changed by updating the Cholesky factor by incorporating the weighted Cholesky factor of
the most recent measurement using (29) and (30).
To resolve this issue, we adopt the approach of the UKF on
⊞
-manifolds of [
7
] (see
also (7)–(9)), which we transfer to the SRUKF. First, we compute the state deviation vector
δt∈Rn
in (44), which is given by the innovation
zt⊟ˆ
zt
weighted by the Kalman gain
Kt
. The innovation is, in turn, determined using the
⊟
-operator since the measurement
may be in a manifold space. Then the Cholesky factor is updated in (45) and (46) in the
same way as in the classical SRUKF. However, the result
S′
t
does not become the posterior
Cholesky factor but is used as an intermediate value. In particular, new sigma points
X′
t∈ S
are generated in (47) by adding
δt
and
S′
t
as perturbation using the
⊞
-operator.
Finally, the posterior mean
µt
and Cholesky factor
St
are retrieved from the sigma points.
For calculating the mean in (48), the iterative approach of Table 1is used. The Cholesky
factor is computed by incorporating the sigma points 1, .. . , 2nusing a QR decomposition
in (49), followed by a Cholesky up- or downdate in (50) for the 0-th sigma point. The
difference between the sigma points and the posterior mean is again determined using the
⊞-operator.
4.2.3. Computational Complexity
Up to (46), the difference in computational complexity between the
⊞
-manifold ver-
sions of the UKF and the SRUKF is the same as the difference between the classical UKF
and SRUKF. The latter is analyzed in [
5
], the conclusion of which is that both are in
O(n3)
.
However, even though the theoretical complexity is in the same class, the implementation
of particular steps can be more efficient in the SRUKF compared to the UKF. For example,
the generation of sigma points requires a Cholesky decomposition of complexity
O(n3/
6
)
in the UKF, while in the SRUKF in (16), (21), (32), and (37) no extra computations are
required since the Cholesky factor is already given. Furthermore, the back-substitutions
Sensors 2024,24, 6622 9 of 19
for calculating the Kalman gain in (27) and (43) of the SRUKFs are more efficient than the
matrix inverse needed in the UKFs.
The same holds true for the extension in (47)–(50), which is necessary for updating the
Cholesky factor correctly in the manifold case. In particular, the generation of sigma points
in (47) requires no expensive Cholesky decomposition as in the UKF on
⊞
-manifolds. The
complexity of the QR decomposition (49) is
O(n3)
, while the Cholesky update (50) is in
O(n2)
. In the UKF on
⊞
-manifolds, those steps correspond to computing the posterior
covariance, which is implemented using
∑2n
i=0wi(X′
t,i⊟µt)(X′
t,i⊟µt)⊤
. Assuming that
X′
t,i⊟µtis precomputed, the complexity of this step is O(2n2+2n3).
In summary, the SRUKF with the modifications for
⊞
-manifolds is still in
O(n3)
and
thus in the same theoretical complexity class as the UKF on
⊞
-manifolds. But individual
steps can be implemented more efficiently in practice, which has the potential for a faster
execution time. This is investigated empirically in the next section. Furthermore, according
to [
5
], the numerical properties of the SRUKF are improved over the UKF, similar to the
square-root Kalman filter [38].
5. Square-Root Cubature Kalman Filter on Manifolds
Due to the similarities between the SRUKF and the SCKF, it requires only a few
changes to turn the SRUKF on manifolds into a SCKF on manifolds. The main difference is
that the SRUKF uses sigma points as sample points, while the SCKF uses so-called cubature
points. Those are generated in a slightly different way. Furthermore, the recovery of the
parameters of the normal distribution from the sample points needs to be adapted. The
particular changes to the SRUKF on manifolds algorithm in Table 2are as follows:
• The generation of sample points in (32), (37), and (47) is changed to
Xt−1=(µt−1⊞ηSt−1µt−1⊞−ηSt−1), (52)
¯
Xt=(¯
µt⊞η¯
St¯
µt⊞−η¯
St), (53)
X′
t=¯
µt⊞η(δt+S′
t)¯
µt⊞η(δt−S′
t), (54)
respectively. Note that there is no sample point anymore that corresponds to the
mean. Accordingly, the total number of sample points is now 2
n
instead of 2
n+
1. For
consistency with the other equations, we change the indexing to
i=
1,
. . .
, 2
n
, i.e., the
index i=0 is removed.
• The scaling factor for generating the cubature point is set to η=√n.
•
The weights of the sample points in (35), (40), (42), and (49) are changed to
wi=1
2n
,
i=1, . . . , 2n.
•
Since there is no 0-th sample point anymore, the sum in (42) becomes
∑2n
i=1
, i.e., its
lower bound is changed from 0 to 1.
•
There is no weight anymore that could have a negative value, and all sample points are
already considered in the QR decompositions in (35), (40), and (49). Accordingly, the
Cholesky up- or downdates in (36), (41), and (50) are skipped. Note that the series of
Cholesky downdates in (46) is still required for computing the (intermediate) updated
Cholesky factor.
Furthermore, the
MEANOFSI GMA POINTS
algorithm in Table 1needs to be adapted as
well to match the changes to the sample points:
• The weights in (12) are changed to wi=1
2n,i=1, . . . , 2n.
• The sum in (12) becomes ∑2n
i=1.
•
There is no 0-th sample point anymore that corresponds to the mean and can be used
for the initialization in (11). Since the remaining sample points are spread around the
mean, choosing any other one would lead to a slow convergence of the limit operation
in (13). Accordingly, the initial value is set to
µ′
0=Y1⊞1
2(Yn+1⊟Y1). (55)
Sensors 2024,24, 6622 10 of 19
The sample points
Y1
and
Yn+1
are approximately opposite to each other (with respect
to the mean), and thus, the chosen value for
µ′
0
is sufficiently close to the actual mean
to alow for a fast convergence.
Regarding the computational complexity, the general findings for the SRUKF on
manifolds (see Section 4.2.3) hold true as well. The SCKF on manifolds has a slight speed
advantage though, since it uses one sample point less (2
n
instead of 2
n+
1) and three
Cholesky up- or downdates are skipped. Accordingly, the computational complexity is
improved by a constant factor, but the algorithm remains in the same complexity class.
6. Evaluation
We compare the proposed SRUKF on manifolds and SCKF on manifolds to the UKF
on manifolds [
7
] and the CKF on manifolds. For the latter, we modify the UKF on man-
ifolds analogously to the changes discussed in Section 5. All filters are implemented in
C++ using the Eigen library (https://eigen.tuxfamily.org, accessed on 26 August 2024).
For the implementation of the SRUKF and the SCKF, there are a few points to consider:
The QR decomposition in Eigen is based on Househoulder transformations. To gain a
speed advantage, we implement the QR decomposition using the modified Gram-Schmidt
algorithm [
39
] (Sect. 5.2.8), which is further optimized for computing
R
only, since we are
not interested in
Q
. Furthermore, Eigen’s matrix storage order should be taken into account
to benefit from CPU caching. The Cholesky update or downdate is computed in-place. One
could also convert constant covariance matrices (e.g., for sensor noise) to their square-root
forms once in order to save time for the additional computations in each step, but this is
not performed here for the commonality with the non-square-root filters.
In this section, we first introduce the example application as well as the corresponding
dataset and used parameters. After that, we evaluate the performance of the filters in terms
of state estimation accuracy and runtime.
6.1. Example Application: Inertial Odometry
We use relative localization (i.e., odometry) of an autonomous car as an example
application for our evaluation. For this purpose, the vehicle [
40
] is equipped with an
inertial measurement unit (IMU), wheel odometers, and a sensor measuring the steering
wheel angle. The application, all corresponding models, and the use of the UKF on
manifolds to solve the estimation problem are described in detail in [
41
]. The values to
be estimated are the 3D position in
R3
, the 3D orientation in
SO(
3
)
, the 3D velocity in
R3
, as well as the 3D accelerometer and 3D gyroscope biases, each in
R3
. Since
SO(
3
)
is
a manifold, the joint state space is a manifold as well, and a corresponding filter needs
to be used.
One challenge with this particular estimation task is that there is no external source for
directly or indirectly correcting the position and the rotation around the global z-axis. This
is no issue for the application itself since we are only interested in odometry, i.e., estimating
the relative movement of the vehicle. But it results in a growing covariance matrix, which
may result in instabilities of the filter. Moreover, the (SR)UKF and the (S)CKF may fail
completely, since the generated sample points are not meaningful anymore for very large
covariance values.
The solution proposed in [
41
] is to estimate the uncertainty with respect to a reference
state, which is moved forward in time. By that means, the covariance matrix remains within
a reasonable range. The moving reference is implemented using two Kalman filters, which
process the same measurements but use different references. After a defined period of time
or driven distance, the uncertainty of one filter is transferred to the other filter, while the
uncertainty of the first filter is set to zero. Again, full details and all derivations can be
found in the original publication.
In the UKF and the CKF, the uncertainty transfer and the uncertainty reset are each
implemented using a propagation of sample points followed by recovering the mean and
covariance. To apply this concept to the SRUKF on manifolds, we employ the familiar mod-
Sensors 2024,24, 6622 11 of 19
ifications: The sigma points are generated using the Cholesky factor without the need for an
additional Cholesky factorization, the mean is calculated using the
MEANOFSI GMA POINTS
function (see
Table 1
), and the updated Cholesky factor is retrieved using a QR decom-
position followed by a Cholesky up- or downdate. This is, in essence, equivalent to the
prediction step (see (32)–(36) in Table 2) except for that the state transition function
f
is replaced by the functions for the uncertainty transfer or the uncertainty reset defined
in [
41
]. Furthermore, no additional noise is introduced, i.e.,
Ut
is omitted. For the SCKF,
the uncertainty transfer and the uncertainty reset are implemented in the same way as for
the SRUKF, but with the changes discussed in Section 5, i.e., we generate cubature points
instead of sigma points, the scaling factor and the weights are adapted, and there is no
need for a Cholesky up- or downdate to retrieve the updated Cholesky factor. Furthermore,
the modified version of ME ANOFSI GMA POINTS is used.
6.2. Dataset and Parameters
To the best of our knowledge, there exists no public dataset that contains all desired
measurements (IMU, speed, steering angle, and RTK reference system) for our application.
For that reason, we use a self-recorded dataset, which consists of four complementary
scenarios, which are listed in Table 3. The first scenario takes place on a parking lot
with partially harsh maneuvers, including sharp accelerating and decelerating as well as
high turning speeds. In the second scenario, we are driving through a suburb of Bremen,
Germany, with comparably low speeds. The third scenario is recorded in the inner city
of Chemnitz, Germany, and has medium speeds. The fourth and longest scenario also
takes place in Chemnitz, Germany, but the majority of the time, we are driving on an urban
freeway with higher speeds.
Table 3. Datasets.
Name Duration Distance Median Speed Max. Speed Trajectory
Parking lot 11:11 3.01 km 17.5 km/h 48.8 km/h Figure 1a
Suburb 20:46 4.51 km 13.8 km/h 51.0 km/h Figure 1b
City 13:19 5.52 km 21.1 km/h 60.9 km/h Figure 1c
Urban freeway 42:28 25.80 km 38.6 km/h 101.6 km/h Figure 1d
(a) (b)
Figure 1. Cont.
Sensors 2024,24, 6622 12 of 19
(c) (d)
Figure 1. Ground-truth trajectories of the evaluation dataset. (Maps generated using UMap based on
OpenStreetMaps data). (a) Parking Lot; (b) Suburb; (c) City; (d) Urban freeway.
The ground-truth position is given by RTK-corrected GNSS measurements provided
by a ublox F9P receiver at 10 Hz. When the GNSS solution has a quality of “RTK fix”
(i.e., integer solution), the position accuracy is within 0.05 m. All data points with a
worse quality are excluded. For the datasets “Parking lot”, “City”, and “Urban freeway”,
the vehicle was equipped with a second ublox F9P receiver operating in moving base
mode, which provides a precise heading (and pitch) estimate with an accuracy of less
than 1°. In the case of the “Suburb” dataset, a second receiver was not available, and the
heading is estimated from the vehicle’s movement. The assumed accuracy is about 5°. The
ground-truth trajectories are shown in Figure 1.
All parameters used for the evaluation are listed in Table 4. They are equal for both filters.
If possible, the noise parameters for the sensors have been estimated from the data. Otherwise,
the values have been set by experts and were tuned for the best performance of the filters.
Table 4. Parameters (noise given as standard deviation).
Name Value Remark
accelerometer noise 0.15 m/s2for each axis
gyroscope noise 0.15 °/s for each axis
accelerometer bias random walk 0.01 m/s2/s for each axis
gyroscope bias random walk 10−6°/s/ s for each axis
accelerometer bias init noise 1.0 m/s2for each axis
gyroscope bias init noise 1.0 °/s for each axis
speed noise 0.1 m/s
zero speed noise y-axis 0.3 m/s see [41] for further details
zero speed noise z-axis 0.25 m/s see [41] for further details
steering angle noise 2.0°
covariance reference time 10 s see [41] for further details
covariance reference distance 0.5 m see [41] for further details
6.3. Relative Trajectory Error
Since the state is estimated without absolute position corrections, the relative trajectory
error is the most relevant accuracy measure. As proposed in [
42
], we use segments of
constant lengths (100 m, 200 m, 500 m, 1000 m, and 2000 m), align the first pose estimate of
each segment with the corresponding ground-truth pose, and then compute the translation
and rotation error for the pose at the end of the segment. The values are normalized
Sensors 2024,24, 6622 13 of 19
with respect to the respective segment length, resulting in the translation error being in
percent, while the rotation error is given in degrees per meter. For computing the errors
and generating the plots, the script of [43] is used.
The resulting box plots are shown in Figure 2. The median of the translation error
is between 7% and 1%, while the rotation error is between 0.04 °/m and 0.005 °/m. For
all datasets, the rotation error decreases with the segment length. For the “Parking lot”
and the “Urban freeway” scenario (see Figure 2a,d), this also holds true for the translation
error, while for “Suburb” and “City” (see Figure 2b,c), it is rather constant. The smallest
translation error can be observed for “Parking lot”, which can be explained by the short
overall distance. The “Urban freeway” scenario, with its long distance and high speeds, has
the highest translation error, particularly for short segment lengths. The “Suburb” dataset
has the highest rotation error, which is caused by the fact that a different IMU is used here.
Regarding the scope of this work, the most significant insight is that the SRUKF and the
SCKF have exactly the same performance as the UKF and the CKF.
(a)
(b)
(c)
Figure 2. Cont.
Sensors 2024,24, 6622 14 of 19
(d)
Figure 2. Relative translation and rotation error for different segment lengths. (a) Parking Lot;
(b) Suburb; (c) City; (d) Urban freeway.
6.4. Absolute Trajectory Error
Due to the lack of absolute position corrections (see above), the absolute trajectory
error is less relevant. It is nevertheless analyzed for completeness. A difficulty is that a small
estimation error at the beginning (in particular in the orientation) can have a huge impact
on the overall error, even though the remaining trajectory has been estimated with high
accuracy. To mitigate this effect, we align the estimated trajectory with the ground-truth
using an
SE(
2
)
transformation (see [
43
] for further details). After that, the root-mean-square
error (RMSE) of the position is computed over the whole trajectory.
The aligned trajectories are shown in Figures 3and 4. All estimated trajectories show
a small drift compared to the ground-truth, which cannot be avoided without absolute
position corrections. Most noticeable is a deviation at the beginning of the “City” scenario
(top right in Figures 3c and 4c). This is likely caused by the fact that the estimate of the IMU
biases did not match the true values while they converged later. It is again evident that the
performance of all filters is equivalent since the estimated trajectories of the UKF and the
SRUKF, as well as the trajectories of the CKF and the SCKF, align perfectly with each other.
This is confirmed by the quantitative errors given in Table 5. In case of “Parking lot”,
the RMSE of all filters is exactly the same, except for the SCKF, where the RMSE is 1 mm
smaller. For “Suburb” the RMSE differs between 3 mm and 7 mm. The biggest difference
can be seen in the “City” scenario, where the RMSE of the cubature filters is almost 0.5 m
larger than the RMSE of the unscented filters. However, as discussed above, all filters had
difficulties estimating the IMU biases correctly at the beginning of that scenario and a minor
orientation error has a large impact on the absolute positioning error. Finally, in the “Urban
freeway” scenario, the RMSE of the UKF and the CKF are equal, and the RMSE of the SRUKF
and the SCKF are equal as well. At the same time, the error of the square-root filters is 8 mm
smaller than the error of the non-square-root variants. Note that the errors are calculated over
several kilometers, i.e., the deviations between the filters are negligibly small.
Table 5. Absolute root mean square (RMS) translation error.
Algorithm Parking Lot Suburb City Urban Freeway
UKF 2.612 m 45.509 m 35.990 m 57.536 m
SRUKF 2.612 m 45.512 m 35.991 m 57.528 m
CKF 2.612 m 45.513 m 36.444 m 57.536 m
SCKF 2.611 m 45.516 m 36.466 m 57.528 m
Sensors 2024,24, 6622 15 of 19
(a) (b)
(c) (d)
Figure 3. Estimated trajectories of the UKF and SRUKF aligned using an
SE(
2
)
transformation to the
ground-truth trajectories. Note the different scale of the figures. (a) Parking Lot; (b) Suburb; (c) City;
(d) Urban freeway.
(a) (b)
Figure 4. Cont.
Sensors 2024,24, 6622 16 of 19
(c) (d)
Figure 4. Estimated trajectories of the CKF and SCKF aligned using an
SE(
2
)
transformation to the
ground-truth trajectories. Note the different scale of the figures. (a) Parking Lot; (b) Suburb; (c) City;
(d) Urban freeway.
6.5. Runtime
Finally, we evaluate the runtime of the algorithms on a desktop computer with an
Intel® Core™ i7 13700K (16 cores, 5.40 GHz, Intel Corporation, Santa Clara, CA, USA) and
64 GB RAM. Note that the filters do not make use of parallelization and, thus, utilize only
one core. The results are averaged over 10 runs.
The total duration for processing each dataset, as well as the percentage difference
between the filters, are given in Table 6. Related to the duration of the respective scenarios
(see Table 3), all filters are able to process the measurements in real time. The speed
advantage of the SRUKF over the UKF is between 3% and 5%, while the speed advantage of
the SCKF over the CKF is between 3% and 6%. The CKF is slightly faster than the UKF, with
an advantage of up to 3%, while in the “City” scenario, it is 0.5% slower. In comparison to
the SRUKF, the SCKF is between 1% and 2% faster. Consequently, the largest difference is
between the UKF and the SCKF, where the latter has a speed advantage of 4% to 7%. Overall,
this confirms the expectations: The square-root filters are faster than the non-square-root
filters, while the cubature filters are, in general, faster than the unscented filters.
Table 6. Runtime of the algorithms and percentage difference.
Algorithm Parking Lot Suburb City Urban Freeway
UKF 6.31 s 6.47 s 9.72 s 29.21 s
SRUKF 6.15 s 6.14 s 9.42 s 28.17 s
CKF 6.26 s 6.31 s 9.77 s 28.46 s
SCKF 6.08 s 5.99 s 9.19 s 27.63 s
UKF vs. SRUKF 97.5% 94.9% 96.9% 96.4%
UKF vs. CKF 99.2% 97.5% 100.5% 97.4%
UKF vs. SCKF 96.4% 92.6% 94.5% 94.6%
CKF vs. SRUKF 98.2% 97.3% 96.4% 99.0%
CKF vs. SCKF 97.1% 94.9% 94.1% 97.1%
SRUKF vs. SCKF 98.9% 97.6% 97.6% 98.1%
7. Conclusions
In this work, we modified the SRUKF and the SCKF for use on manifolds. More precisely,
not only the state but also the measurements can be elements in almost arbitrary manifold
spaces. This is, for example, particularly relevant for state estimation when an orientation is
Sensors 2024,24, 6622 17 of 19
part of a state vector or a measured quantity, where the classical SRUKF and SCKF cannot
be applied. We employ the
⊞
-method for our approach, which offers a sound way to
encapsulate manifold spaces. In contrast to other solutions, our SRUKF on manifolds and
our SCKF on manifolds are both generic and mathematically coherent. They have the same
theoretical complexity as the UKF on manifolds and the CKF on manifolds, respectively, but
the practical implementation can be faster. In addition, as the classical SRUKF and SCKF, they
have improved numerical properties compared to the UKF and the CKF. Furthermore, for
parameter estimation, the algorithm can be modified for a lower computational complexity.
The only disadvantage is that the implementation effort is a little bit greater.
We demonstrated the effectiveness of our approach using the example of relative
localization (i.e., odometry) of an autonomous car, where the 3D orientation of the vehicle
is part of the state vector, and thus, the state space is a manifold. For this purpose, we used
four real-world datasets of different scenarios (parking lot, suburb, city, and urban freeway).
It was shown that the SRUKF and the SCKF on manifolds have the same localization
performance as the UKF and the CKF on manifolds in terms of relative translation and
rotation error as well as the absolute positioning error. At the same time, our SRUKF and
our SCKF have lower computational demands than their non-square-root counterparts.
One of our next steps is to compare the SRUKF and the SCKF to the UKF and the CKF
for parameter estimation on manifolds. Furthermore, we plan to apply our SRUKF and
our SCKF in different applications. One example is extended object tracking, where the
state of a dynamic object (e.g., another traffic participant) is estimated. This state consists
of a position, orientation, size, and velocity, and thus, it is an element of a manifold [
44
].
Another possible application is maritime robotics, where we aim to use the proposed
approach for localizing an unmanned surface vehicle. The state space is also a manifold
here since the 3D orientation is part of the state. Furthermore, angular heading and pitch
measurements are incorporated, so we have a manifold measurement space as well.
Author Contributions: Conceptualization, J.C. and C.W.; methodology, J.C.; software, J.C. and C.W.;
validation, J.C. and C.W.; formal analysis, J.C.; investigation, J.C.; resources, J.C. and C.W.; data
curation, J.C. and C.W.; writing—original draft preparation, J.C.; writing—review and editing, J.C.
and C.W.; visualization, J.C.; supervision, J.C.; project administration, J.C.; funding acquisition, J.C.
and C.W. All authors have read and agreed to the published version of the manuscript.
Funding: This research was partially funded by German Aerospace Center (DLR) Space Admin-
istration with financial means of the German Federal Ministry for Economic Affairs and Climate
Action (BMWK) on the basis of a decision by the German Bundestag, projects “OPA3L” (grant
No. 50 NA 1909), “MUTIG-VORAN” (grant No. 50 NA 2202A), “Safety Control Center” (grant
No. 50 NA2302B), and “iMarEx” (grant No. 50 NA 2206A).
Institutional Review Board Statement: Not applicable.
Data Availability Statement: The data used in this research are available at https://github.com/
JoachimClemens/AD-Datasets (accessed on 25 August 2024).
Acknowledgments: The authors thank IAV GmbH, Department Automated Driving Functions,
Germany, for providing the research vehicle and the test driver for recording the datasets in Chemnitz.
Conflicts of Interest: The authors declare no conflict of interest. The funders had no role in the design
of the study, in the collection, analyses, or interpretation of data, in the writing of the manuscript, or
in the decision to publish the results.
Abbreviations
The following abbreviations are used in this manuscript:
CKF cubature Kalman filter
CPU central processing unit
EKF extended Kalman filter
GNSS global navigation satellite system
ICKF invariant cubature Kalman filter
Sensors 2024,24, 6622 18 of 19
IEKF invariant extended Kalman filter
IMM-EKF interacting multi-model extended Kalman filter
IMM-MSCKF interacting multi-model multi-state constraint Kalman filter
IMM-UKF interacting multi-model unscented Kalman filter
IMU inertial measurement unit
IUKF invariant unscented Kalman filter
MSCKF multi-state constraint Kalman filter
RAM random access memory
RMSE root-mean-square error
RTK real-time kinematics
SCKF square-root cubature Kalman filter
SRUKF square-root unscented Kalman filter
UKF unscented Kalman filter
References
1. Kalman, R.E. A new approach to linear filtering and prediction problems. J. Fluids Eng. 1960,82, 35–45. [CrossRef]
2. Jazwinski, A.H. Stochastic Processes and Filtering Theory; Academic Press: New York, NY, USA, 1970.
3.
Julier, S.J.; Uhlmann, J.K. New extension of the Kalman filter to nonlinear systems. In Proceedings of the 11th International Sym-
posium on Aerospace/Defense Sensing, Simulation and Controls (AeroSense), Orlando, FL, USA, 21–23 April 1997;
pp. 182–193.
[CrossRef]
4. Arasaratnam, I.; Haykin, S. Cubature Kalman Filters. IEEE Trans. Autom. Control 2009,54, 1254–1269. [CrossRef]
5.
Van der Merwe, R.; Wan, E. The square-root unscented Kalman filter for state and parameter-estimation. In Proceedings of the
IEEE International Conference on Acoustics, Speech, and Signal Processing, Salt Lake City, UT, USA, 7–11 May 2001; Volume 6,
pp. 3461–3464. [CrossRef]
6.
Crassidis, J.L.; Markley, F.L.; Cheng, Y. Survey of nonlinear attitude estimation methods. J. Guid. Control Dyn. 2007,30, 12–28.
[CrossRef]
7.
Hertzberg, C.; Wagner, R.; Frese, U.; Schröder, L. Integrating generic sensor fusion algorithms with sound state representations
through encapsulation of manifolds. Inf. Fusion 2013,14, 57–77. [CrossRef]
8.
Bourmaud, G.; Mégret, R.; Giremus, A.; Berthoumieu, Y. Discrete Extended Kalman Filter on Lie groups. In Proceedings of the
21st European Signal Processing Conference (EUSIPCO), Marrakech, Morocco, 9–13 September 2013; pp. 1–5.
9.
Markovi´c, I.; ´
Cesi´c, J.; Petrovi´c, I. On Wrapping the Kalman Filter and Estimating with the SO(2) Group. In Proceedings of the
IEEE 19th International Conference on Information Fusion (FUSION), Heidelberg, Germany, 5–8 July 2016; pp. 2245–2250.
10.
Clemens, J.; Schill, K. Extended Kalman Filter with Manifold State Representation for Navigating a Maneuverable Melting Probe.
In Proceedings of the IEEE 19th International Conference on Information Fusion (FUSION), Heidelberg, Germany, 5–8 July 2016;
pp. 1789–1796.
11.
Nakath, D.; Clemens, J.; Schill, K. Multi-Sensor Fusion and Active Perception for Autonomous Deep Space Navigation.
In Proceedings of the IEEE 21th International Conference on Information Fusion (FUSION), Cambridge, UK, 10–13 July 2018;
pp. 2596–2605.
12.
Brossard, M.; Bonnabel, S.; Condomines, J. Unscented Kalman filtering on Lie groups. In Proceedings of the IEEE/RSJ
International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, 24–28 September 2017;
pp. 2485–2491.
[CrossRef]
13.
Sang, X.; Li, J.; Yuan, Z.; Yu, X.; Zhang, J.; Zhang, J.; Yang, P. Invariant Cubature Kalman Filtering-Based Visual-Inertial Odometry
for Robot Pose Estimation. IEEE Sensors J. 2022,22, 23413–23422. [CrossRef]
14.
Xian, Z.; Lian, J.; Shan, M.; Zhang, L.; He, X.; Hu, X. A square root unscented Kalman filter for multiple view geometry based
stereo cameras/inertial navigation. Int. J. Adv. Robot. Syst. 2016,13, 1–11. [CrossRef]
15.
Tang, X.; Yan, J.; Zhong, D. Square-root sigma-point Kalman filtering for spacecraft relative navigation. Acta Astronaut. 2010,
66, 704–713. [CrossRef]
16.
Zhang, D.; Deng, Z.; Wang, B.; Fu, M. The application of Square-Root Cubature Kalman Filter in the SINS/CNS integrated
navigation system. In Proceedings of the IEEE Chinese Guidance, Navigation and Control Conference (CGNCC), Nanjing, China,
12–14 August 2016; pp. 2331–2335. [CrossRef]
17.
Yu, H.; Lihua, W.; Qiang, Y. Underwater square-root cubature attitude estimator by use of quaternion-vector switching and
geomagnetic field tensor. J. Syst. Eng. Electron. 2020,31, 804–814. [CrossRef]
18.
Menegaz, H.M.; Ishihara, J.Y. Unscented and square-root unscented Kalman filters for quaternionic systems. Int. J. Robust
Nonlinear Control 2018,28, 4500–4527. [CrossRef]
19.
Tang, X.; Liu, Z.; Zhang, J. Square-root quaternion cubature Kalman filtering for spacecraft attitude estimation. Acta Astronaut.
2012,76, 84–94. [CrossRef]
20.
Sipos, B.J. Application of the manifold-constrained unscented Kalman filter. In Proceedings of the IEEE/ION Position, Location
and Navigation Symposium, Monterey, CA, USA, 5–8 May 2008; pp. 30–43. [CrossRef]
Sensors 2024,24, 6622 19 of 19
21.
Sang, X.; Yuan, Z.; Yu, X. A Cubature Kalman Filtering Algorithm for Robot Pose Estimation. In Proceedings of the IEEE
10th International Conference on Information, Communication and Networks (ICICN), Zhangye, China, 19–22 August 2022;
pp. 543–549. [CrossRef]
22.
Brossard, M.; Bonnabel, S.; Barrau, A. Invariant Kalman Filtering for Visual Inertial SLAM. In Proceedings of the 21st International
Conference on Information Fusion (FUSION), Cambridge, UK, 10–13 July 2018; pp. 2021–2028. [CrossRef]
23.
Menegaz, H.M. Unscented Kalman Filtering on Euclidean and Riemannian Manifolds. Ph.D. Thesis, Universidade de Brasília,
Brasília, Brazil, 2016.
24.
Barrau, A.; Bonnabel, S. The Invariant Extended Kalman Filter as a Stable Observer. IEEE Trans. Autom. Control 2017,62, 1797–1812.
[CrossRef]
25.
Kümmerle, R.; Grisetti, G.; Strasdat, H.; Konolige, K.; Burgard, W. g
2
o: A general framework for graph optimization. In Proceed-
ings of the IEEE International Conference on Robotics and Automation (ICRA), Shanghai, China, 9–13 May 2011;
pp. 3607–3613.
[CrossRef]
26.
Grisetti, G.; Kümmerle, R.; Stachniss, C.; Burgard, W. A Tutorial on Graph-Based SLAM. IEEE Intell. Transp. Syst. Mag. 2010,
2, 31–43. [CrossRef]
27.
Koller, T.L.; Frese, U. The Interacting Multiple Model Filter and Smoother on Boxplus-Manifolds. Sensors 2021,21, 4164.
[CrossRef] [PubMed]
28.
Toledo-Moreo, R.; Zamora-Izquierdo, M.A.; Ubeda-Minarro, B.; Gomez-Skarmeta, A.F. High-Integrity IMM-EKF-Based Road
Vehicle Navigation With Low-Cost GPS/SBAS/INS. IEEE Trans. Intell. Transp. Syst. 2007,8, 491–511. [CrossRef]
29.
Kim, B.; Yi, K.; Yoo, H.J.; Chong, H.J.; Ko, B. An IMM/EKF Approach for Enhanced Multitarget State Estimation for Application
to Integrated Risk Management System. IEEE Trans. Veh. Technol. 2015,64, 876–889. [CrossRef]
30.
Cork, L.; Walker, R. Sensor Fault Detection for UAVs using a Nonlinear Dynamic Model and the IMM-UKF Algorithm.
In Proceedings of the Information, Decision and Control, Adelaide, SA, Australia, 12–14 February 2007;
pp. 230–235.
[CrossRef]
31.
Yao, Y.; Xu, X.; Yang, D.; Xu, X. An IMM-UKF Aided SINS/USBL Calibration Solution for Underwater Vehicles. IEEE Trans. Veh.
Technol. 2020,69, 3740–3747. [CrossRef]
32.
Mourikis, A.I.; Roumeliotis, S.I. A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation. In Proceedings of
the IEEE International Conference on Robotics and Automation, Rome, Italy, 10–14 April 2007; pp. 3565–3572. [CrossRef]
33.
Nguyen, T.; Mann, G.K.I.; Vardy, A.; Gosine, R.G. Developing a Cubature Multi-state Constraint Kalman Filter for Visual-Inertial
Navigation System. In Proceedings of the 14th Conference on Computer and Robot Vision (CRV), Edmonton, AB, Canada,
17–19 May 2017; pp. 321–328. [CrossRef]
34.
Sun, W.; Li, Y.; Ding, W.; Zhao, J. A Novel Visual Inertial Odometry Based on Interactive Multiple Model and Multistate
Constrained Kalman Filter. IEEE Trans. Instrum. Meas. 2024,73, 1–10. [CrossRef]
35.
Guo, H.; Liu, H.; Zhou, Y.; Li, J. Quaternion Invariant Cubature Kalman Filtering for Attitude Estimation. In Proceedings of the
3rd World Conference on Mechanical Engineering and Intelligent Manufacturing (WCMEIM), Shanghai, China, 4–6 December
2020; pp. 67–72. [CrossRef]
36.
Van der Merwe, R.; Doucet, A.; de Freitas, N.; Wan, E. The Unscented Particle Filter. In Proceedings of the Advances in Neural
Information Processing Systems, Denver, CO, USA; Leen, T., Dietterich, T., Tresp, V., Eds.; MIT Press: Cambridge, MA, USA, 2000;
Volume 13.
37.
Wei, W.; Gao, S.; Zhong, Y.; Gu, C.; Hu, G. Adaptive Square-Root Unscented Particle Filtering Algorithm for Dynamic Navigation.
Sensors 2018,18, 2337. [CrossRef] [PubMed]
38. Sayed, A.; Kailath, T. A state-space approach to adaptive RLS filtering. IEEE Signal Process. Mag. 1994,11, 18–60. [CrossRef]
39. Golub, G.H.; Van Loan, C.F. Matrix Computations, 4th ed.; Johns Hopkins University Press: Baltimore, MD, USA, 2013.
40.
Folkers, A.; Wellhausen, C.; Rick, M.; Li, X.; Evers, L.; Schwarting, V.; Clemens, J.; Dittmann, P.; Shubbak, M.;
Bustert, T.; et al.
The OPA3L System and Testconcept for Urban Autonomous Driving. In Proceedings of the IEEE 25th IEEE International
Conference on Intelligent Transportation Systems (ITSC), Macau, China, 8–12 October 2022; pp. 1949–1956. [CrossRef]
41.
Clemens, J.; Wellhausen, C.; Koller, T.L.; Frese, U.; Schill, K. Kalman Filter with Moving Reference for Jump-Free, Multi-Sensor
Odometry with Application in Autonomous Driving. In Proceedings of the IEEE 23rd International Conference on Information
Fusion (FUSION), Rustenburg, South Africa, 6–9 July 2020. [CrossRef]
42.
Geiger, A.; Lenz, P.; Urtasun, R. Are we ready for autonomous driving? The KITTI vision benchmark suite. In Proceedings of the
IEEE Conference on Computer Vision and Pattern Recognition (CVPR), Providence, RI, USA, 16–21 June 2012;
pp. 3354–3361.
[CrossRef]
43.
Zhang, Z.; Scaramuzza, D. A Tutorial on Quantitative Trajectory Evaluation for Visual(-Inertial) Odometry. In Proceedings of the
IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Madrid, Spain, 1–5 October 2018;
pp. 7244–7251.
[CrossRef]
44.
Giefer, L.A.; Clemens, J.; Schill, K. Extended Object Tracking on the Affine Group Aff(2). In Proceedings of the 23rd International
Conference on Information Fusion (FUSION), Rustenburg, South Africa, 6–9 July 2020; pp. 1–8. [CrossRef]
Disclaimer/Publisher’s Note: The statements, opinions and data contained in all publications are solely those of the individual
author(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to
people or property resulting from any ideas, methods, instructions or products referred to in the content.