2018 21st International Conference on Information Fusion (FUSION)

Simultaneous Localization and Mapping

Using a Novel Dual Quaternion Particle Filter
Kailai Li, Gerhard Kurz, Lukas Bernreiter and Uwe D. Hanebeck
Intelligent Sensor-Actuator-Systems Laboratory (ISAS)
Institute for Anthropomatics and Robotics
Karlsruhe Institute of Technology (KIT), Germany
[email protected], [email protected], [email protected], [email protected]

Abstract—In this paper, we present a novel approach to

perform simultaneous localization and mapping (SLAM) for
planar motions based on stochastic filtering with dual quaternion
particles using low-cost range and gyro sensor data. Here, SE(2)
states are represented by unit dual quaternions and further get
stochastically modeled by a distribution from directional statistics
such that particles can be generated by random sampling. To
build the full SLAM system, a novel dual quaternion particle
filter based on Rao–Blackwellization is proposed for the tracking
block, which is further integrated with an occupancy grid
mapping block. Unlike previously proposed filtering approaches,
our method can perform tracking in the presence of multi-modal
noise in unknown environments while giving reasonable mapping
results. The approach is further evaluated using a walking robot
with on-board ultrasonic sensors and an IMU sensor navigating
in an unknown environment in both simulated and real-world
scenarios.

Keywords—simultaneous localization and mapping, stochastic

filtering, directional statistics, Rao–Blackwellized particle filtering,
low-cost range and gyro sensors

Figure 1: Dual quaternion particles of SE(2) states propa-

gating through system dynamics of different noise levels and
I. I NTRODUCTION
getting updated based on importance sampling with simulated
Simultaneous Localization and Mapping denotes the tech- likelihoods. The simulation is given by a single run with 1000
nique of constructing or updating a map of unknown sur- particles.
roundings while at the same time tracking an agent’s location.
It plays a key role in a variety of applications such as First, the planar rigid body motions mathematically belong
extended object tracking [1], autonomous driving [2] and robotic to the Special Euclidean Group SE(2), which incorporates both
perception [3], localization [4] as well as navigation [5] in orientation and position and have a highly nonlinear structure
unknown environments. Over recent years, an extensive number due to the underlying manifold. There has been continuing effort
of approaches have been proposed to solve this problem, among in adapting the tracking problem from the nonlinear manifold
which the stochastic methods are most popular. For instance, to an approximated linearized local domain, e.g., with EKF and
the tracking problems are typically solved by some stochastic UKF. However, the linearization operations in these tracking
filtering approaches, such as the well-known Kalman Filter methods have typically the assumption of slow motions as well
(KF), Extended Kalman Filter (EKF), Unscented Kalman Filter as low noise level and distributions used in conventional Kalman
(UKF) [6] or the Particle Filter (PF) [7]. However, the mapping filter family lack probabilistic interpretation of the nonlinear
technique normally depends on the application scenario and the manifold as well as the correlation between orientation and
employed sensors. For example, in autonomous driving tasks position. For handling these issues, dual quaternions can be used
it is always appealing to maintain a sufficiently detailed map to represent SE(2) states due to their advantages compared
where only the interested object features should be dynamically to other representation methods such as better numerical
perceived. In some visual-based perception applications where stability (compared with homogeneous transformation matrix),
dense reconstruction is required, some numerical approaches less ambiguity (compared to Euler angles where a gimbal
should be applied and techniques such as bundle adjustment lock can occur). Moreover, they can represent the orientation
should be used to enforce a globally consistent map. In this and position simultaneously in the same domain. In order to
paper, we focus on a specific application scenario where model uncertain dual quaternions, some specific distributions
simultaneous robotic localization and mapping is performed from the field of directional statistics are proposed, e.g.,
based on some low-cost sensors, such as ultrasonic and gyro the Projected-Gaussian Distribution (PGD) [8] or Partially-
sensors, whose measurement performance may suffer from Conditioned Gaussian (PCG) [9], but they either lack the
high noise level. The following issues can make solving such probabilistic interpretation of the correlation between the real
a SLAM problem challenging. and dual parts or still rely on local linearization. In [10]

978-0-9964527-7-9 ©2018 ISIF 1680

 2018 21st International Conference on Information Fusion (FUSION)

and [11], a new distribution from the exponential family of enables flexible sensor fusion for particle update, which shows
probability density function is proposed to model uncertain promising potential for real-world SLAM scenarios.
dual quaternions and a UKF-like filtering scheme is developed.
However, its assumed measurement model is restricted to be In Sec. II, preliminaries such as pose representation using
identity thus not suitable to be directly used for SLAM. dual quaternions and the applied distribution from directional
statistics are introduced. The core part is introduced in Sec. III
Second, states noise can be multi-modal, especially in real- including particle propagation and update as well as grid-
world scenarios. This can be solved, e.g., by adapting the based mapping. In Sec. IV, evaluation with both simulation
proposed distribution to mixture models as in [12] and [13]. and experiments are presented. The work is concluded and
However, they typically rely on an approximation in the discussed finally in Sec. V.
Bayesian update step and are hard to be integrated into a
probabilistic mapping block. Fig. 1 further gives an illustration II. BACKGROUND
of this issue. The depicted results are from a dual quaternion-
based PF for overall 5 steps, where the magenta quivers and The theory of dual quaternions is essentially the combination
green curve indicate ground truth orientations and trajectory of quaternions and dual theory, which was first introduced by
respectively. During each step, the dual quaternion particles are Clifford [19]. For dual quaternion arithmetics, the readers can
first translated by t = [15, 15]T then rotated by θ = 30◦ respect refer to [20] and [16]. Unit dual quaternions, which are dual
to the last pose. Then they are propagated with uncertainty quaternions of unit length, provide a convenient method for pose
according to the distribution proposed in [10]. The system representation as well as manipulation in a similar manner as
noise is zero-centered and a larger absolute diagonal entry in the unit quaternions do for orientation. In this section, we focus
the parameter matrix Cw indicates a lower noise level. The on introducing unit dual quaternions in the context of planar
simulated measurement noise has two independently distributed motions and further the Bingham-like distribution from the
components, where the orientation part follows a zero-centered exponential family of density function for modeling uncertain
wrapped normal distribution [14] and the position measurement planar dual quaternions.
noise is Gaussian-distributed. Particles after update based on
importance sampling are denoted by the red quivers. It is A. Unit Dual Quaternions and Planar Motion
intuitive to see that the state particles tend to be multi-modal Without loss of generality, poses belonging to SE(2) can
under higher system noise which can make the approximation be assumed to incorporate orientations around z-axis of angle
to a uni-modal distribution risky during the update step. θ and positions t in (x, y)-coordinates. Thus, the orientation
Third, a SLAM system typically relies on multi-sensor quaternion can be represented by the following quaternion
fusion and possesses nonlinearity especially to the measurement
   
θ θ
model. This can be cleverly solved by using a progressive xq = cos + k sin . (1)
2 2
update approach [15] to the Bayesian update step as we
proposed in [16], but it still relies on the sampling and Here, k denotes the unit vector along z-axis, such that xq can
re-approximation scheme and not suitable for multi-modal be ensured to be of unit length via
noise and mapping. Moreover, for our specific application xq
x∗q
scenarios where range and gyro sensors are employed, the          
typical probabilistic mapping approach is to use occupancy θ θ θ θ
= cos + k sin
cos − k sin
grid maps. The Monte Carlo localization approach can be 2 2 2 2
further incorporated for handling the underlying nonlinearity   2   2
θ θ
as proposed in FastSLAM [17] where Rao–Blackwellization = cos + sin = 1,
2 2
is used to reduce the high uncertainty space dimension.
But conventional PF-based SLAM systems [17], [18] lack with
denoting the Hamilton product [21] and x∗q the conjugate
consideration of the nonlinear structure of the underlying of xq . Given a unit quaternion representing the aforementioned
manifold. rotation angle and axis, a vector v ∈ R2 can be rotated to
v0 accordingly through v0 = xq
v
x∗q . Moreover, unit
In this paper, we introduce a novel scheme of simultaneous quaternions representing planar orientations belong to the unit
localization and mapping for planar motions using low-cost circle S1 in Euclidean space.
range and gyro sensors. The SLAM system takes the idea of the
grid-based FastSLAM [7] technique. Unlike the conventional PF The dual quaternions are essentially two paired quaternions.
where a Gaussian distribution is assumed to model the uncertain In order to represent both positions and orientations, dual
position and orientation angle, we use dual quaternions to quaternions are defined as
represent SE(2) states and further use a Bingham-like distri-
x = xq +  xp , (2)
bution to stochastically model them for particle generation and
propagation. An occupancy grid map is hereby employed for the with xq as in (1) representing the orientation part. Here 
mapping part. In order to handle the potential high-dimensional denotes the dual unit with 2 = 0 and the dual part, i.e., the
uncertainty domain during Bayesian inference, we use Rao- translation quaternion, is defined as
Blackwellization for dimension reduction. Here, each dual
1 1
quaternion is assigned with its own occupancy grid map and xp = t
xq = Qq · t , (3)
gets resampled during the update step given the likelihood of its 2 2
observation. Our approach inherently considers the nonlinear which is half the product of position and orientation quaternion.
structure of the SE(2), allows multi-modal state noise and Furthermore, there are overall three kinds of conjugate defined

1681
 2018 21st International Conference on Information Fusion (FUSION)

for dual quaternions as introduced in Appendix A. The Hamilton The distribution is antipodally symmetric since f (x) = f (−x).
product here can be also realized with the normal matrix-vector Some detailed probabilistic interpretation of the applied distri-
multiplication with bution can also be found in [16].
 
xq,1 xq,2
Qq = ∈ SO(2) , (4) III. G RID - BASED SLAM U SING
−xq,2 xq,1
D UAL Q UATERNION PARTICLE F ILTER
which is a two-dimensional rotation matrix. A proof can be In this paper, we specify our application scenario to be
found in Appendix B. With real and dual part defined as in (1) planar SLAM using on-board range and gyro sensors with robot
and (3) respectively, the dual quaternions in (2) are inherently pose represented by unit dual quaternions. From a probabilistic
guaranteed to be of unit length. A vector v ∈ R2 can also be viewpoint, a full SLAM problem is formalized as estimating
transformed to v0 by getting first rotated by xq then translated the posterior of both the poses x and map M of all time steps,
by t via i.e.,
v 0 = x
v
x◦ , (5) p(x0:k , M|z1:k , u1:k ) . (10)
with x◦ = diag(1, −1, 1, 1) · x denoting the full conjugate Here x0:k ∈ S1 × R2 denote overall robot pose chain, z1:k and
of the dual quaternion x. The unit dual quaternions give a u1:k are the measurements and system input up to the current
convenient method for performing planar transformation in time step k respectively.
a similar manner as unit quaternions do for planar rotation.
Corresponding proofs are presented in Appendix C and D We propose a recursive estimation scheme for pose tracking
respectively. The manifold of unit dual quaternions representing and use grid-based occupancy maps for mapping. In order to
planar motions is thus the Cartesian product of the unit circle handle the potential case of multi-modal noise, we use an
and the two-dimensional Euclidean space and it is embedded approach based on particle filtering with dual quaternion states.
in the four-dimensional Euclidean space, namely The particles are generated according to the distribution in (7)
  and get propagated through
x
x = q ∈ S1 × R2 ⊂ R4 , (6)
xp xk+1 = a(xk , uk )
wk , (11)
Furthermore, two antipodal unit dual quaternions, namely x with a(·, ·) denoting the system equation and wk ∈ S × R2 1

and −x denote the same planar rigid body motion. the dynamics noise. Due to the advantages discussed in II-B,
the system noise is assumed to follow also the aforementioned
distribution in (7) . Each particle gives a hypothesis of the robot
pose and has its own estimated map based on its observation
B. Stochastic Modeling of Uncertain Dual Quaternions given the measurement model
zk = h(xk ) ⊗ vk , (12)
In order to generate unit dual quaternion particles for our
SLAM system, a distribution proposed in [10] and [22] is with function h(·) mapping the dual quaternion state to the
employed. It belongs to the exponential family, inherently measurement domain. Here, the measurement noise vk can
incorporates the underlying nonlinear structure of SE(2) be either additive or non-additive (denoted by ⊗, which is
states and gives further consideration of correlation between invertible) and it can be of arbitrary measurement domain. The
orientation and position. The proposed distribution is defined dual quaternion particles are then resampled according to its
as observation likelihood and the global map also gets updated
1 correspondingly. A grid-based mapping approach is used here
f (x) = exp(xT Cx) , (7)
N (C) by which the map is discretized into
with x denoting dual quaternions of SE(2) states in (5). The M = {mi }i=1:m , (13)
parameter matrix C is defined as
  with mi denoting the map grid cell indexed by i.
C1 CT2
C= , (8) In this section, we focus on presenting the central blocks
C2 C3
of our proposed SLAM scheme including the tracking method
with Ci ∈ R2×2 , symmetric C1 , arbitrary C2 and symmetric based on particle filtering using dual quaternions as well as the
negative definite C3 . It also determines the normalization con- mapping technique based on occupancy grid maps.
stant N (C) [10]. The distribution can be further decomposed
as A. Rao-Blackwellization for Grid-based SLAM
f (x) = fxq (xq )fxp |xq (xp |xq ) Since our approach is PF-based, directly sampling in the
1 posterior domain formalized in (10) typically suffers from the
= exp(xTq T1 xq + (xp − T2 xq )T C3 (xp − T2 xq )) ,
N (C) curse of dimensionality. We thus decompose the full SLAM
posterior based on Rao-Blackwellization [7] into, i.e.,
with T1 = C1 −CT2 C−1 −1
3 C2 and T2 = C3 C2 . Thus, it can be
viewed as the product of a Bingham distribution [23] modeling p(x0:k , M|z1:k , u1:k ) =
(14)
the orientation part and a Gaussian distribution modeling the p(x0:k |z1:k , u1:k ) · p(M|x1:k , z1:k , u1:k ) ,
translation part conditioned on each individual orientation, i.e.,
where the first part denotes the trajectory posterior and
xq ∼ B(T1 ), xp |xq ∼ N (T2 xq , −0.5C−1
3 ). (9) the second part denotes the map posterior. However, direct

1682
 2018 21st International Conference on Information Fusion (FUSION)

estimation of the map posterior for each single particle can also occupancy probability of cell mi after updating with the current
be intractable especially in the case of large-scale mapping, as measurement can then be computed as
the posterior is actually the joint probability of all the grid cells 1
mi . A typical solution used in grid-based SLAM is to assume p(mi |z1:k , x1:k ) = 1 − .
each of the cell occupancy to be independently distributed such 1 + exp(lk,i )
that the map posterior can be approximated as the product of A detailed algorithm for updating the occupancy grid maps
all the marginals of all dual quaternion particles {xk,j }j=1:n given the current
m
Y measurement zk is introduced in Alg. 1. The function in line
p(M|z1:k , u1:k ) = p(mi |z1:k , u1:k ) , (15) 4 updates each cell of the old map Mk−1,j with the newly
i=1 calculated occupancy log odds.
with p(mi |z1:k , u1:k ) denoting the probability that cell mi is
occupied. With the assumption in (14) and (15), a tractable Algorithm 1 Occupancy Grid Map Update
grid-based SLAM formalization can be derived as procedure updateMap (zk , {(xk,j , Mk−1,j )}j=1:n )
p(x0:k , M|z1:k , u1:k ) = 1: for j = 1 to n do
m 2: for i = 1 to m do
Y (16) 3: lk,i ← lk−1,i + g(mk−1,j,i , xk,j , zk ) − l0,i ;
p(x0:k |z1:k , u1:k ) · p(mi |x1:k , z1:k ) .
4: Mk,j ← updateCellOccupancy (Mk−1,j , lk,i );
i=1
5: end for
6: end for
7: return {Mk,j }j=1:n

B. Occupancy Grid Mapping end procedure

Mapping with low-cost sensors, e.g., ultrasonic sensors, can

suffer from the common issue of high measurement noise level C. Particle Generation
as well as very sparse measurement resolution. One of the most
Dual quaternion particles are generated by random sampling
popular mapping approaches for range sensor perception is to
on the manifold S1 × R2 in a manner according to the
use an occupancy grid map, where the map gets discretized
conditional probability introduced in (9). First, we randomly
into grid cells assigned to the occupancy probabilities, namely
sample the rotation quaternions from the Bingham part [24].
the probability of a cell being occupied or obstacle-free. For
Second, conditioned on each orientation quaternion particle we
better numerical stability, the posterior occupancy probability
sample from the Gaussian for the translation quaternion part. In
of the ith cell in (15) is typically represented in its log odds
the end we give the dual quaternion particles by concatenating
form, namely
  the two sets. The detailed sampling scheme is introduced in
p(mi |x1:k , z1:k ) Alg. 2.
lk,i = log .
1 − p(mi |x1:k , z1:k )
Since the occupancy probability of each cell is typically Algorithm 2 Particle Generation
initialized to be 0.5, we have the log odds procedure sampleRandom (C, n)
−1
1: T1 ← C1 − CT 2 C3 C2 ;
l0,i = log(0.5/(1 − 0.5)) = 0 . −1
2: T2 ← −C3 C2 ;
At each recursive estimation step, the sensor takes a new 3: {xq,j }j=1··· ,n ←sampleRandomBingham (T1 );
−1
measurement and the occupancy map should also be updated, 4: {xp,j }j=1··· ,n ←sampleRandomGaussian (0,−0.5C3 );
for which the inverse sensor model is typically applied, i.e., 5: for j = 1 to n do
  6: xp,j ← T2 xq,j + xp,j ;
p(mi |xk , zk ) 7: xj ← [xTq,j , xTp,j ]T ;
g(mi , xk , zk ) = log . (17)
1 − p(mi |xk , zk ) 8: end for
9: return {xj }j=1:n
Compared to the forward sensor model p(zk |xk , mi ), the
inverse sensor model gives occupancy probability hypothesis end procedure
based on the measurement. Regarding the ultrasonic sensors,
for instance, the inverse sensor model represents the functional
principle of a measurement by means of creating a cone with
a filling that has the probability of being free and endpoints D. Particle Prediction
with a probability of being occupied [7, Table 9.2]. The log
odds occupancy of cell i can thus be updated using the binary For each recursive step, the dual quaternion particles
Bayes filter [7, Table 4.2] according to estimated from last step {xek−1,j }j=1:n are propagated through
lk,i = lk−1,i + g(mi , xk , zk ) − l0,i , (18) the system equation a(·, ·) and further propagated with uncertain
noise terms that are also randomly sampled from the system
where lk,i and lk−1,i denote the log odds occupancy of the noise distribution characterized by Cw . The detailed approach
current and the last time step respectively. The posterior is introduced in Alg. 3.

1683
 2018 21st International Conference on Information Fusion (FUSION)

Algorithm 3 Particle Prediction Algorithm 4 Particle Update

procedure predictParticles (uk , {xek−1,j }j=1:n ) procedure updateParticles ({(xpk,j , Mek,j )}j=1:n , zk )
1: {xw,j }j=1:n ←sampleRandom (Cw ); 1: {(xek,j , wk,j )}j=1:n ← ∅;
2: for j = 1 to n do 2: for j = 1 to n do
3: xpk,j ← a(xek−1,j , uk )
xw,j ; 3: wk,j ← fv ((hMek,j (xpk,j ))−1 ⊗ zk );
4: end for 4: end for
p p
5: return {xk,j }j=1:n 5: {xek,j }j=1:n ←selectiveResample ({xk,j }j=1:n , wk,j );
end procedure 6: return {(xek,j , wk,j )}j=1:n
end procedure

E. Particle Update
grid map and finally update the particles based on selective
In general, the update step follows the same idea of the Rao-
resampling according to the likelihood field. In order to
Blackwellized particle filter based on the occupancy grid map.
visualize a globally consistent map after each recursive step,
Here, dual quaternion particles are resampled according to their
each grid cell is given a log odds occupancy by the particle
likelihoods. More specifically, given the measurement model
with the largest weight.
(for both additive and non-additive noise) in (12), the likelihood
for a dual quaternion particle given the current measurement
Algorithm 5 SLAM with Dual Quaternion Particles
zk can be computed using Bayes’ theorem as follows
procedure DQPFSLAM ({(xek−1,j , Mek−1,j )}j=1:n , uk , zk )
f (zk |xk,j ) p
Z 1: {xk,j }j=1:n ← predictParticles (uk , {xek−1,j }j=1:n );
p
= f (zk , vk |xk,j )dvk,j 2: {Mek,j }j=1:n ← updateMap (zk , {(xk,j , Mek−1,j )}j=1:n );
ZZ 3: {(xek,j , wk,j )}j=1:n ← updateParticles (
= f (zk |vk,j , xk,j )f (vk,j )dvk,j (19) 4: {(xpk,j , Mek,j )}j=1:n , zk );
ZZ 5: return {(xek,j , Mek,j )}j=1:n

= δ(vk,j − (hMj (xk,j ))−1 ⊗ zk )f (vk,j )dvk,j end procedure

Z
= fv ((hMj (xk,j ))−1 ⊗ zk ) ,
IV. E VALUATION
with Z denoting the measurement domain and fv the mea-
surement noise distribution. As mentioned in (12), the function In this section, the proposed SLAM approach is evaluated
hMj (·) : S1 × R2 7→ Z gives a measurement hypothesis for with a miniature walking robot (shown in Fig. 2) performing
each dual quaternion particle xj . planar rigid body motions in a static and unknown environment.
The robot aims to estimate its own poses and simultaneously
For our application scenario where range sensors are
map the unknown surroundings using low-cost on-board
employed, this function is implemented based on the likelihood
sensors, which are four ultrasonic sensors and an IMU. The
field model [7, Chapter 6.4], in which the range measurement
evaluation is performed in both simulated and real-world
hypothesis is given by calculating its Euclidean distance to
experiment scenarios.
the nearest occupied cell of the estimated grid map assigned
to the particle xk,j . If no obstacle cell is found by xj , its For each step of movement, the robot is given a dual
likelihood is directly assigned to a value indicating the uniform quaternion input uk ∈ S1 × R2 following a dynamics model
distribution, i.e., f (zk |xk,j ) = 1/dmax , with dmax denoting the
maximum range. The dual quaternion particles can then be xk+1 = xk
uk
wk , (20)
resampled based on selective sampling given their likelihoods, 1 2
with the noise term wk ∈ S × R following the distribution
in which we first calculate the normalized Effective Sample introduced in (7) characterized by a parameter matrix Cw =
Size (ESS) [25] defined as diag(−1, −100, −100, −100) indicating zero-centered noise.
1 Xn The measurement domain Z = S1 × R4 , which comprises the
=n· wj , orientation measurement qk ∈ S1 from the IMU and four range
ESS j=1 measurements dk = [d1k , d2k , d3k , d4k ]T ∈ R4 from the ultrasonic
sensors. The IMU measurement is assumed to have a non-
with n denoting particle size. And the importance sampling is additive and zero-centered noise that is Bingham-distributed
only performed when the ESS is less than a given threshold, characterized by Cort = diag(0, −50). The noise of each range
e.g., 0.5. The scheme for updating particles can be found in measurement is assumed to be additive and Gaussian-distributed
Alg. 4. as vd ∼ N (0, 2). We further assume that all of the measurement
noises are independently distributed, thus the likelihood of
F. SLAM Using Dual Quaternion Particles particle xk,j during the update step can be calculated by simply
taking the product of each, i.e.,
The full scheme of the proposed SLAM approach comprises
the aforementioned mapping and particle filtering components. f (zk |xk,j )
For each recursive step, as shown in Alg. 5, we first propagate 4
Y (21)
the dual quaternion particles according to the system equation = fvq (x−1
k,q
qk ) fvd (drk − srMk,j (xk,j )) ,
and the assumed noise distribution, then update the occupancy r=1

1684
 2018 21st International Conference on Information Fusion (FUSION)

Figure 4: Robot navigating in the experimental setup. Using

our proposed SLAM approach, the robot aims to estimate itself
for both orientation and position in an unknown environment
Figure 2: Sketches of a walking robot equipped with four with surrounding cardboard obstacles.
ultrasonic sensors and an IMU performing planar rigid body
motions in static obstacles. A. Simulation Scenario
During the simulation run, the robot is ordered to move step-
wise in an environment of similar shape as in Fig. 2 without
predefined map information. The map has an approximated
area of 7800 cm2 . Fig. 3 shows the tracking result as well as
the result of grid-based mapping. The estimated occupancy
map is visualized by directly taking the log odds of each cell
as a grayscale pixel, with a larger intensity indicating a higher
probability of being occupied by obstacles, e.g., the black
pixels. Despite of the high-level noise and sparse perception
of the ultrasonic sensors, our approach is still able to give a
reasonable reconstruction of the map and a good estimation of
the trajectory. We get the result from a single simulation run
with 200 dual quaternion particles.

Figure 3: Estimated map and trajectory using our proposed B. Experiment Scenario
SLAM approach. The red and blue curve denote the ground
truth and estimated trajectory respectively and the mapping For the first time, filtering approaches based on the proposed
ground truth is shown with red dot curve. distribution in [10] are evaluated in a real-world scenario. Here
we set up an environment with static obstacles shown in Fig. 4,
which is unknown for the robot. During the controlled and step-
wise movements in the map, the robot aims to localize itself by
where the function srMk,j (·) gives the expected distance using the on-board four ultrasonic sensors and IMU. For this
measurement based on the occupancy grid map assigned to the specific test case, both of the system and measurement noise
particle. In the case of unknown areas, the likelihood for single level can be very high, partially due to the fact that, e.g., the
range measurement is assumed to be uniformly distributed, i.e., robot is likely to slip, the low-cost ultrasonic sensors can give
fvd = 1/dmax , with maximum detection range dmax = 50 cm, unreliable measurements, and the potential multiple reflection
as discussed in III-E. among the walls as well as the drift issue of IMU cannot be
ignored. The ground truth of robot’s pose is measured by using
a ceiling camera from above. Due to the aforementioned issues,
The evaluations are performed as follows. First, simulations only tracking accuracy is evaluated.
are carried out with an emphasis of showing the possibility
of employing our proposed SLAM approach for simultaneous We compare the tracking accuracy of our proposed SLAM
tracking and mapping. Second, a real-world scenario is designed system based on dual quaternion particles with the SLAM
in which the previously mentioned robot navigates in an systems based on the ordinary particle filter as well as the
unknown environment and the tracking accuracy is evaluated progressive dual quaternion filter [16]. In order to have the
for both orientation and position based on multiple Monte same noise level of system dynamics between the ordinary
Carlo runs. In both simulation and experiment, the ultrasonic PF and the dual quaternion-based filters, we approximate the
sensor has a beam angle αus = 15◦ and an effective detection Gaussian distribution of the ordinary PF from dual quaternion
range of 2 cm ∼ 65 cm. And the robot is able to move for samples randomly drawn from the distribution in (20). And we
about 1.6 cm in each direction and rotate for 16◦ per step. apply the same Bingham-distributed orientation noise for all
The implementation of our proposed SLAM approach is based the filters. The evaluation is performed based on 100 Monte
on [26]. Carlo runs of one single recorded dataset and the result is

1685
 2018 21st International Conference on Information Fusion (FUSION)

depicted in Fig. 5. Because all of the filtering approaches use B. Proof 1

roughly the same orientation distribution for system noise, the
orientation RMS errors of the three approaches are on the same Since xq ∈ S1 , it can be proven that
level. However, our proposed dual quaternion particle filter
  
xq,1 xq,2 xq,1 −xq,2
outperforms the others for position estimation. This is mainly Qq QTq = = I2×2 .
−xq,2 xq,1 xq,2 xq,1
because our proposed approach gives better consideration of
the nonlinearity underlying the structure of SE(2) compared Similarly we also have QTq Qq = I2×2 . Moreover, we have
to the ordinary PF and is able to handle the multi-modal noise
compared to the progressive dual quaternion filter where the det(Qq ) = ||xq || = 1 ,
noise is assumed to be uni-modal. thus it belongs to the two-dimensional rotation matrix group,
i.e., Qq ∈ SO(2).

V. C ONCLUSION AND O UTLOOK

C. Proof 2
In this paper, we presented a novel SLAM approach using With the real and dual part of a dual quaternion defined as
range and gyro sensors based on a dual quaternion particle (1) and (3), the norm of it can be computed as follows
filter. This approach incorporates a better understanding of the
nonlinear structures of SE(2) by using unit dual quaternions x
x∗ = (xq +  xp )
(x∗q +  x∗p )
for pose representation and the proposed distribution introduced = xq
x∗q +  (xq
x∗p + xp
x∗q )
in II-B to propagate system uncertainty. Moreover, the particle- 
based filtering approach is able to handle multi-modal noise = xq
x∗q + (xq
x∗q
t∗ + t
xq
x∗q )
2
distribution, which theoretically outperforms the approaches 
based on uni-modal noise distribution, e.g., [22] and [16]. The = xq
x∗q + (t∗ + t)
2
proposed filtering approach using dual quaternion particles is = xq
x∗q = 1 .
further integrated with grid-based mapping methods into a full
SLAM scheme based on Rao-Blackwellization. The proposed Here 2 = 0 and the conjugate of vector t in quaternion
SLAM system is able to give reasonable mapping results and form is simply its opposite. Moreover, x∗ denotes the classical
outperforms the typical uni-modal-assumed filters as well as conjugate of the dual quaternion x where each of its composing
ordinary PF for pose estimation in real-world scenarios. quaternion is conjugated.

There are still a few corresponding extensions that can be

made in the future. For example, the proposed filter is only for D. Proof 3
performing tracking on a plane. It would be appealing to extend Given a unit dual quaternion defined in (6) for planar
this filtering approach to general spatial movements, namely motions, a vector v ∈ R2 can get transformed according to
the group SE(3). Second, the aforementioned SLAM system
is designed for range sensor-based perception scenarios where v0 = x
v
x◦
the occupancy grid map is employed. It should be possible to  
= (xq + t
xq )
(1 +  v)
(xq + t ⊗ xq )◦
further integrate this dual quaternion particle-based approach 2 2
 
into some more generic SLAM scenarios, e.g., some visual- = (xq +  xq
v + t
xq )
(x∗q −
x∗q ⊗ t∗ )
based perception or SLAM systems. Third, efficiency issue is 2 2
not discussed in this paper. However, it should be investigated = xq
x∗q +  xq
v ⊗ x∗q +
later for its employment in practical applications. 
(t
xq ⊗ x∗q − xq
x∗q
t∗ )
2
= 1 +  (xq
v
x∗q + t) ,
A PPENDIX where v first gets rotated by rotation quaternion xq then
translated by t.
A. Dual Quaternion Conjugates
For an arbitrary dual quaternion defined as in (2), whose ACKNOWLEDGMENT
vector form x ∈ R8 , it has overall three kinds of conjugates
as follows. First, the dual conjugate takes the conjugate of the This work is supported by the German Research Foundation
dual unit  and can be written as (DFG) under grant HA 3789/16-1 “Recursive Estimation of
Rigid Body Motions”.
x• = diag(1, 1, 1, 1, −1, −1, −1, −1) · x .
Second, the classical conjugate only conjugates each individual R EFERENCES
quaternion, namely [1] K. Granström, “Extended target tracking using PHD filters,” Ph.D.
x∗ = diag(1, −1, −1, −1, 1, −1, −1, −1) · x . dissertation, Linköping University Electronic Press, 2012.
[2] J. Zhu, “Image gradient-based joint direct visual odometry for stereo
Third, the full conjugate combines the former two conjugations camera,” in Int. Jt. Conf. Artif. Intell, 2017, pp. 4558–4564.
as [3] J. Engel, J. Stückler, and D. Cremers, “Large-scale direct SLAM
with stereo cameras,” in 2015 IEEE/RSJ International Conference on
x◦ = (x• )∗ = diag(1, −1, −1, −1, −1, 1, 1, 1) · x . Intelligent Robots and Systems (IROS). IEEE, 2015, pp. 1935–1942.

1686
 2018 21st International Conference on Information Fusion (FUSION)

Crawler Trajectory Position Error in X-direction Orientation Cumulative RMS Error

18 2
DQ Progresssive Fitler DQ Progresssive Fitler
80 16 Particle Filter 1.8 Particle Filter
DQ Particle Filter DQ Particle Filter
1.6
14
1.4
70 12

Orientation/radian
Map
1.2

X-error/cm
Ground Truth 10
DQ Progresssive Fitler
1
60 Particle Filter
DQ Particle Filter
8
0.8
6
0.6
50 4
0.4

2 0.2
40
0 0
0 20 40 60 80 100 0 20 40 60 80 100
Time steps Time steps
30
Y

Position Error in Y-direction Position Cumulative RMS Error

20 18 12
DQ Progresssive Fitler DQ Progresssive Fitler
16 Particle Filter Particle Filter
DQ Particle Filter 10 DQ Particle Filter

10 14

12 8

Position Error/cm
0 Y-error/cm 10
6
8

-10 6 4

4
2
-20 2

0 0
-30 -20 -10 0 10 20 0 20 40 60 80 100 0 20 40 60 80 100
X Time steps Time steps

Figure 5: Evaluation result for robot tracking in the real-world experiment set up in Fig.4. The picture on the left shows the
digitalized cardboard map. In the map, we plot the estimated trajectory using our proposed filtering approach (blue), the ordinary
particle filter (red) and the progressive filtering approach [16] (green). We plot the cumulative RMS error to show the tracking
accuracy and the per-step error on x and y to show how the error evolves. The results are based on 100 Monte Carlo runs based
on the same recorded data using 200 dual quaternion particles.

[4] F. Dellaert, D. Fox, W. Burgard, and S. Thrun, “Monte carlo localization Fusion (FUSION), July 2014.
for mobile robots,” in Proceedings 1999 IEEE International Conference [16] K. Li, G. Kurz, L. Bernreiter, and U. D. Hanebeck, “Nonlinear
on Robotics and Automation, vol. 2. IEEE, 1999, pp. 1322–1328. progressive filtering for SE(2) Estimation,” in 21st International
[5] H. Temeltas and D. Kayak, “SLAM for robot navigation,” IEEE Conference on Information Fusion (FUSION). IEEE, 2018.
Aerospace and Electronic Systems Magazine, vol. 23, no. 12, pp. 16–19, [17] M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit et al., “FastSLAM: A
2008. factored solution to the simultaneous localization and mapping problem,”
[6] S. J. Julier and J. K. Uhlmann, “Unscented filtering and nonlinear Aaai/iaai, vol. 593598, 2002.
estimation,” Proceedings of the IEEE, vol. 92, no. 3, pp. 401–422, 2004.
[18] M. Montemerlo and S. Thrun, “FastSLAM 2.0,” FastSLAM: A scalable
[7] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics. MIT press, method for the simultaneous localization and mapping problem in
2005. robotics, pp. 63–90, 2007.
[8] J. Goddard, “Pose and motion estimation from vision using dual
quaternion-based extended Kalman filtering. University of Tennessee, [19] Clifford, “Preliminary sketch of biquaternions,” Proceedings of the
Knoxville,” Ph.D. dissertation, 1997. London Mathematical Society, vol. s1-4, no. 1, pp. 381–395, 1871.
[9] J. E. Darling and K. J. DeMars, “Uncertainty Propagation of correlated [20] B. Kenwright, “A beginners guide to dual-quaternions: what they are,
quaternion and Euclidean states using partially-conditioned Gaussian how they work, and how to use them for 3D,” in The 20th International
mixtures,” in 19th International Conference on Information Fusion Conference in Central Europe on Computer Graphics, Visualization and
(FUSION), July 2016, pp. 1805–1812. Computer Vision, WSCG 2012 Conference Proceedings, 2012, pp. 1–13.
[10] I. Gilitschenski, G. Kurz, S. J. Julier, and U. D. Hanebeck, “A new [21] W. R. Hamilton, “Ii. on quaternions; or on a new system of imaginaries
probability distribution for simultaneous representation of uncertain po- in algebra,” Philosophical Magazine Series 3, vol. 25, no. 163, pp.
sition and orientation,” in 17th International Conference on Information 10–13, 1844.
Fusion (FUSION). IEEE, 2014.
[11] ——, “Unscented orientation estimation based on the Bingham distri- [22] I. Gilitschenski, G. Kurz, and U. D. Hanebeck, “A stochastic filter for
bution,” IEEE Transactions on Automatic Control, vol. 61, no. 1, pp. planar rigid-body motions,” in 2015 IEEE International Conference
172–177, 2016. on Multisensor Fusion and Integration for Intelligent Systems (MFI).
IEEE, 2015, pp. 13–18.
[12] M. Lang and W. Feiten, “MPG-fast forward reasoning on 6 dof pose
uncertainty,” in ROBOTIK 2012; 7th German Conference on Robotics. [23] C. Bingham, “An antipodally symmetric distribution on the sphere,” The
VDE, 2012, pp. 1–6. Annals of Statistics, pp. 1201–1225, 1974.
[13] W. Feiten, M. Lang, and S. Hirche, “Rigid motion estimation using [24] G. Kurz, I. Gilitschenski, F. Pfaff, and L. Drude, “libDirectional,” 2015.
mixtures of projected gaussians,” in 16th International Conference on [Online]. Available: https://github.com/libDirectional
Information Fusion (FUSION). IEEE, 2013, pp. 1465–1472.
[14] K. V. Mardia and P. E. Jupp, Directional statistics. John Wiley & [25] B. Ristic, S. Arulampalam, and N. Gordon, Beyond the Kalman filter:
Sons, 2009, vol. 494. Particle filters for tracking applications. Artech house, 2003.
[15] J. Steinbring and U. D. Hanebeck, “Progressive Gaussian filtering using [26] J. Steinbring, “Nonlinear Estimation Toolbox,” 2015. [Online].
explicit likelihoods,” in 17th International Conference on Information Available: https://bitbucket.org/nonlinearestimation/toolbox

1687