# Causal Deterministic SLAM: Representation and Compensation of Independent Systems of Errors

Joel Tari, Patrick Danès

<figure><img src="image_1.png" /></figure>

## To cite this version:

Joel Tari, Patrick Danès. Causal Deterministic SLAM: Representation and Compensation of Independent Systems of Errors. 2026. hal-05511061

HAL Id: hal-05511061

https://hal.science/hal-05511061v1

Preprint submitted on 14 Feb 2026

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_2.png" /></figure>

Distributed under a Creative Commons CC BY 4.0 - Attribution - International License
# Causal Deterministic SLAM:
Representation and Compensation of Independent
Systems of Errors

Joel Tari, Patrick Danès

LAAS-CNRS, Université de Toulouse

Email: {joel.tari,patrick.danes}@laas.fr

***Abstract***—This paper addresses the emblematic Simultaneous Localization And Mapping (SLAM) problem through the lens of causality. The proposed approach explicitly represents the causal deterministic relationships on the robot's relative motion and actuator/sensor signals, together with the effect of exogenous random errors, into a structural causal model and associated causal graph.

The SLAM solution is obtained by “compensating” these errors. This adjustment process can take place only when a loop closure occurs, and proceeds in two stages: first, the errors are partitioned into conditionally independent subsets through d-separation analysis; then, each obtained subset is compensated separately by means of a dedicated algorithm which ensures the respect of Gaussian assumptions and the satisfaction of constraints. The estimation of the robot trajectory and landmark poses comes as a by-product of error compensation. Throughout the paper, the conceptual differences with the standard probabilistic view of SLAM are highlighted. The strengths and limits of the alternative causal approach are discussed, and illustrated on experiments built with synthetic and real data.

## I. INTRODUCTION

A SLAM (Simultaneous Localization and Mapping) is generally understood as a system enabling a robot or device to build a map of an unknown or partially known environment while simultaneously tracking its own pose, or sequence of poses, within that map. Typically, errors affecting actuator/sensor signals accumulate, resulting in unrestrained growth of uncertainty and in turn compromising downstream tasks (e.g., navigation), unless information about revisited places is incorporated. The probabilistic statement of SLAM, initiated by Smith and Cheeseman [38] and consolidated by many contributions (see reviews [16, 3, 5, 37, 20, 8]), posits that robot coordinate frames (e.g., poses) are hidden variables encoded in the factors of their joint probability distribution. This pdf constitutes the cornerstone of their estimation after incorporating data (e.g., odometry, loop closures, etc.). A large body of SLAM literature is based on this paradigm, which has demonstrated impressive successes [43, 19, 14, 36, 32, 8].

<figure><img src="image_3.png" /><figcaption>Fig. 1: Overview of the approach on a pose graph example.</figcaption></figure>

This work was supported by the MIDOC Federation (Intelligent and Sustainable Mobility in Occitanie). Part of the contribution was developped while JT was at Akkodis (PhD program grant ANR2018/0508). Authors are grateful for the help received on the outdoor experiment by: the autOCampus platform steering staff (Occitanie Region), TwinswHeel's for providing d'Artagnan robot, GUIDE-GNSS Gelocation Test Lab for the RTK-GNSS track, and former LAAS interns Augusto Capone and Anushree Shrivastava.

However, the search for an alternative representation is not an exhausted subject, especially when considering the broader context of the emulation of a form of spatial intelligence in robots.

Meanwhile, for the last decade, causal inference—especially Structural Causal Models (SCMs)—is increasingly popular in the field of AI [33, 35]. Probabilistic models do capture independence relationships and answer inferential queries on *seeing* (*what is* the joint/marginal posterior pdf conditioned on observations), yet only causal models, and particularly SCMs, can answer queries on *doing* (*what if* an action is done on a set of variables) and on *imagining* (*why*, i.e., *what if another action were to be done*, in a situation different from
the unfolded) [4].

As we believe that these capabilities are essential for spa-
tial understanding, the representation of the SLAM problem
through the lens of this causal framework is hereafter explored.

As such, this paper contains three main contributions: (a) a
rigorous modeling of canonical SLAM problems supported by
well-founded causal assumptions; (b) a key distinction, from
model implications, between a problem statement for model
testing and another for adjustment; (c) an estimation schemes
for both problems.

Section II motivates the use of causal determinism as a
potent paradigm for the SLAM problem. Section III states
the assumptions necessary to address SLAM with Structural
Causal Models. The rigorous incorporation of loop closures
is thoroughly discussed. Directional separation (d-separation)
analysis is leveraged on associated causal graphs to par-
tition errors into conditionally independent sets which can
be processed separately. Section IV is concerned with the
"compensation" of errors, i.e., the corrections to be brought
to these variables so that they can explain measurements. We
distinguish the estimation of feasible error vectors, compliant
with the assumed model, from the estimation of error vec-
tors which minimizes the expected squared distance to their
true hidden outcomes. A variety of experiments is presented
in Section V, on existing synthetic and real datasets, for
comparison with the established probabilistic approach. The
proposed representation is shown to provides new insights on
the analysis of a new real scale experiment. Code and datasets
are provided [42].

II. BACKGROUND

Causality can be considered as a critical feature of trust-
worthy robotics systems [22]. A number of applications has
recently popped up, such as helping to predict and prevent
failures [15], improving tasks in navigation [23, 11], manip-
ulation [6], understanding 3D object relations [29], possibly
in shared environment [9]. Its interest stems from modeling
transparency and enforcement of invariants such as temporal
precedence.

To our best knowledge, the deterministic viewpoint which
is key in the SCM framework [33, §1.4] is not leveraged
in SLAM existing work. However, SLAM actually has a
great conceptual compatibility and not commonly recognized
relation to causality (specifically causal determinism). Indeed,
while the problem of adjustment of triangles in geodesy
has been studied as an ancestor of SLAM systems [1], our
recent historical study [41] has emphasized the early role
of causal determinism in the late 18th to early 19th century
for the “compensation” of measurement errors in “chain of
triangles” of meridian and geographical surveys. Correcting
errors in geodesy required to combine equations of uncertain
observations. The problem was emblematic in the period [40]
and studied by well known scholars, such as Boscovich, the
Cassini-s, Maupertuis, Laplace, Legendre, Gauss etc.

Alongside the study of motion of comets, it served as
a primary driver for two key contributions in probabilistic

inference: the discovery of least squares (1795-1809) and the
first proof of the central limit theorem (1810)<fnref n="1" />.

At the time, causal determinism was a common background
conception [44] which stated that physical processes are
deterministic and probability is due partly to knowledge, and
partly to ignorance. Consequently, in geodesy, the variables to
be compensated were typically the measurement “errors” since
they were the source of ignorance. Causality was however only
informal, until contemporary advances in Causal AI justify a
renewed interest for such viewpoints. We recall below the key
mathematical causal objects to be used in the paper.

**Definition 1** (Structural Causal Model [33]). An SCM is a
4-tuple, SCM =< V, E, p(E), {fᵢ} >, where:

• V is the set of endogenous variables.

• E is the set of exogenous variables, or “errors”.

• p(E) is the joint pdf over the errors.

• F = {fᵢ(., .)}ᵢ=₁<sup>|V|</sup> is a set of structural equations

$$
\forall v_i \in V, v_i := f_i (pa_i, \mathcal{E}_i)
$$

involving for each endogenous variable $v_i$ the set $pa_i \subset V$ of its parents and the set $\mathcal{E}_i \subseteq \mathcal{E}$ of the errors affecting it. Importantly, the assignment operator $:==$ means that its LHS $v_i$ comes as the effect of the causes appearing in its RHS.

An SCM does convey a causal deterministic viewpoint of
the world, in that the functions fᵢ(., .)'s are deterministic
and the errors are the single source of uncertainty and have no
cause. These express the modelers' limited knowledge in the
form of the pdf p(E) over them. In fact, this is precisely how
probability theory was framed by Laplace<fnref n="2" /> in the so-called
"theory of errors" era [28, Intro].

**Definition 2 (Causal Graph [33]).** Any SCM M can be associated with a directed acyclical graph (DAG) G<sub>M</sub>, termed “Causal Graph”, such that: each node corresponds to a variable in E ∪ V; for each v<sub>i</sub> ∈ V, directed edges point from the members of pa<sub>i</sub> ∪ E<sub>i</sub> involved in f<sub>i</sub>(., .) to v<sub>i</sub>; nodes with no incident edge, e.g., error nodes e<sub>j</sub> ∈ E, are said “root nodes”.

Causal graphs transparently depict model assumptions, and
expose conditional independences through d-separation (Ap-
pendix VI-A). To sum up, we formulate the SLAM problem
as a set of causal and deterministic relations, submitted to
uncertainty through the effect of exogenous errors. Those
errors are modeled to appear alongside robot actuation and
sensing data. This is referred as the representation, which
logical implications will in turn guide the estimation scheme.

<footnote n="1">¹Outstanding applications were the Dunkirk-Barcelona chain of triangles (1792-1799), used for the definition of the metric system [2][28, p. 531], and the surveying of the Kingdom of Hanover by Gauss [17] (1821-1825).</footnote>

<footnote n="2">²Laplace's framing of probability theory includes a famous thought experiment, known later as the 'Laplace demon'. Notably, similar metaphorical statements can be found in the (sometimes preceding) work of other scholars incidentally also familiar with geodetic adjustment: e.g., Boscovich, Maupertuis, and Liagre [44, 26, 41].</footnote>
### III. CAUSAL REPRESENTATION

In the sequel, bold variables (e.g., $\mathbf{x}$, $\mathbf{u}$, $\mathbf{z}$, $\mathbf{\Lambda}$), live in a Lie manifold—typically, $SO(2)$, $SE(2)$, $SO(3)$, $SE(3)$, $SGAL(3)$. Regular variables (e.g., $x$) represent vectors in Euclidean space. Capital letters stand for matrices. For mathematical operators, the notations of Solà et al. [39] are used, whereby: ◦ terms the matrix Lie group composition; · stands for the group action of a manifold member onto a vector (e.g., in $\mathbf{\Lambda} \cdot \mathbf{y}$); Log is the mapping from an element of the manifold to the Euclidean space isomorphic to its Lie algebra; Exp is the inverse mapping. Fundamentals of the causal representation are introduced alongside the following toy SLAM scenario.

**Scenario 1.** (Figure 2) From a starting pose $\mathbf{x}_0$, a robot operates relative motions from times $t_1$ to $t_5$. A first pointwise landmark $l_1$ is discovered through the robot exteroceptive perception system at time $t_1$ and resighted at $t_4$. A second pointwise landmark $l_2$ is discovered at $t_2$ and re-sighted at $t_3$. Control inputs and measurements are corrupted by noise.

<figure><img src="image_4.png" /><figcaption>Fig. 2: Depiction of Scenario 1.</figcaption></figure>

This Scenario 1 involves a sequence of control inputs $U = \{\mathbf{u}_0^1, \mathbf{u}_1^2, \mathbf{u}_2^3, \mathbf{u}_3^4, \mathbf{u}_4^5\} \subset SE(d)$. Their associated errors are generically noted $w_i^{i+1} \in \mathfrak{SE}(d)^\vee = \mathbb{R}^{\frac{d(d+1)}{2}}$. Unless indicated, measurements live in the group $SE(d)$ acts on, i.e., in $\mathbb{R}^d$. They will be explained to be noted $\mathcal{Y} = \{\bar{y}_1^{+0}, \bar{y}_2^{+0}, \bar{y}_2^{+1}, \bar{y}_1^{+3}\} \subset \mathbb{R}^d$. Their additive sighting errors will be referred to as $\nu_i^{+j} \in \mathbb{R}^d$. So, one gets $\mathcal{E} = \{w_0^1, \dots, w_4^5, \nu_1^{+0}, \dots, \nu_1^{+3}\}$.

#### A. Causal statements

The formulation of the SLAM SCM is guided by underlying assumptions. Unlike probabilistic approaches, these take the form of plausible causal statements. Each one will be presented in turn in relation to Scenario 1, to build incrementally the global causal graph of Figure 3. Some statements are trivial, while others will be justified in a follow-up discussion. Importantly, in the causal graph, grayed single-circled nodes are observed variables while grayed double-circled nodes correspond to interventions, i.e., to variables acted upon.

**Causal Statement 1.** A control input $\mathbf{u}$ and an associated error $\mathbf{w}$ define the relative robot motion $\mathbf{\Lambda}$, e.g., $\mathbf{\Lambda}_0^1 := \mathbf{u}_0^1 \circ \text{Exp}(w_0^1)$ in Figure 3a.

**Causal Statement 2.** The composition of a past robot pose $\mathbf{x}_i$ and relative motion $\mathbf{\Lambda}_i^j$ from time $i$ to $j > i$, defines a new robot pose $\mathbf{x}_j$, e.g., $\mathbf{x}_1 := \mathbf{x}_0 \circ \mathbf{\Lambda}_0^1$ in Figure 3a.

The perception of landmarks requires more involved notations. Let $y_k^{+n}$ stand for the true relative pose of a landmark $l_k$ w.r.t. the frame of the observing pose, where the superscript $+n$

denotes the elapsed time since the discovery of this landmark (so, $y_k^{+0}$ characterizes $l_k$ at discovery). The measurement value and error are respectively termed $\bar{y}_k^{+n}$ and $\nu_k^{+n}$. Sometimes, the notation $l_k$ will also stand for the landmark's relative position w.r.t. the robot starting pose $\mathbf{x}_0$.

**Causal Statement 3.** When a landmark $l_k$ is discovered, its measurement value $\bar{y}_k^{+0}$ and error $\nu_k^{+0}$ define its true relative position w.r.t. the robot, e.g., $y_1^{+0} := \bar{y}_1^{+0} + \nu_1^{+0}$ (Figure 3a).

**Causal Statement 4.** A landmark position $l_k$ w.r.t. the robot starting pose $\mathbf{x}_0$ is defined by the robot pose and the true landmark relative position w.r.t. the robot, both at discovery time, e.g., $l_1 := \mathbf{x}_1 \cdot y_1^{+0}$.

In view of Causal Statements 1-4, the SCM can be built at the first step of Scenario 1 (i.e., until time $t_1$):

$$ (\text{SCM}_{t_1}) : \left\{ \begin{array}{l} \mathbf{\Lambda}_0^1 := \mathbf{u}_0^1 \circ \text{Exp}(w_0^1) \\ \mathbf{x}_1 := \mathbf{x}_0 \circ \mathbf{\Lambda}_0^1 \\ y_1^{+0} := \bar{y}_1^{+0} + \nu_1^{+0} \\ l_1 := \mathbf{x}_1 \cdot y_1^{+0} \end{array} \right. \qquad (1) $$

The SCM at the second step is the union of $(\text{SCM}_{t_1})$ and the novel structural equations (colored in Figure 3b):

$$ (\text{SCM}_{t_2}) : (\text{SCM}_{t_1}) \cup \left\{ \begin{array}{l} \mathbf{\Lambda}_1^2 := \mathbf{u}_1^2 \circ \text{Exp}(w_1^2) \\ \mathbf{x}_2 := \mathbf{x}_1 \circ \mathbf{\Lambda}_1^2 \\ y_2^{+0} := \bar{y}_2^{+0} + \nu_2^{+0} \\ l_2 := \mathbf{x}_2 \cdot y_2^{+0} \end{array} \right. \qquad (2) $$

The necessary considerations to process the two landmark resights in Scenario 1 are stated next.

**Causal Statement 5.** The relative motion between steps $i$ and $j > i + 1$ is defined by the cumulative composition of atomic relative motions, e.g., $\mathbf{\Lambda}_1^4 := \mathbf{\Lambda}_1^2 \circ \mathbf{\Lambda}_2^3 \circ \mathbf{\Lambda}_3^4$ (Figure 3d).

**Causal Statement 6.** The true relative position $y_k^{+0}$ of landmark $l_k$ w.r.t. the robot at discovery time, together with the robot relative motion from discovery to resight of $l_k$, define the value of the relative position $y_k^{+n}$ of $l_k$ w.r.t. the new robot pose, e.g., $y_2^{+1} := (\mathbf{\Lambda}_2^3)^{-1} \cdot y_2^{+0}$ (Figure 3c), and $y_1^{+3} := (\mathbf{\Lambda}_1^4)^{-1} \cdot y_1^{+0}$ (Figure 3d).

**Causal Statement 7.** In the vein of Causal Statement 3, the measurement value $\bar{y}_k^{+n}$ of landmark $l_k$ at any resight time is defined by its relative position $y_k^{+n}$ and associated measurement error $\nu_k^{+n}$, e.g., $\bar{y}_2^{+1} := y_2^{+1} + \nu_2^{+1}$ (Figure 3c).

The full model $\text{SCM}_{t_5}$ can be easily built on the top of (2), but is not inserted for space reasons. For completeness, since pose-graph problems are frequently encountered, a last causal statement on pose based loop closure measurements follows.

**Causal Statement 8.** The measurement value $z_i^j$ of the resight of a robot pose is defined by the true robot relative motion $\mathbf{\Lambda}_i^j$ between times $i$ and $j$ and associated measurement error $\zeta_i^j$, e.g., $z_1^4 := \mathbf{\Lambda}_1^4 \circ \text{Exp}(\zeta_1^4)$ (Figure 1).
<figure><img src="image_5.png" /><figcaption>Fig. 3: Sequence of causal graphs representing Scenario 1, from time $t_1$ (a), to $t_5$ (e). At each step, colored strokes emphasize new nodes and edges.</figcaption></figure>

## B. Discussion

A distinctive aspect of our representation is the explicit modeling of relative motions $\mathbf{\Lambda}$. One could have considered, for instance, that the measurement $z_4^1$ in Figure 1 is defined directly by $x_1^{-1} \circ x_4$. Instead, as Causal Statement 8 indicates, $z_4^1$ is the effect of a cumulative relative motion $\mathbf{\Lambda}_1^4$. But here, the trajectory appears only as a by-product. The reason is that from a causal perspective, the (relative) motions are considered a fact of the experience, while the poses are nothing more than made-up sequential integration of these motions from an arbitrary starting pose $x_0$. Therefore, it is considered that poses can not be causal ancestors of observations: only the motions can explain their existence.

Concerning landmark discovery, it can be noted that the discovery error remains independent of any other errors, at least until resight (e.g., $\nu_1^{+0} \perp w_0^1$). Positions advocating some of the causal statements can be found in the literature: one of SLAM pioneering paper by Chatila and Laumond [10, § 1.3] noted that corrections can only occur once a place is re-observed (excluding obviously overlapping loop closures), what this approach formalizes. Last, note a difference in the causal interpretation of landmark resight and landmark discovery, despite being the same physical process of measurement: the former represents the measure as an observation, a consequence of past relative position and relative motions; while the latter is treated as an intervention (i.e., double-

circled) which creates the landmark (as $y_k^{+0}$ comes as an ancestor of $l_k$). This can be justified on the basis that the causal process being enforced is not one of a world model, but one of internalized topography. An analogy of this view can be made when people engage in cartography with a set of magnets on a white board: the ‘discovery’ consists in placing the magnet on the board, while the resight amounts to displacing it.

All in all, as depicted in Figure 3c, these considerations, concurrently with the d-separation criterion, enables to partition an otherwise monolithic SLAM system into subsets of correlated errors. This partitioning scheme is exact with regard to the problem formulation.

## IV. COMPENSATION OF ERRORS

The compensation, or “adjustment”, of errors designates the computation of estimates that are consistent with measured variables. Fundamentally, once the errors of an SCM have been partitioned into independent subsets conditionally on the data, this process is applied independently to each partition. As discussed in Section III-B, systems of errors which do not include loop closures cannot be compensated as, strictly speaking, they feature no observation (only interventions). This is the case for $w_0^1$ and $w_4^5$ in Figure 4c, their estimates will remain at whatever prior distribution is attached to them. We suppose that the errors are Gaussian distributed, an assumption —conventional in SLAM [32, § 1.3] and geodesy [1, 41]— not required in the above modeling stage. So, any $\epsilon_i \in \mathcal{E}$
<figure><img src="image_6.png" /><figcaption>(c) Causal graph from Figure 3e. The pink ribbon depicts paths of belief propagation between errors, as determined by d-separation analysis.</figcaption></figure>

Fig. 4: Probabilistic and causal graphical models for Scenario 1. The proposed partitioning is possible neither for factor graphs nor for Bayes networks, partly due to fact that they neither directly express errors, nor differentiate landmark discovery and resight.

satisfies $\epsilon_i \sim \mathcal{N}(0, L_i L_i^\top)$, with $L_i$ its (assumed full-rank) covariance matrix lower triangular Cholesky factor<fnref n="3" />. Without loss of generality, stacking the vectors $e_i = L_i^{-1} \epsilon_i$ leads to the hereafter called “standardized” error vector $e \sim \mathcal{N}(0, I_n)$.

### A. Feasibility of a solution

Hard constraints on errors arise upon incorporation of loop-closures. In Scenario 1, the two closures lead to

$$ \bar{y}_1^{+3} = (\mathbf{u}_1^2 \circ \mathrm{Exp}(w_1^2) \circ \mathbf{u}_3^2 \circ \mathrm{Exp}(w_3^2) \circ \mathbf{u}_3^4 \circ \mathrm{Exp}(w_3^4))^{-1} \cdot (\bar{y}_1^{+0} + \nu_1^{+0}) + \nu_1^{+3} \quad (3) $$

$$ \bar{y}_2^{+1} = (\mathbf{u}_2^3 \circ \mathrm{Exp}(w_2^3))^{-1} \cdot (\bar{y}_2^{+0} + \nu_2^{+0}) + \nu_2^{+1} \quad (4) $$

By replacing the error terms by their standardized versions, the set of all constraints can be generically written as $h(e) = b$, with $h()$ a—generally nonlinear—function and $b$ a vector.

Besides, a key consequence of the SCM set up in Section III and the above Gaussian assumption is that the true hidden standardized error vector outcome $e_{\text{true}}$ must be a sample of $\mathcal{N}(0, I_n)$. Hence, $\|e_{\text{true}}\|^2 = e_{\text{true}}^\top e_{\text{true}}$ must pass the test of checking if it is distributed along the chi-squared distribution with $n$ degrees of freedom, or, in first approximation for large $n$, if $e_{\text{true}}^\top e_{\text{true}} \sim \chi^2(n) \underset{n \to \infty}{\approx} \mathcal{N}(n, 2n)$. A well-suited alternative is the Jarque-Bera test [24] (Appendix VI-B).

In Table I, we study, for two synthetic benchmarks, those test scores on two standardized error vectors: $e_{\text{true}}$ (deduced

<table><caption>TABLE I: Jarque-Bera and squared norm normality tests on standardized error vector outcomes $e_{\text{true}}$ and $e_{\text{MAP}}$ for Manhattan datasets.</caption><thead><tr><th>Dataset</th><th>n ≜ dim(e)</th><th>e<sup>T</sup><sub>true</sub>e<sub>true</sub></th><th>JB<sub>0,1</sub>(e<sub>true</sub>)</th><th>e<sup>T</sup><sub>MAP</sub>e<sub>MAP</sub></th></tr></thead><tbody><tr><td>M3500</td><td>16 359</td><td>16 421.4</td><td>1.88</td><td>5 742.2</td></tr><tr><td>M10k</td><td>192 933</td><td>191 905.9</td><td>1.05</td><td>163 247.5</td></tr></tbody></table>

from the ground truth of poses) and $e_{\text{MAP}}$. The latter is issued from Maximum Likelihood Estimates (MLE) / Maximum A Posteriori (MAP) of probabilistic paradigm underlying many state-of-the-art SLAM solvers. We deem “unfeasible” an error candidate $e$ for which one of the tests scores sits outside the 99% confidence interval: i.e., $JB_{0,1}(e) > 9.21$ or $e^\top e \notin [n - 2.576\sqrt{2n}, n + 2.576\sqrt{2n}]$, where $n \triangleq \text{dim}(e)$. Table I shows that vector $e_{\text{true}}$ adheres by those conditions, as expected, while the study of $e_{\text{MAP}}^\top e_{\text{MAP}}$ suffices to invalidate the feasibility of $e_{\text{MAP}}$.

In summary, we consider a candidate standardized error to be feasible if it (a) satisfies closure constraints, and (b) does not fail a normality test.

### B. Minimum norm solution

As the name suggests it, the aim is to bring the least amplitude of corrections which satisfy all the constraints.

**Problem 1.** The minimum norm solution is defined by

$$ \hat{e} = \arg \min_{e} e^{\top} e \text{ subject to } h(e) = b. \quad (5) $$

**Remark 1.** The solution $\hat{e}$ to Problem 1 can be viewed as the MAP estimate of the standardized error vector under a perfect likelihood (the deterministic constraint). However, we prefer avoiding Bayesian considerations in favor of the first principles that have led to the proposed paradigm to SLAM. Observe that, in contrast to feasible solutions as per Section IV-A, $\hat{e}$ is not concerned with—and may fail at—normality tests in the same way as the MLE/MAP estimates referred to in the last line of Table I.

To sum up, there are two distinct perspectives on what counts as a solution for our approach. However, what may appear as a dilemma can be reconciled under the following consideration: the minimum norm solution can serve as the major estimation objective, while solution feasibility can provide an answer from the standpoint of model testing. The latter view can be useful to detect wrong models, what cannot be done solely with the minimum norm solution. This is supported by a dedicated experiment in Section V.

### C. Estimation scheme for minimum norm

Problem 1 can be solved by iterative local search from an initial error guess $\tilde{e}$. Let the Taylor expansion of the constraints write as $h(\tilde{e} + \delta e) \approx h(\tilde{e}) + H \delta e$, with $H$ the Jacobian matrix $[\frac{\partial h(e)}{\partial e}]_{e=\tilde{e}}$. Its expressions for landmark resight and cumulative motion closures are omitted for space, but are provided in Appendix VI-C. Note that $H \in \mathbb{R}^{m \times n}$ is a wide

<footnote n="3">³Note that errors $\epsilon_i$ are zero-mean. If some random noises of the problem are suspected to be nonzero-mean, then this bias should explicitly appear in the endogenous part of the SCM, keeping $\epsilon_i \sim \mathcal{N}(0, L_i L_i^\top)$ unchanged.</footnote>
$(m < n)$ sparse full (row) rank matrix, for the number of closures is smaller than the number of errors. After forming the Lagrangian of the problem, the optimum increment $\delta\hat{e}$ can be shown to write as

$$ \hat{\delta e} = [I_n \quad 0] M^{-1} \begin{bmatrix} -\hat{e} \\ \tilde{b} \end{bmatrix}, \quad (6) $$

$$ \text{with } M = \begin{bmatrix} I_n & H^\top \\ H & 0 \end{bmatrix} \text{ and } \tilde{b} = b - h(\hat{e}). $$

The minimum norm solution is obtained by successive linearizations and iterations $\tilde{e} \leftarrow \tilde{e} + \delta\hat{e}$ until a fixed point is reached up to a precision threshold, where one sets $\hat{e} \leftarrow \tilde{e}$. While the computations of Jacobian matrices $H$ has a significant cost, the suitable decomposition of $M$ leading to $\delta\hat{e}$ is the major bottleneck of the procedure.

### D. Estimation scheme for feasible solution

To find a feasible solution, the following empirical procedure is proposed. Starting from a minimum norm solution $\hat{e}$, $K$ candidate for feasibility (indexed by $k$) are built by setting

$$ e_f^{(k)} = H^\dagger \tilde{b} + \alpha(z^{(k)})(I - H^\dagger H)z^{(k)}, \text{ with } z^{(k)} \sim \mathcal{N}(0, I_n). \quad (7) $$

In (7), $H^\dagger = H^T(HH^T)^{-1}$ is entailed in the minimum norm error $\hat{e} = H^\dagger \tilde{b}$ of the linearized system at the last iteration, and $P = I - H^\dagger H$ is the projector onto the null space of $H$. The norm correction factor $\alpha(z^{(k)}) \in \mathbb{R}$ is set to

$$ \alpha(z^{(k)}) = \sqrt{\frac{r - \|\hat{e}\|^2}{\|Pz^{(k)}\|^2}}, \quad (8) $$

with $r \sim \mathcal{N}(n, 2n)$ so as to ensure that $\|e_f^{(k)}\|^2 \sim \mathcal{N}(n, 2n)$. Then, every $e_f^{(k)}$ is locally refined by searching its closest point satisfying the nonlinear constraints, what is not developed for space reasons. It is expected that some of the $K$ candidates may be rejected by the normality test. The requirement is that at least one feasible solution survives the process.

### E. SLAM solution

Robot poses and landmark positions are then straightforwardly deduced from the error vector estimate by passing it through the relevant equations of the SCM. So, they come as a by-product of error compensation.

## V. EXPERIMENTS

We consider commonly used datasets in SLAM benchmarks, synthetic and real, as well as a new outdoor dataset collected by a mobile robot on a campus (Figures 5 and 9). We propose several experiments to give a broad answer to the following questions. How often can the partitioning scheme be leveraged? Is the overhead of causal graph manipulation significant? Can the interest of feasible solutions be highlighted for model testing? How can the approach help improve result analysis? What is its comparative efficiency?

Experiments are run on an Intel i7-8665U CPU with 32 GB RAM.

### A. Lifecycle of independences

We run the selected representative SLAM scenarios incrementally. We study the number of independent systems of errors over time, and the proportion of them which can be compensated (Figure 6). It could be argued that as several scenarios close a loop on the beginning of their trajectory, the scope of error partitioning is limited. We show that for some of them, the partitioning can still be beneficial during the intermediate time steps preceding large loop closures. Some scenarios, such as Sphere2500, entail a single system of errors, while others feature a varying number of independent partitions. Table II shows that the overhead of causal graph manipulation is negligible, as its total time (cumulated from the first to the last step) is in the order of a second for scenarios typically lasting tens of minutes.

### B. Model falsification

We consider the solution of a simulated medium-sized Manhattan-like scenario, with three models assuming different noise covariances: the correct one (characterizing the genuine noise); an underconfident model (where the genuine noise standard deviations are multiplied by 100); an overconfident model (dividing the genuine standard deviations by 10). As depicted in Figure 7, the obtained estimations are visually similar. As experimenters, we know $e_{\text{true}} \in \mathbb{R}^{1146}$. We study the capacity for each model to generate feasible solutions. Figure 8a plots $\|e - e_{\text{true}}\|^2$ for the correct model: this squared distance to ground truth is plotted in red when $e$ is the minimum norm solution $\hat{e}$, and portrayed by a box plot for 1000 feasible solutions $e$ randomly generated around $e_{\text{true}}$. The minimum norm solution is closer to the error ground truth, what supports its choice as the major estimation objective.

An interesting point concerns the study of the norm of feasible solutions for each model. Whether for the underconfident or the overconfident model, we cannot generate solutions which pass normality tests. This is illustrated in Figure 8b, whereby no generated solution from the underconfident model has a squared norm within the confidence interval of the $\chi^2(1146) \approx \mathcal{N}(1146, 2292)$ distribution. This provides a basis to reject both underconfident and overconfident (not represented, at its squared norm is much higher) solutions. Conversely, generating feasible solutions with the correct model raises no issue.

This experiment vindicates feasible solutions introduced in Section IV, especially given the fact that the minimum norm solution of the underconfident model has a smaller norm than the one from the correct model (red dots in Figure 8b). Note, as a first limitation, that it is harder to falsify the underconfident model (a sd factor of 100 is needed, while a division by 10 suffices for the overconfident model). Secondly, for this curated experiment, the sources of wrongness, the covariances, were known in advance. This means that in practical applications, when encountering a similar situation (no feasible solution found), this method would not *by itself* indicate whether the issue comes from wrong covariances, outliers, false measurements, etc.
<figure><img src="image_7.png" /><figcaption>Fig. 5: Trajectories obtained from our proposed SLAM solution on a mix of publicly available datasets [31, 25, 7, 21]. From left to right: M3500, CSAIL, Sphere2500, Kitti (×5), Intel, Victoria Park and Parking Garage.</figcaption></figure>

<table><caption>TABLE II: Total overhead partitioning timings (s) on incremental runs (from first step to last step).</caption><thead><tr><th></th><th>M3500</th><th>CSAIL</th><th>Sphere2500</th><th>kitti00</th><th>kitti05</th><th>kitti06</th><th>kitti07</th><th>kitti09</th><th>Intel</th><th>Victoria Park</th><th>Parking Garage</th></tr></thead><tbody><tr><th>Cumulative Partitioning time</th><td>1.07</td><td>0.18</td><td>0.20</td><td>0.76</td><td>0.74</td><td>0.74</td><td>0.86</td><td>0.75</td><td>0.20</td><td>2.97</td><td>1.73</td></tr></tbody></table>

<table><caption>TABLE III: Performance results on datasets. The lines ‘RSoS’ (as in Residual Sum-of-Squares), ‘g2o Timing’ [27], commit d89ab60a], ‘GTSAM Timing’ [12, v4.2], and ‘PSLAM Timing’ indicate results obtained by probabilistic SLAM solvers (baseline). We can see that the RSoS is very similar to the squared norm of minimum errors $e^T e$. Timings are given in seconds. The main cost of our approach is realized in the sparse decomposition step, and to a lesser extent in the time to compute Jacobian blocks.</caption><thead><tr><th>Metrics</th><th>M3500</th><th>CSAIL</th><th>Sphere2500</th><th>kitti00</th><th>kitti05</th><th>kitti06</th><th>kitti07</th><th>kitti09</th><th>Intel</th><th>Victoria Park</th><th>Parking Garage</th></tr></thead><tbody><tr><th>dim(e)</th><td>16359</td><td>3510</td><td>29694</td><td>13971</td><td>7983</td><td>3390</td><td>3198</td><td>4731</td><td>5505</td><td>28169</td><td>37614</td></tr><tr><th>Nb iterations</th><td>5</td><td>3</td><td>4</td><td>3</td><td>3</td><td>3</td><td>3</td><td>3</td><td>3</td><td></td><td>3</td></tr><tr><th>e<sup>T</sup>e</th><td>5742.2</td><td>40.40</td><td>1351.4</td><td>98.32</td><td>157.1</td><td>25.95</td><td>12.66</td><td>34.383</td><td>546.3</td><td>426.51</td><td>1.268</td></tr><tr><th>Jacobian time</th><td>2.4</td><td>0.22</td><td>3.68</td><td>1.09</td><td>0.27</td><td>0.12</td><td>0.05</td><td>0.05</td><td>0.98</td><td>8.51</td><td>78.5</td></tr><tr><th>Decomposition time</th><td>10.6</td><td>0.29</td><td>11.8</td><td>2.64</td><td>0.68</td><td>0.36</td><td>0.04</td><td>0.07</td><td>5.13</td><td>1221.7</td><td>704.6</td></tr><tr><th>Found feasible solutions (Y/N)</th><td>Y</td><td>Y</td><td>Y</td><td>Y</td><td>Y</td><td>Y</td><td>Y</td><td>Y</td><td>Y</td><td>N (100 tries)</td><td>N (100 tries)</td></tr><tr><th>RSoS (PSLAM)</th><td>5742.2</td><td>40.40</td><td>1351.4</td><td>98.32</td><td>157.1</td><td>25.95</td><td>12.66</td><td>34.383</td><td>546.24</td><td>615.9</td><td>1.268</td></tr><tr><th>PSLAM Timing</th><td>0.64</td><td>0.07</td><td>2.54</td><td>0.44</td><td>0.26</td><td>0.064</td><td>0.054</td><td>0.087</td><td>0.097</td><td>18.9</td><td>1.07</td></tr><tr><th>g2o Timing</th><td>0.14</td><td>0.067</td><td>1.134</td><td>0.24</td><td>0.044</td><td>0.062</td><td>0.031</td><td>0.036</td><td>0.052</td><td>0.198</td><td>0.226</td></tr><tr><th>GTSAM Timing</th><td>0.38</td><td>0.1</td><td>0.336</td><td>0.29</td><td>0.19</td><td>0.1</td><td>0.2</td><td>0.1</td><td>0.14</td><td></td><td>0.185</td></tr></tbody></table>

<figure><img src="image_8.png" /><figcaption>Fig. 6: Number of independent (blue) and compensable (orange) systems of errors over time for various datasets. Baseline (black) corresponds to a single system.</figcaption></figure>

<figure><img src="image_9.png" /><figcaption>Fig. 7: Manhattan-like scenario compensated using 3 different covariance values in the model: underconfident (left), correct (middle), and overconfident (right). The trajectories from their respective minimum norm solution are indistinguishable.</figcaption></figure>

### C. Outdoor experiment

A mobile robot is manually navigated in a campus (Figure 9). Dedicated April Tags landmarks are registered and localized by vision. The robot’s absolute track is computed by

<figure><img src="image_10.png" /><figcaption>Fig. 8: Tests for model acceptation or falsification.</figcaption></figure>

a RTK-GNSS system. Its path features a total of 6797 poses and 203 landmarks relative position measurements (including 4 discoveries). The problem is planar: the robot poses live in $SE(2)$, the motion errors are in $se(2)^{\vee} = \mathbb{R}^3$, and the landmark relative position measurement errors belong to $\mathbb{R}^2$.

The SLAM solution begins by identifying, from the causal representation, a set of 1743 independent systems of errors to be adjusted, all but one of which are made of a lone, independent error: 979 lone motion errors at the beginning, before the first landmark re-observation; 763 lone motion errors after the last landmark re-observation. The independent errors are associated with the poses close to the starting and final point (blue triangle in the parking). Only the “main” system of errors can be adjusted. It is composed of 5054 motion errors and
199 errors on re-observations of landmarks. All these errors are mutually dependent conditioned on the measurements. 199 constraints of the form (3)–(4) are exhibited, and the MAP estimate is sought for. The covariance of the motion errors and of the landmark relative position measurements are respectively set to $\Sigma_{rm} = (10^{-7} \quad 10^{-9} \quad 10^{-6})$, and $\Sigma_l = (1/44 \quad 1/44 \quad 1/44)$. Starting from the initialization guess 0, the adjusted values of the error vector are obtained after 6 linearized constrained least squares iterations. The purple, green and gray paths on Figure 9 respectively depict the MAP estimate, the RTK-GNSS ground truth, and the odometry.

<figure><img src="image_11.png" /><figcaption>Fig. 9: Outdoor experiment with a mobile robot.</figcaption></figure>

### D. Performances

Figure 5 shows a number of SLAM scenarios being solved successfully by the proposed approach. Failed compensations include datasets such as ‘M10k’, ‘grid3D’ and ‘Torus3D’, these are generally larger and denser datasets. The limitation of the approach, in its current form, comes from the fact that there are denser block rows in the Jacobians (from large loop closures) which translates to considerably less efficient sparse matrix decompositions.

Similarly, successful compensations in the Figure 5 are also much slower than efficient graphSLAM implementations as indicated in Table III. Last, we note that an equal score between the residual sum of squares (RSoS) of graphSLAM techniques [43] and the proposed estimation scheme, which was not obvious, since the decomposed matrix in both approaches are quite different. For large problems, such as Victoria Park, residual scores become unequal, which could be attributed to different numerical behavior and/or initial guess. This calls for further mathematical analysis on the equivalences between the two optimization.

## VI. CONCLUSION

This paper presented a causal deterministic view of SLAM with an emphasis on the representation by causal graphs and on the compensation of partitions of correlated errors. Overall, the motivation of the causal SLAM work is to serve as the basis for future processing of causal queries, provided downstream tasks also have a causal expression. The framework is based on causal considerations justified in III. In our view, the proposed modeling provides novel useful insights when addressing the issue of compensations,

for instance for model falsifiability. Compared to state-of-the-art SLAM implementations, numerical and performance issues arise when facing large problems. Ongoing work addresses the composition of motions to recover sparsity. Among other planned developments are the investigation of causal interventions.

# APPENDIX

## A. d-separation

Directional separation (d-separation) is a graphical criterion for the identification of conditional independences in the underlying probability distribution of a causal graph $G$.

**Definition 3 (d-separation).** *From Pearl et al. [34, def 2.4.1]. A path between two nodes A and C of $G$ is said to be blocked by a set of nodes $V_z$ if and only if:*

1) the path contains a chain of nodes $A \to B \to C$, or $C \to B \to A$, or a fork $A \leftarrow B \to C$ such that the middle node $B \in V_z$, or,

2) the path contains a collider $A \to B \leftarrow C$ such that neither the collision node B nor any of its descendants belong to $V_z$.

If $V_z$ blocks every path between two nodes A and C, then A and C are said to be d-separated conditioned on $V_z$.

## B. Jarque-Bera normality test

Given a set $\mathcal{X}$ of $n$ scalar samples $x_i$, of mean $\bar{x}$, the Jarque-Bera variable writes as [24]:

$$ \mathrm{JB}(\mathcal{X}) \triangleq \frac{n}{6} \left( S(\mathcal{X})^2 + \frac{(K(\mathcal{X}) - 3)^2}{4} \right), \quad (9) $$

with:

$$ \left\{ \begin{aligned} S(\mathcal{X}) &\triangleq \frac{\frac{1}{n}\sum_{i=1}^{n}(x_i - \bar{x})^3}{\left(\frac{1}{n}\sum_{i=1}^{n}(x_i - \bar{x})^2\right)^{\frac{3}{2}}} \\ K(\mathcal{X}) &\triangleq \frac{\frac{1}{n}\sum_{i=1}^{n}(x_i - \bar{x})^4}{\left(\frac{1}{n}\sum_{i=1}^{n}(x_i - \bar{x})^2\right)^2}. \end{aligned} \right. \quad (10) $$

By property, if the samples of $\mathcal{X}$ are normally distributed, the Jarque-Bera variable asymptotically follows a chi-squared statistics: $JB(\mathcal{X}) \sim \chi^2(2)$ as $n \to +\infty$.

A modified version, proposed by Glinskiy et al. [18], can incorporate in the test whether the samples have a known mean $\bar{x}$ and variance $\sigma^2$:

$$ \mathrm{JB}_{\bar{x}, \sigma^2}(\mathcal{X}) \triangleq n \left( \frac{S_{\bar{x}, \sigma^2}(\mathcal{X})^2}{15} + \frac{(K_{\bar{x}, \sigma^2}(\mathcal{X}) - 3)^2}{96} \right), \quad (11) $$

with

$$ \left\{ \begin{aligned} S_{\bar{x}, \sigma^2}(\mathcal{X}) &\triangleq \frac{\frac{1}{n}\sum_{i=1}^{n}(x_i - \bar{x})^3}{\left(\frac{1}{n}\sum_{i=1}^{n}(x_i - \bar{x})^2\right)^{\frac{3}{2}}} \\ K_{\bar{x}, \sigma^2}(\mathcal{X}) &\triangleq \frac{\frac{1}{n}\sum_{i=1}^{n}(x_i - \bar{x})^4}{\left(\frac{1}{n}\sum_{i=1}^{n}(x_i - \bar{x})^2\right)^2}. \end{aligned} \right. \quad (12) $$

If the components of $x$ are drawn from $N(\bar{x}, \sigma^2)$, then $JB_{\bar{x}, \sigma^2}(\mathcal{X}) \sim \chi^2(2)$ as $n \to +\infty$.
### C. Constraint Jacobians (as supplements)

1) Motion closures: Consider the constraint equation of a measurement closure on 3 motions:

$$ \mathbf{u}_0 \circ \mathrm{Exp}(w_0) \circ \mathbf{u}_1 \circ \mathrm{Exp}(w_1) \circ \mathbf{u}_2 \circ \mathrm{Exp}(w_2) = \mathbf{z} \circ \mathrm{Exp}(-\zeta), \quad (13) $$

which can be arranged as:

$$ \begin{aligned} & \mathrm{Exp}(\mathbf{Ad}_{\mathbf{u}_1 \circ \mathbf{u}_2}^{-1} w_0) \circ \mathrm{Exp}(\mathbf{Ad}_{\mathbf{u}_1}^{-1} w_1) \circ \mathrm{Exp}(w_2) \circ \mathrm{Exp}(\zeta) \\ & = (\mathbf{u}_0 \circ \mathbf{u}_1 \circ \mathbf{u}_2)^{-1} \circ \mathbf{z} \quad (14) \end{aligned} $$

where the adjoint relation Ad has been used (see [39]).

Let $e \triangleq \{w_0, w_1, w_2, \zeta\}$ be the error vector, where we assume, for simplicity of notations, and without loss of generality, that members of $e$ distributes in a standard Gaussian (i.e. no need to premultiply each error by the Cholesky decomposition of the associated covariance matrix). The desired form $h(e) = d$ described in IV is obtained through log-mapping:

$$ \left\{ \begin{array}{ll} h(e) & \triangleq \mathrm{Log} \left( \mathrm{Exp} \left( \mathbf{Ad}_{\mathbf{u}_2 \circ \mathbf{u}_1}^{-1} w_0 \right) \circ \mathrm{Exp} \left( \mathbf{Ad}_{\mathbf{u}_2}^{-1} w_1 \right) \right. \\ & \qquad \left. \circ \mathrm{Exp} (w_2) \circ \mathrm{Exp} (\zeta) \right) \\ d & \triangleq \mathrm{Log} \left( (\mathbf{u}_0 \circ \mathbf{u}_1 \circ \mathbf{u}_2)^{-1} \circ \mathbf{z} \right), \end{array} \right. \quad (15) $$

Keeping notations consistent with [39], the form $J_r(\zeta)$ is the derivative of $\mathrm{Exp}(\zeta)$. The Jacobian expressions of $h$ around the point $\bar{e}$ are given by:

$$ \left. \frac{\partial h}{\partial \zeta} \right|_{\bar{e}} = J_r^{-1}(h(\bar{e})) J_r(\bar{\zeta}) \quad (16) $$

$$ \left. \frac{\partial h}{\partial w_2} \right|_{\bar{e}} = J_r^{-1}(h(\bar{e})) \mathbf{Ad}_{\mathrm{Exp}(\bar{\zeta})}^{-1} \mathbf{J}_r(\bar{w}_2) \quad (17) $$

$$ \left. \frac{\partial h}{\partial w_1} \right|_{\bar{e}} = J_r^{-1}(h(\bar{e})) \mathbf{Ad}_{\mathrm{Exp}(\bar{w}_2) \circ \mathrm{Exp}(\bar{\zeta})}^{-1} \mathbf{J}_r(\mathbf{Ad}_{\mathbf{u}_2}^{-1} \bar{w}_1) \mathbf{Ad}_{\mathbf{u}_2}^{-1} \quad (18) $$

$$ \left. \frac{\partial h}{\partial w_0} \right|_{\bar{e}} = J_r^{-1}(h(\bar{e})) \mathbf{Ad}_{\mathrm{Exp}(\mathbf{Ad}_{\mathbf{u}_2 \circ \mathbf{u}_1}^{-1} \bar{w}_1) \circ \mathrm{Exp}(\bar{w}_2) \circ \mathrm{Exp}(\bar{\zeta})}^{-1} \mathbf{J}_r(\mathbf{Ad}_{\mathbf{u}_2 \circ \mathbf{u}_1}^{-1} \bar{w}_0) \mathbf{Ad}_{\mathbf{u}_2 \circ \mathbf{u}_1}^{-1} \quad (19) $$

Consider a more general case of motion closure, with $m$ motions. The constraint equation writes as:

$$ \left( \prod_{i=0}^{m-1} \mathrm{Exp} \left( \mathbf{Ad}_{\prod_{j=i+1}^{m-1} \mathbf{u}_j}^{-1} \mathbf{u}_i \right) \right) \circ \mathrm{Exp}(\zeta) = \left( \prod_{i=0}^{m-1} \mathbf{u}_j \right)^{-1} \mathbf{z}. \quad (20) $$

Then, the corresponding expressions of $\left\{ \left. \frac{\partial h}{\partial w_i} \right|_{\bar{e}} \right\}_{i=0}^{m-1}$ are:

$$ \left. \frac{\partial h}{\partial w_i} \right|_{\bar{e}} = J_r^{-1}(h(\bar{e})) \mathbf{Ad}_{\mathrm{Exp}(\zeta)}^{-1} \mathbf{Ad}_{\prod_{k=i+1}^{m-1} \mathrm{Exp}(\mathbf{Ad}_{\prod_{j=k+1}^{m-1} \mathbf{u}_j}^{-1} \bar{w}_k)}^{-1} \times J_r \left( \mathbf{Ad}_{\prod_{j=i+1}^{m-1} \mathbf{u}_j}^{-1} \bar{w}_i \right) \mathbf{Ad}_{\prod_{j=i+1}^{m-1} \mathbf{u}_j}^{-1} \quad (21) $$

2) Landmark resight closures: Consider the constraint equation of a landmark resighted 3 steps after discovery:

$$ y^{+3} = \mathbf{\Lambda} \cdot [y^{+0} + \nu^{+0}] + \nu^{+3}, \quad (22) $$

with $\mathbf{\Lambda} \triangleq (\mathbf{u}_0 \circ \mathrm{Exp}(w_0) \circ \mathbf{u}_1 \circ \mathrm{Exp}(w_1) \circ \mathbf{u}_2 \circ \mathrm{Exp}(w_2))^{-1}$.

Let the error vector $e \triangleq \{w_0, w_1, w_2, \nu^{+0}, \nu^{+3}\}$. Same as above, we assume $e \sim \mathcal{N}(0, I)$ to put aside Cholesky decomposition terms of their respective covariance matrices. The desired form $h(e) = d$ is obtained for:

$$ \left\{ \begin{array}{ll} h(e) & \triangleq \mathbf{\Lambda} \cdot [y^{+0} + \nu^{+0}] + \nu^{+3} \\ d & \triangleq y^{+3}. \end{array} \right. \quad (23) $$

Let $\mathbf{J}_{\mathbf{\Lambda}}^{\mathbf{\Lambda} \cdot p}|_{\bar{e}}$ and $\mathbf{J}_{p}^{\mathbf{\Lambda} \cdot p}|_{\bar{e}}$ denote the Jacobian matrices of the action operator $\cdot$, which expression depends on the Lie group considered, we refer the reader towards the appendix in Solà et al. [39] where formulas for $\mathrm{SE}(2) \times \mathbb{R}^2$ and $\mathrm{SE}(3) \times \mathbb{R}^3$ are given. The Jacobian expressions of $h$ around the point $\bar{e}$ are given by:

$$ \left. \frac{\partial h}{\partial \nu^{+0}} \right|_{\bar{e}} = \mathbf{J}_{p}^{\mathbf{\Lambda} \cdot p}|_{\bar{e}} \quad (24) $$

$$ \left. \frac{\partial h}{\partial \nu^{+3}} \right|_{\bar{e}} = I \quad (25) $$

$$ \left. \frac{\partial h}{\partial w_0} \right|_{\bar{e}} = - \mathbf{J}_{\mathbf{\Lambda}}^{\mathbf{\Lambda} \cdot p}|_{\bar{e}} \mathbf{Ad}_{\mathbf{u}_0} \mathbf{J}_r(-\bar{w}_0) \quad (26) $$

$$ \left. \frac{\partial h}{\partial w_1} \right|_{\bar{e}} = - \mathbf{J}_{\mathbf{\Lambda}}^{\mathbf{\Lambda} \cdot p}|_{\bar{e}} \mathbf{Ad}_{\mathbf{u}_0 \circ \mathrm{Exp}(\bar{w}_0) \circ \mathbf{u}_1} \mathbf{J}_r(-\bar{w}_1) \quad (27) $$

$$ \left. \frac{\partial h}{\partial w_2} \right|_{\bar{e}} = - \mathbf{J}_{\mathbf{\Lambda}}^{\mathbf{\Lambda} \cdot p}|_{\bar{e}} \mathbf{Ad}_{\mathbf{u}_0 \circ \mathrm{Exp}(\bar{w}_0) \circ \mathbf{u}_1 \circ \mathrm{Exp}(\bar{w}_1) \circ \mathbf{u}_2} \mathbf{J}_r(-\bar{w}_2). \quad (28) $$

It is easy to generalize the expressions $\left\{ \left. \frac{\partial h}{\partial w_i} \right|_{\bar{e}} \right\}_{i=0}^{m-1}$ to an arbitrary number of $m$ motions:

$$ \left. \frac{\partial h}{\partial w_i} \right|_{\bar{e}} = - \mathbf{J}_{\mathbf{\Lambda}}^{\mathbf{\Lambda} \cdot p}|_{\bar{e}} \mathbf{Ad}_{\prod_{j=0}^{i-1} \mathbf{u}_j \circ \mathrm{Exp}(\bar{w}_j)} \mathbf{Ad}_{\mathbf{u}_i} \mathbf{J}_r(-\bar{w}_i). \quad (29) $$

## REFERENCES

[1] Pratik Agarwal, Wolfram Burgard, and Cyrill Stachniss. A Survey of Geodetic Approaches to Mapping and the Relationship to Graph-Based SLAM. IEEE RAM, 2014.

[2] Ken Alder. The measure of all things: the seven-year odyssey and hidden error that transformed the world. Free Press, New York, 1. ed edition, 2003. ISBN 978-0-7432-1676-0.

[3] T. Bailey and H. Durrant-Whyte. Simultaneous localization and mapping (SLAM): part II. IEEE Robotics & Automation Magazine, 13(3):108–117, 2006.

[4] Elias Bareinboim, Juan D. Correa, Duligur Ibeling, and Thomas Icard. On Pearl's Hierarchy and the Foundations of Causal Inference, page 507–556. Association for Computing Machinery, New York, NY, USA, 1 edition, 2022. ISBN 9781450395861.
[5] Cesar Cadena, Luca Carlone, Henry Carrillo, Yasir Latif, Davide Scaramuzza, Jose Neira, Ian Reid, and John J. Leonard. Past, Present, and Future of Simultaneous Localization and Mapping: Toward the Robust-Perception Age. *IEEE Transactions on Robotics*, 32(6):1309–1332, December 2016. ISSN 1552-3098, 1941-0468.

[6] Ricardo Cannizzaro, Michael Groom, Jonathan Routley, Robert Osazuwa Ness, and Lars Kunze. COBRA-PPM: A Causal Bayesian Reasoning Architecture Using Probabilistic Programming for Robot Manipulation Under Uncertainty, August 2025.

[7] Luca Carlone, Roberto Tron, Kostas Daniilidis, and Frank Dellaert. Initialization techniques for 3D SLAM: A survey on rotation estimation and its use in pose graph optimization. In *2015 IEEE International Conference on Robotics and Automation (ICRA)*, pages 4597–4604, Seattle, WA, USA, May 2015. IEEE. ISBN 978-1-4799-6923-4.

[8] Luca Carlone, Ayoung Kim, Timothy Barfoot, Daniel Cremers, and Frank Dellaert. Slam handbook: From localization and mapping to spatial intelligence, 2025.

[9] Luca Castri, Gloria Beraldo, and Nicola Bellotto. Causality-enhanced decision-making for autonomous mobile robots in dynamic environments, 2026. arXiv: 2504.11901 [cs.RO].

[10] R Chatila and J Laumond. Position referencing and consistent world modeling for mobile robots. In *Proceedings. 1985 IEEE International Conference on Robotics and Automation*, volume 2, pages 138–145. IEEE, 1985.

[11] Julian Gerald Dcruz, Sam Mahoney, Jia Yun Chua, Adoundeth Soukhabandith, John Mugabe, Weisi Guo, and Miguel Arana-Catania. Causal reinforcement learning for optimisation of robot dynamics in unknown environments. In *2024 IEEE international smart cities conference (ISC2)*, pages 1–6. IEEE, 2024.

[12] Frank Dellaert. Factor Graphs and GTSAM: A Hands-on Introduction. 2012.

[13] Frank Dellaert and Michael Kaess. Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing. *The International Journal of Robotics Research*, 25(12):1181–1203, December 2006. ISSN 0278-3649, 1741-3176.

[14] Frank Dellaert and Michael Kaess. Factor Graphs for Robot Perception. *Foundations and Trends in Robotics*, 6(1-2):1–139, 2017. ISSN 1935-8253, 1935-8261.

[15] Maximilian Diehl and Karinne Ramirez-Amaro. A causal-based approach to explain, predict and prevent failures in robotic tasks. *Robotics and Autonomous Systems*, 162:104376, 2023. ISSN 0921-8890.

[16] H. Durrant-Whyte and T. Bailey. Simultaneous localization and mapping: part I. *IEEE Robotics & Automation Magazine*, 13(2):99–110, 2006. doi: 10.1109/MRA.2006.1638022.

[17] Carl Friedrich Gauss. *Supplementum theoriae combinatoris observationum erroribus minimis obnoxiae*. 1826. English translation by Charles Henry Davis, 1857.

[18] V Glinskiy, Y Ismayilova, S Khrushchev, A Logachov, O Logachova, L Serga, A Yambartsev, and K Zaykov. Modifications to the Jarke–Bera Test. *Mathematics*, 12 (16):2523, 2024. ISSN 2227-7390.

[19] Giorgio Grisetti, Rainer Kummerle, Cyrill Stachniss, Udo Frese, and Christoph Hertzberg. Hierarchical optimization on manifolds for online 2D and 3D mapping. In *2010 IEEE International Conference on Robotics and Automation*, pages 273–278, Anchorage, AK, May 2010. IEEE. ISBN 978-1-4244-5038-1.

[20] Giorgio Grisetti, Rainer Kümmerle, Cyrill Stachniss, and Wolfram Burgard. *An Updated Tutorial on Graph-Based SLAM*, pages 255–285. Springer Nature Switzerland, Cham, 2025. ISBN 978-3-319-74095-9.

[21] Jose Guivant, Eduardo Nebot, Juan Nieto, and Favio Masson. Navigation and Mapping in Large Unstructured Environments. *The International Journal of Robotics Research*, 23(4-5):449–472, April 2004. ISSN 0278-3649, 1741-3176.

[22] Thomas Hellström. The relevance of causation in robotics: A review, categorization, and analysis. *Paladyn, Journal of Behavioral Robotics*, 12(1):238–255, April 2021. ISSN 2081-4836.

[23] Rhys Howard and Lars Kunze. Extending Structural Causal Models for Autonomous Vehicles to Simplify Temporal System Construction & Enable Dynamic Interactions Between Agents, March 2025. arXiv:2406.01384 [cs].

[24] Carlos M. Jarque and Anil K. Bera. *A Test for Normality of Observations and Regression Residuals*. *International Statistical Review / Revue Internationale de Statistique*, 55(2):163, August 1987. ISSN 03067734.

[25] Michael Kaess, Hordur Johannsson, Richard Roberts, Viorela Ila, John Leonard, and Frank Dellaert. iSAM2: Incremental smoothing and mapping with fluid relin-earization and incremental variable reordering. In *2011 IEEE International Conference on Robotics and Automation*, pages 3281–3288, Shanghai, China, May 2011. IEEE. ISBN 978-1-61284-386-5.

[26] Boris Kožnjak. Who let the demon out? Laplace and Boscovich on determinism. *Studies in History and Philosophy of Science Part A*, 51:42–52, June 2015. ISSN 00393681.

[27] Rainer Kummerle, Giorgio Grisetti, Hauke Strasdat, Kurt Konolige, and Wolfram Burgard. G2o: A general framework for graph optimization. In *2011 IEEE International Conference on Robotics and Automation*, pages 3607–3613, Shanghai, China, May 2011. IEEE. ISBN 978-1-61284-386-5.

[28] Pierre-Simon Laplace. *Théorie analytique des probabilités*, volume Tome 7 of Oeuvres Complètes de Laplace. Éd. Gabay, Paris, courcier edition, 1820. ISBN 978-2-87647-161-0.

[29] Weiyang Liu, Zhen Liu, Liam Paull, Adrian Weller, and Bernhard Schölkopf. Structural Causal 3D Reconstruc- tion. In *Computer Vision – ECCV 2022: 17th European
*Conference, Tel Aviv, Israel, October 23–27, 2022, Proceedings, Part I*, page 140–159, Berlin, Heidelberg, 2022. Springer-Verlag. ISBN 978-3-031-19768-0.

[30] Michael Montemerlo, Sebastian Thrun, Daphne Koller, Ben Wegbreit, et al. *FastSLAM: A factored solution to the simultaneous localization and mapping problem*. Aaai/iaai, 593598(2):593–598, 2002.

[31] E. Olson, J. Leonard, and S. Teller. Fast iterative alignment of pose graphs with poor initial estimates. In *Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA 2006.*, pages 2262–2269, Orlando, FL, USA, 2006. IEEE. ISBN 978-0-7803-9505-3.

[32] Joseph Ortiz, Talfan Evans, and Andrew J. Davison. *A visual introduction to Gaussian Belief Propagation*, 2021. arXiv:2107.02308 [cs].

[33] Judea Pearl. *Causality*. Cambridge University Press, 2009.

[34] Judea Pearl, Madelyn Glymour, and Nicholas P. Jewell. *Causal inference in statistics: a primer*. Wiley, Chichester, reprinted with revisions edition, 2021. ISBN 978-1-119-18684-7.

[35] Atul Rawal, Adrienne Raglin, Danda B. Rawat, Brian M. Sadler, and James McCoy. *Causality for Trustworthy Artificial Intelligence: Status, Challenges and Perspectives*. ACM Computing Surveys, 57(6):1–30, June 2025. ISSN 0360-0300, 1557-7341.

[36] David M Rosen, Luca Carlone, Afonso S Bandeira, and John J Leonard. *SE-Sync: A certifiably correct algorithm for synchronization over the special Euclidean group*. *The International Journal of Robotics Research*, 38(2-3):95-125, March 2019. ISSN 0278-3649, 1741-3176.

[37] David M Rosen, Kevin J Doherty, Antonio Terán Espinoza, and John J Leonard. *Advances in inference and representation for simultaneous localization and mapping*. *Annual Review of Control, Robotics, and Autonomous Systems*, 4(1):215–242, 2021.

[38] Randall C. Smith and Peter Cheeseman. *On the Representation and Estimation of Spatial Uncertainty*. *The International Journal of Robotics Research*, 5(4):56–68, December 1986. ISSN 0278-3649, 1741-3176.

[39] Joan Solà, Jeremie Deray, and Dinesh Atchuthan. *A micro Lie theory for state estimation in robotics*, December 2021. arXiv:1812.01537 [cs].

[40] Stephen M. Stigler. *The history of statistics: the measurement of uncertainty before 1900*. Belknap Pr. of Harvard Univ. Pr, Cambridge, Mass., 9. print edition, 1986. ISBN 978-0-674-40341-3 978-0-674-40340-6.

[41] Joel Tari. *Causal Representation of the Simultaneous Localization And Mapping Problem*. PhD Thesis, Université de Toulouse, December 2024.

[42] Joel Tari and Danès Patrick. *CARLIT: Causal Approach to Represent and Internalize Topography*. https://github.com/JoelTari/Carlit.jl, 2026.

[43] S Thrun, W Burgard, and D Fox. *Probabilistic robotics*. Intelligent robotics and autonomous agents. MIT Press, Cambridge, Mass. 2005. ISBN 978-0-262-20162-9. OCLC: ocm58451645.

[44] Marij Van Strien. *On the origins and foundations of Laplacian determinism. Studies in History and Philosophy of Science Part A*, 45:24–31, March 2014. ISSN 00393681.