# Quantum dynamics of impurities in a Bose-Einstein Condensate

Javed Akram  Department of Physics, COMSATS University Islamabad, 44000,
Islamabad Pakistan<EMAIL_ADDRESS>
###### Abstract

We study the quantum dynamics of the two impurities in a trapped quasi-one-
dimensional Bose-Einstein Condensate (BEC). We explore the effect of impurity-
BEC and impurity-impurity interaction strengths on the dynamics of impurities
inside the Bose-Einstein condensate. By studying the auto-correlation function
of impurities and the BEC, we analyze and quantify the trapping of impurities
inside the BEC. We find out that for the small value of inter-species coupling
strength the BEC starts to oscillate inside the trap. For mild coupling
strengths, attractive and repulsive impurities are captured after a few cycles
of oscillation inside the BEC. In the strong interaction strength regime, the
to-and-fro motion of impurities suppressed quite fast. Our conclusion
indicates that quench dynamics can be a tool for studying impurity BEC
interactions or impurity-impurity interactions. Our analysis shows that the
generation of Phonon, shock-waves, soliton-train, and self-trapping is
strongly dependent on the impurity-BEC coupling coefficient.

###### pacs:

(020.1335) Atom optics; (020.1475) Bose-Einstein condensates; (020.2070)
Effects of collisions.

##  I Introduction

Impurities dynamics in a Bose–Einstein condensate (BEC) offer an appealing
platform to investigate profoundly imbalanced multicomponent systems Catani et
al. (2012); Hohmann et al. (2015); Hu et al. (2016); Jørgensen et al. (2016).
Ultracold atoms provide one to manipulate and examine systems with population
imbalance Morales-Molina and Gong (2008); Spethmann et al. (2012); Galteland
and Sudbø (2016), and analogous impurity-impurity induced interactions Naidon
(2018); Lingua et al. (2018); Yan et al. (2020); Mistakidis et al. (2020). The
impurity interaction with the ultracold atoms offers the possibility for novel
applications. For example, in the low-temperature limit, the interaction of a
magnetic single impurity with the neighboring electron gas leads to appealing
phenomena called Kondo effect Hewson (1993). Transport of spin-impurity
through a strongly interacting one-dimensional Bose gas reveal fundamental
properties of spin transport Zvonarev et al. (2007); Palzer et al. (2009) and
spin-charge separation Kleine et al. (2008). A spin-impurity can be
represented as a qubit that helps to probe the system, where this setup helps
to implement impurity as a single atom transistor Micheli et al. (2004). A
single impurity ion can be used to probe the system of an ultracold atom cloud
dynamics Schmid et al. (2010). The transfer of quantum information between the
atoms can be achieved by two impurity atoms through phonon exchange in a Bose-
Einstein condensate Klein and Fleischhauer (2005). Bright shock waves and
gray/dark bisolitons can be generated by quenching the attractive and
repulsive interspecies coupling strength of impurity and the BEC respectively
Akram and Pelster (2016a); Akram and Pelster (2016b, c); Akram (2018); Edmonds
et al. (2019).

Figure 1: (Color online) Sketch of the system. The BEC and two impurities
initially trapped in separate harmonic traps. At the time $t=0$, impurities
traps switched off and let them evolve in the BEC trap. Here, the
$Left$-impurity and $Right$-impurity have repulsive ($G_{L}>0$) and attractive
($G_{R}<0$) interaction strengths with the BEC, respectively.  Figure 2:
(Color online) We plot mean-position of impurities for (a) $G_{L}=-G_{R}=1$,
(b) $G_{L}=-G_{R}=12$, (c) $G_{L}=-G_{R}=30$ and (d) $G_{L}=-G_{R}=50$. All
other dimensionless parameters are $G_{B}=1$, $G_{II}=0$, and
$x_{0R}=-x_{0L}=10$.

In this research, we study the dynamics of the two separated impurity atoms
with the BEC, we also discuss the emerging of soliton trains, shock-waves, and
self-trapping during the time of flight. By determining the auto-correlation
function of impurities, we discuss different regimes, when impurities undergo
a state-dependent scattering during the collision with the condensate atoms.
This change of states happens due to the generation of phonons. This
conditional shift in the density profile of impurities and the BEC is studied,
which strongly depends on the interaction strength of impurities and the BEC.
In addition to this, we also investigate the transmission and reflection of
impurities through the BEC for varying interaction strengths.

In this respect, we organize this paper as follows. We discuss the theoretical
model in Sec. II. A detailed discussion of dynamics of impurities inside the
trapped BEC is given in Sec. III, where this section has two subsections, in
one we study the behavior of impurities without impurity-impurity interaction,
and in the second we investigate the dynamics of impurities in the presence of
the impurity-impurity interaction strength. In Sec. IV, we present the time-
of-flight dynamics of impurities and the BEC, where impurities and the BEC
traps are switched off and we investigate the possibility of the self-trapping
and the generation of soliton-train. The summary and conclusion are given in
Sec. V and the last Sec. VI is assigned for acknowledgment.

##  II Theoretical Model

The BEC and impurities dynamics in a quasi-1D regime is governed by three
coupled Gross-Pitaevskii equations (GPEs) in dimensionless form, Ao and Chui
(1998); Timmermans (1998); Akram and Pelster (2016c); Akram (2018)

$\displaystyle i\frac{\partial}{\partial t}\psi_{B}\left(x,t\right)$
$\displaystyle=\left\\{-\frac{1}{2}\frac{\partial^{2}}{\partial
x^{2}}+\frac{x^{2}}{2}+G_{B}|\psi_{B}\left(x,t\right)|^{2}+G_{R}|\psi_{RI}(x,t)|^{2}\right.$
$\displaystyle\left.+G_{L}|\psi_{LI}(x,t)|^{2}\right\\}\psi_{B}\left(x,t\right),$
$\displaystyle i\frac{\partial}{\partial t}\psi_{k}\left(x,t\right)$
$\displaystyle=\left\\{-\frac{\alpha^{2}}{2}\frac{\partial^{2}}{\partial
x^{2}}+\frac{V_{I}}{2\alpha^{2}}+G_{I}|\psi_{B}(x,t)|^{2}+G_{II}|\psi_{\bar{k}}(x,t)|^{2}\right\\}$
$\displaystyle\times\psi_{k}\left(x,t\right).$

with the normalization condition $\int|\psi_{k}(x,t)|^{2}dx=1$, here
$k\in\\{B,RI,LI\\}$. Here $\psi_{B}(x,t)$ and $\psi_{k}(x,t)$ represents the
BEC wave function and impurity wave function respectively, where the subscript
“$k$”denotes for the $Left$ and $Right$ side impurities. Additionally, the
subscript $\bar{k}$ specify the opposite side of the impurity, e.g., if
subscript $k$ describes the $Right$-impurity then the subscript $\bar{k}$
represents the $Left$-impurity and vice-versa. We define
$\alpha=\sqrt{m_{B}/m_{I}}$, here $m_{B}$ describes the mass of the BEC and
$m_{I}$ denotes the mass of impurities, in this paper we let the same species
for the BEC and for impurities, therefore, $\alpha=1$. The dimensionless time
defines as $t$ and the $x$ stands for a dimensionless 1D-space coordinate. For
impurities, we let the harmonic confinement as
$V_{I}=\frac{(x-x_{0I})^{2}}{2}$, where for $Right$-impurity $x_{0R}=10$ and
for $Left$-impurity $x_{0L}=-10$. Here,
$G_{B}=2N_{B}\omega_{r}a_{B}/\left(\omega_{x}l_{x}\right)$ represents the
dimensionless atom-atom coupling strength for the BEC in a quasi one-
dimension, this interaction strength can be tuned by using the the s-wave
scattering length,
$G_{I}=2a_{IB}\omega_{r}f\left(\omega_{Ir}/\omega_{r}\right)/\omega_{x}l_{x}$
stands for the dimensionless impurity-BEC coupling strength, here,
$f\left(\omega_{Ir}/\omega_{r}\right)=\left[1+\left(m_{B}/m_{I}\right)\right]/\left[1+\left(m_{B}\omega_{r}\right)/\left(m_{I}\omega_{Ir}\right)\right]$
represents a geometric function for more detail please see this Ref. Akram and
Pelster (2016c), which depends on the ratio of the trap frequencies and
$G_{II}$ illustrates the impurity-impurity interaction strength. The inter-
species coupling strength can be tune in experiments by using a number of
atoms or by using Feshbach resonances, or by using trap-frequencies ratios
Stan et al. (2004); Pilch et al. (2009); Gröbner et al. (2017). In this paper,
we consider single impurities on both ends of the BEC as shown in Fig. 1,
therefore, the on side impurity-impurity atoms interaction has not been
considered. To make coupled GPEs (II,II) dimensionless, we define time in
$\omega_{x}^{-1}$, length of the harmonic oscillator depict in
$\sqrt{\hbar/{m_{B}\omega_{x}}}$ and energy characterize in $\hbar\omega_{x}$,
here $\omega_{x}$ describes the oscillator length along the $x$-axis.

To do numerical simulations, we do discretization of the dimensionless GPEs
(II,II), with space step $\Delta{x}=0.0177$ and the time step
$\Delta{t}=0.0001$. To perform numerical simulation, we use the time-splitting
spectral method Vudragovic et al. (2012); Kumar et al. (2015); Sataric et al.
(2016); Loncar et al. (2016); Young-S. et al. (2016). To find the ground state
wave function of the BEC and impurities, we minimize the energy by simulating
in imaginary time $\tau=\iota t$, where we let initially the Gaussian wave
functions as a test wave function. To achieve the equilibrium, we trap the BEC
in a harmonic confinement $V(x)=\frac{x^{2}}{2}$ and impurities in the shifted
confinements $V_{I}=\frac{(x-x_{0I})^{2}}{2}$. For the dynamical evolution of
the wave function of the BEC and impurities, it is eminent to mention that the
ground state wave function serves as an initial condition for the rest of the
numerical simulations. To find out equilibrium wave functions of BEC and
impurities, we run the numerical program for separated traps as shown in
cartoon Fig. 1. After achieving the equilibrium, we switched off the
impurities trap and see the interaction of impurities with the BEC in the one-
dimensional confinement $V(x)=x^{2}/2$. We also investigate another scenario
in which all traps are switched off to determine the time-of-flight dynamics
during this free fall.

Figure 3: (Color online) We plot auto-correlation of impurities and the BEC
for (a) $G_{L}=-G_{R}=1$, (b) $G_{L}=-G_{R}=12$, (c) $G_{L}=-G_{R}=30$ and (d)
$G_{L}=-G_{R}=50$. All other dimensionless parameters are $G_{B}=1$,
$G_{II}=0$, and $x_{0R}=-x_{0L}=10$.

##  III Dynamics of Impurities inside the trapped BEC

###  III.1 Without impurity-impurity interaction

Initially, we trap impurities far away from the BEC, the so-called
$Left$-impurity ($Right$-impurity) is trapped at $x_{0L}=10~{}(x_{0R}=-10)$.
In our calculation, for the rest of this research article, the inter-species
interaction strength between the $Left$-impurity and the BEC is repulsive
$G_{L}>0$ and the inter-species interaction strength between the
$Right$-impurity and the BEC is attractive i.e., $G_{R}<0$. In the present
scenario, we let the weak BEC coupling strength $G_{B}=1$ and interaction
between the $Right$-impurity and $Left$-impurity is zero i.e., $G_{II}=0$.
After achieving the equilibrium, we switch off impurities traps and let
impurities evolve inside the trapped BEC. For weak BEC-impurity interaction
strengths, impurities move to-and-fro motion inside the BEC as shown in Fig.
2(a). Where, the mean-position of impurities is calculated as $<x_{k}>=\int
x|\psi_{k}\left(x,t\right)|^{2}dx$, here, the subscript $k$ represents the $B$
for BEC, $RI$ for $Right$-impurity, and $LI$ for $Left$-impurity. To study the
dynamics of impurities inside the BEC, we plot in Fig. 3(a-d) the auto-
correlation function of the BEC and impurities, which we calculate as

$C_{k}=\int\psi_{k}(x,t=0)\psi_{k}(x,t)dx.$ (3)

We note that the auto-correlation function for impurities and the BEC is not
affected by low interspecies interaction strengths $G_{L}=-G_{R}=1$
(Fig.2(a)), so we can say that impurities preserve their shapes. For the lower
values interspecies interaction strengths, the density profile of the BEC is
not affected by the movement of impurities, as shown in Fig. 3(a). We also
notice that for mild interspecies interaction strength ($G_{L}=-G_{R}=12$) of
impurities and the BEC mean position is not too much affected as presented in
Fig. 2(b), and the BEC dynamics shows the signature of oscillations,
therefore, the auto-correlation of the BEC is minimized from its maximum value
$one$ during time intervals $t=[(6,12),(13,17),(20,25),(26,29)]$ as shown in
Fig. 3(b). Additionally, we also note that the auto-correlation function for
the $Left$-impurity and $Right$-impurity is also start to decrease from $one$
after dimensionless time $t\approx 30$ as depicted in Fig. 3(b). But for
strong BEC-impurity interspecies interaction strength $G_{L}=-G_{R}=30$ and
$G_{L}=-G_{R}=50$ the mean position of impurities is started to localize at
the center of the BEC as predicted in Fig. 2(c,d) respectively. For this
special case, the BEC start effecting by impurities collisions and quasi-
particles (phonons) are generated as impurities are colliding again and again
with the BEC atoms during their to-and-fro motion, which leads to the
localization of impurities in the center of the trap as depicted in Fig.
2(c-d). The self-trapping of impurities in the center of the BEC are the
results of the nonlinearity of inter- and intra-species interactions strength
in GPEs as shown in Fig. 3(c,d). At the same time, the auto-correlation graph
of the BEC and impurities shows that the BEC and impurities lose their shapes
due to the strong interaction strength for a long period of time as depicted
in Fig. 3(c,d). We also observe that the $Left$-impurity is localized faster
at the center of the BEC as compared to the $Right$-impurity as illustrated in
Fig. 2(c,d). Therefore, we conclude that for experimentalists the repulsive
inter-species impurities are preferable to trap inside the BEC at a short time
scale. For intermediate couplings, i.e., around $G_{L}=-G_{R}=30$ and
$G_{L}=-G_{R}=50$ regardless of the impurity-BEC coupling strength sign, after
a few cycles of oscillation the impurity is trapped by the BEC cloud, and
drastically changes its amplitude of oscillation as shown in Fig. 2(c-d), this
phenomenon is also reported by another study Lingua et al. (2018).

Figure 4: (Color online) We plot auto-correlation of impurities and BEC in
(a,d), mean-position of impurities in (b,d) and the temporal density of BEC in
(c,f). Here in the upper-row and in the lower-row, we have interspecies
interaction strength $G_{L}=-G_{R}=10$ and $G_{L}=-G_{R}=30$ respectively. All
other dimensionless parameters are $G_{B}=1$, $G_{II}=10$, and
$x_{0R}=-x_{0L}=10$.

###  III.2 With $Right$-impurity and $Left$-impurity interaction

In this subsection, we discuss the dynamics of impurities in the BEC, during
the presence of the mild $Right$-impurity and $Left$-impurity repulsive
interaction strength $G_{II}=10$. We would like to study the effect of the
repulsive interaction strength on the localization of impurities. We notice
that for the weak attracted impurity-impurity interactions and for the small
BEC-impurities attractive and repulsive interaction strengths, the dynamics of
impurities is not too much different from the previously discussed cases Fig.
2, and Fig. 3, therefore, we do not present these results here. However, a
very interesting case happens as we increase the impurity-impurity interaction
strength $G_{II}=10$, the auto-correlation of impurities shows that impurities
preserved their shape as shown in Fig. 4(a) and also the mean position of
impurities does not effect by impurity-impurity interaction as depicted in
Fig. 4(b). On the other hand, the BEC temporal density plot shows that the BEC
starts oscillating due to the collisions of impurities as can be seen in Fig.
4(c), this shows the transfer of momentum from impurities to the BEC. For our
proposed model, we consider that the total energy and momentum are conserved,
therefore the transfer of momentum decreases the oscillation of impurities as
predicted in Fig. 4(b) for time $t\approx 30$. This situation can be made
clearer by increasing the impurity-BEC interaction $G_{L}=-G_{R}=30$ and can
be seen in Fig. 4(e). This phenomenon helps to localize impurities in the
center of the BEC as predicted in Fig. 4(e), however, this transfer of
momentum due to impurities collisions create quasi-particles and solitons in
the BEC as shown in Fig. 4(f) for more detail please see the Refs. Akram and
Pelster (2016a); Akram and Pelster (2016b, c); Akram (2018). In addition to
that, the shape of the density profile of impurities is also be compromised
therefore, the auto-correlation of impurities is less than one as plotted in
Fig. 4(d). We also note that the BEC preserves its shapes as its auto-
correlation oscillate between $40\%$ to $60\%$. To study explicitly, we
consider attractive ($Right$-Impurity) and repulsive ($Left$-Impurity)
interaction strengths of impurities with the BEC, nevertheless, both
impurities approximately are localized at the same time scale as shown in Fig.
4(e). Additionally, our study also reveals that the $Left$-impurity has
squeezed oscillation in the BEC as compared to the $Right$-Impurity, which is
due to the repulsive interaction strength of the impurity with the BEC as
presented in Fig. 4(e). Furthermore, the $Left$-impurity loses its density-
profile shape as can be seen from the auto-correlation graph Fig. 4(d),
therefore, it is suggested that the attractive interaction between the BEC and
the impurity preferred for the trapping of the impurity inside the BEC. We
also notice that for the weak attractive impurity-impurity interaction
strength, the dynamics of impurities inside the BEC are not so much changed
therefore to avoid repeat discussion, we do not present these results here.

Figure 5: (Color online) We plot density profile of the BEC (a-c),
$Right$-impurity (d-f) and $Left$-impurity (g-i) for different values of
impurity-BEC interspecies interaction strength $G_{L}=-G_{R}=1$ (a,d,g),
$G_{L}=-G_{R}=10$ (b,e,h) and $G_{L}=-G_{R}=30$ (c,f,i). All other
dimensionless parameters are $G_{B}=1$, $G_{II}=0$, and $x_{0R}=-x_{0L}=10$.

##  IV Time-of-flight dynamics of impurities and the BEC

In this section, we discuss the dynamics of the BEC and impurities in the
time-of-flight picture. The time-of-flight dynamics of the BEC and impurities
can be achieved by switching off the confinement traps of the BEC and
impurities as shown in Fig. 5(a-i). We note that the $Right$-impurity which
has attractive interaction strength between impurity and the BEC is attracted
towards the BEC as shown in Fig. 5(d-f). On the other hand, the
$Left$-impurity which has repulsive interaction strength between impurity and
the BEC is squeezed from one side and repealed away from the other side as
shown in Fig. 5(g-i). The repulsion gets stronger for the strong impurity-BEC
interaction strength as predicted in Fig. 6(b,c), where we plot the mean-
position of the BEC. Hence the BEC temporal density is squeezed from one side
and broaden from the other side. In addition to that during the collision of
impurities and the BEC, Phonons, and shock waves are generated as can be seen
in Fig. 5(c,f,i). For the strong $G_{L}=-G_{R}=30$ interaction strength
soliton train is generated as shown in Fig. 5(c). The $Right$-impurity is
broke up into solitonic trains, however, the $Left$-impurity is confined and
repealed away from the BEC. We also note that the trapping of BEC fragment
between two consecutive soltions as presented in Fig. 5(c) and Fig. 5(f). In
addition to that, we also note that the mean-position of impurities is not
affected during the time-of-flight for very small impurity-BEC interaction
strength $G_{L}=-G_{R}=1$. However, for $G_{L}=-G_{R}=10$, the mean-position
of the impurity is slightly attractive towards the BEC as shown in Fig. 6(a-b)
and the $Right$-impurity is repelled away from the BEC center as predicted in
Fig. 6(a-b). In the very strong impurity-BEC interaction regime, for
$G_{L}=-G_{R}=30$ a very strange phenomenon happens, firstly, the
$Right$-impurity is attractive towards the center of the BEC but after some
time around $t\approx 15$ the impurity repelled away as can be noticed in Fig.
6(c) which is due to the generation of shock-waves and solitons, which means
that the most of the weight of the impurity is pushed away from the BEC. For
$Left$-impurity, we notice that the mean-position of the impurity is not
affected by strong impurity-BEC interaction strength, and it continuously
repealed from the center of the BEC as shown in Fig. 6(c). The width of the
BEC and impurities grow linearly for small impurity-BEC interaction strength
$G_{L}=-G_{R}=1$ as displayed in Fig. 6(d), however, for the strong
interaction strength $G_{L}=-G_{R}=10$ the $Right$-impurity width grows linear
but the $Left$-impurity width growth is squeezed due to the repulsion of the
BEC as presented in Fig. 6(e), this phenomena quite visible as shown in Fig.
5(i). We also note that for small impurity-BEC interaction strengths the auto-
correlation of the BEC decays exponentially however, for strong impurity-BEC
interaction strengths, impurities auto-correlation function decays
exponentially to zero, we did not present this result here.

Figure 6: (Color online) Mean-position (a-c) and width (d-f) of the of
impurities. Here, the impurity-BEC interspecies interaction strength
$G_{L}=-G_{R}=1$ (a,d), $G_{L}=-G_{R}=10$ (b,e) and $G_{L}=-G_{R}=30$ (c,f).
All other dimensionless parameters are $G_{B}=1$, $G_{II}=0$, and
$x_{0R}=-x_{0L}=10$.

##  V Conclusion

In this research paper, we study the impact of the dynamics of impurities in
the presence of the Bose-Einstein condensate. We model our system by using
three coupled GPEs, where impurities start from the outside of the BEC with
some initial momentum and collide with the BEC. We let our system be very
specific, however, the results are quite general, and different scenarios can
be classified depending on the inter-species interaction strengths. Our mean-
field results can be beneficial for future experimental and can also help to
develop a theory on impurity-BEC collisions. However, better results can be
build by utilizing more extended time-dependent methods where quantum and
thermal fluctuations are taken into account. We find out that the impurity-BEC
interaction strength strongly affects the dynamics of the impurity inside the
BEC. We also note that the impurity scattering with the BEC generates Phonons,
shock-waves, and soliton trains. Our findings suggest that the impurity
switching from one end of the BEC to the other end depends on system
parameters. Additionally, we also note that the self-trapping of BEC fragments
depends on system parameters and the impurity-BEC interaction strength. In the
future, we would like to investigate how the finite size corrections upon the
BEC can affect the self-trapping of impurities Yukalov (2005, 2011);
Castellanos (2016). Additionally, we would like to investigate the generation
and dynamics of quantum droplets in the presence of impurities.

##  VI Acknowledgment

Javed Akram thank Hamza Qayyum, Tasawar Abbas and Waqas Masood for the
insightful discussion.

## References

## References

  * Catani et al. (2012) J. Catani, G. Lamporesi, D. Naik, M. Gring, M. Inguscio, F. Minardi, A. Kantian, and T. Giamarchi, Phys. Rev. A 85, 023623 (2012), URL https://link.aps.org/doi/10.1103/PhysRevA.85.023623.
  * Hohmann et al. (2015) M. Hohmann, F. Kindermann, B. Gänger, T. Lausch, D. Mayer, F. Schmidt, and A. Widera, EPJ Quantum Technology 2, 23 (2015), ISSN 2196-0763, URL http://doi.org/10.1140/epjqt%2Fs40507-015-0036-y.
  * Hu et al. (2016) M.-G. Hu, M. J. Van de Graaff, D. Kedar, J. P. Corson, E. A. Cornell, and D. S. Jin, Phys. Rev. Lett. 117, 055301 (2016), URL https://link.aps.org/doi/10.1103/PhysRevLett.117.055301.
  * Jørgensen et al. (2016) N. B. Jørgensen, L. Wacker, K. T. Skalmstang, M. M. Parish, J. Levinsen, R. S. Christensen, G. M. Bruun, and J. J. Arlt, Phys. Rev. Lett. 117, 055302 (2016), URL https://link.aps.org/doi/10.1103/PhysRevLett.117.055302.
  * Morales-Molina and Gong (2008) L. Morales-Molina and J. Gong, Phys. Rev. A 78, 041403 (2008), URL https://link.aps.org/doi/10.1103/PhysRevA.78.041403.
  * Spethmann et al. (2012) N. Spethmann, F. Kindermann, S. John, C. Weber, D. Meschede, and A. Widera, Phys. Rev. Lett. 109, 235301 (2012), URL https://link.aps.org/doi/10.1103/PhysRevLett.109.235301.
  * Galteland and Sudbø (2016) P. N. Galteland and A. Sudbø, Phys. Rev. B 94, 054510 (2016), URL https://link.aps.org/doi/10.1103/PhysRevB.94.054510.
  * Naidon (2018) P. Naidon, Journal of the Physical Society of Japan 87, 043002 (2018).
  * Lingua et al. (2018) F. Lingua, L. Lepori, F. Minardi, V. Penna, and L. Salasnich, New Journal of Physics 20, 045001 (2018), URL https://doi.org/10.1088%2F1367-2630%2Faab1ae.
  * Yan et al. (2020) Z. Z. Yan, Y. Ni, C. Robens, and M. W. Zwierlein, Science 368, 190–194 (2020), ISSN 0036-8075, URL https://science.sciencemag.org/content/368/6487/190.
  * Mistakidis et al. (2020) S. I. Mistakidis, A. G. Volosniev, and P. Schmelcher, Phys. Rev. Research 2, 023154 (2020), URL https://link.aps.org/doi/10.1103/PhysRevResearch.2.023154.
  * Hewson (1993) A. C. Hewson, _The Kondo Problem to Heavy Fermions_ , Cambridge Studies in Magnetism (Cambridge University Press, 1993).
  * Zvonarev et al. (2007) M. B. Zvonarev, V. V. Cheianov, and T. Giamarchi, Phys. Rev. Lett. 99, 240404 (2007), URL https://link.aps.org/doi/10.1103/PhysRevLett.99.240404.
  * Palzer et al. (2009) S. Palzer, C. Zipkes, C. Sias, and M. Köhl, Phys. Rev. Lett. 103, 150601 (2009), URL https://link.aps.org/doi/10.1103/PhysRevLett.103.150601.
  * Kleine et al. (2008) A. Kleine, C. Kollath, I. P. McCulloch, T. Giamarchi, and U. Schollwöck, Phys. Rev. A 77, 013607 (2008), URL https://link.aps.org/doi/10.1103/PhysRevA.77.013607.
  * Micheli et al. (2004) A. Micheli, A. J. Daley, D. Jaksch, and P. Zoller, Phys. Rev. Lett. 93, 140408 (2004), URL https://link.aps.org/doi/10.1103/PhysRevLett.93.140408.
  * Schmid et al. (2010) S. Schmid, A. Härter, and J. H. Denschlag, Phys. Rev. Lett. 105, 133202 (2010), URL https://link.aps.org/doi/10.1103/PhysRevLett.105.133202.
  * Klein and Fleischhauer (2005) A. Klein and M. Fleischhauer, Phys. Rev. A 71, 033605 (2005), URL https://link.aps.org/doi/10.1103/PhysRevA.71.033605.
  * Akram and Pelster (2016a) J. Akram and A. Pelster, Laser Physics 26, 065501 (2016a), URL https://doi.org/10.1088%2F1054-660x%2F26%2F6%2F065501.
  * Akram and Pelster (2016b) J. Akram and A. Pelster, Phys. Rev. A 93, 023606 (2016b), URL https://link.aps.org/doi/10.1103/PhysRevA.93.023606.
  * Akram and Pelster (2016c) J. Akram and A. Pelster, Phys. Rev. A 93, 033610 (2016c), URL https://link.aps.org/doi/10.1103/PhysRevA.93.033610.
  * Akram (2018) J. Akram, Laser Physics Letters 15, 025501 (2018), URL https://doi.org/10.1088%2F1612-202x%2Faa8ec4.
  * Edmonds et al. (2019) M. J. Edmonds, J. L. Helm, and T. Busch, New Journal of Physics 21, 053019 (2019), ISSN 1367-2630, URL http://doi.org/10.1088/1367-2630%2Fab171f.
  * Ao and Chui (1998) P. Ao and S. T. Chui, Phys. Rev. A 58, 4836–4840 (1998), URL https://link.aps.org/doi/10.1103/PhysRevA.58.4836.
  * Timmermans (1998) E. Timmermans, Phys. Rev. Lett. 81, 5718–5721 (1998), URL https://link.aps.org/doi/10.1103/PhysRevLett.81.5718.
  * Stan et al. (2004) C. A. Stan, M. W. Zwierlein, C. H. Schunck, S. M. F. Raupach, and W. Ketterle, Phys. Rev. Lett. 93, 143001 (2004), URL https://link.aps.org/doi/10.1103/PhysRevLett.93.143001.
  * Pilch et al. (2009) K. Pilch, A. D. Lange, A. Prantner, G. Kerner, F. Ferlaino, H.-C. Nägerl, and R. Grimm, Phys. Rev. A 79, 042718 (2009), URL https://link.aps.org/doi/10.1103/PhysRevA.79.042718.
  * Gröbner et al. (2017) M. Gröbner, P. Weinmann, E. Kirilov, H.-C. Nägerl, P. S. Julienne, C. R. Le Sueur, and J. M. Hutson, Phys. Rev. A 95, 022715 (2017), URL https://link.aps.org/doi/10.1103/PhysRevA.95.022715.
  * Vudragovic et al. (2012) D. Vudragovic, I. Vidanovic, A. Balaz, P. Muruganandam, and S. K. Adhikari, Comp. Phys. Commun. 183, 2021–2025 (2012), ISSN 0010-4655, URL http://www.sciencedirect.com/science/article/pii/S0010465512001270.
  * Kumar et al. (2015) R. K. Kumar, L. E. Young-S., D. Vudragovic, A. Balaz, P. Muruganandam, and S. Adhikari, Comput. Phys. Commun. 195, 117–128 (2015), ISSN 0010-4655.
  * Sataric et al. (2016) B. Sataric, V. Slavnic, A. Belic, A. Balaz, P. Muruganandam, and S. K. Adhikari, Comput. Phys. Commun. 200, 411 (2016), ISSN 0010-4655, URL http://www.sciencedirect.com/science/article/pii/S0010465515004440.
  * Loncar et al. (2016) V. Loncar, A. Balaz, A. Bogojevic, S. Skrbic, P. Muruganandam, and S. K. Adhikari, Computer Physics Communications 200, 406 (2016), ISSN 0010-4655, URL https://www.sciencedirect.com/science/article/pii/S0010465515004361.
  * Young-S. et al. (2016) L. E. Young-S., D. Vudragovic, P. Muruganandam, S. K. Adhikari, and A. Balaz, Computer Physics Communications 204, 209 (2016), ISSN 0010-4655, URL https://www.sciencedirect.com/science/article/pii/S001046551630073X.
  * Yukalov (2005) V. I. Yukalov, Phys. Rev. A 72, 033608 (2005), URL https://link.aps.org/doi/10.1103/PhysRevA.72.033608.
  * Yukalov (2011) V. I. Yukalov, Physics of Particles and Nuclei 42, 460 (2011), ISSN 1063-7796,1531-8559, URL http://doi.org/10.1134/s1063779611030063.
  * Castellanos (2016) E. Castellanos, Modern Physics Letters B 30, 1650307 (2016), eprint https://doi.org/10.1142/S0217984916503073, URL https://doi.org/10.1142/S0217984916503073.


©IEEE. This paper is a preprint version.

# End-to-End UAV Simulation for Visual SLAM and Navigation

Shengyang Chen1, Han Chen2, Weifeng Zhou1, Chih-yung Wen1,2, and Boyang Li2
1Shengyang Chen, Weifeng Zhou, and Chih-yung Wen are with Department of
Mechanical Engineering, The Hong Kong Polytechnic University, Hong Kong
(shengyang.chen@connect.polyu.edu.hk).2Han Chen and Boyang Li are with
Interdisciplinary Division of Aeronautical and Aviation Engineering, The Hong
Kong Polytechnic University, Hong Kong.

###### Abstract

Visual Simultaneous Localization and Mapping (v-SLAM) and navigation of
multirotor Unmanned Aerial Vehicles (UAV) in an unknown environment have grown
in popularity for both research and education. However, due to the complex
hardware setup, safety precautions, and battery constraints, extensive
physical testing can be expensive and time-consuming. As an alternative
solution, simulation tools lower the barrier to carry out the algorithm
testing and validation before field trials. In this letter, we customize the
ROS-Gazebo-PX4 simulator in deep and provide an end-to-end simulation solution
for the UAV v-SLAM and navigation study. A set of localization, mapping, and
path planning kits were also integrated into the simulation platform. In our
simulation, various aspects, including complex environments and onboard
sensors, can simultaneously interact with our navigation framework to achieve
specific surveillance missions. In this end-to-end simulation, we achieved
click and fly level autonomy UAV navigation. The source code is open to the
research community.

## Supplementary Materials

Demo video: https://youtu.be/9BQMSVQlo7A

##  I Introduction

With modern artificial intelligence algorithms, the multi-rotor UAVs are
empowered to become smart agents that can navigate in the unknown environment.
Given a destination, the UAV can precept the environment, reconstruct the
environment map, and dynamically plan a trajectory to the destination. In
detail, three sorts of tool kits are applied in such scenarios: localization,
mapping, and planning.

The localization (or named pose estimation) kit utilizes the onboard sensor
information, such as a stereo camera, to estimate the vehicle’s 6 degree of
freedom (DoF) pose in real-time. The pose feeds into the flight control unit
(FCU) to achieve position level control. Given the vehicle pose and sensor
input, such as point cloud, the mapping kit reconstructs the environment
throughout the mission. Typically, the environment is presented by a 3D
occupancy voxel map with the euclidean signed distance information [1]. The
path planning kit, finds the path to the destination with the minimum cost,
avoids the obstacle, and generates a trajectory. The trajectory is then sent
to the FCU in the time sequence and navigate the vehicle.

Verifying such UAV navigation system under realistic scenarios can be effort-
intensive, and a testing failure may lead to damage to the vehicle. The
simulator provides simulated hardware components such as perception sensors to
researchers. It also helps researchers with the ease of reconfiguration, and
the flexibility of environmental setup.

(a) System overview of the proposed simulator.

(b) UAV in the simulator, the right hand side images are the real-time color
and depth image from the on-board cameras.

(c) Click and fly navigation in the unknown environment (the blue path is the
traveled path and the red path is global planned path from the current
position to the destination).

Figure 1: End-to-end UAV v-SLAM and navigation simulator.

Various UAV simulation works exist nowadays. However, most of them barely
focus on a specific task. The flight dynamic oriented simulation ignore all
environment information. The simulation for the v-SLAM study simplified the
dynamic model of the vehicle. The navigation-oriented simulation’s environment
setting contains the 3D information of the environment but neglecting the
features of these 3D object. Some simulation works [2] [3] do achieve
autonomous navigation in a certain level, but the flexibility is limited and
the source codes are not released. The motivation of this work is to construct
an end-to-end simulation environment for research and education purposes.
Here, the end-to-end refers to the capability of verifying all the perception,
reaction, and control algorithm in one simulator (Figure 1(a)).

Base on the popular ROS-Gazebo-PX4 toolchain, we made several improvements to
meet the requirement of UAV v-SLAM and navigation simulation (Figure 1(b)).
These improvements include: (a) construction of a simulation world, (b)
customized UAV models, (c) adding a stereo camera model, and (d) configuration
of the vision-based control setup . At the end of this paper, we demonstrate
an end-to-end UAV navigation simulation (Figure 1(c)). In summary, the
contributions of this work include:

  * •

Customization of the ROS-Gazebo-PX4 simulator in terms of the support of
stereo inertial vision estimation, vision feedback control, and ground truth
level evaluation.

  * •

Integration of functions including localization, mapping, and planning into
tool kits and achievement of click and fly level autonomy in the simulation
environment.

  * •

Release of the simulation setup111https://github.com/HKPolyU-
UAV/mav_sim_gazebo, together with the localization
kit222https://github.com/HKPolyU-UAV/FLVIS, mapping
kit333https://github.com/HKPolyU-UAV/glmapping and planning
kit444https://bitbucket.org/arclabadmin/fuxi-planner as open-source tools for
the research community.

##  II Related Works

###  II-A UAV Simulator

We sort the UAV simulator into two categories concerning the scope of
simulation: flight dynamics simulator and environment integrated simulator.
The first kind of simulator focus on simulating the dynamics for different UAV
platforms. All environmental information is neglected except for the gravity
force. Based on Matlab Simulink, Quad-Sim [4] is suitable to test the flight
control algorithms for different dynamic models. Sun et al. [5] developed
another Simulink-based simulator. It includes a comprehensive aerodynamic
model of the tail-sitter VTOL UAV. The perception supported simulator, as it
named, includes the model of the perception sensors and the environment
information. Users can access the simulated sensor output, such as the camera
images and the point cloud from the Lidar sensor [6]. Schmittle et al. [7]
developed an easy access web-based UAV testbed for education and research.
With the Containers as a Service (CaaS) technology, the simulator was deployed
on the cloud, which means the user does not need a high-performance computer
to execute the simulation.

###  II-B UAV v-SLAM and Navigation System

Typically, the v-SLAM and navigation system consists localization, mapping,
and planning modules, we review the related works in sequence.

####  II-B1 Localization

Visual localization or visual pose estimation has monocular or stereo camera
solutions. The monocular solution has an advantage in terms of simple
structure, light-weight, and cost-efficiency. However, recover the scale
correctly is the challenge for such a system. Researchers integrate IMU
information [8] or use predefining the object pattern [9, 10] of the
environment to eliminate this problem. Nowadays, stereo camera solutions are
off the shelf. As the depth information can be directly extracted from every
frame, the accuracy and the robustness of the system are better than the
monocular setup. Admittedly, the stereo data stream is more massive than the
monocular one. The powerful onboard computers can compensate for this issue.

In the UAV application, the visual information is usually fused with the IMU
data through either a filter-based framework or an optimization-based
framework. In the filter-based framework, the pose and the landmark are in the
system states. IMU input propagates the pose states and the relevant con-
variance matrix [11, 12]. In the optimization-based framework, the IMU engaged
through a pre-integration edge [13]. According to Delmerico et al. [14], the
optimization-based approach outperforms the filter-based approach in terms of
accuracy but requires more computation resources.

####  II-B2 Mapping

The mapping system, which provides a foundation for onboard motion planning,
is an essential component in the perception-planning-control pipeline. A
mapping system needs to balance the measurements’ accuracy and the overhead of
storage. Three kinds of maps have successful stories in the UAV navigation
application: point cloud map [15], occupancy map [16], and Euclidean Signed
Distance Fields (ESDFs) [17] map.

We can easily get the point cloud map by stitching points measurement.
However, this kind of map only suitable for high precision sensors in static
environments since the sensor noise and dynamic objects cannot be accessed and
modified. Occupancy maps, such as Octomap [16], store occupancy probabilities
in a hierarchical octree structure. These approaches’ main restriction is the
fixed-size voxel grid, which requires a known map size in advance and cannot
be dynamically changed [17]. Nowadays, Euclidean Signed Distance Fields
(ESDFs) map gains popularity [18]. This kind of map is suitable for
dynamically growing maps and have the advantage of evaluating the distance and
gradient information against obstacles.

####  II-B3 Planning

For UAV route planning, algorithms can be classified into two main categories,
sampling-based [19] and optimization-based [20]. Rapidly-exploring random tree
(RRT) [19] is the representative of the sampling-based algorithm. In this
method, samples are drawn randomly from the configuration space and guide the
tree to grow towards the target [21]. Rapidly-exploring random graph (RRG)
[22] is an extension of the RRT algorithm, and it is asymptotic optimal. Even
though the sampling-based method is suitable for finding safe paths, it is not
smooth for UAV to follow. The minimum snap [20] algorithm can be applied to
generate a smooth trajectory. It formulates the trajectory generation problem
as a quadratic programming (QP) problem. By minimizing the cost function
instantly, the trajectory can be represented in piecewise polynomial
functions. The cost function includes two terms: the penalty for the
trajectory with the potential of collisions and the smoothness of the
trajectory itself. In optimization-based methods, another way to add
constraints to the optimization problem is first obtaining a series of
waypoints by sample search or grid search, then optimize the motion primitives
to generate a smooth trajectory through the waypoints under the UAV’s dynamic
constraints [23]. It combines the advantages of the two categories, and
computation efficiency has a great advantage over those pure optimization-
based algorithms. However, the safe radius and other parameters must be tuned
carefully.

###  II-C UAV SLAM and Navigation Simulation

Some similar simulations have been conducted prior to this work. Zhang et al.
[2] presented a quadrotor UAV simulator integrated with a hierarchical
navigation system. The UAV equips with two laser scanners and one monocular
camera. One laser scanners were mounted on the bottom for altitude control and
3D map construction, while another one was attached to the top for navigation
purposes. The monocular camera is attached to a tilting mechanism for target
detection and vision guidance. The fused data were constructed into Octomap
and facilitate a trajectory from an A* global path planner. However, in this
work, the environment setup is quite simple and does not include vision
features. The navigation is based on the Laser SLAM result. In the simulation
from Alzugaray et al. [3], a point-to-point planner algorithm was designed to
work with the SLAM estimation of a monocular-inertial system. The UAV in the
simulator fly around the building and reconstruct the environment. However,
the UAV is flying outside the building and the obstacle avoidance feature is
not considered in this work.

##  III Simulation Platform

###  III-A Overview

In the robotic society, the robot operating system (ROS555https://www.ros.org)
[24] is undoubtedly the most convenient platform, which provides powerful
developer tools and software packages from drivers to state-of-the-art
algorithms. Besides, many navigation kits have the ROS version package and
very convenient to be integrated. Moreover, Gazebo666http://gazebosim.org
[25], an open-source robotics simulator, is the most widely used simulator in
ROS. And we selected the widely used open-source UAV autopilot stack
PX4777https://px4.io [26]. It supports software in the loop (SITL) simulation.
Our simulation platform is based on the ROS-Gazebo-PX4 toolchain.

As shown in Figure 2, the upper part is the SITL simulator, and the bottom
part is the mapping and navigation system. All components were coordinate
through different ROS topics. Especially, the communication between the
navigation system and PX4 are through MAVROS888http://wiki.ros.org/mavros. We
list details of important ROS topics in Appendix. In the simulator, PX4
communicates with the Gazebo to receive sensor data from the simulated world
and send the motor and actuator commands back. The Extended Kalman Filter
(EKF) based state estimator and motion control module are running on the PX4
stack. In Gazebo, it has an environment map called the world and a simulated
UAV model; the UAV model supports the dynamic simulation. Besides, serials of
onboard sensors, including GPS, IMU, barometer, and a custom defined depth
camera are attached to the UAV model through the Gazebo plugins.

Figure 2: Framework of simulation.

###  III-B Dynamics of UAV Model

The dynamic model in the simulator follows the conventional quadcopter dynamic
model which can be found in general papers about dynamics and control such as
[27, 28, 29]. The UAV is modeled as a 6 DoF rigid body (3 DoF in position,3
DoF in rotation). The position of the center of gravity (C.G.) of the vehicle
in the inertial frame is defined by
$\boldsymbol{\xi}=[X~{}Y~{}Z]^{\mathsf{T}}\in\mathbb{R}^{3}$, the orientation
of the vehicle is denoted by the rotation matrix from body frame ($B$) to
inertial frame ($I$) $\mathbf{R}_{B}^{I}\in SO(3)$, the velocity, as the
derivatives of the position, in the inertial frame is described by
$\mathbf{v}=[\dot{X}~{}\dot{Y}~{}\dot{Z}]^{\mathsf{T}}\in\mathbb{R}^{3}$, and
the angular velocity is denoted by $\boldsymbol{\omega}$. The kinematics and
dynamics of the position and attitude can be denoted by:

$\displaystyle\dot{\boldsymbol{\xi}}$ $\displaystyle=\mathbf{v},$ (1)
$\displaystyle m\dot{\mathbf{v}}$
$\displaystyle=\mathbf{R}_{B}^{I}\mathbf{F}_{B},$
$\displaystyle\dot{\mathbf{R}}_{B}^{I}$
$\displaystyle=\mathbf{R}_{B}^{I}\boldsymbol{\omega}_{\times},$
$\displaystyle\mathbf{I}\dot{\boldsymbol{\omega}}$
$\displaystyle=-\boldsymbol{\omega}\times(\mathbf{I}\boldsymbol{\omega})+\mathbf{M}_{B},$

where $m$ denotes the mass and $\mathbf{I}$ denotes the inertia matrix of the
vehicle. $\boldsymbol{\omega}_{\times}$ denotes the skew-symmetric matrix such
that
$\boldsymbol{\omega}_{\times}\mathbf{v}=\boldsymbol{\omega}\times\mathbf{v}$
for any vector $\mathbf{v}\in\mathbb{R}^{3}$. The $\mathbf{F}_{B}$ and
$\mathbf{M}_{B}$ are total force and moment acting on the body frame,
respectively. The dynamic simulation of the UAV is achieved by the model in
the Gazebo environment. The PX4 SITL simulator handles all the inner loop
attitude, velocity, and position control. The user commands the UAV in the
offboard control mode by sending the target position or velocity commands
through MAVROS.

###  III-C On-board Sensors

Improved from the 3DR-IRIS model, we added a depth camera and customized the
IMU sensor to support the visual-inertial pose estimator. Here we introduce
the camera and IMU models. In the Appendix, we introduced how to transplant to
other pose estimators. The coordinate definition of the body and IMU is shown
in Figure 3.

####  III-C1 Visual sensor

Figure 3: Modified 3DR-IRIS model and the installation geometry of the vision
sensor and IMU (X-Y-Z axis in reference frame are colored in red, green, and
blue).

The visual sensor is bases on
librealsense_gazebo_plugin999https://github.com/pal-
robotics/realsense_gazebo_plugin. The visual sensor in the simulator consists
of two grey scale cameras ($C_{0}$ and $C_{1}$), a color camera ($C_{c}$), and
a depth camera ($C_{d}$). All these cameras are based on the non-distortion
pine hole camera model. The output of the sensor includes two grey scale
images, a color image, a depth image, and a point cloud. All these outputs are
time synchronized. The horizon field of view ($HFOV$) and resolution
($width\times height$) define the intrinsic camera parameters ($f_{x}$,
$f_{y}$, $c_{x}$, $c_{y}$) by

$f_{x}=f_{y}=\frac{width}{2\cdot tan(HFOV/2)}$ (2)
$c_{x}=c_{x}=\frac{width}{2};c_{y}=\frac{height}{2}.$ (3)

Besides, the installation geometry in the SDFormat (Simulation Description
Format) file defines the extrinsic parameters. For convenience, the color
camera, the depth camera, and the left pine hole camera are installed in the
same link, which means the color image, depth image, left grey scale image,
and the point cloud are all aligned.

####  III-C2 IMU

In the simulator, the IMU consists of a 3-axis accelerometer and a 3-axis
gyroscope. The measured angular velocity and acceleration can be described by
the following models:

$\displaystyle\boldsymbol{\omega}_{m}=\boldsymbol{\omega}_{real}+\boldsymbol{b}_{\omega}+\boldsymbol{n}_{\omega}$
(4)
$\displaystyle\boldsymbol{a}_{m}=\boldsymbol{a}_{real}+\boldsymbol{b}_{a}+\boldsymbol{n}_{a}$
(5)
$\displaystyle\boldsymbol{n}_{\omega}\sim\mathcal{N}(\boldsymbol{0},\boldsymbol{\sigma}_{\omega}^{2});\boldsymbol{n}_{a}\sim\mathcal{N}(\boldsymbol{0},\boldsymbol{\sigma}_{a}^{2})$
(6)
$\displaystyle\boldsymbol{\dot{b}}_{\omega}\sim\mathcal{N}(\boldsymbol{0},\boldsymbol{\sigma}_{\omega_{b}}^{2});\boldsymbol{\dot{b}}_{a}\sim\mathcal{N}(\boldsymbol{0},\boldsymbol{\sigma}_{a_{b}}^{2})$
(7)

where $\boldsymbol{n}_{\omega}$ and $\boldsymbol{n}_{a}$ refer to the
intrinsic noises of the sensor which follow the Gaussian distributions. The
biases ($\boldsymbol{\omega}_{b}$ and $\boldsymbol{a}_{b}$) are affected by
the temperature and change over time. The slow variations in the sensor biases
are modelled with or random walk noise in discrete time. That is, the time
derivatives of the biases ($\boldsymbol{\dot{b}}_{\omega}$ and
$\boldsymbol{\dot{b}}_{a}$) follow the Gaussian distributions. Furthermore, in
the IRIS model provided by PX4 Firmware, the update rate of IMU is constrained
by the MAVROS. To cross the speed limit, we add another IMU plugin, which
publishes the IMU information at the rate of 200 Hz.

###  III-D Simulation World Setup

First, we added obstacles such as walls and boxes into a $20\times 20~{}m$
empty world. Then, to meet the requirement of v-SLAM simulation, we furnish
all these items and the ground plane with the wallpapers, which contains rich
visual features, shown in Figure 4.

Figure 4: The $20\times 20~{}m$ simulation world with obstacles (wall papers
contain rich visual features to support the visual tracking).

##  IV MAV Navigation Framework

###  IV-A Localization

We integrate FLVIS[30], a stereo visual-inertial pose estimator developed by
our group, as the localization kits (Figure 5). Compared to other monocular
vSLAM solutions, stereo visual-inertial pose estimator has the advantages of
robustness, accuracy, and scale-consistency. They could be further explained
as Robustness: the pose between consecutive visual frames can be estimated by
IMU when visual tracking is lost. Accuracy: more measurements are fused in the
pose estimation process to get better accuracy. Scale-consistency: the depth
information can be extracted directly from stereo images without any motion.
FLVIS use the feedback/feedforward loops to fuse the data from IMU and
stereo/RGB-D camera and achieve high accuracy on resource-limited computation
platform.

Figure 5: FLVIS in the simulation, the markers in the image refer to the
landmarks, different color refer to the distance between the landmark and the
camera.

###  IV-B Mapping

A global-local mapping kit ‘glmapping’ was integrated into the simulator as
shown in Figure 6. This mapping kit is a 3D occupancy voxel map designed for
the MAV or mobile robot navigation applications. Currently, most of the
navigation strategies are the combination of global planning and local
planning algorithms. Global planning focuses on finding the least cost path
from the current position to the destination. Furthermore, local planning is
used to replan the trajectory to avoid obstacles. This mapping kit processes
the perception information separately. The global map, on the cartesian
coordinate system, is a probability occupancy map. The local map, on
cylindrical coordinates system, has an excellent dynamic performance. The
mapping kit also supports the projected 2D occupancy grid map and ESDFs map
output.

Figure 6: Visualization of Global/Local/ESFDs map in Rviz. In the global map,
the color (blue-purple) refers to the height of the obstacle; the ESFDs map’s
color (red-yellow-green) refers to the signed distance value; the highlight
white spheres refer to the local map.

###  IV-C Path Planning

Algorithm 1 fuxi-planner

1: while goal not reached: do

2: Receive a global 3D voxel map ${Pcl_{m}}$, and its projecttion on the
ground (${Map1}$) as the 2D pixel map for path finding

3: Cut off blank edge of ${Map1}$ and apply obstacle inflation on ${Map1}$,
output ${Map2}$

4: Find the shortest ${Path1}$ to goal

5: Calculate the optimal local goal by the Bezier curve

6: end while

7: while goal not reached: do

8: Receive the local goal

9: Find the next waypoint $N_{k}$ by heuristic angular search

10: if ${foundafeasiblewaypoint:}$ then

11: Run the minimum acceleration motion planner to get motion primitives

12: else

13: Run the backup plan for safety, then go to 5

14: end if

15: Send the motion primitives to the UAV flight controller

16: end while

We integrated fuxi-Planner [23, 31] as our path planning kit. Algorithm 1
illustrates the planning process. The planner is composed of a global path
planner and a local planner. The global planner works on a 2D global grid map
to find the shortest 2D path and output the local goal for the local planner.
And the local planner works directly on point cloud to avoid the potential
collision with obstacles and plan a kinematically feasible trajectory to the
local goal. The local planner’s core component is a sample-based waypoint
search method called the heuristic angular search (HAS) method. The engagement
of the global planner prevents the local planner from failing into the kidnap
situation. The global planner utilizes Jump Point Search (JPS) algorithm [32]
to output a serial of waypoints, which present the shortest path. The first
three waypoints are used as the control points to draw a Bezier curve. The
local path planner’s goal is to locate at the tangent at the Bezier curve’s
first waypoint. The two planners work in parallel. Although the global
planner’s outer loop frequency is relatively low, the local planner inner loop
still maintains a high update rate and can continuously command the UAV.

##  V Results and Discussion

In this section, we conducted two experiments to show to performance of the
proposed simulator. In the first experiment, we flew the vehicle manually in
the simulation world with the keyboard to verify the localization and mapping
kits. In the second experiment, we demonstrated the click and fly level
autonomous navigation.

###  V-A Manual Exploration

In this case, we integrated the localization kit and mapping kit. With the
first person view (FPV) from the colored camera and the real-time
reconstructed map view, we controlled the UAV to explore the $20m\times 20m$
unknown environment. This exploration mission cost 7 minutes and 24 seconds,
and the UAV traveled 82 meters in the simulation world.

The performance of the localization and mapping kit were then evaluated. The
excellent agreement between the ground truth path and the estimated path form
the localization kit can be observed in Figure 7. We used a tool from Michael
Grupp [33] to evaluate the accuracy of the localization kit. The absolute
trajectory error (ATE) of the translation drift in the form of root mean
square error (RMSE) is 0.3 m.

Figure 7: Comparison of the ground truth and the estimated pose from the
localization kit. Figure 8: Top-down view of the simulation world (left) and
the projected 2D occupancy grid map generated by the mapping kit (right).
Figure 9: Oblique view of the simulation world (left) and reconstructed map by
the mapping kit (right).

We capture the image of the simulation world and reconstructed map from
different views. Good agreement between them and the detail of the map can be
observed in Figure 8 and 9. The voxel size of the map is $0.2\times 0.2\times
0.2$ meter.

###  V-B Click and Fly Level Autonomy

Figure 10: Click and fly navigation (the blue path is the traveled path and
the white mark in the image is the given way point).

In this case, we further integrated the path planning kit into the simulation.
We only give a desired destination on the map to the UAV. Then the UAV will
plan a path, avoid the obstacle, and fly to the destination automatically. As
shown in Figure 10, six waypoints were set during the mission. The drone
precept the environment, plan a path to visit these waypoints in sequence
fully automatically. The fly path kept a safe distance to the nearest obstacle
to avoid the collision.

Figure 11: The record of position, velocity, and attitude in click and fly
navigation.

Figure 11 displays the curve of position, velocity, and attitude of the
simulation. We can see the velocity is well controlled under the max speed
limit (1 m/s) and the position curves are fluent and pass all the six goal
points stably. The pitch and roll angles are small so that the vision
localization kit work at high performance.

TABLE A1: IMPORTANT ROS TOPICS Topic name | Topic type | Update rate(Hz) | Comment
---|---|---|---
Simulator
/camera/infra1/image_raw | sensor_msgs/Image | 30 | Image of the left grey scale camera
/camera/infra2/image_raw | sensor_msgs/Image | 30 | Image of the right grey scale camera
/camera/color/image_raw | sensor_msgs/Image | 30 | Image of the color camera
/camera/depth_aligned_to_color_and_infra1/image_raw | sensor_msgs/Image | 30 | Depth Image
/camera/depth/color/points | sensor_msgs/PointCloud2 | 30 | Point Cloud (aligned to color)
/iris/imu | sensor_msgs/Imu | 200 | IMU data
/gt_iris_base_link_imu | nav_msgs/Odometry | 50 | Ground truth of the IMU link
with Localization Kit
/imu_path | nav_msgs/Path | 200 | Path at IMU rate
/vision_path | nav_msgs/Path | 30 | Path at vision rate
/vo_img0 | sensor_msgs/Image | 30 | Output image with colored landmanks
/vo_img1 | sensor_msgs/Image | 30 | Colored depth Image
with Mapping kit
/globalmap | visualization_msgs/Marker | 10 | Visualize global map
/localmap | visualization_msgs/Marker | 10 | Visualize local map
/occupancygrid | nav_msgs/OccupancyGrid | 30 | Projected occupancy grid map
/esfd_map | visualization_msgs/Marker | 10 | Visualized ESFD map
with Planning kit
/jps_path | nav_msgs/Path | 10-20 | Global path on grid map
/local_wp | visualization_msgs/Marker | 50-70 | waypoint of the local planner
/global _goal | geometry_msgs/Point | 10-60 | Temporary goal for local planner
/mavros/setpoint_velocity/cmd_vel | geometry_msgs/PoseStamped | 50-70 | Velocity command for PX4 controller

###  V-C Performance Analysis

The configurations for the navigation system in the simulation are as follows:
in localization kit, the input image resolution is $640\times 360$; in mapping
kit, the voxel size is $0.2m\times 0.2m\times 0.2m$ and the map contains
181500 ($110\times 110\times 15$) voxels. The simulation has been verified on
two different computers. The processing times are listed in the Table 1. The
average time factor refer to the ratio of the real time over the simulation
time. The value of time factor is 1 means the simulation runs in real-time.

TABLE 1: PROCESSING TIME OF THE NAVIGATION SYSTEM AND THE SIMULATION TIME
FACTOR

Kits | Computer 1 | Computer 2
---|---|---
Localization (with out loop clousure) | 28 ms | 22 ms
Mapping | Global map | 26 ms | 18 ms
Local map | 4 ms | 4 ms
Projected ESDFs map | 70 ms | 64ms
Planning | Global planning | 90 ms | 65ms
Local planning | 20 ms | 16ms
Simulator | Average time factor | 0.6 | 0.92

Note: Computer 1: i5-8250u CPU, 8GB RAM, GeForce MX150 graphic card; Computer
2: i7-8550u CPU, 16GB RAM, GeForce MX150 graphic card.

##  VI CONCLUSIONS AND FUTURE WORK

In this letter, we introduced an end-to-end UAV simulation platform for SLAM
and navigation research and applications, with the detailed simulator setup
and an the out of box localization, mapping, and navigation system. We also
demonstrated the click and fly level autonomy navigation by the simulator. The
flight results show that the simulator could provide trustworthy data stream
and also the versatile interfaces for autonomous functions development. We
offered all the kits for public access to promote further research and
development of the autonomous UAV system based on this framework.Future work
will focus on two aspects. One is to support more open source notable
navigation-related kits. Moreover, to design the benchmark scenario in the
simulator to evaluate the performance of these kits. Another aspect is to
expand the current simulator to encompass more perception sensors, more UAV
platforms, and more challenging environments for a variety of potential tasks.

## APPENDIX

###  VI-A Important ROS Topics

Several important ROS topics are list in Table A1.

###  VI-B Support for Stereo Visual Inertial Pose Estimator

In default simulator setup. The resolution of the camera is $640\times 360$
with the $HFOV=1.5~{}rad$. According to Equation 2 and 3, the camera intrinsic
is

$[f_{x},f_{y},c_{x},c_{y}]=[343.4963,343.4963,320,180].$

The extrinsic parameter including the transformation from right camera to left
camera $\mathbf{T}_{C_{1}}^{C_{0}}$ and the transformation from left camera to
IMU $\mathbf{T}_{C_{0}}^{I}$. By the default installation geometry setup, they
are:

$\mathbf{T}_{C_{1}}^{C_{0}}=\begin{bmatrix}1&0&0&0.05\\\ 0&1&0&0\\\ 0&0&1&0\\\
0&0&0&1\end{bmatrix},\mathbf{T}_{C_{0}}^{I}=\begin{bmatrix}0&0&1&0.12\\\
-1&0&0&0\\\ 0&-1&0&0\\\ 0&0&0&1\end{bmatrix}.$

## References

  * [1] H. Oleynikova, A. Millane, Z. Taylor, E. Galceran, J. Nieto, and R. Siegwart, “Signed distance fields: A natural representation for both mapping and planning,” in RSS 2016 Workshop: Geometry and Beyond-Representations, Physics, and Scene Understanding for Robotics, University of Michigan, 2016.
  * [2] M. Zhang, H. Qin, M. Lan, J. Lin, S. Wang, K. Liu, F. Lin, and B. M. Chen, “A high fidelity simulator for a quadrotor uav using ros and gazebo,” in IECON 2015-41st Annual Conference of the IEEE Industrial Electronics Society, pp. 002846–002851, 2015.
  * [3] I. Alzugaray, L. Teixeira, and M. Chli, “Short-term uav path-planning with monocular-inertial slam in the loop,” in 2017 IEEE International Conference on Robotics and Automation (ICRA), pp. 2739–2746, 2017.
  * [4] D. Hartman, K. Landis, M. Mehrer, S. Moreno, and J. Kim, “dch33/quad-sim.” https://github.com/dch33/Quad-Sim, 2014.
  * [5] J. Sun, B. Li, C.-Y. Wen, and C.-K. Chen, “Design and implementation of a real-time hardware-in-the-loop testing platform for a dual-rotor tail-sitter unmanned aerial vehicle,” Mechatronics, vol. 56, pp. 1–15, 2018.
  * [6] F. Furrer, M. Burri, M. Achtelik, and R. Siegwart, Robot Operating System (ROS): The Complete Reference (Volume 1), ch. RotorS—A Modular Gazebo MAV Simulator Framework, pp. 595–625.  Cham: Springer International Publishing, 2016.
  * [7] M. Schmittle, A. Lukina, L. Vacek, J. Das, C. P. Buskirk, S. Rees, J. Sztipanovits, R. Grosu, and V. Kumar, “Openuav: a uav testbed for the cps and robotics community,” in 2018 ACM/IEEE 9th International Conference on Cyber-Physical Systems (ICCPS), pp. 130–139, 2018.
  * [8] T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004–1020, 2018.
  * [9] D. Frost, V. Prisacariu, and D. Murray, “Recovering stable scale in monocular slam using object-supplemented bundle adjustment,” IEEE Transactions on Robotics, vol. 34, no. 3, pp. 736–747, 2018.
  * [10] B. Pfrommer and K. Daniilidis, “Tagslam: Robust slam with fiducial markers,” arXiv preprint arXiv:1910.00679, 2019.
  * [11] A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint kalman filter for vision-aided inertial navigation,” in Proceedings 2007 IEEE International Conference on Robotics and Automation, pp. 3565–3572, 2007.
  * [12] M. Bloesch, M. Burri, S. Omari, M. Hutter, and R. Siegwart, “Iterated extended kalman filter based visual-inertial odometry using direct photometric feedback,” The International Journal of Robotics Research, vol. 36, no. 10, pp. 1053–1072, 2017.
  * [13] S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale, “Keyframe-based visual–inertial odometry using nonlinear optimization,” The International Journal of Robotics Research, vol. 34, no. 3, pp. 314–334, 2015.
  * [14] J. Delmerico and D. Scaramuzza, “A benchmark comparison of monocular visual-inertial odometry algorithms for flying robots,” in 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 2502–2509, 2018\.
  * [15] F. Gao, W. Wu, W. Gao, and S. Shen, “Flying on point clouds: Online trajectory generation and autonomous navigation for quadrotors in cluttered environments,” Journal of Field Robotics, vol. 36, no. 4, pp. 710–733, 2019.
  * [16] A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, and W. Burgard, “Octomap: An efficient probabilistic 3d mapping framework based on octrees,” Autonomous robots, vol. 34, no. 3, pp. 189–206, 2013.
  * [17] H. Oleynikova, Z. Taylor, M. Fehr, R. Siegwart, and J. Nieto, “Voxblox: Incremental 3d euclidean signed distance fields for on-board mav planning,” in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 1366–1373, 2017.
  * [18] L. Han, F. Gao, B. Zhou, and S. Shen, “Fiesta: Fast incremental euclidean distance fields for online motion planning of aerial robots,” arXiv preprint arXiv:1903.02144, 2019.
  * [19] S. M. LaValle, “Rapidly-exploring random trees: A new tool for path planning,” tech. rep., Computer Science Department, Iowa State University, 1998\.
  * [20] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in 2011 IEEE international conference on robotics and automation, pp. 2520–2525, 2011.
  * [21] F. Gao, Y. Lin, and S. Shen, “Gradient-based online safe trajectory generation for quadrotor flight in complex environments,” in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 3681–3688, 2017.
  * [22] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” The international journal of robotics research, vol. 30, no. 7, pp. 846–894, 2011.
  * [23] H. Chen, P. Lu, and C. Xiao, “Dynamic obstacle avoidance for uavs using a fast trajectory planning approach,” in 2019 IEEE International Conference on Robotics and Biomimetics (ROBIO), pp. 1459–1464, 2019.
  * [24] M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote, J. Leibs, R. Wheeler, and A. Y. Ng, “Ros: an open-source robot operating system,” in ICRA workshop on open source software, vol. 3, p. 5, Kobe, Japan, 2009.
  * [25] N. Koenig and A. Howard, “Design and use paradigms for gazebo, an open-source multi-robot simulator,” in 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)(IEEE Cat. No. 04CH37566), vol. 3, pp. 2149–2154, 2004.
  * [26] L. Meier, D. Honegger, and M. Pollefeys, “Px4: A node-based multithreaded open source robotics framework for deeply embedded platforms,” in 2015 IEEE international conference on robotics and automation (ICRA), pp. 6235–6240, 2015\.
  * [27] T. Lee, M. Leok, and N. H. McClamroch, “Geometric tracking control of a quadrotor uav on se (3),” in 49th IEEE conference on decision and control (CDC), pp. 5420–5425, 2010.
  * [28] R. Mahony, V. Kumar, and P. Corke, “Multirotor aerial vehicles: Modeling, estimation, and control of quadrotor,” IEEE Robotics & Automation Magazine, vol. 19, pp. 20–32, sep 2012.
  * [29] S. Verling, B. Weibel, M. Boosfeld, K. Alexis, M. Burri, and R. Siegwart, “Full attitude control of a VTOL tailsitter UAV,” in IEEE Int. Conf. on Robotics and Automation, 2016.
  * [30] S. Chen, C.-Y. Wen, Y. Zou, and W. Chen, “Stereo visual inertial pose estimation based on feedforward-feedback loops,” arXiv preprint arXiv:2007.02250, 2020.
  * [31] H. Chen and P. Lu, “Computationally efficient obstacle avoidance trajectory planner for uavs based on heuristic angular search method,” arXiv preprint arXiv:2003.06136, 2020.
  * [32] D. D. Harabor and A. Grastien, “Improving jump point search,” in Twenty-Fourth International Conference on Automated Planning and Scheduling, Citeseer, 2014.
  * [33] M. Grupp, “evo: Python package for the evaluation of odometry and slam..” https://github.com/MichaelGrupp/evo, 2017.