* Howe and Cutkosky (1996) Howe RD and Cutkosky MR (1996) Practical force-motion models for sliding manipulation.  _The International Journal of Robotics Research_ 15(6): 557–572.
  * Hutchinson et al. (1996) Hutchinson S, Hager GD and Corke PI (1996) A tutorial on visual servo control.  _IEEE transactions on robotics and automation_ 12(5): 651–670.
  * Ioffe and Szegedy (2015) Ioffe S and Szegedy C (2015) Batch normalization: Accelerating deep network training by reducing internal covariate shift.  In: _International conference on machine learning (ICML)_. PMLR, pp. 448–456.
  * James and Lepora (2020) James JW and Lepora NF (2020) Slip detection for grasp stabilization with a multifingered tactile robot hand.  _IEEE Transactions on Robotics_ 37(2): 506–519.
  * James et al. (2018) James JW, Pestell N and Lepora NF (2018) Slip detection with a biomimetic tactile sensor.  _IEEE Robotics and Automation Letters_ 3(4): 3340–3346.
  * Jia and Erdmann (1999) Jia YB and Erdmann M (1999) Pose and motion from contact.  _The International Journal of Robotics Research_ 18(5): 466–487.
  * Julier and Uhlmann (1997) Julier SJ and Uhlmann JK (1997) New extension of the Kalman filter to nonlinear systems.  In: _Signal processing, sensor fusion, and target recognition VI_ , volume 3068. Spie, pp. 182–193.
  * Julier et al. (1995) Julier SJ, Uhlmann JK and Durrant-Whyte HF (1995) A new approach for filtering nonlinear systems.  In: _Proceedings of 1995 American Control Conference-ACC’95_ , volume 3. pp. 1628–1632.
  * Kalman (1960) Kalman RE (1960) A new approach to linear filtering and prediction problems.  _Transactions of the ASME–Journal of Basic Engineering_ 82(Series D): 35–45.
  * Kalman and Bucy (1961) Kalman RE and Bucy RS (1961) New results in linear filtering and prediction theory.  _ASME Transactions–Journal of Basics Engineering_ 83(Part D): 95–103.
  * Kappassov et al. (2020) Kappassov Z, Corrales JA and Perdereau V (2020) Touch driven controller and tactile features for physical interactions.  _Robotics and Autonomous Systems_ 123: 103332.
  * Kelestemur et al. (2022) Kelestemur T, Platt R and Padir T (2022) Tactile pose estimation and policy learning for unknown object manipulation.  In: _21st International Conference on Autonomous Agents and Multiagent Systems (AAMAS 2022)_. pp. 742–750.
  * Kopicki et al. (2011) Kopicki M, Zurek S, Stolkin R, Mörwald T and Wyatt J (2011) Learning to predict how rigid objects behave under simple manipulation.  In: _2011 IEEE international conference on robotics and automation_. pp. 5722–5729.
  * Krivic and Piater (2019) Krivic S and Piater J (2019) Pushing corridors for delivering unknown objects with a mobile robot.  _Autonomous Robots_ 43: 1435–1452.
  * Lee and Cutkosky (1991) Lee SH and Cutkosky MR (1991) Fixture planning with friction.  _Journal of Engineering for Industry_ 113(3): 320–327.
  * Lepora (2021) Lepora NF (2021) Soft biomimetic optical tactile sensing with the TacTip: A review.  _IEEE Sensors Journal_ 21(19): 21131–21143.
  * Lepora et al. (2017) Lepora NF, Aquilina K and Cramphorn L (2017) Exploratory tactile servoing with active touch.  _IEEE Robotics and Automation Letters_ 2(2): 1156–1163.
  * Lepora et al. (2019) Lepora NF, Church A, De Kerckhove C, Hadsell R and Lloyd J (2019) From pixels to percepts: Highly robust edge perception and contour following using deep learning and an optical biomimetic tactile sensor.  _IEEE Robotics and Automation Letters_ 4(2): 2101–2107.
  * Lepora et al. (2022) Lepora NF, Lin Y, Money-Coomes B and Lloyd J (2022) DigiTac: A DIGIT-TacTip hybrid tactile sensor for comparing low-cost high-resolution robot touch.  _IEEE Robotics and Automation Letters_ 7(4): 9382–9388.
  * Lepora and Lloyd (2020) Lepora NF and Lloyd J (2020) Optimal deep learning for robot touch: Training accurate pose models of 3d surfaces and edges.  _IEEE Robotics & Automation Magazine_ 27(2): 66–77.
  * Lepora and Lloyd (2021) Lepora NF and Lloyd J (2021) Pose-based tactile servoing: Controlled soft touch using deep learning.  _IEEE Robotics & Automation Magazine_ 28(4): 43–55.
  * Li et al. (2018) Li JK, Lee WS and Hsu D (2018) Push-net: Deep planar pushing for objects with unknown physical properties.  In: _Robotics: Science and Systems_ , volume 14. pp. 1–9.
  * Li et al. (2020) Li Q, Kroemer O, Su Z, Veiga FF, Kaboli M and Ritter HJ (2020) A review of tactile information: Perception and action through touch.  _IEEE Transactions on Robotics_ 36(6): 1619–1634.
  * Li et al. (2013) Li Q, Schürmann C, Haschke R and Ritter HJ (2013) A control framework for tactile servoing.  In: _Robotics: Science and systems_. Citeseer.  10.15607/RSS.2013.IX.045.
  * Lloyd and Lepora (2021) Lloyd J and Lepora NF (2021) Goal-driven robotic pushing using tactile and proprioceptive feedback.  _IEEE Transactions on Robotics_ 38(2): 1201–1212.
  * Lloyd et al. (2021) Lloyd J, Lin Y and Lepora NF (2021) Probabilistic Discriminative Models address the Tactile Perceptual Aliasing Problem.  In: _Proceedings of Robotics: Science and Systems_.  10.15607/RSS.2021.XVII.057.
  * Luo et al. (2017) Luo S, Bimbo J, Dahiya R and Liu H (2017) Robotic tactile perception of object properties: A review.  _Mechatronics_ 48: 54–67.
  * Lynch et al. (1992) Lynch K, Maekawa H and Tanie K (1992) Manipulation and active sensing by pushing using tactile feedback.  In: _Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems_ , volume 1. pp. 416–421.
  * Lynch and Mason (1996) Lynch KM and Mason MT (1996) Stable pushing: Mechanics, controllability, and planning.  _The international journal of robotics research_ 15(6): 533–556.
  * Lynch and Park (2017) Lynch KM and Park FC (2017) _Modern robotics_.  Cambridge University Press.
  * Makansi et al. (2019) Makansi O, Ilg E, Cicek O and Brox T (2019) Overcoming limitations of mixture density networks: A sampling and fitting framework for multimodal future prediction.  In: _Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition_. pp. 7144–7153.
  * Martinez-Hernandez et al. (2017) Martinez-Hernandez U, Dodd TJ, Evans MH, Prescott TJ and Lepora NF (2017) Active sensorimotor control for tactile exploration.  _Robotics and Autonomous Systems_ 87: 15–27.
  * Martinez-Hernandez et al. (2013) Martinez-Hernandez U, Metta G, Dodd TJ, Prescott TJ, Natale L and Lepora NF (2013) Active contour following to explore object shape with robot touch.  In: _2013 World Haptics Conference (WHC)_. pp. 341–346.
  * Mason (1986) Mason MT (1986) Mechanics and planning of manipulator pushing operations.  _The International Journal of Robotics Research_ 5(3): 53–71.
  * Meier et al. (2016) Meier M, Walck G, Haschke R and Ritter HJ (2016) Distinguishing sliding from slipping during object pushing.  In: _2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. pp. 5579–5584.
  * Murray et al. (2017) Murray RM, Li Z and Sastry SS (2017) _A mathematical introduction to robotic manipulation_.  CRC press.
  * Pestell et al. (2022) Pestell N, Griffith T and Lepora NF (2022) Artificial SA-I and RA-I afferents for tactile sensing of ridges and gratings.  _Journal of the Royal Society Interface_ 19(189): 20210822.
  * Petersen et al. (2008) Petersen KB, Pedersen MS et al. (2008) The matrix cookbook.  _Technical University of Denmark_ 7(15): 510.
  * Särkkä (2013) Särkkä S (2013) _Bayesian filtering and smoothing_.  3\. Cambridge university press.
  * Sikka et al. (2005) Sikka P, Zhang H and Sutphen S (2005) Tactile servo: Control of touch-driven robot motion.  In: _Experimental Robotics III: The 3rd International Symposium, Kyoto, Japan, October 28–30, 1993_. Springer, pp. 219–233.
  * Silverman (2018) Silverman BW (2018) _Density estimation for statistics and data analysis._ Routledge.
  * Simon (2015) Simon C (2015) Generating uniformly distributed numbers on a sphere.  http://corysimon.github.io/articles/uniformdistn-on-sphere/.  Accessed: 2022-11-23.
  * Smith et al. (2003) Smith P, Drummond T and Roussopoulos K (2003) Computing map trajectories by representing, propagating and combining pdfs over groups.  In: _ICCV_. pp. 1275–1282.
  * Sola et al. (2018) Sola J, Deray J and Atchuthan D (2018) A micro Lie theory for state estimation in robotics.  _arXiv preprint arXiv:1812.01537_ .
  * Srivastava et al. (2014) Srivastava N, Hinton G, Krizhevsky A, Sutskever I and Salakhutdinov R (2014) Dropout: a simple way to prevent neural networks from overfitting.  _The journal of machine learning research_ 15(1): 1929–1958.
  * Stüber et al. (2020) Stüber J, Zito C and Stolkin R (2020) Let’s push things forward: A survey on robot pushing.  _Frontiers in Robotics and AI_ : 8.
  * Suresh et al. (2021) Suresh S, Bauza M, Yu KT, Mangelson JG, Rodriguez A and Kaess M (2021) Tactile slam: Real-time inference of shape and pose from planar pushing.  In: _2021 IEEE International Conference on Robotics and Automation (ICRA)_. pp. 11322–11328.
  * Sutanto et al. (2019) Sutanto G, Ratliff N, Sundaralingam B, Chebotar Y, Su Z, Handa A and Fox D (2019) Learning latent space dynamics for tactile servoing.  In: _2019 International Conference on Robotics and Automation (ICRA)_. pp. 3622–3628.
  * Villalonga et al. (2021) Villalonga MB, Rodriguez A, Lim B, Valls E and Sechopoulos T (2021) Tactile object pose estimation from the first touch with geometric contact rendering.  In: _Conference on Robot Learning_. PMLR, pp. 1015–1029.
  * Ward-Cherrier et al. (2018) Ward-Cherrier B, Pestell N, Cramphorn L, Winstone B, Giannaccini ME, Rossiter J and Lepora NF (2018) The TacTip family: Soft optical tactile sensors with 3d-printed biomimetic morphologies.  _Soft robotics_ 5(2): 216–227.
  * Yu et al. (2016) Yu KT, Bauza M, Fazeli N and Rodriguez A (2016) More than a million ways to be pushed. a high-fidelity experimental dataset of planar pushing.  In: _2016 IEEE/RSJ international conference on intelligent robots and systems (IROS)_. pp. 30–37.
  * Yuan et al. (2017) Yuan W, Dong S and Adelson EH (2017) GelSight: High-resolution robot tactile sensors for estimating geometry and force.  _Sensors_ 17(12): 2762.
  * Yuan et al. (2015) Yuan W, Li R, Srinivasan MA and Adelson EH (2015) Measurement of shear and slip with a gelsight tactile sensor.  In: _2015 IEEE International Conference on Robotics and Automation (ICRA)_. pp. 304–311.
  * Zhang and Chen (2000) Zhang H and Chen NN (2000) Control of contact via tactile sensing.  _IEEE Transactions on Robotics and Automation_ 16(5): 482–495.
  * Zhou et al. (2018) Zhou J, Mason MT, Paolini R and Bagnell D (2018) A convex polynomial model for planar sliding mechanics: theory, application, and experimental validation.  _The International Journal of Robotics Research_ 37(2-3): 249–265.

##  Appendix A Notation and mathematical preliminaries

In this appendix, we define our notation and give some basic properties of
matrix Lie groups and algebras, focussing on the Special Euclidean group
$SE(3)$ of rotations and translations in 3D. We also describe how we represent
probability distributions in $SE(3)$. A more comprehensive introduction to Lie
groups applied to robotics can be found in Barfoot (2017); Sola et al. (2018).

A _Lie group_ is a group that is also a smooth, differentiable manifold.
Hence, the group composition and inversion operations are smooth,
differentiable operations. A _matrix Lie group_ $G$ is a smooth manifold in
the set of $\mathbb{R}^{n\times n}$ matrices that is closed under composition
and where the composition and inversion operations are matrix multiplication
and inversion, respectively. The group identity is the $n\times n$ identity
matrix $\boldsymbol{\mathrm{1}}_{n\times n}$. In this paper, we focus on the
Special Euclidean Group of rotations and translations in 3D, $SE(3)$, because
it can be used to represent poses and shears, pose-and-shear transformations
or changes of coordinate frame in 3D. The elements of $SE(3)$ can be
represented as

$SE(3)\,=\,\\{\boldsymbol{\mathrm{X}}=\begin{bmatrix}\boldsymbol{\mathrm{C}}&\boldsymbol{\mathrm{r}}\\\
\boldsymbol{\mathrm{0}}^{\top}&1\end{bmatrix}:\,\boldsymbol{\mathrm{C}}\in
SO(3),\,\boldsymbol{\mathrm{r}}\in\mathbb{R}^{3}\\},$ (26)

where $\boldsymbol{\mathrm{C}}$ is a $3\times 3$ orthonormal matrix
($\boldsymbol{\mathrm{C}}^{\top}\boldsymbol{\mathrm{C}}=\boldsymbol{\mathrm{C}}\boldsymbol{\mathrm{C}}^{\top}=\boldsymbol{\mathrm{1}}_{3\times
3}$) giving the rotational component of the transformation, and the column
vector $\boldsymbol{\mathrm{r}}$ gives the translational component.

Because Lie groups are manifolds, they have tangent spaces, which at the
origin is called the _Lie algebra_ , representing directions of motion in the
group. For $SE(3)$, the Lie algebra $\mathfrak{se}(3)$ is isomorphic to a
6-dimensional vector space $\mathbb{R}^{6}$, representing the three
translational and three rotational degrees of freedom. Elements
$\boldsymbol{\Xi}$ of the Lie algebra map onto corresponding elements
$\boldsymbol{\mathrm{X}}$ of the Lie group via the _exponential map_ ,
$\exp(\boldsymbol{\Xi})$, where:

$\boldsymbol{\mathrm{X}}\,=\,\exp(\boldsymbol{\Xi}),\hskip
20.00003pt\boldsymbol{\Xi}\,=\,\ln(\boldsymbol{\mathrm{X}}).$ (27)

This exponential function is defined by an infinite series analogous to the
corresponding scalar exponential series, where the successive powers of
$\boldsymbol{\Xi}$ are found recursively by matrix multiplication. For the
inverse map, elements of the matrix group are mapped into the Lie algebra
using the _logarithmic map_ , $\ln(\boldsymbol{\mathrm{X}})$, defined by an
infinite series analogous to the corresponding scalar logarithmic series.

We use the notation $\left[\cdot\right]^{\wedge}$ to represent mapping an
element $\boldsymbol{\xi}$ of the Euclidean vector space onto its
corresponding element $\boldsymbol{\Xi}$ of the Lie algebra. For
$\mathfrak{se}(3)$, this operation is defined as:

$\boldsymbol{\Xi}\,=\,\boldsymbol{\xi}^{\wedge}\,=\,\begin{bmatrix}\boldsymbol{\rho}\\\
\boldsymbol{\phi}\end{bmatrix}^{\wedge}\,=\,\begin{bmatrix}\boldsymbol{\phi}^{\wedge}&\boldsymbol{\rho}\\\
\boldsymbol{\mathrm{0}}^{\top}&0\end{bmatrix},$ (28)

where $\boldsymbol{\rho},\boldsymbol{\phi}\in\mathbb{R}^{3}$ represents the
three translational and three rotational components of $\boldsymbol{\xi}$,
with $\boldsymbol{\phi}^{\wedge}$ the skew-symmetric matrix representation of
$\boldsymbol{\phi}$:

$\boldsymbol{\phi}^{\wedge}\,=\,\begin{bmatrix}\phi_{1}\\\ \phi_{2}\\\
\phi_{3}\end{bmatrix}^{\wedge}\,=\,\begin{bmatrix}0&-\phi_{3}&\phi_{2}\\\
\phi_{3}&0&-\phi_{1}\\\ -\phi_{2}&\phi_{1}&0\end{bmatrix}.$ (29)

In this paper, we follow the convention adopted in Barfoot (2017) and Murray
et al. (2017) and use the first three components of $\boldsymbol{\xi}$ to
represent $\boldsymbol{\rho}$ and the last three components to represent
$\boldsymbol{\phi}$. This differs from the convention used in Lynch and Park
(2017), which reverses this order.

Note that we use the notation $\left[\cdot\right]^{\vee}$ to represent the
inverse mapping: $\boldsymbol{\xi}\,=\,\boldsymbol{\Xi}^{\vee}$. In robot
kinematics, $\boldsymbol{\xi}$ is often referred to as a _velocity twist_.

Elements of the Lie group $\boldsymbol{\mathrm{X}}\in G$ act upon elements of
the Lie algebra $\boldsymbol{\Xi}\in\mathfrak{g}$ using the _adjoint
representation_ ,
$\mathrm{Ad}_{\boldsymbol{\mathrm{X}}}\\!\left(\boldsymbol{\mathrm{X}}\right)\boldsymbol{\Xi}=\boldsymbol{\mathrm{X}}\boldsymbol{\Xi}\boldsymbol{\mathrm{X}}^{-1}$,
which for $SE(3)$ is represented by:

$\begin{split}\mathrm{Ad}\\!\left(\boldsymbol{\mathrm{X}}\right)&\,=\,\mathrm{Ad}\\!\left(\begin{bmatrix}\boldsymbol{\mathrm{C}}&\boldsymbol{\mathrm{r}}\\\
\boldsymbol{\mathrm{0}}^{\top}&1\end{bmatrix}\right)\,=\,\begin{bmatrix}\boldsymbol{\mathrm{C}}&\boldsymbol{\mathrm{r}}^{\wedge}\boldsymbol{\mathrm{C}}\\\
\boldsymbol{\mathrm{0}}_{3\times
3}&\boldsymbol{\mathrm{C}}\end{bmatrix}.\end{split}$ (30)

acting on the $\mathbb{R}^{6}$ vector space representation
$\boldsymbol{\xi}=\left[\boldsymbol{\rho},\boldsymbol{\phi}\right]^{\top}$ of
$\mathfrak{se}(3)$. Then the adjoint representations of the group composition
and inverse operations are
$\mathrm{Ad}\\!\left(\boldsymbol{\mathrm{X}}_{1}\boldsymbol{\mathrm{X}}_{2}\right)=\mathrm{Ad}\\!\left(\boldsymbol{\mathrm{X}}_{1}\right)\mathrm{Ad}\\!\left(\boldsymbol{\mathrm{X}}_{2}\right)$
and
$\mathrm{Ad}\\!\left(\boldsymbol{\mathrm{X}}\right)^{-1}=\mathrm{Ad}\\!\left(\boldsymbol{\mathrm{X^{-1}}}\right)$.
Likewise, the corresponding adjoint representation of elements in the Lie
algebra $\boldsymbol{\Xi}\in\mathfrak{g}$ is denoted as
$\mathrm{ad}\\!\left(\boldsymbol{\Xi}\right)$ and for $\mathfrak{se}(3)$ is:

$\mathrm{ad}\\!\left(\boldsymbol{\Xi}\right)=\boldsymbol{\xi}^{\curlywedge}=\mathrm{ad}\\!\left(\boldsymbol{\xi}^{\wedge}\right)=\mathrm{ad}\\!\left(\begin{bmatrix}\boldsymbol{\rho}\\\
\boldsymbol{\phi}\end{bmatrix}^{\wedge}\right)=\begin{bmatrix}\boldsymbol{\phi}^{\wedge}&\boldsymbol{\rho}^{\wedge}\\\
\boldsymbol{\mathrm{0}}&\boldsymbol{\phi}^{\wedge}\end{bmatrix},$ (31)

which acts on the $\mathbb{R}^{6}$ vector space representation
$\boldsymbol{\xi}=\left[\boldsymbol{\rho},\boldsymbol{\phi}\right]^{\top}$ of
$\mathfrak{se}(3)$ with components $\rho_{k}$ and $\phi_{k}$.

The product of two exponentials in $\mathfrak{se}(3)$ can be computed using
the following approximation, based on the _Baker-Campbell-Hausdorff_ (BCH)
formula (Barfoot (2017)):

$\ln\\!\big{(}\\!\exp(\boldsymbol{\Xi}_{1})\,\exp(\boldsymbol{\Xi}_{2})\big{)}^{\vee}\,=\,\ln\\!\big{(}\\!\exp(\boldsymbol{\xi}^{\wedge}_{1})\,\exp(\boldsymbol{\xi}^{\wedge}_{2})\big{)}^{\vee}\\\
\approx\begin{cases}\mathcal{J}(\boldsymbol{\xi}_{2})^{-1}\boldsymbol{\xi}_{1}+\boldsymbol{\xi}_{2}&\text{if
$\boldsymbol{\xi}_{1}$ small}\\\
\boldsymbol{\xi}_{1}+\mathcal{J}(-\boldsymbol{\xi}_{1})^{-1}\boldsymbol{\xi}_{2}&\text{if
$\boldsymbol{\xi}_{2}$ small}\\\ \end{cases}$ (32)

Here, $\mathcal{J}\in\mathbb{R}^{6\times 6}$ is the _left Jacobian_ of
$SE(3)$, which can be written as the following series expansion:

$\mathcal{J}\left(\boldsymbol{\xi}\right)=\sum_{n=0}^{\infty}\frac{\left(\boldsymbol{\xi}^{\curlywedge}\right)^{n}}{\left(n+1\right)!},\
\
\boldsymbol{\xi}^{\curlywedge}=\mathrm{ad}\left(\boldsymbol{\xi}^{\wedge}\right)=\begin{bmatrix}\boldsymbol{\phi}^{\wedge}&\boldsymbol{\rho}^{\wedge}\\\
\boldsymbol{\mathrm{0}}&\boldsymbol{\phi}^{\wedge}\end{bmatrix}.$ (33)

Similarly, the inverse (left) Jacobian can be written as another series
expansion:

$\mathcal{J}\left(\boldsymbol{\xi}\right)^{-1}\,=\,\sum_{n=0}^{\infty}\frac{B_{n}}{n!}\left(\boldsymbol{\xi}^{\curlywedge}\right)^{n},$
(34)

where $B_{n}$ are the _Bernoulli numbers_ ,
$B_{0}=1,\,B_{1}=-\frac{1}{2},\,B_{2}=\frac{1}{6},\,B_{3}=0,\,B_{4}=-\frac{1}{30},\,\cdots$.
In this paper, if we need to calculate the Jacobian or its inverse, we
typically truncate the corresponding series after second-order terms because
we have found that this is sufficiently accurate for our purposes (this is
also consistent with the findings of Barfoot and Furgale (2014)). Note that it
is also possible to define a _right Jacobian_ of $SE(3)$ but we will not need
it here and so will just refer to $\mathcal{J}$ as the _Jacobian_.

##  Appendix B SE(3) random variables and probabilistic transformations

It is well known that the PDF of a linearly-transformed Gaussian random
variable with added Gaussian noise is itself Gaussian, and its parameters can
be computed analytically without having to explicitly evaluate a
marginalisation integral like the one in Equation 7. So, if we were using a
linear-Gaussian state dynamics model with our Bayesian filter, we could
efficiently compute the prediction step in Equation 7 in closed form. In this
section, we will derive an analogous simplification for $SE(3)$ random
variables.

Let us consider a random variable $\boldsymbol{\mathrm{X}}\in SE(3)$.
Following Barfoot and Furgale (2014); Barfoot (2017); Bourmaud et al. (2016),
we define a PDF:

$\boldsymbol{\mathrm{X}}=\exp({\boldsymbol{\epsilon}}^{\wedge})\bar{\boldsymbol{\mathrm{X}}};\hskip
10.00002pt\boldsymbol{\epsilon}\sim\mathcal{N}(\boldsymbol{\mathrm{0}},\boldsymbol{\Sigma}),\
\ \bar{\boldsymbol{\mathrm{X}}}\in SE(3),$ (35)

with deterministic mean $\bar{\boldsymbol{\mathrm{X}}}\in SE(3)$ left-
multiplied by a small zero-mean Gaussian perturbation
$\boldsymbol{\epsilon}\in\mathbb{R}^{6}$ with covariance matrix
$\boldsymbol{\Sigma}$. The Gaussian perturbation $\boldsymbol{\epsilon}$
induces a non-Gaussian PDF over $\boldsymbol{\mathrm{X}}\\!\in\\!SE(3)$
(Barfoot and Furgale (2014))

$p\left(\boldsymbol{\mathrm{X}}\right)\,=\,\beta\left(\boldsymbol{\epsilon}\right)\exp\left(-\textstyle\frac{1}{2}\boldsymbol{\epsilon}^{\top}\boldsymbol{\mathrm{\Sigma}}^{-1}\boldsymbol{\epsilon}\right),$
(36)

where
$\boldsymbol{\epsilon}=\ln\big{(}\boldsymbol{\mathrm{X}}\bar{\boldsymbol{\mathrm{X}}}^{-1}\big{)}^{\vee}$
and
$\beta\left(\boldsymbol{\epsilon}\right)={\eta}/{\left|\mathrm{det}(\mathcal{J}(\boldsymbol{\epsilon}))\right|}$.
The non-constant normalisation factor
$\beta\left(\boldsymbol{\epsilon}\right)$ originates from the relationship
between infinitesimal volume elements in $\mathfrak{se}(3)$ and $SE(3)$
(Barfoot (2017)):
$d\boldsymbol{\mathrm{X}}\,=\,\left|\mathrm{det}(\mathcal{J}(\boldsymbol{\epsilon}))\right|\,d\boldsymbol{\epsilon}$,
which is the reason for the non-Gaussianity.

Sometimes, it is necessary to map $\boldsymbol{\mathrm{X}}$ to a random
variable $\boldsymbol{\xi}\in\mathbb{R}^{6}$ in the vector space
representation of the Lie algebra, and find its PDF. To do this, we first map
$\boldsymbol{\mathrm{X}}$ to the Lie algebra using the logarithmic map and
then approximate it using the BCH formula (Equation 32):

$\boldsymbol{\xi}\,=\,\ln(\boldsymbol{\mathrm{X}})^{\vee}\,\approx\,\boldsymbol{\mu}+\mathcal{J}\left(\boldsymbol{\mu}\right)^{-1}\boldsymbol{\epsilon},$
(37)

where
$\boldsymbol{\mu}=\ln(\bar{\boldsymbol{\mathrm{X}}})^{\vee}\in\mathbb{R}^{6}$.
Then, noting that the second term on the right hand side is just a linear
transform of the Gaussian random variable $\boldsymbol{\epsilon}$, we find
that the PDF of $\boldsymbol{\xi}$ is:

$\boldsymbol{\xi}\,\sim\,\mathcal{N}(\boldsymbol{\mu},\,\mathcal{J}^{-1}(\boldsymbol{\mu})\,\boldsymbol{\Sigma}\,\mathcal{J}^{-\top}(\boldsymbol{\mu})),$
(38)

which is also approximately Gaussian. In the methodology (Section 3.2.2), we
use this expression to map the sensor observations from a distribution defined
by $\boldsymbol{\mu}\in\mathbb{R}^{6}$ with covariance matrix
$\boldsymbol{\Sigma}$ to a distribution defined by
$\bar{\boldsymbol{\mathrm{X}}}\in SE(3)$ with perturbation covariance matrix
$\mathcal{J}(\boldsymbol{\mu})\,\boldsymbol{\Sigma}\,\mathcal{J}^{\top}(\boldsymbol{\mu})$.

Another useful operation is to compound an $SE(3)$ random variable
$\boldsymbol{\mathrm{X}}$ with distribution parameters
$\bar{\boldsymbol{\mathrm{X}}}$ and $\boldsymbol{\Sigma}$ (Equation 35) with a
second $SE(3)$ variable sampled from another PDF:

$\boldsymbol{\mathrm{T}}=\exp({\boldsymbol{\phi}}^{\wedge})\bar{\boldsymbol{\mathrm{T}}};\hskip
10.00002pt\boldsymbol{\phi}\sim\mathcal{N}(\boldsymbol{\mathrm{0}},\boldsymbol{\Sigma}_{\boldsymbol{\phi}}),\
\ \bar{\boldsymbol{\mathrm{T}}}\in SE(3),$ (39)

where $\bar{\boldsymbol{\mathrm{T}}}$, the deterministic $SE(3)$ mean, is
close to $\mathbf{1}$, and $\boldsymbol{\phi}$ is a second small Gaussian
perturbation with covariance matrix $\boldsymbol{\Sigma}_{\boldsymbol{\phi}}$.
For
$\boldsymbol{\mathrm{X}}^{\prime}=\boldsymbol{\mathrm{T}}\boldsymbol{\mathrm{X}}$,
the transformed PDF is to first order in the covariance matrix (see Barfoot
and Furgale (2014) for a higher order treatment):

$\boldsymbol{\mathrm{X}}^{\prime}=\exp\left({\boldsymbol{\epsilon}}^{\prime\wedge}\right)\bar{\boldsymbol{\mathrm{X}}}^{\prime};\hskip
10.00002pt\boldsymbol{\epsilon^{\prime}}\sim\mathcal{N}(\boldsymbol{\mathrm{0}},\boldsymbol{\Sigma}^{\prime}),\
\ \bar{\boldsymbol{\mathrm{X}}}^{\prime}\in SE(3),$ (40)

with corresponding $SE(3)$ mean and covariance matrix:

$\bar{\boldsymbol{\mathrm{X}}}^{\prime}=\bar{\boldsymbol{\mathrm{T}}}\bar{\boldsymbol{\mathrm{X}}},\
\ \ \
\boldsymbol{\Sigma}^{\prime}=\mathrm{Ad}(\bar{\boldsymbol{\mathrm{T}}})\,\boldsymbol{\Sigma}\,\mathrm{Ad}(\bar{\boldsymbol{\mathrm{T}}})^{\top}+\boldsymbol{\Sigma}_{\boldsymbol{\phi}}.$
(41)

In the methodology (Section 3.2.2), we use this expression to give the
prediction step in the $SE(3)$ discriminative Bayesian filter (Algorithm 2).

Equation 41 is derived by composing Equation 35 on the left with a
deterministic $SE(3)$ transform $\bar{\boldsymbol{\mathrm{T}}}$ to get another
random variable of the same form (Barfoot and Furgale (2014); Barfoot (2017)):

$\bar{\boldsymbol{\mathrm{T}}}\boldsymbol{\mathrm{X}}\,=\,\bar{\boldsymbol{\mathrm{T}}}\exp\left(\boldsymbol{\epsilon}^{\wedge}\right)\bar{\boldsymbol{\mathrm{X}}}\,=\,\exp\left(\left(\mathrm{Ad}(\bar{\boldsymbol{\mathrm{T}}})\boldsymbol{\epsilon}\right)^{\wedge}\right)\bar{\boldsymbol{\mathrm{T}}}\bar{\boldsymbol{\mathrm{X}}}.$

The probabilistic transformation to the new random variable
$\boldsymbol{\mathrm{X}}^{\prime}$ is completed by adding some Gaussian noise
$\boldsymbol{\phi}\sim\mathcal{N}(\boldsymbol{\mathrm{0}},\,\boldsymbol{\Sigma}_{\boldsymbol{\phi}})$
to the transformed perturbation
$\mathrm{Ad}(\bar{\boldsymbol{\mathrm{T}}})\boldsymbol{\epsilon}$:

$\boldsymbol{\mathrm{X}}^{\prime}\,=\,\exp\left(\left(\mathrm{Ad}(\bar{\boldsymbol{\mathrm{T}}})\boldsymbol{\epsilon}+\boldsymbol{\phi}\right)^{\wedge}\right)\bar{\boldsymbol{\mathrm{T}}}\bar{\boldsymbol{\mathrm{X}}}.$

Therefore, the transformed random variable can be written:

$\boldsymbol{\mathrm{X}}^{\prime}=\exp\left({\boldsymbol{\epsilon}}^{\prime\wedge}\right)\bar{\boldsymbol{\mathrm{X}}}^{\prime},\
\ \
\bar{\boldsymbol{\mathrm{X}}}^{\prime}=\bar{\boldsymbol{\mathrm{T}}}\bar{\boldsymbol{\mathrm{X}}},\
\ \
\boldsymbol{\epsilon}^{\prime}=\mathrm{Ad}(\bar{\boldsymbol{\mathrm{T}}})\,\boldsymbol{\epsilon}+\boldsymbol{\phi}.$

Since
$\boldsymbol{\epsilon}^{\prime}=\mathrm{Ad}(\bar{\boldsymbol{\mathrm{T}}})\,\boldsymbol{\epsilon}+\boldsymbol{\phi}$
represents a linear transformation of a Gaussian random variable
$\boldsymbol{\epsilon}$ with added Gaussian noise $\boldsymbol{\phi}$, and
both are small, it also has a Gaussian distribution:

${\boldsymbol{\epsilon}}^{\prime}\,\sim\,\mathcal{N}(\boldsymbol{\mathrm{0}},\,\mathrm{Ad}(\bar{\boldsymbol{\mathrm{T}}})\,\boldsymbol{\Sigma}\,\mathrm{Ad}(\bar{\boldsymbol{\mathrm{T}}})^{\top}+\boldsymbol{\Sigma}_{\boldsymbol{\phi}}),$
(42)

which completes the derivation of Equation 41.

##  Appendix C SE(3) probabilistic data fusion

In the correction step of our discriminative Bayesian filter, we need to fuse
two PDFs by computing their normalised product (Equation 11). For the
multivariate Gaussian case, we can do this by computing the normalised product
of two Gaussian PDFs, which is also a Gaussian PDF. However, in $SE(3)$, the
PDFs are defined over a curved manifold instead of a Euclidean vector space,
which necessitates an iterative approach to solve this problem (Barfoot and
Furgale (2014); Bourmaud et al. (2016); Smith et al. (2003)).

Suppose that our $SE(3)$ state PDFs have the form described in Appendix B:

$p\left(\boldsymbol{\mathrm{X}}\right)\,=\,\beta\left(\boldsymbol{\epsilon}\right)\exp\left(-\textstyle\frac{1}{2}\boldsymbol{\epsilon}^{\top}\boldsymbol{\mathrm{\Sigma}}^{-1}\boldsymbol{\epsilon}\right),$

where
$\boldsymbol{\epsilon}=\ln\big{(}\boldsymbol{\mathrm{X}}\bar{\boldsymbol{\mathrm{X}}}^{-1}\big{)}^{\\!\vee}$
and
$\beta\left(\boldsymbol{\epsilon}\right)=\eta/\left|\mathrm{det}(\mathcal{J}(\boldsymbol{\epsilon}))\right|$.
This PDF can then be approximated by (Bourmaud et al. (2016)):

$p\left(\boldsymbol{\mathrm{X}}\right)\approx\eta\exp\left(-\textstyle\frac{1}{2}\ln\left(\boldsymbol{\mathrm{X}}\bar{\boldsymbol{\mathrm{X}}}^{-1}\right)^{\vee\top}\boldsymbol{\mathrm{\Sigma}}^{-1}\ln\left(\boldsymbol{\mathrm{X}}\bar{\boldsymbol{\mathrm{X}}}^{-1}\right)^{\vee}\right).$
(43)

Here, we replace the variable normalisation factor
$\beta\left(\boldsymbol{\epsilon}\right)$ with the constant normalisation
factor $\eta$, because from Equation 33,
$\mathcal{J}(\boldsymbol{\epsilon})=\boldsymbol{\mathrm{1}}+\frac{1}{2}\epsilon^{\curlywedge}+\dots\approx\boldsymbol{\mathrm{1}}$
if the perturbation covariance matrix is sufficiently small (i.e., the maximum
eigenvalue is sufficiently small).

This _concentrated Gaussian on a Lie group_ assumption is made explicitly in
Bourmaud et al. (2016) and is implicit in the data fusion algorithm described
in Barfoot and Furgale (2014) and Barfoot (2017). In the latter case, the
assumption is implied by the authors’ choice of Mahalanobis cost function that
they minimise to solve the fusion problem. The reason this assumption is
required for probabilistic fusion in $SE(3)$ is that a normalised product of
Lie group PDFs is not, in general, equal to another Lie group PDF of the same
form (in contrast to the normalised product of Gaussian PDFs in a Euclidean
vector space). In fact, the normalised product of two Gaussian Lie group PDFs
can only be _approximated_ by another PDF of that form.

Using the concentrated Gaussian on a Lie group assumption, the normalised
product of two $SE(3)$ PDFs is approximately:

$p_{*}\\!\left(\boldsymbol{\mathrm{X}}\right)\,=\,\alpha\,p_{1}\\!\left(\boldsymbol{\mathrm{X}}\right)\,p_{2}\\!\left(\boldsymbol{\mathrm{X}}\right)\\\
\,\approx\,\alpha\eta_{1}\eta_{2}\,\exp\left(-\textstyle\frac{1}{2}\ln\left(\boldsymbol{\mathrm{X}}\bar{\boldsymbol{\mathrm{X}}}_{1}^{-1}\right)^{\vee\top}\\!\boldsymbol{\mathrm{\Sigma}}_{1}^{-1}\ln\left(\boldsymbol{\mathrm{X}}\bar{\boldsymbol{\mathrm{X}}}_{1}^{-1}\right)^{\vee}\right)\\\
\times\exp\left(-\textstyle\frac{1}{2}\ln\left(\boldsymbol{\mathrm{X}}\bar{\boldsymbol{\mathrm{X}}}_{2}^{-1}\right)^{\vee\top}\\!\boldsymbol{\mathrm{\Sigma}}_{2}^{-1}\ln\left(\boldsymbol{\mathrm{X}}\bar{\boldsymbol{\mathrm{X}}}_{2}^{-1}\right)^{\vee}\right),$
(44)

where $\alpha$ is a normalisation factor that ensures the product PDF
integrates to $1$ over its support. It can then be shown that this product of
$SE(3)$ PDFs can be written as

$p_{*}\\!\left(\boldsymbol{\epsilon}\right)\,\approx\,\alpha\eta_{1}\eta_{2}\,\exp\left(-\textstyle\frac{1}{2}\left(\boldsymbol{\epsilon}-\boldsymbol{\mu}_{1}^{\prime}\right)^{\top}{\boldsymbol{\mathrm{\Sigma}}}_{1}^{\prime-1}\left(\boldsymbol{\epsilon}-\boldsymbol{\mu}_{1}^{\prime}\right)\right)\\\
\times\exp\left(-\textstyle\frac{1}{2}\left(\boldsymbol{\epsilon}-\boldsymbol{\mu}_{2}^{\prime}\right)^{\top}{\boldsymbol{\mathrm{\Sigma}}}_{2}^{\prime-1}\left(\boldsymbol{\epsilon}-\boldsymbol{\mu}_{2}^{\prime}\right)\right),$
(45)

where the two means and convariance matrices are

$\displaystyle\boldsymbol{\mu}_{1}^{\prime}=-\mathcal{J}\left(\boldsymbol{\xi}_{1}\right)\boldsymbol{\xi}_{1},\
\ \ \
\boldsymbol{\mathrm{\Sigma}}_{1}^{\prime}=\mathcal{J}\left(\boldsymbol{\xi}_{1}\right)\boldsymbol{\mathrm{\Sigma}}_{1}\mathcal{J}\left(\boldsymbol{\xi}_{1}\right)^{\top},$
(46)
$\displaystyle\boldsymbol{\mu}_{2}^{\prime}=-\mathcal{J}\left(\boldsymbol{\xi}_{2}\right)\boldsymbol{\xi}_{2},\
\ \ \
\boldsymbol{\mathrm{\Sigma}}_{2}^{\prime}=\mathcal{J}\left(\boldsymbol{\xi}_{2}\right)\boldsymbol{\mathrm{\Sigma}}_{2}\mathcal{J}\left(\boldsymbol{\xi}_{2}\right)^{\top}.$
(47)

where
$\boldsymbol{\xi}_{1}=\ln\big{(}\bar{\boldsymbol{\mathrm{X}}}\bar{\boldsymbol{\mathrm{X}}}_{1}^{-1}\big{)}^{\\!\vee}$,
$\boldsymbol{\xi}_{2}=\ln\big{(}\bar{\boldsymbol{\mathrm{X}}}\bar{\boldsymbol{\mathrm{X}}}_{2}^{-1}\big{)}^{\\!\vee}$.

To show this, we consider a change of (random) variable using
$\boldsymbol{\mathrm{X}}\,=\,\exp(\boldsymbol{\epsilon}^{\wedge})\,\bar{\boldsymbol{\mathrm{X}}}$
and
$\boldsymbol{\epsilon}\,=\,\ln\big{(}\boldsymbol{\mathrm{X}}\bar{\boldsymbol{\mathrm{X}}}^{-1}\big{)}^{\vee}$,
so that:

$\displaystyle\boldsymbol{\mathrm{X}}\bar{\boldsymbol{\mathrm{X}}}_{1}^{-1}=\exp(\boldsymbol{\epsilon}^{\wedge})\,\bar{\boldsymbol{\mathrm{X}}}\bar{\boldsymbol{\mathrm{X}}}_{1}^{-1}=\exp(\boldsymbol{\epsilon}^{\wedge})\,\exp(\boldsymbol{\xi}_{1}^{\wedge}),$
$\displaystyle\boldsymbol{\mathrm{X}}\bar{\boldsymbol{\mathrm{X}}}_{2}^{-1}=\exp(\boldsymbol{\epsilon}^{\wedge})\,\bar{\boldsymbol{\mathrm{X}}}\bar{\boldsymbol{\mathrm{X}}}_{2}^{-1}=\exp(\boldsymbol{\epsilon}^{\wedge})\,\exp(\boldsymbol{\xi}_{2}^{\wedge}).$

Since $\boldsymbol{\epsilon}$ is small because of the concentrated Gaussian on
a Lie group assumption, we can use the BCH approximation (Equation 32) to
write:

$\begin{gathered}\ln\big{(}\boldsymbol{\mathrm{X}}\bar{\boldsymbol{\mathrm{X}}}^{-1}\big{)}^{\vee}\approx\mathcal{J}\left(\boldsymbol{\xi}\right)^{-1}\boldsymbol{\epsilon}+\boldsymbol{\xi}=\mathcal{J}\left(\boldsymbol{\xi}\right)^{-1}\left(\boldsymbol{\epsilon}+\mathcal{J}\left(\boldsymbol{\xi}\right)\boldsymbol{\xi}\right).\end{gathered}$

Substituting this expression in Equation 44 and rearranging terms, we get
Equations 45-47.

Equation 45 is recognizable as a normalized product of Gaussian PDFs, which is
itself equal to a Gaussian PDF (Bromiley (2003); Petersen et al. (2008)):

$p_{*}\\!\left(\boldsymbol{\epsilon}\right)\,\approx\,\eta_{*}\,\exp\left(-\textstyle\frac{1}{2}\left(\boldsymbol{\epsilon}-\boldsymbol{\mu}_{*}^{\prime}\right)^{\top}{\boldsymbol{\mathrm{\Sigma}}}_{*}^{\prime-1}\left(\boldsymbol{\epsilon}-\boldsymbol{\mu}_{*}^{\prime}\right)\right),$
(48)

with normalization factor $\eta_{*}=\alpha\eta_{1}\eta_{2}\gamma$, and the
mean and covariance matrices are given by:

$\begin{gathered}{\boldsymbol{\mathrm{\mu}}}_{*}^{\prime}={\boldsymbol{\mathrm{\Sigma}}}_{*}^{\prime}\left({\boldsymbol{\mathrm{\Sigma}}}_{1}^{\prime-1}{\boldsymbol{\mathrm{\mu}}}_{1}^{\prime}+{\boldsymbol{\mathrm{\Sigma}}}_{2}^{\prime-1}{\boldsymbol{\mathrm{\mu}}}_{2}^{\prime}\right),\
{\boldsymbol{\mathrm{\Sigma}}}_{*}^{\prime}=\left({\boldsymbol{\mathrm{\Sigma}}}_{1}^{\prime-1}+{\boldsymbol{\mathrm{\Sigma}}}_{2}^{\prime-1}\right)^{-1}\\!\\!.\end{gathered}$
(49)

This scaled product of Gaussian PDFs represents a _non-zero-mean_ Gaussian
perturbation around a mean $\bar{\boldsymbol{\mathrm{X}}}_{*}\in SE(3)$.

Finally, we obtain an approximation for the normalised product PDF as a _zero-
mean_ Gaussian perturbation:

$p_{*}\left(\boldsymbol{\mathrm{X}}\right)\approx\eta_{*}\exp\left(-\textstyle\frac{1}{2}\ln\big{(}\boldsymbol{\mathrm{X}}\bar{\boldsymbol{\mathrm{X}}}_{*}^{-1}\big{)}^{\vee\top}\boldsymbol{\mathrm{\Sigma}}_{*}^{-1}\ln\big{(}\boldsymbol{\mathrm{X}}\bar{\boldsymbol{\mathrm{X}}}_{*}^{-1}\big{)}^{\vee}\right)$
(50)

where
$\boldsymbol{\xi}_{*}=\ln\big{(}\bar{\boldsymbol{\mathrm{X}}}\bar{\boldsymbol{\mathrm{X}}}_{*}^{-1}\big{)}^{\vee}$
defines:

$\begin{gathered}\bar{\boldsymbol{\mathrm{X}}}_{*}=\exp\left(\left(-\boldsymbol{\xi}_{*}\right)^{\wedge}\right)\bar{\boldsymbol{\mathrm{X}}},\
\ \ \
\boldsymbol{\mathrm{\Sigma}}_{*}=\mathcal{J}\left(\boldsymbol{\xi}_{*}\right)^{-1}\boldsymbol{\mathrm{\Sigma}}_{*}^{\prime}\mathcal{J}\left(\boldsymbol{\xi}_{*}\right)^{-\top}\\!\\!.\end{gathered}$
(51)

Under the concentrated Gaussian on a Lie group assumption, the infinitesimal
volume elements are related by $d\boldsymbol{\mathrm{X}}\approx
d\boldsymbol{\epsilon}$, and so by an analogous BCH approximation to above:

$\ln\big{(}\boldsymbol{\mathrm{X}}\bar{\boldsymbol{\mathrm{X}}}_{*}^{-1}\big{)}^{\\!\vee}\approx\mathcal{J}\\!\left(\boldsymbol{\xi}_{*}\right)^{\\!-1}\\!\boldsymbol{\epsilon}+\boldsymbol{\xi}_{*}\\\
=\mathcal{J}\\!\left(\boldsymbol{\xi}_{*}\right)^{\\!-1}\\!\left(\boldsymbol{\epsilon}+\mathcal{J}\\!\left(\boldsymbol{\xi}_{*}\right)\boldsymbol{\xi}_{*}\right)$

Substituting this in Equation 50 and rearranging terms, we can rewrite the
mean and covariance in Equation 49 as:

$\begin{gathered}\boldsymbol{\mu}_{*}^{\prime}=-\mathcal{J}\left(\boldsymbol{\xi}_{*}\right)\boldsymbol{\xi}_{*},\
\ \ \
\boldsymbol{\mathrm{\Sigma}}_{*}^{\prime}\,=\,\mathcal{J}\left(\boldsymbol{\xi}_{*}\right)\boldsymbol{\mathrm{\Sigma}}_{*}\mathcal{J}\left(\boldsymbol{\xi}_{*}\right)^{\top}.\end{gathered}$
(52)

These are established by reversing the steps used in the initial change of
variables, by substituting analogous expressions to those of Equations 46,47.

Now, let us consider the $SE(3)$ data fusion algorithm, which we construct in
the form of a fixed-point iterative method because we cannot compute the
deterministic mean $\bar{\boldsymbol{\mathrm{X}}}_{*}$ in closed form, since
it would require solving a non-linear equation in $\boldsymbol{\xi}_{*}$ and
$\boldsymbol{\mu}_{*}^{\prime}$ (Equation 52). Therefore, we approximate the
Jacobian in this equation as
$\mathcal{J}\left(\boldsymbol{\xi}_{*}\right)\approx\boldsymbol{\mathrm{1}}$
(which becomes more accurate as $\boldsymbol{\xi}_{*}$ becomes smaller) and
then substitute for $\boldsymbol{\xi}_{*}$ in Equation 51 to obtain an
approximate solution:

$\begin{gathered}\bar{\boldsymbol{\mathrm{X}}}_{*}\,\approx\,\exp\left(\boldsymbol{\mu}_{*}^{\prime\wedge}\right)\bar{\boldsymbol{\mathrm{X}}},\
\ \ \
\boldsymbol{\mathrm{\Sigma}}_{*}\,\approx\,\boldsymbol{\mathrm{\Sigma}}_{*}^{\prime}.\end{gathered}$
(53)

We solve for $\boldsymbol{\mu}_{*}^{\prime}$ by updating the operating point
$\bar{\boldsymbol{\mathrm{X}}}$ to equal our approximation
$\bar{\boldsymbol{\mathrm{X}}}_{*}$, and then iterate until convergence:

$\bar{\boldsymbol{\mathrm{X}}}\,\leftarrow\,\exp\left(\boldsymbol{\mu}_{*}^{\prime\wedge}\right)\bar{\boldsymbol{\mathrm{X}}}.$
(54)

At convergence,
$\boldsymbol{\mu}_{*}^{\prime}\rightarrow\boldsymbol{\mathrm{0}}$ and we
obtain the mean and covariance parameters of the normalised product of $SE(3)$
PDFs from Equation 53 as
$\bar{\boldsymbol{\mathrm{X}}}_{*}\,=\,\bar{\boldsymbol{\mathrm{X}}}$ and
$\boldsymbol{\mathrm{\Sigma}}_{*}\,=\,\boldsymbol{\mathrm{\Sigma}}_{*}^{\prime}$,
which we found to typically converge in 3-4 steps, consistent with the
findings presented in Barfoot and Furgale (2014). Since we are operating in a
real-time context where we want our code to run with predictable timing, we
use 5 iterations.

To simplify the implementation, we expanded the solution in Equation 49 in
terms of the original factor PDF means and covariances, and in terms of the
inverse Jacobian. We computed the inverse Jacobian up to second order in its
series expansion (Equation 34), which we found was sufficiently accurate (once
again, this is consistent with findings presented in Barfoot and Furgale
(2014)). We also initialised the operating point
$\bar{\boldsymbol{\mathrm{X}}}$ to equal the mean of the left-hand factor PDF
of the product. Taken together, this results in Algorithm 3 of the methodology
(Section 3.2.2).

While we recognise that our data fusion algorithm is essentially identical to
the algorithm proposed in Barfoot and Furgale (2014), our method of deriving
it is very different. In our case, we follow an algebraic approach, where the
iteration arises in finding an approximate solution to a non-linear equation.
This contrasts with the original derivation, which is based on iterative
Gauss-Newton minimization of a Mahalanobis cost function. We believe that our
derivation offers some new insights that were not present in the original
derivation. For example, in Barfoot and Furgale (2014), the authors refer to
their method qualitatively as a “data fusion” method, but do not show that it
finds a solution to the normalised product of $SE(3)$ PDFs. However, finding
this product is essential for implementing the correction step of our $SE(3)$
discriminative Bayesian filter (Equation 11).

##  Appendix D Controller parameters

Table 6: Controller parameters (gain matrices, clipping ranges, feedback
reference pose and feedforward velocity) used in tactile servoing controller
for follower robot arm in object tracking experiments. The feedback reference
pose is expressed as a 6-vector, where the first three elements denote the 3D
position and the last three elements denote the 3D rotation in extrinsic-$xyz$
Euler format.

Parameter | Value
---|---
$\boldsymbol{\mathrm{K}}_{p}$ | $\mathrm{diag}(5,5,5,2,2,0)$
$\boldsymbol{\mathrm{K}}_{i}$ | $\mathrm{diag}(0.5,0.5,0.5,0.2,0.2,0.2)$
$\boldsymbol{\mathrm{K}}_{d}$ | $\mathrm{diag}(0.5,0.5,0.5,0.2,0.2,0.2)$
Integral error clipping | Not used
Feedback ref pose | $(0,0,6,0,0,0)$
Feedforward ref velocity | $(0,0,0,0,0,0)$

Table 7: Controller parameters used in tactile servoing controller for surface
following experiments. The feedback reference pose is expressed as a 6-vector,
as in Table 7.

Parameter | Value
---|---
$\boldsymbol{\mathrm{K}}_{p}$ | $\mathrm{diag}(0,0,2,2,2,0)$
$\boldsymbol{\mathrm{K}}_{i}$ | $\mathrm{diag}(0,0,0.1,0.1,0.1,0)$
$\boldsymbol{\mathrm{K}}_{d}$ | $\mathrm{diag}(0,0,0.05,0.05,0.05,0)$
Integral error clipping |  $\left[-25,25\right]$ all components
Feedback ref pose | $(0,0,3,0,0,0)$
Feedforward ref velocity | Task-dependent

Table 8: Pushing controller parameters used for single-arm and dual-arm
(leader) object pushing experiments. The feedback reference pose is expressed
as a 6-vector, as in Table 7.

Parameter | Value
---|---
PID 1 (MIMO) |
$\boldsymbol{\mathrm{K}}_{p}$ | $\mathrm{diag}(1,0,0,1,0,0)$
$\boldsymbol{\mathrm{K}}_{i}$ | $\mathrm{diag}(0.1,0,0,0.1,0,0)$
$\boldsymbol{\mathrm{K}}_{d}$ | $\mathrm{diag}(0.1,0,0,0.1,0,0)$
Integral error clipping |  $\left[-25,25\right]$ all components
Feedback ref pose | $(0,0,0,0,0,0)$
Feedforward ref velocity | $(0,0,10,0,0,0)$
PID 2 (SISO) |
$K_{p}$ | $0.9$
$K_{i}$ |  $0.3$ (single-arm) / $0.5$ (dual-arm)
$K_{d}$ | $0.9$
Integral error clipping | $\left[-10,10\right]$
Controller output clipping | $\left[-15,15\right]$
Ref bearing | $0$

Table 9: Tracking (stabilising) controller parameters used for dual-arm
(follower) object pushing experiments. The feedback reference pose is
expressed as a 6-vector, as in Table 7.

Parameter | Value
---|---
$\boldsymbol{\mathrm{K}}_{p}$ | $\mathrm{diag}(5,0,5,1,0,0)$
$\boldsymbol{\mathrm{K}}_{i}$ | $\mathrm{diag}(0.5,0,0.5,0.1,0,0)$
$\boldsymbol{\mathrm{K}}_{d}$ | $\mathrm{diag}(0.5,0,0.5,0.1,0,0)$
Integral error clipping |  $\left[-200,200\right]$ all components
Feedback ref pose | $(0,0,3,0,0,0)$
Feedforward ref velocity | $(0,0,0,0,0,0)$