# Conservative Filtering for Heterogeneous Decentralized Data Fusion in
Dynamic Robotic Systems*

Ofer Dagan1 and Nisar R. Ahmed1 *This work was not supported by any
organization1 Ofer Dagan and Nisar R. Ahmed are with the Smead Aerospace
Engineering Sciences Department, University of Colorado Boulder, Boulder, CO
80309 USA<EMAIL_ADDRESS><EMAIL_ADDRESS>
###### Abstract

This paper presents a method for Bayesian multi-robot peer-to-peer data fusion
where any pair of autonomous robots hold non-identical, but overlapping parts
of a global joint probability distribution, representing real world inference
tasks (e.g., mapping, tracking). It is shown that in dynamic stochastic
systems, filtering, which corresponds to marginalization of past variables,
results in direct and hidden dependencies between variables not mutually
monitored by the robots, which might lead to an overconfident fused estimate.
The paper makes both theoretical and practical contributions by providing (i)
a rigorous analysis of the origin of the dependencies and and (ii) a
conservative filtering algorithm for heterogeneous data fusion in dynamic
systems that can be integrated with existing fusion algorithms. This work uses
factor graphs as an analysis tool and an inference engine. Each robot in the
network maintains a local factor graph and communicates only relevant parts of
it (a sub-graph) to its neighboring robot. We discuss the applicability to
various multi-robot robotic applications and demonstrate the performance using
a multi-robot multi-target tracking simulation, showing that the proposed
algorithm produces conservative estimates at each robot.

##  I INTRODUCTION

Consider a team of robots operating in an unknown environment and
collaborating on a global joint inference task such as building a map of an
unknown environment [1], tracking a set of targets [2] or cooperatively
localizing themselves [3, 4, 5]. To enable a scalable operation of such teams,
a common practice is to distribute the global joint inference task into
smaller, overlapping tasks, and allow robots to fuse only data relevant to
their local inference task. Here, for example, relevant data might be an
overlapping part of the map; subset of targets or a robot’s immediate
neighboring vehicles, respectively. Assuming each robot is an independent
entity that needs to send and locally fuse messages, the examples above
describe different instances of _heterogeneous_ fusion [2].

There are several approaches for heterogeneous multi-robot cooperation, such
as consensus [6], or by using a server/cloud computing [7, 8]. This paper
explores a peer-to-peer Bayesian heterogeneous decentralized data fusion (DDF)
approach, as it has advantages in robustness and flexibility. Briefly, in DDF
robots communicate their current posterior probability distribution function
(pdf) based on their locally available data [9].

In previous work [2], [10] we define peer-to-peer Bayesian heterogeneous DDF
as the process of fusing two non-equal, but overlapping, pdfs.111To be
precise, we differentiate between the more general problem of forming a
posterior pdf over a set of random variables, which might describe a random
state vector, and the Bayesian point estimate of those variables, given for
example by finding the minimum squared error (MMSE) of that pdf. One of the
main challenges of Bayesian DDF and more acutely of heterogeneous DDF is
accounting for dependencies in the data gathered by the different robots.
Incorrect treatment of these dependencies might lead to ‘double counting’ data
more than once, which results in an overconfident estimate. For the case of
homogeneous DDF, i.e. where the communicated and posterior pdfs describe the
same set of random variables, it is common to either (i) explicitly track
dependencies, which can be done by keeping a pedigree of the incoming data
[11] or by adding a channel filter (CF) [12] for example; or (ii) implicitly
account for dependencies by discounting data using covariance intersection
(CI) [13] or with the geometric mean density (GMD) [14], for example. Notice
that these methods are not application-specific and used to solve a variety of
problems. On the other hand, in the case of heterogeneous fusion, solutions to
account for the data dependencies have so far been rather application-
specific.

In [15] a multi-robot SLAM problem is solved, where dynamic robots share
static variables (the landmarks describing the unknown map) and the solution
is given for a smoothing problem, i.e. robot’s local position variables are
augmented. Thus this algorithm does not solve the filtering or fixed-lag
smoothing problem, which are relevant for target tracking and cooperative
localization applications for example. In cooperative localization (CL),
robots take relative measurements to their neighboring robots, which directly
couples the robots’ states. To the best of our knowledge, in current
approaches, robots either explicitly keep track of the dependencies of all
[16] or groups [4] of vehicles in the network, or implicitly account for
dependencies with CI to approximate the joint coupled covariance by its block
diagonal elements and then marginalize out the other robot [17, 5]. While
these approaches work for cooperative localization, they might not adapt well
for other cooperative applications, e.g., target tracking or SLAM. Thus there
is a need to look at heterogeneous fusion for robotics as a wider, non-
application specific problem, and develop the necessary analysis tools and
algorithms. For that reason, previous work [10] aimed at gaining insight into
the heterogeneous fusion problem.

In [2, 10] it is shown that in order for the fused posterior to be
conservative (doesn’t underestimate the uncertainty) over all the robot’s
variables, conditional independence has to be maintained between non-mutual
variables, given the common variables, e.g., in multi-robot SLAM, non-mutual
position variables are independent given common landmark variables. An
algorithm, named the _Heterogeneous State Channel Filter_ (HS-CF) was
developed, and shown to be conservative for static problems and for dynamic
problems when a smoothing approach was applied. However, for a filtering
scenario, the algorithm was slightly overconfident (non-conservative). It was
hypothesized that marginalization of past variables (states) in the filtering
stage resulted dependencies between non-mutual variables.

The goal of this work is to understand the nature of the dependencies between
non-mutual variables resulting from filtering in heterogeneous DDF
applications. To explicitly track and account for dependencies between common
variables, the CF framework is used, which requires the communication graph
between robots to be an undirected a-cyclic graph, so data can flow only in
one route (i.e., can not ‘circle’ back). Factor graphs, representing the local
robot’s pdf are analysed to gain insight into the structure of the problem and
the cause of dependencies between non-mutual variables. While this paper is
motivated by a search for rigorous understanding of the heterogeneous fusion
problem, to develop new theory and thus focus less on a specific application,
the presented approach can be readily applied to different robotic
applications.

The major contributions of this paper are twofold: theoretical and practical.
From a theoretical point of view, factor graph analysis i) reveals ‘hidden
variable dependency dynamics’ for Bayesian heterogeneous DDF and ii) sheds
light on the interplay between groups of common variables, showing that
preserving conditional independence structure through the filtering stage is
key to helping ensure conservativeness. From a practical point of view, i) an
algorithm for conservative filtering that can be integrated with other fusion
algorithms is developed and ii) multi-robot target tracking simulation
verifies that the presented approach overcomes previous difficulties for
dynamic variables.

The rest of the paper is organized as follows: Sec. II gives the necessary
background on heterogeneous DDF problems and factor graphs; Sec. III defines
the problem of conservative filtering in the context of heterogeneous DDF and
Sec. IV details the analysis, presents the suggested solution and algorithm.
Simulation results, discussion and conclusions are given in Sec. V and Sec.
VI.

##  II BACKGROUND

Consider a set $N_{r}$ of $n_{r}$ autonomous robots jointly tasked with
monitoring a global set of random variables $V$. These random variables can be
static, dynamic or even parameters of the distribution, and can describe, for
example, the random position states of the $n_{r}$ robots and $n_{t}$ targets
tracked by the robots. Since the size $V$ scales with the number of robots and
targets and might become quite heavy to process locally by each robot, there
is interest in distributing it and tasking each robot $i\in N_{r}$ with only a
subset of random variables $\chi^{i}\subset V$. To allow for collaboration
between robots we take a Bayesian DDF approach.

In Bayesian DDF, each robot $i$ is an independent entity, collecting data over
a set of random variables of interest $\chi^{i}$ from its local sensors and/or
via communication with neighboring robots. The robots can then individually
(locally) infer the posterior distribution over their random variables of
interest via Bayes’ rule $p(\chi^{i}|Z^{i})=p(\chi^{i})p(Z^{i}|\chi^{i})$,
where $Z^{i}$ is the locally available data at robot $i$ and $p(\chi^{i})$ is
the prior distribution. Note that if two robots $i$ and $j$ hold distributions
over overlapping sets of variables, i.e.,
$\chi^{i}\cap\chi^{j}\neq\varnothing$ and
$\chi^{i}\setminus\chi^{j}\neq\varnothing$, where ‘$\setminus$’ is the set
exclusion operation, [10] defines the resulting fusion process as an instance
of _heterogeneous_ fusion.

###  II-A Heterogeneous Bayesian Decentralized Data Fusion

In heterogeneous DDF a robot’s set of random variables of interest $\chi^{i}$
can be divided into: (i) a set of variables it has in common with at least one
of its set of neighbors $N_{r}^{i}$, such that $\chi^{i}_{C}=\bigcup_{j\in
N_{r}^{i}}\chi^{ij}_{C}$, where $\chi^{ij}_{C}$ are the variables common to
robots $i$ and $j$, and (ii) a set of local variables $\chi^{i}_{L}$ which are
not monitored by any other robot in the network. Robot $i$’s set of variables
is thus $\chi^{i}=\chi^{i}_{L}\bigcup\chi^{i}_{C}$.

Assume that non-mutual variables to robots $i$ and $j$, defined by
$\chi^{i\setminus
j}=\chi^{i}_{L}\cup\\{\chi^{i}_{C}\setminus\chi^{ij}_{C}\\}$, are independent
given the common variables, i.e., $\chi^{i\setminus j}\perp\chi^{j\setminus
i}|\chi^{ij}_{C}$. Then using a distributed version of Bayes’ rule, [2] shows
that the _Heterogeneous State_ (HS) fusion rule at robot $i$ is,

$\begin{split}p_{f}(\chi^{i}|Z^{i,+}_{k})\propto\frac{p(\chi^{ij}_{C}|Z^{i,-}_{k})p(\chi^{ij}_{C}|Z^{j,-}_{k})}{p^{ij}_{c}(\chi^{ij}_{C}|Z^{i,-}_{k}\cap
Z^{j,-}_{k})}\cdot p(\chi^{i\backslash
j}|\chi^{ij}_{C},Z^{i,-}_{k}).\end{split}$ (1)

Here $Z^{i,-}_{k}$ and $Z^{i,+}_{k}$ are the data at time step $k$, available
at robot $i$ before and after fusion, respectively, and it is assumed that all
measurements acquired by each robot are conditionally independent given the
random variable of interest.

There are two key points concerning the above fusion rule. First, the validity
of the equation is based on the assumption that non-mutual variables are
conditionally independent given common variables between the communicating
robots. However, when the variables of interest are dynamic and are
marginalized out successively over time, dependencies between non-mutual
variables arise and the fusion rule (1) is no longer valid. This problem is at
the core of this paper and is demonstrated, analyzed and solved in the next
sections. The second point is the importance of accounting for the posterior
distribution over the common variables, given the data common to both robots,
$p^{ij}_{c}(\chi^{ij}_{C}|Z^{i,-}_{k}\cap Z^{j,-}_{k})$, in the denominator of
(1).

There are several methods to approximate this distribution (e.g., CI [13], GMD
[14]) when the dependencies in the data held by two robots are unknown.
However, the focus of current work is understanding the nature and origin of
these dependencies. Thus we choose to use a CF [12] to try to explicitly track
the data common and dependencies between the robots.

###  II-B The Channel Filter

The Channel filter (CF) was suggested in [12] as a method to track
dependencies in the data in a network of robots. The main idea is to add a
filter on the communication channel between any two communicating robots. It
was shown [12], that for homogeneous fusion, if: (i) data does not circle back
to its sender, i.e., the communication graph is undirected and a-cyclic (e.g.,
tree or chain); (ii) there is full rate communication (i.e., communication at
every time step, without delays); (iii) incoming data is processed
sequentially and (iv) assuming linear dynamic and measurement models and
additive white Gaussian noise (AWGN), then the CF allows for each robot to
recover the optimal centralized state estimate.

For heterogeneous fusion, since robots only reason over parts of the global
set of random variables, they cannot achieve the optimal estimate.
Nevertheless, in [2], [10] an extended version of the original homogeneous CF
is developed - the _Heterogeneous State CF_ (HS-CF). In the HS-CF algorithm
each CF maintains a distribution over only the common variables between any
two communicating robots (as opposed to the full set of variables in the
original CF), given the common data. Thus the distribution
$p^{ij}_{c}(\chi^{ij}_{C}|Z^{i,-}_{k}\cap Z^{j,-}_{k})$ is explicitly tracked
and can be accounted for when fusing according to (1).

The HS-CF was shown ([10]) to be conservative for static problems and for
dynamic problems with a smoothing approach. However, for a filtering scenario,
it resulted in a slightly overconfident state minimum mean squared error
(MMSE) estimates. These results revealed a gap in both the understanding of
the dependencies resulting from marginalizing out past variables (filtering)
and in the tools available to analyze heterogeneous DDF, more specifically the
conditional independence structure of such problems.

###  II-C Factor Graphs

In recent years factor graphs [18] have been used to study and solve a variety
of robotic applications [19]. Factor graphs are arguably the most general
framework to analyze and express conditional independence, as such they
directly express the sparse structure of decentralized problems.

A factor graph is an undirected bipartite graph $\mathcal{F}=(U,V,E)$ that
represents a function, proportional to the joint pdf over all random variable
nodes $v_{m}\in V$, and factorized into smaller functions given by the factor
nodes $f_{l}\in U$. An edge $e_{lm}\in E$ in the graph only connects a factor
node _l_ to a variable node _m_. The joint distribution over the graph is then
proportional to the global function $f(V)$:

$p(V)\propto f(V)=\prod_{l}f_{l}(V_{l}),$ (2)

where $f_{l}(V_{l})$ is a function of only those variables $v_{m}\in V_{l}$
connected to the factor _l_.

In [20] a factor graph based DDF framework, _FG-DDF_ , was suggested and
demonstrated on a static heterogeneous fusion problem. Here, we further use
factor graphs to analyze conservative filtering in dynamic problems.

##  III PROBLEM STATEMENT

Consider a robot $i$ from the network of $n_{r}$ robots described in Sec. II.
Without loss of generality, assume that this robot has two neighbors $j$ and
$m$ and the three robots are part of an undirected a-cyclic communication
topology, such that $j-i-m$ (i.e., $j$ does not have a way to receive data
from $m$ other than through $i$). Robot $i$ exchanges data over its sets of
common variables $\chi^{ij}_{C}$ and $\chi^{im}_{C}$ with robots $j$ and $m$,
respectively, via the HS-CF fusion rule (1). Suppose that $V$, the global set
of variables monitored by the $n_{r}$ robots, describes some systems’ random
state vector, constructed out of dynamic and static states.

Every robot $i$ gathers data and updates its prior pdf in two ways: (i) by
independent sensor measurements $y^{i,l}_{k}\in Y^{i}_{k}$, described by the
conditional likelihood
$p(Y^{i}_{k}|\chi^{i}_{k})=\prod_{l}p(y^{i,l}_{k}|\chi^{i,l}_{k})$ and updated
using Bayes’ rule, where $\chi^{i,l}_{k}$ is the subset $l$ of states,
measured by the $y^{i,l}_{k}$ measurement, taken by robot $i$ at time step
$k$; (ii) peer-to-peer heterogeneous fusion of data $Z^{j,-}_{k}$ from a
neighboring robot $j\in N_{r}^{i}$, updated via (1).

We begin our analysis from time step 1, with the initial pdf
$p(\chi^{i}_{2:1}|Z^{i,+}_{1})$ (post fusion) and conditional independence
structure of the type shown in Fig. 1(a), where $\chi^{i}_{2:1}$ denotes the
augmented state of time steps $1$ and $2$, and $Z^{i,+}_{1}$ is all the data
gathered by robot $i$ up to and including time step 1. The graph also shows
‘neighborhood variables’ not monitored (hidden) from robot $i$ in the dashed
nodes and hidden dependencies are noted by the dashed lines.

$(a)$$\chi^{i}_{L}$$\chi^{m}_{L}$$\chi^{j}_{L}$$\chi^{im\setminus
j}_{C,1}$$\chi^{ijm}_{C,1}$$\chi^{ij\setminus
m}_{C,1}$$f^{i,1}_{1}$$f^{i,2}_{1}$$f^{i,3}_{1}$$f^{im}_{1}$$f^{ij}_{1}$$f^{im}_{2|1}$$\chi^{im\setminus
j}_{C,2}$$f^{ijm}_{2|1}$$\chi^{ijm}_{C,2}$$f^{ij}_{2|1}$$\chi^{ij\setminus
m}_{C,2}$$(b)$$\chi^{i}_{L}$$\chi^{m}_{L}$$\chi^{j}_{L}$$\chi^{ijm}_{C,2}$$\chi^{im\setminus
j}_{C,2}$$\chi^{ij\setminus m}_{C,2}$$f^{i}_{2}$

Figure 1: Neighborhood graph perspective: factor graph representing robot $i$’
local pdf with hidden local variables of neighboring robots $m$ and $j$.
Dashed nodes and grey factors are hidden from robot $i$. (a) graph before
marginalization of time step 1 (b) fully connected graph after
marginalization.

It can be seen that non-mutual variables are independent given the common
variables, e.g.,

$\chi_{L}^{m}\perp\chi_{L}^{i},\chi_{C,1}^{ij\setminus
m},\chi_{C,2}^{ij\setminus m}|\chi_{C,1}^{im},\ \ \
\chi_{C,1}^{im}=\chi_{C,1}^{ijm}\cup\chi_{C,1}^{im\setminus j}.$

Here common variables are separated into three different sets
$\chi_{C}^{ijm}$, $\chi_{C}^{ij\setminus m}$ and $\chi_{C}^{im\setminus j}$,
respectively representing variables common to the three robots, variables
common to $i$ and $j$ but not to $m$, and variables common to $i$ and $m$ but
not to $j$. Dependencies between these subsets are created by fusion, which
introduces the factors $f^{im}_{1}$ and $f^{ij}_{1}$, coupling the variables
in the common sets $\chi^{im}_{C,1}$ and $\chi^{ij}_{C,1}$, respectively, as a
result of marginalization of non-mutual variables at the communicating robots
$m$ and $j$, e.g., $p(\chi^{ij}_{C}|Z^{j,-}_{k})$ in (1) [20]. A dictionary
for the different types of factors and their interpretation is given in Table
I.

TABLE I: Factors dictionary, giving examples for different types of factors, their notation and their pdf interpretation.  Factor | Type | Proportional to
---|---|---
$f^{i,l}_{k}$ | Local measurement | $p(y^{i,l}_{k}|\chi^{i,l}_{k})$
$f^{ij\setminus m}_{2|1}$ | Dynamic prediction | $p(\chi^{ij\setminus m}_{C,2}|\chi^{ij\setminus m}_{C,1})$
$f_{k}^{ij}$ | Fusion |  $p(\chi^{ij}_{C}|Z^{j,-}_{k})$ (from (1))
$f^{i}_{2}$ | Dense marginalization | $\int p(\chi^{i}_{2:1}|Z^{i,+}_{1})d\chi_{C,1}^{i}$
$\tilde{f}^{ij}_{2}$ | Approximate marginalization |  $\int\tilde{p}(\chi^{i}_{2:1}|Z^{i,+}_{1})d\chi_{C,1}^{i}$ (6)

As can be seen from Fig 1(a), the graph encodes the following local pdf
factorization,

$\begin{split}&p(\chi^{i}_{2:1}|Z^{i,+}_{1})=p(\chi_{L}^{i})\cdot\\\
&p(\chi^{ijm}_{C,1}|\chi_{L}^{i})\cdot p(\chi^{im\setminus
j}_{C,1}|\chi^{ijm}_{C,1},\chi_{L}^{i})\cdot p(\chi^{ij\setminus
m}_{C,1}|\chi^{ijm}_{C,1},\chi_{L}^{i})\cdot\\\
&p(\chi^{ijm}_{C,2}|\chi^{ijm}_{C,1})\cdot p(\chi^{im\setminus
j}_{C,2}|\chi^{im\setminus j}_{C,1})\cdot p(\chi^{ij\setminus
m}_{C,2}|\chi^{ij\setminus m}_{C,1}),\end{split}$ (3)

where the conditioning on the data $Z_{1}^{i,+}$ is omitted from the right
side of the equations for the rest of the paper for brevity.

In Fig. 1(b), marginalization of past common nodes of time step 1 (filtering)
results in a dense factor, causing both ‘hidden’ and ‘visible’ dependencies.
Here hidden refers to variables and dependencies being hidden from robot’s $i$
perspective (dashed variables and grey factors in Fig. 1), and visible
suggests the the variables, thus the dependencies, exist in the robot’s local
graph.

The problem is therefore formulated in the following way: we seek a method to
filtering that: (i) results in a conservative posterior pdf after the next
fusion step and (ii) maintains the conditional independence requirement
between non-mutual variables. That is, we ask for a conservative approximation
$\hat{p}(\cdot)$ of the pdf $p(\cdot)$,

$\begin{split}\hat{p}(\chi^{i}_{2}|Z^{i,+}_{1})\succeq\int
p(\chi^{i}_{2:1}|Z^{i,+}_{1})d\chi_{C,1}^{i},\end{split}$ (4)

where ‘$\succeq$’ denotes conservative. The question regarding the meaning of
conservative in the case of general pdfs is beyond the scope of this paper and
the reader is referred to [21] and [10] for further discussion. It will be
assumed later that the distributions can be described using their first two
moments (mean and covariance), so that then a commonly used definition of
conservativeness exists.

We define a conservative posterior pdf as one that does not underestimate the
uncertainty over the robot’s set of random variables $\chi^{i}_{k}\subseteq
V_{k}$ relative to the marginal pdf over $\chi^{i}_{k}$ of a consistent
centralized pdf. Consistency here means that the fused result does not
overestimate or underestimate the true uncertainty. The centralized pdf refers
to the posterior pdf over the full global set of random variables $\chi_{k}$,
conditioned on all the available data from all the robots up and including
time step $k$, $p(V_{k}|\bigcup_{i\in N_{r}}Z^{-}_{i,k})$. For a more complete
discussion on conservative pdfs and point estimates, see [10].

##  IV CONSERVATIVE FILTERING

Our proposed solution to the conservative filtering problem is inspired by the
graph-SLAM literature, where the process of removing edges between nodes is
commonly referred to as _sparsification_. In graph-SLAM, removal of the
robot’s pose nodes induces dependencies between landmarks and results in a
dense information matrix. Several methods have been proposed to enforce
conditional independence between the landmarks in a conservative manner, which
differ in the ‘timing’ of the edge removal. For example, the _Exactly Sparse
Extended Information Filter_ (ESEIF) algorithm [22] removes motion links
_prior_ to marginalization of pose nodes, to induce a sparse marginalized
information matrix. On the other hand, the algorithms in [23], [24] enforce
conditional independence _after_ marginalization by manipulation of the
marginalized dense information matrix. Next, we make use of these two concepts
to derive a new conservative filtering algorithm. We divide the problem into
two parts, first to account for the hidden dependencies between local
variables, and second to enforce conditional independence between sets of
visible non-mutual variables.

###  IV-A Local Variables - Hidden Dependencies

To understand the problem of hidden dependencies between local variables it is
sufficient to study the reduced problem of only two robots. Consider two
robots $i$ and $j$, with local subsets of variables $\chi_{L}^{i}$ and
$\chi_{L}^{j}$ and a common subset $\chi_{C}^{ij}$. Fig. 2(a)-(b) shows an
example of hidden correlation due to filtering, demonstrated on robot $i$’s
local graph. Robot $i$ has no knowledge on robot $j$’s local variables, which
now have hidden dependencies with its local variables due to marginalization.
To avoid this hidden coupling robot $i$ needs to take preventive action prior
to marginalization based on its local understanding of the distribution.

$(a)$$\chi_{L}^{i}$$\chi_{L}^{j}$$\chi_{C,1}^{ij}$$f^{ij}_{2|1}$$\chi_{C,2}^{ij}$$(b)$$\chi_{L}^{i}$$\chi_{L}^{j}$$\chi_{C,2}^{ij}$$f^{ij}_{2}$$(c)$$\chi_{L}^{i}$$\chi_{L}^{j}$$\chi_{C,1}^{ij}$$\chi_{C,2}^{ij}$$(d)$$\chi_{L}^{i}$$\chi_{L}^{j}$$\chi_{C,2}^{ij}$$\tilde{f}^{ij}_{2}$
Figure 2: Example of hidden correlation due to filtering, demonstrated on
robot $i$’s local graph, only shown factors relevant to the example. Dashed
variable nodes and edges are hidden from robot $i$. (a) robot $i$’s graph
after first measurement, coupling local and common variables, fusion and
prediction step. (b) filtering, with the marginalization of $\chi_{C,1}^{ij}$
results in coupling shown by the red factor, which is hidden from the robot
(as $i$ has no knowledge about $\chi_{L}^{j}$) but evident in the full graph.
(c)-(d) demonstrates the proposed solution, approximating by marginal
distributions prior to filtering.

The local pdf at robot $i$, prior to marginalization (Fig. 2(a)), can be
factorized as,

$\begin{split}p(\chi^{i}_{2:1}|Z^{i,+}_{1})=p(\chi_{C,1}^{ij},\chi_{C,2}^{ij})\cdot
p(\chi^{i}_{L}|\chi_{C,1}^{ij}).\end{split}$ (5)

By removing the dependency between the local and common variables prior to
marginalization, i.e., approximating the pdf by its marginals (Fig. 2(c)),

$\begin{split}\tilde{p}(\chi^{i}_{2:1}|Z^{i,+}_{1})=p(\chi_{C,1}^{ij},\chi_{C,2}^{ij})\cdot
p(\chi^{i}_{L}),\end{split}$ (6)

and only then marginalize out $\chi_{C,1}^{ij}$ (Fig. 2(d)),

$\begin{split}\tilde{p}(\chi^{i}_{2}|Z^{i,+}_{1})=p(\chi_{C,2}^{ij})\cdot
p(\chi^{i}_{L}),\end{split}$ (7)

the hidden dependencies are prevented. Notice that from robot $i$’s
perspective, it didn’t have to ‘know’ if robot $j$ is tasked with more local
variables, but rather it just assumes that there are other variables that are
not known locally and takes preemptive measures to avoid hidden dependencies.

###  IV-B Common Variables - Visible Dependencies

To analyze the visible dependencies between the different sets of common
variables, consider again the 3 robots $j-i-m$ described in Sec. III and
assume that edges to the local variables have been removed prior to
marginalization (6)-(7). Thus the current factor graph at robot $i$ has the
form depicted in Fig. 3(a) and it describes the approximate pdf,

$\begin{split}\tilde{p}(\chi^{i}_{2}|Z^{i,+}_{1})=p(\chi_{L}^{i})\cdot
p(\chi^{ijm}_{C,2},\chi^{im\setminus j}_{C,2},\chi^{ij\setminus
m}_{C,2}).\end{split}$ (8)

$(a)$$\chi^{i}_{L}$$\chi^{m}_{L}$$\chi^{j}_{L}$$\chi^{ijm}_{C,2}$$\chi^{im\setminus
j}_{C,2}$$\chi^{ij\setminus
m}_{C,2}$$(b)$$\chi^{i}_{L}$$\chi^{m}_{L}$$\chi^{j}_{L}$$\chi^{ijm}_{C,2}$$\chi^{im\setminus
j}_{C,2}$$\chi^{ij\setminus
m}_{C,2}$$(c)$$\chi^{i}_{L}$$\chi^{m}_{L}$$\chi^{j}_{L}$$\chi^{ijm}_{C,2}$$\chi^{im\setminus
j}_{C,2}$$\chi^{ij\setminus
m}_{C,2}$$\tilde{f}^{im}_{2}$$\tilde{f}^{ij}_{2}$$(d)$$\chi^{i}_{L}$$\chi^{m}_{L}$$\chi^{j}_{L}$$\chi^{ijm}_{C,2}$$\chi^{im\setminus
j}_{C,2}$$\chi^{ij\setminus m}_{C,2}$$\tilde{f}^{im}_{2}$$\tilde{f}^{ij}_{2}$
Figure 3: Visible dependencies with full graph perspective. (a) graph after
filtering (b) addition of measurement factors and dependencies at time step 2,
red dense factor breaks conditional independence assumption required for
heterogeneous fusion. (c)-(d) our proposed method to regain conditional
independence by factorizing into smaller local factors.

The robots now take a second measurement shown by the black and grey factors
in Fig. 3(b). Here the grey factors show measurements taken by robots $j$ and
$m$, which are not available at robot $i$ prior to the next fusion episode. It
can be seen from the factor graph (Fig. 3(b)) that the conditional
independence assumption between non-mutual variables given common variables
does not hold, i.e., $\chi_{L}^{j}\not\perp\chi_{C,2}^{im\setminus
j}\cup\chi_{L}^{i}|\chi_{C}^{ij}$ for robots $i$ and $j$ and similarly
$\chi_{L}^{m}\not\perp\chi_{C,2}^{ij\setminus
m}\cup\chi_{L}^{i}|\chi_{C}^{im}$ for robots $i$ and $m$. This breaks the
assumption allowing heterogeneous fusion according to (1) and might cause
overconfidence in robot $i$’s estimate.

Fig. 3(d) shows that by requiring the inner-structure/dependencies between the
subsets of common variables at time step 2 to be identical to the dependencies
of those subsets at time step 1 prior to marginalization, the conditional
independence required for heterogeneous fusion is recovered. For example, if
$\chi_{C,1}^{im\setminus j}\perp\chi_{C,1}^{ij\setminus
m}|\chi_{C,1}^{ijm},\chi_{L}^{i}$ prior to marginalization (Fig. 1(a)), we
require $\chi_{C,2}^{im\setminus j}\perp\chi_{C,2}^{ij\setminus
m}|\chi_{C,2}^{ijm},\chi_{L}^{i}$ post marginalization (Fig. 3(d)). This
requirement also bypasses the need for ‘bookkeeping’ of the different subset
of variables, as it simply keeps the same structure as before filtering.

The method to enforce this structure is by factorizing the dense factor
induced by marginalization shown in Fig. 3(a) to two smaller factors
$\tilde{f}^{im}_{2}(\chi_{C}^{im})$ and $\tilde{f}^{ij}_{2}(\chi_{C}^{ij})$,
connecting separately the variables common to $i$ and $m$ and $i$ and $j$,
respectively. In other words, we wish to further approximate the pdf
$\tilde{p}(\chi^{i}_{2}|Z^{i,+}_{1})$ shown in Fig. 3(a) with the pdf
$\hat{p}(\chi^{i}_{2}|Z^{i,+}_{1})$ shown in Fig. 3(c), such that:

$\begin{split}&\hat{p}(\chi^{i}_{2}|Z^{i,+}_{1})=\\\ &p(\chi_{L}^{i})\cdot
p(\chi_{C,2}^{ijm})\cdot p(\chi_{C,2}^{ij\setminus m}|\chi_{C,2}^{ijm})\cdot
p(\chi_{C,2}^{im\setminus j}|\chi_{C,2}^{ijm}).\end{split}$ (9)

The steps taken thus far have led to an approximate pdf that accounts for the
hidden and visible dependencies in the data in a way that preserves the
conditional independence assumptions. However, we have yet to guarantee the
conservativeness of the approximate pdf with respect to (w.r.t) the true pdf
as required in the problem statement (4), where true pdf here means the dense
pdf that would have been resulted without the approximations. While no
assumption on the type of distribution have been made until now, there is no
commonly used formal definition of conservativeness for general pdfs. Thus we
now focus our attention to the case where the pdf can be represented by a
Gaussian distribution (or the first two moments). These are used in many
applications across robotics [19].

Assume that the factors are Gaussian functions, described by their information
matrix (inverse of covariance). Then the factorization, or sparsification
given in (9), can be preformed using various methods, e.g., [23], [24], [25].
The basic idea of all of these methods is the similar: given the true Gaussian
distribution, shown in Fig. 1(b) and characterized by its _dense_ information
matrix and vector $\mathcal{N}(\zeta_{tr},\Lambda_{tr})$. We wish to make the
approximate sparse Gaussian pdf,
$\hat{p}(\chi^{i}_{2}|Z^{i,+}_{1})\sim\mathcal{N}(\zeta_{sp},\Lambda_{sp})$,
shown in Fig. 3(c) and given in (9), conservative in the positive semi-
definite (PSD) sense,

$\begin{split}\Lambda_{tr}-\Lambda_{sp}\succeq 0.\end{split}$ (10)

Due to its relative simplicity and the fact that it does not require
optimization, we choose to use the method suggested by [25] and generalized in
[10]. Briefly, we solve for the deflation constant $\lambda_{min}$, the
minimal eigenvalue of
$\tilde{Q}=\Lambda_{sp}^{-\frac{1}{2}}\Lambda_{tr}\Lambda_{sp}^{-\frac{1}{2}}$
and enforce the mean of the sparse distribution to equal the true one. The
conservative approximate pdf is,

$\begin{split}\hat{p}(\chi^{i}_{2}|Z^{i,+}_{1})\sim\mathcal{N}(\lambda_{min}\Lambda_{sp}\Lambda_{tr}^{-1}\zeta_{tr},\lambda_{min}\Lambda_{sp}).\end{split}$
(11)

Note that to apply the deflation back to the factor graph expressing this pdf,
factors need to be manipulated. In the case of Gaussian distributions, the
information matrix, like the corresponding factor graph, directly expresses
the conditional independence structure. Thus zero terms in the information
matrix indicate conditional independence (lack of factor) between the
variables, and non-zero terms correspond to a factor between variables. We
then re-factorize the graph in Fig. 3(c) in the following way:222Since the
information form is related to the log of the distribution, it can be
expressed as summation instead of multiplication [26].

$\begin{split}\hat{p}(\chi^{i}_{2}|Z^{i,+}_{1})\propto&f(\chi_{L}^{i})+f(\chi_{C,2}^{im\setminus
j})+f(\chi_{C,2}^{ijm})+f(\chi_{C,2}^{ij\setminus m})+\\\
&f(\chi_{C,2}^{ijm},\chi_{C,2}^{im\setminus
j})+f(\chi_{C,2}^{ijm},\chi_{C,2}^{ij\setminus m}),\end{split}$ (12)

where factors over one set of variables, e.g., $f(\chi_{C,2}^{ijm})$, are
Gaussians with the vector and block diagonal elements of the information
vector and matrix in (11) corresponding to the variables in the set. Factors
over two sets of variables hold a zero information vector and the matrix of
off-block diagonal elements of those sets are zero block-diagonal elements,
e.g.,

$\begin{split}&f(\chi_{C,2}^{ijm})=\mathcal{N}(\zeta_{\chi_{C,2}^{ijm}},\Lambda_{\chi_{C,2}^{ijm},\chi_{C,2}^{ijm}}),\\\
&f(\chi_{C,2}^{ijm},\chi_{C,2}^{im\setminus
j})=\mathcal{N}(\begin{bmatrix}\underline{0}\\\
\underline{0}\end{bmatrix},\begin{bmatrix}\underline{\underline{0}}&\Lambda_{\chi_{C,2}^{ijm},\chi_{C,2}^{im\setminus
j}}\\\ \Lambda_{\chi_{C,2}^{ijm},\chi_{C,2}^{im\setminus
j}}^{T}&\underline{\underline{0}}\end{bmatrix}).\end{split}$

Here $\underline{0}$ and 0 denotes the zero vector and matrix in the correct
dimensions, respectively.

###  IV-C Updating the CF

The last step of the proposed method is to guarantee that data is not over-
discounted in the fusion step. Remember that on every communication channel a
robot has with its neighbors, a CF was added to track dependencies in the data
(Sec. II-B), in the form of another factor graph over the common variables.
The conservative filtering operations so far did not affect the CF while
discounting information on the robot’s graph. This might lead to a situation
where the CF has ‘more information’, in the sense of information matrix, over
the common variables than the robot itself. During fusion this might result
robot $i$ sending ‘negative information’ to its neighbors. To avoid this
situation, we update every CF graph the robot maintains by deflating the
factors of the graph with the deflation constant $\lambda_{min}$. In practice
that means multiplying the information vector and matrix of every factor $f$
in the CF graph by $\lambda_{min}$.

###  IV-D Algorithm

The full conservative filtering approach is now summarized in Algorithm 1,
which is written from the perspective of one robot $i$ communicating with its
$n^{i}_{r}$ neighbors and is applied recursively at every filtering step. Note
that while the algorithm defines operations on a factor graph and in the
example of this paper is integrated with the FG-DDF framework, it is not
limited to a specific type of graphical model nor a pdf or a fusion algorithm.
The main steps, given in lines 4-7, can in general, be defined by various
methods, with the only caveat being guaranteeing conservativeness for non-
Gaussian pdfs, which, in practice is frequently solved by approximating with a
Gaussian as done in many robotic applications.

Algorithm 1 Conservative Filtering

1:Input: Local factor graph $\mathcal{F}^{i}$

2:Prediction step

3:Create a copy of the ‘true’ graph $\mathcal{F}^{i}_{tr}$

4:Approximate $\mathcal{F}^{i}$ with marginal pdfs $\triangleright$ Eq. (6)

5:Marginalize out past nodes in $\mathcal{F}^{i}$ (7) and
$\mathcal{F}^{i}_{tr}$

6:Regain conditional independence in $\mathcal{F}^{i}$ $\triangleright$ Eq.
(9)

7:Guarantee conservativeness w.r.t $\mathcal{F}^{i}_{tr}$ $\triangleright$ Eq.
(11)

8:Re-factorize graph $\triangleright$ Eq. (12)

9:for Every neighbor $j\in N_{r}^{i}$ do

10: Update CF graph $\mathcal{F}^{ij}_{CF}$

11:end for

12:return

##  V SIMULATION RESULTS and DISCUSSION

To test the proposed method, a simulation of 4 robots tracking 5 dynamic
targets was preformed. The robots are connected in a chain $(1\leftrightarrow
2\leftrightarrow 3\leftrightarrow 4)$ with bidirectional communication. Each
robot is tasked with estimating the $2D$ (north, east) position and velocity
$x^{t}=[n^{t},\dot{n}^{t},e^{t},\dot{e}^{t}]^{T}$ of a subset of the 5
targets, and its own constant (but unknown) robot-to-target relative position
measurement bias $s^{i}=[b^{i}_{n},b^{i}_{e}]^{T}$, similar to the bias in
[27]. Robots tasks, described by their local random state vector, are as
follows:

$\begin{split}\chi^{1}_{k}=\begin{bmatrix}x^{1}_{k}\\\ x^{2}_{k}\\\
s^{1}\end{bmatrix},\chi^{2}_{k}=\begin{bmatrix}x^{2}_{k}\\\ x^{3}_{k}\\\
s^{2}\end{bmatrix},\chi^{3}_{k}=\begin{bmatrix}x^{3}_{k}\\\ x^{4}_{k}\\\
x^{5}_{k}\\\ s^{3}\end{bmatrix},\chi^{4}_{K}=\begin{bmatrix}x^{4}_{k}\\\
x^{5}_{k}\\\ s^{4}\end{bmatrix}.\end{split}$ (13)

For example, robot 1 is tasked with tracking targets 1 and 2 and its own local
bias. Common states between robots are those states in the intersection
between the state vectors, e.g, $\chi^{12}_{C,k}=x^{2}_{k}$ is the common
state vector between robots 1 and 2, and
$\chi^{1}_{L,k}=[(x^{1}_{k})^{T},(s^{1})^{T}]^{T}$ are the local state vectors
at robot 1. Notice that in homogeneous fusion, all 4 robots reason and
communicate the full global state, including 28 states. On the other hand, in
heterogeneous fusion robots only reason over their local tasks, including
maximum 14 states (robot 3) and communicate over maximum 8 common states
(robots 3-4). For Gaussian distributions that translates to more than $95\%$
reduction in communication and computation costs for robots 1,2,4 and about
$90\%$ for robot 3.

In every time step $k$, each robot $i$ takes two types of measurements, a
relative measurement to target $t$, $y^{i,t}_{k}$, and a measurement to a
known landmark, $m^{i}_{k}$,

$\begin{split}&y^{i,t}_{k}=x^{t}_{k}+s^{i}+v^{i,1}_{k},\ \
v^{i,1}_{k}\sim\mathcal{N}(0,R^{i,1}_{k}),\\\ &m^{i}_{k}=s^{i}+v^{i,2}_{k},\ \
v^{i,2}_{k}\sim\mathcal{N}(0,R^{i,2}_{k}).\end{split}$ (14)

As in many target tracking problems [28], each target $t$ dynamics is model by
the following kinematic model,

$\begin{split}&x^{t}_{k+1}=Fx^{t}_{k}+Gu^{t}_{k}+\omega_{k},\ \
\omega_{k}\sim\mathcal{N}(0,0.08\cdot I_{n_{x}\times n_{x}}),\\\
&F=\begin{bmatrix}1&\Delta t&0&0\\\ 0&1&0&0\\\ 0&0&1&\Delta t\\\
0&0&0&1\end{bmatrix},\quad G=\begin{bmatrix}\frac{1}{2}\Delta t^{2}&0\\\
\Delta t&0\\\ 0&\frac{1}{2}\Delta t^{2}\\\ 0&\Delta
t\end{bmatrix}.\end{split}$ (15)

The simulation uses the FG-DDF algorithm [20], with the addition of the
conservative filtering algorithm 1. Each robot then maintains and reasons over
its local dynamic factor graph and estimates the current MMSE estimate (mean
and covariance) using the sum-product (message passing) algorithm on the
factor graph [18].

The algorithm’s performance was tested with 250 Monte Carlo simulations.
Consistency of each robot’s estimate was tested using the normalized
estimation error squared (NEES) test [28]. Fig. 4 shows the four robots’
results with $95\%$ confidence bounds for robots 1,2,4 in dashed red centered
around 10 (number of estimated states) and for robot 3 in dashed blue around
14. Results show that all robots are consistent, underestimating their
uncertainty due to due to the information matrix deflation (covariance
inflation) in the conservative filtering process.

Figure 4: NEES test results from 250 Monte Carlo of 4-robots, 5-targets
tracking simulations. Shown are $95\%$ confidence bounds.

To validate that the robots’ estimates are conservative, the local uncertainty
of each robot was compared with a centralized estimator over the full 28
random state vector, getting all data from all the robots at every time step.
We thus require the robot’s covariance to be larger than the centralized
covariance in the PSD sense,
$\Sigma_{\chi^{i}}-\Sigma_{\chi^{i}}^{cent}\succeq 0$, where
$\Sigma_{\chi^{i}}^{cent}$ is the marginal covariance over the local random
state vector of robot $i$, $\chi^{i}$, taken from the joint centralized
covariance over the full system state $\chi$. In practice, this is verified by
computing the minimal eigenvalue of the above covariance difference and
testing that it is not negative. The left plot of Fig. 5 shows the minimal
eigenvalue of the covariance difference with and without the conservative
filtering algorithm. It can be seen that without the algorithm the
heterogeneous fusion results are not conservative. On the other hand, with the
algorithm it takes about $1-1.5$ seconds for the robots’ estimates to become
conservative, but once it is conservative, it stays conservative.

The right plot of Fig. 5 shows the change in the deflation constant in time
across all robots. Intuitively, as robots accumulate more data, approximations
by detaching dependencies have lower impact on the pdf, i.e., the sparse
approximation is ‘closer’ to the dense pdf. Thus the deflation constant
approaches to some limit, depending on the problem statistics and structure.
Also, notice that robots 1 and 4, which only communicate with one robot each,
and thus accumulate less data, need smaller constants to regain
conservativeness.

Another interesting observation is that both the minimal eigenvalues and
deflation constant are constant across simulations. We attribute it to the
fact that these are functions of the problem statistics, i.e., measurement and
dynamic model noise, and of the communication network, all where kept constant
across simulations. Thus in general, these can be analyzed and studied priori,
with no connection to the actual measurements received.

Figure 5: Minimal eigenvalue of the covariance difference (left) and deflation
constant (right) of the four robots. Shown are results with and without the
conservative filtering algorithm.

##  VI CONCLUSIONS

Heterogeneous Bayesian DDF enables a system of robots to cooperatively share
information in a scalable way, such that every robot only reasons about its
variables of interest. These can include locally observed variables or any
other subset of the global set of variables. By reducing their local set and
communicating only common variables of interest, robots can significantly
reduce their communication and computation costs.

This paper rigorously develops the theory for conservative filtering in
decentralized multi-robot dynamic applications. The analysis and methods are
developed in a Bayesian framework using factor graphs, representing each
robot’s local pdf. When the notion of conservativeness can be defined, e.g.,
for Gaussian distributions, a practical algorithm is developed and it is shown
to yield a conservative estimate for a multi-robot multi-target tracking
application. The methods developed are based on only the local knowledge a
robot has regarding the structure of the pdf. Thus, robots can independently
reason about their local tasks without the need for some global graph
knowledge, and opportunistically fuse data for cooperative robots to obtain
conservative state estimates.

The suggested conservative filtering algorithm bridges the gap found in [10],
and allows, together with the FG-DDF framework [20] to solve different robotic
problems, such as target tracking [10] and terrain height mapping [1].

Future research will explore non-linear problems, hardware implementation and
cyclic network topologies.

## References

  * [1] J. R. Schoenberg and M. Campbell, “Distributed terrain estimation using a mixture-model based algorithm,” in _2009 12th International Conference on Information Fusion (FUSION)_ , Jul. 2009, pp. 960–967.
  * [2] O. Dagan and N. R. Ahmed, “Heterogeneous decentralized fusion using conditionally factorized channel filters,” in _2020 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI)_ , Sep. 2020, pp. 46–53.
  * [3] I. Loefgren, N. Ahmed, E. Frew, C. Heckman, and S. Humbert, “Scalable event-triggered data fusion for autonomous cooperative swarm localization,” in _2019 22th International Conference on Information Fusion (FUSION)_ , Jul. 2019, pp. 1–8.
  * [4] H. Li and F. Nashashibi, “Cooperative multi-vehicle localization using split covariance intersection filter,” in _2012 IEEE Intelligent Vehicles Symposium_ , Jun. 2012, pp. 211–216, iSSN: 1931-0587.
  * [5] J. Zhu and S. S. Kia, “Cooperative localization under limited connectivity,” _IEEE Transactions on Robotics_ , vol. 35, no. 6, pp. 1523–1530, Dec. 2019\.
  * [6] Y. Sun and D. B. Work, “Scaling the Kalman Filter for large-scale traffic estimation,” _IEEE Transactions on Control of Network Systems_ , vol. 5, no. 3, pp. 968–980, Sep. 2018.
  * [7] S. S. Kia, J. Hechtbauer, D. Gogokhiya, and S. Martínez, “Server-assisted distributed cooperative localization over unreliable communication links,” _IEEE Transactions on Robotics_ , vol. 34, no. 5, pp. 1392–1399, Oct. 2018\.
  * [8] P. Schmuck and M. Chli, “CCM-SLAM: Robust and efficient centralized collaborative monocular simultaneous localization and mapping for robotic teams,” _Journal of Field Robotics_ , vol. 36, no. 4, pp. 763–781, 2019.
  * [9] C. Y. Chong, E. Tse, and S. Mori, “Distributed estimation in networks,” in _1983 American Control Conference (ACC)_ , Jun. 1983, pp. 294–300.
  * [10] O. Dagan and N. R. Ahmed, “Exact and approximate heterogeneous Bayesian decentralized data fusion,” _arXiv:2101.11116 [cs, eess]_ , Sep. 2021.
  * [11] T. W. Martin and K. C. Chang, “A distributed data fusion approach for mobile ad hoc networks,” in _2005 7th International Conference on Information Fusion (FUSION)_ , vol. 2, Jul. 2005, pp. 1062–1069.
  * [12] S. Grime and H. Durrant-Whyte, “Data fusion in decentralized sensor networks,” _Control Engineering Practice_ , vol. 2, no. 5, pp. 849–863, Oct. 1994.
  * [13] S. J. Julier and J. K. Uhlmann, “A non-divergent estimation algorithm in the presence of unknown correlations,” in _Proceedings of the 1997 American Control Conference (ACC)_ , vol. 4, Jun. 1997, pp. 2369–2373 vol.4.
  * [14] T. Bailey, S. Julier, and G. Agamennoni, “On conservative fusion of information with unknown non-Gaussian dependence,” in _2012 15th International Conference on Information Fusion (FUSION)_ , Jul. 2012, pp. 1876–1883.
  * [15] A. Cunningham, V. Indelman, and F. Dellaert, “DDF-SAM 2.0: consistent distributed smoothing and mapping,” in _2013 IEEE International Conference on Robotics and Automation (ICRA)_ , May 2013, pp. 5220–5227, iSSN: 1050-4729.
  * [16] L. Luft, T. Schubert, S. I. Roumeliotis, and W. Burgard, “Recursive decentralized collaborative localization for sparsely communicating robots,” in _Robotics: Science and Systems XII_. Robotics: Science and Systems Foundation, 2016.
  * [17] S. S. Kia, S. Rounds, and S. Martinez, “Cooperative localization for mobile agents: a recursive decentralized algorithm based on Kalman-Filter decoupling,” _IEEE Control Systems Magazine_ , vol. 36, no. 2, pp. 86–101, Apr. 2016.
  * [18] B. J. Frey, F. R. Kschischang, H.-A. Loeliger, and N. Wiberg, “Factor graphs and algorithms,” in _In Proc. 35th Allerton Conf. Communications, Control, and Computing_ , 1997, pp. 666–680.
  * [19] F. Dellaert, “Factor graphs: exploiting structure in robotics,” _Annual Review of Control, Robotics, and Autonomous Systems_ , vol. 4, no. 1, pp. 141–166, 2021.
  * [20] O. Dagan and N. R. Ahmed, “Factor graphs for heterogeneous Bayesian decentralized data fusion,” in _2021 IEEE 24th International Conference on Information Fusion (FUSION)_ , Nov. 2021, pp. 1–8.
  * [21] S. Lubold and C. N. Taylor, “Formal definitions of conservative PDFs,” _arXiv:1912.06780v2 [ math.ST]_ , May 2021.
  * [22] M. R. Walter, R. M. Eustice, and J. J. Leonard, “Exactly sparse extended information filters for feature-based SLAM,” _The International Journal of Robotics Research_ , vol. 26, no. 4, pp. 335–359, Apr. 2007.
  * [23] J. Vial, H. Durrant-Whyte, and T. Bailey, “Conservative sparsification for efficient and consistent approximate estimation,” in _2011 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , Sep. 2011, pp. 886–893, iSSN: 2153-0866.
  * [24] N. Carlevaris-Bianco and R. M. Eustice, “Conservative edge sparsification for graph SLAM node removal,” in _2014 IEEE International Conference on Robotics and Automation (ICRA)_ , May 2014, pp. 854–860, iSSN: 1050-4729.
  * [25] R. Forsling, Z. Sjanic, F. Gustafsson, and G. Hendeby, “Consistent distributed track fusion under communication constraints,” in _2019 22th International Conference on Information Fusion (FUSION)_ , Jul. 2019, pp. 1–8.
  * [26] D. Koller and N. Friedman, _Probabilistic graphical models: principles and techniques_ , ser. Adaptive computation and machine learning. Cambridge, MA: MIT Press, 2009.
  * [27] B. Noack, S. J. Julier, and U. D. Hanebeck, “Treatment of biased and dependent sensor data in graph-based SLAM,” in _2015 18th International Conference on Information Fusion (Fusion)_ , Jul. 2015, pp. 1862–1867.
  * [28] Y. Bar-Shalom, X. R. Li, and T. kirubarajan, “Linear estimation in static systems,” in _Estimation with Applications to Tracking and Navigation_. John Wiley & Sons, Ltd, 2001, pp. 121–177.