Exploiting Symmetries to Design EKFs with Consistency Properties for Navigation and SLAM

TL;DR

This paper introduces a novel invariant EKF framework that ensures consistency in navigation and SLAM by properly capturing unobservable directions, demonstrated through theoretical proofs and empirical evaluations.

Contribution

It develops an alternative EKF based on a nonlinear error that inherently respects symmetry properties, ensuring consistency in unobservable scenarios.

Findings

The alternative EKF captures unobservable directions automatically.

The proposed EKF outperforms standard EKF in experiments.

It achieves performance comparable to iSAM in multi-robot SLAM.

Abstract

The Extended Kalman Filter (EKF) is both the historical algorithm for multi-sensor fusion and still state of the art in numerous industrial applications. However, it may prove inconsistent in the presence of unobservability under a group of transformations. In this paper we first build an alternative EKF based on an alternative nonlinear state error. This EKF is intimately related to the theory of the Invariant EKF (IEKF). Then, under a simple compatibility assumption between the error and the transformation group, we prove the linearized model of the alternative EKF automatically captures the unobservable directions, and many desirable properties of the linear case then directly follow. This provides a novel fundamental result in filtering theory. We apply the theory to multi-sensor fusion for navigation, when all the sensors are attached to the vehicle and do not have access to absolute information, as typically occurs in GPS-denied environments. In the context of Simultaneous Localization And Mapping (SLAM), Monte-Carlo runs and comparisons to OC-EKF, robocentric EKF, and optimization-based smoothing algorithms (iSAM) illustrate the results. The proposed EKF is also proved to outperform standard EKF and to achieve comparable performance to iSAM on a publicly available real dataset for multi-robot SLAM.

Equations

$${\mathbf{x}}_n$$

$${\mathbf{y}}_n$$

$$\forall \mathbf{x}\in \mathcal{X}$$

$$\forall \alpha ,\beta \in G,\mathbf{x}\in \mathcal{X}$$

$$\forall \alpha ,\mathbf{x},\mathbf{u},\mathbf{w}{\varphi }_{\alpha }\left(f(\mathbf{x},\mathbf{u},\mathbf{w})\right)=f\left({\varphi }_{\alpha }(\mathbf{x}),\mathbf{u},\mathbf{w}\right),$$

$$\forall \alpha ,\mathbf{x}h({\varphi }_{\alpha }(\mathbf{x}),\mathbf{v})=h(\mathbf{x},\mathbf{v}).$$

$$\mathcal{O}({\mathbf{x}}_{n_0})=\left[\begin{array}{c}{\mathbf{H}}_{n_0}\\ {\mathbf{H}}_{n_0+1}{\mathbf{F}}_{n_0+1}\\ ⋮\\ {\mathbf{H}}_{n_0+N}{\mathbf{F}}_{n_0+N}\cdots {\mathbf{F}}_{n_0+1}\end{array}\right],$$

$$\displaystyle\frac{\partial}{\partial\alpha}\phi_{\alpha}\bigr{|}_{\alpha=Id}\left(f(\mathbf{x},\mathbf{u},\mathbf{w})\right)$$

$$\displaystyle\frac{\partial h}{\partial\mathbf{x}}|_{(\mathbf{x},\mathbf{v})}\frac{\partial}{\partial\alpha}\phi_{\alpha}\bigr{|}_{\alpha=Id}(\mathbf{x})$$

$$\hat{\mathcal{O}}({\hat{\mathbf{x}}}_{n_0})=\left[\begin{array}{c}{\hat{\mathbf{H}}}_{n_0}\\ {\hat{\mathbf{H}}}_{n_0+1}{\hat{\mathbf{F}}}_{n_0+1}\\ ⋮\\ {\hat{\mathbf{H}}}_{n_0+N}{\hat{\mathbf{F}}}_{n_0+N}\cdots {\hat{\mathbf{F}}}_{n_0+1}\end{array}\right],$$

$${\mathbf{e}}_{n-1|n-1}$$

$${\mathbf{e}}_{n|n-1}$$

$${\mathbf{y}}_n-h\left({\hat{\mathbf{x}}}_{n|n-1},0\right)$$

$$\eta \left(f(\mathbf{x}),f(\hat{\mathbf{x}})\right)\simeq {\hat{\mathbf{F}}}^e\eta \left(\mathbf{x},\hat{\mathbf{x}}\right),$$

$$\eta (f({\varphi }_{\alpha }({\hat{\mathbf{x}}}_n)),f({\hat{\mathbf{x}}}_n))$$

$$\simeq \frac{\partial }{\partial \alpha }{|}_{Id}\eta ({\varphi }_{\alpha }(f({\hat{\mathbf{x}}}_n)),f({\hat{\mathbf{x}}}_n))\delta \alpha \in \mathcal{C}$$

$${\hat{\mathbf{H}}}_n^e\mathcal{C}=0,{\hat{\mathbf{F}}}_{n+1}^e\mathcal{C}\subset \mathcal{C},\text{ for any n and x^n. }$$

$$u_n^T({\mathbf{P}}_{n|n}^e{)}^{-1}{u}_n\le u_{n-1}^T({\mathbf{P}}_{n-1|n-1}^e{)}^{-1}{u}_{n-1}.$$

$$u_n^T({\mathbf{P}}_{n|n-1}^e{)}^{-1}{u}_n$$

$$⩽u_{n-1}^T({\hat{\mathbf{F}}}_n^e{)}^T{\left({\hat{\mathbf{F}}}_n^e{\mathbf{P}}_{n-1|n-1}^e({\hat{\mathbf{F}}}_n^e{)}^T\right)}^{-1}{\hat{\mathbf{F}}}_n^e{u}_{n-1}$$

$$u_n^T({\mathbf{P}}_{n|n}^e{)}^{-1}{u}_n=u_n^T\left(({\mathbf{P}}_{n|n-1}^e{)}^{-1}+({\hat{\mathbf{H}}}_n^e{)}^T{\hat{\mathbf{R}}}_n^{-1}{\hat{\mathbf{H}}}_n^e\right)u_n.$$

$$\mathbf{x}=\left(\mathbf{R},\text{leavevmode}{\mathbf{p}}_1,\text{leavevmode}\cdots ,\text{leavevmode}{\mathbf{p}}_l,\text{leavevmode}{\mathbf{v}}_1,\text{leavevmode}\cdots ,\text{leavevmode}{\mathbf{v}}_m,\text{leavevmode}\mathbf{b}\right)\in \mathcal{X},$$

$$\displaystyle\begin{split}\phi_{\alpha}(\mathbf{x})=\big{(}&\mathbf{R}_{\alpha}\mathbf{R},\leavevmode\nobreak\ \mathbf{R}_{\alpha}\mathbf{p}_{i}+\mathbf{p}_{\alpha},\leavevmode\nobreak\ \mathbf{R}_{\alpha}\mathbf{v}_{j},\leavevmode\nobreak\ \mathbf{b}\big{)}.\end{split}$$

$$\displaystyle\begin{split}\frac{\partial}{\partial\alpha}\phi_{\alpha}|_{Id}(\mathbf{x})\delta\alpha=\big{(}&(\delta\bm{\omega})_{\times}\mathbf{R},\leavevmode\nobreak\ (\delta\bm{\omega})_{\times}\mathbf{p}_{i}+\delta\mathbf{p},\leavevmode\nobreak\ (\delta\bm{\omega})_{\times}\mathbf{v}_{j},\leavevmode\nobreak\ \mathbf{b}\big{)}\end{split}$$

$$\mathbf{x}=\left(\mathbf{R},\text{leavevmode}{\mathbf{p}}_R,\text{leavevmode}{\mathbf{p}}_L\right)\in \mathcal{X}=SO(3)\times {\mathbb{R}}^6.$$

$${\varphi }_{\alpha }(\mathbf{x})=\left({\mathbf{R}}_{\alpha }\mathbf{R},\text{leavevmode}{\mathbf{R}}_{\alpha }{\mathbf{p}}_R+{\mathbf{p}}_{\alpha },\text{leavevmode}{\mathbf{R}}_{\alpha }{\mathbf{p}}_L+{\mathbf{p}}_{\alpha }\right).$$

$$\left({\left(\delta \mathbf{\omega }\right)}_{\times }\mathbf{R},\text{leavevmode}(\delta \mathbf{\omega }{)}_{\times }{\mathbf{p}}_R+\delta \mathbf{p},\text{leavevmode}(\delta \mathbf{\omega }{)}_{\times }{\mathbf{p}}_L+\delta \mathbf{p}\right),$$

$$\mathbf{x}=\left(\mathbf{R},\text{leavevmode}{\mathbf{p}}_R,\text{leavevmode}\mathbf{v},\text{leavevmode}\mathbf{b}\right)$$

$${\varphi }_{\alpha }(\mathbf{x})=\left({\mathbf{R}}_{\alpha }\mathbf{R},\text{leavevmode}{\mathbf{R}}_{\alpha }{\mathbf{p}}_R+{\mathbf{p}}_{\alpha },\text{leavevmode}{\mathbf{R}}_{\alpha }\mathbf{v},\text{leavevmode}\mathbf{b}\right),$$

$$\displaystyle\begin{split}\eta(\mathbf{x},\hat{\mathbf{x}})=\big{(}&\mathbf{R}\hat{\mathbf{R}}^{T},\hat{\mathbf{p}}_{i}-\hat{\mathbf{R}}\mathbf{R}^{T}\mathbf{p}_{i},\hat{\mathbf{v}}_{j}-\hat{\mathbf{R}}\mathbf{R}^{T}\mathbf{v}_{j},\mathbf{b}-\hat{\mathbf{b}}\big{)}\end{split}.$$

$$\displaystyle\begin{split}\check{\eta}(\mathbf{x},\hat{\mathbf{x}})&=\big{(}\mathbf{e}_{R},\leavevmode\nobreak\ \hat{\mathbf{p}}-(\mathbf{e}_{R})_{\times}\mathbf{p},\leavevmode\nobreak\ \hat{\mathbf{v}}-(\mathbf{e}_{R})_{\times}\mathbf{v},\leavevmode\nobreak\ \mathbf{b}-\hat{\mathbf{b}}\big{)},\end{split}$$

$$\mathbf{R}{\hat{\mathbf{R}}}^T$$

$$\eta ({\varphi }_{\alpha }(\mathbf{x}),\mathbf{x})$$

Full text

1 Introduction

Multi-sensor fusion for navigation of autonomous and non-autonomous vehicles, or for Simultaneous Localization And Mapping (SLAM) or Visual Inertial Odometry (VIO) is classically handled by the Extended Kalman Filter (EKF). Although powerful alternative techniques have since emerged, the EKF is both the historical algorithm - originally implemented in the Apollo program - and still a prevalent algorithm in the academia and in the industry, see e.g., [13, 7, 40].

One major limitation of the EKF is its inconsistency, that is, the filter returns a covariance matrix that is too optimistic [2], leading to inaccurate estimates. EKF inconsistency in the context of SLAM has been the object of many papers, see e.g. [31, 22, 36, 16, 1, 26, 24, 23, 27, 29]. Theoretical analysis [31, 24, 29] reveals inconsistency is caused by the inability of EKF to reflect the unobservable degrees of freedom of SLAM. Indeed, the filter tends to erroneously acquire information along the directions spanned by these unobservable degrees of freedom. The Observability Constrained (OC)-EKF [29, 26] constitutes one of the most advanced solutions to remedy this problem and has been fruitfully adapted, e.g. for VIO, cooperative localization, and unscented Kalman filter [28, 36, 25]. The idea is to pick a linearization point that is such that the unobservable subspace “seen” by the filter is of appropriate dimension.

In this paper we propose a novel general theory. We first propose to build EKFs based on an alternative error $\mathbf{e}=\eta(\mathbf{x},\hat{\mathbf{x}})$, generalizing the Invariant EKF (IEKF) methodology [5]. This means the covariance matrix $\mathbf{P}$ reflects the dispersion of $\mathbf{e}$, and not of $\mathbf{x}-\hat{\mathbf{x}}$. When unobservability stems from symmetries, the technique may resolve the consistency issues of the EKF. Indeed symmetries are encoded by the action $\phi_{\alpha}(\mathbf{x})$ of a transformation group $G$ [39], where $\alpha\in G$ denotes the corresponding infinitesimal unobservable transformation. Under the simple condition that the image of matrix $\frac{\partial}{\partial\alpha}\eta(\phi_{\alpha}(\mathbf{x}),\mathbf{x})$ is independent of $\mathbf{x}$, the EKF based on $\mathbf{e}$ is proved to possess the desirables properties of the linear case regarding unobservability, and is thus consistent.

1.1 Specific Application to SLAM

The specific application to SLAM was released by the authors on Arxiv in 2015 [4] and encountered immediate successes reported in [42, 41, 20, 12, 14, 6, 21], although the work was never published elsewhere.

More precisely, we noticed back in [10] the SLAM problem bears a nontrivial Lie group structure. In the Arxiv preprint [4] we formalized the group introduced in [10] and called it $SE_{l+1}(3)$, and we proved that for odometry based SLAM, using the right invariant error of $SE_{l+1}(3)$ and devising an EKF based on this error, i.e., a Right-Invariant EKF (RIEKF), the linearized system possesses the desirable properties of the linear case, since it automatically correctly captures unobservable directions for SLAM. Thus, virtually all properties of the linear Kalman filter regarding unobservability may be directly transposed: the information about unobservable directions is non-increasing (see Proposition 2), the dimension of the unobservable subspace has appropriate dimension (this relates to the result of OC-EKF [25, 29, 28]), the filter’s output is invariant to linear unobservable transformations, even if they are stochastic and thus change the EKF’s covariance matrix along unobservable directions [42]. The right-invariant error for the proposed Lie group structure was also recently shown to lead to deterministic observers having exponential convergence properties in [38].

Along the same lines, using the right-invariant error of the group $SE_{2}(3)$ we proposed in [5] also led to alternative consistent IEKF for visual inertial SLAM and VIO applications [41, 20, 12, 14, 21]. In particular, [20] demonstrates that an alternative Invariant MSCKF based on the right-invariant error of $SE_{2}(3)$ naturally enforces the state vector to remain in the unobservable subspace, a consistency property which is preserved when considering point and line features [21], or when a network of magnetometers is available [14].

1.2 Paper’s Organization

Section 2 presents the general theory. Section 3 applies the theory to the general problem of navigation in the absence of absolute measurements, as typically occurs in GPS-denied environments. Section 4 is dedicated to SLAM and compares the proposed EKF to conventional EKF, OC-EKF [29], robocentric mapping filter [15] and iSAM [32, 33]. In Section 5, we show our alternative EKF achieves comparable results to iSAM and outperforms the conventional EKF on a multi-robot SLAM experiment using the UTIAS dataset [35].

Preliminary ideas and results can be found in the 2015 technical report [4]. Although the present paper is a major rewrite, notably including a novel general theory encompassing the particular application to SLAM of [4], and novel comparisons and experiments, [4] serves as preliminary material for the present paper. Matlab codes used for the paper are available at https://github.com/CAOR-MINES-ParisTech/esde.

2 General Theory

Let us consider the following dynamical system in discrete time with state $\mathbf{x}_{n}\in\mathcal{X}$ and observations $\mathbf{y}_{n}\in\mathbb{R}^{p}$:

[TABLE]

where $f(\cdot)$ is the function encoding the evolution of the system, $\mathbf{w}_{n}\sim\mathcal{N}\left(\mathbf{0},\mathbf{Q}_{n}\right)$ is the Gaussian process noise, $\mathbf{u}_{n}$ is the input, $h(\cdot)$ is the observation function and $\mathbf{v}_{n}\sim\mathcal{N}\left(\mathbf{0},\mathbf{R}_{n}\right)$ a Gaussian measurement noise.

We now define mathematical symmetries, see [39, 8].

Definition 1**.**

An action of a (Lie) group $G$ on $\mathcal{X}$ is defined as a family of bijective maps $\phi_{\alpha}:\mathcal{X}\rightarrow\mathcal{X},\leavevmode\nobreak\ \alpha\in G$ satisfying

[TABLE]

where $Id$ corresponds to the identity of the group $G$.

Definition 2**.**

Let $\phi_{\cdot}(\cdot)$ be defined as in (3)-(4). We say that system (1)-(2) is totally invariant under the action of $\phi_{\cdot}(\cdot)$ if

the dynamics are equivariant under $\phi_{\cdot}(\cdot)$, i.e.,

[TABLE] 2. 2.

the observation map $h(\cdot)$ is invariant w.r.t. $\phi_{\cdot}(\cdot)$

[TABLE]

“Symmetry” is defined as invariance to transformations $\phi_{\alpha}(\cdot)$. Throughout the paper we will rather use the term invariant, along the lines of the preceding definition.

As in this paper we pursue the design of consistent EKFs, we will focus on the system “seen” by an EKF: it consists of the linearization of system (1)-(2) about the estimated trajectory $(\hat{\mathbf{x}}_{n})_{n\geq 0}$ in the state space. Along the lines of [24, 28] we use the linearized system about a trajectory.

Let $\left(\mathbf{x}_{n}\right)_{n\geq 0}$ denote a solution of (1) with noise turned off. The local observability matrix [17] at $\mathbf{x}_{n_{0}}$ for the time interval between time-steps $n_{0}$ and $n_{0}+N$ is defined as

[TABLE]

with the Jacobians $\mathbf{F}_{n}=\frac{\partial f}{\partial\mathbf{x}}|_{\mathbf{x}_{n-1},\mathbf{u}_{n},\mathbf{w}_{n}}$, $\mathbf{H}_{n}=\frac{\partial h}{\partial\mathbf{x}}|_{\mathbf{x}_{n},\mathbf{v}_{n}}$.

First we show the directions spanned by the action of $G$ are necessarily unobservable directions of the linearized system, that is, they lie in the kernel of the observability matrix.

Proposition 1**.**

If system (1)-(2) is invariant in the sense of Definition 2, then the directions $\frac{\partial}{\partial\alpha}\phi_{\alpha}|_{Id}(\mathbf{x}_{n_{0}})$ infinitesimally spanned by $\phi_{\cdot}(\cdot)$ at any $\mathbf{x}_{n_{0}}$ necessarily lie in $\operatorname{Ker}\mathcal{O}(\mathbf{x}_{n_{0}})$, with $\mathcal{O}(\mathbf{x}_{n_{0}})$ defined by (7), and are thus unobservable.

Proof.

Differentiating111 On Lie groups differentiation can indeed be rigorously defined as \frac{\partial}{\partial\alpha}\phi_{\alpha}\bigr{|}_{\alpha=Id}(\mathbf{x})\delta\alpha:=\frac{d}{ds}\phi_{\exp(s\delta\alpha)}(\mathbf{x})|_{s=0} with $\delta\alpha$ in the Lie algebra, which mean partial derivative of $\phi_{\alpha}\left(\mathbf{x}\right)$ with respect to $\alpha$ at $\alpha=Id$. (5) and (6) w.r.t. $\alpha$ at $Id$ we obtain

[TABLE]

Let $\left(\mathbf{x}_{n}\right)_{n\geq 0}$ denote a solution of (1) with noise turned off. (9) applied at $\mathbf{x}_{n_{0}}$ yields $\mathbf{H}_{n_{0}}\frac{\partial}{\partial\alpha}\phi_{\alpha}|_{Id}(\mathbf{x}_{n_{0}})=\allowbreak\mathbf{0}$. Considering then (8) at $\mathbf{x}_{n_{0}}$ leads to $\frac{\partial}{\partial\alpha}\phi_{\alpha}|_{Id}\left(f(\mathbf{x}_{n_{0}},\mathbf{u}_{n_{0}+1},\mathbf{w}_{n_{0}+1})\right)=\allowbreak\mathbf{F}_{n_{0}+1}\frac{\partial}{\partial\alpha}\phi_{\alpha}|_{Id}\left(\mathbf{x}_{n_{0}}\right)$. Applying (9) at $\mathbf{x}_{n_{0}+1}\allowbreak=f(\mathbf{x}_{n_{0}},\mathbf{u}_{n_{0}+1},\mathbf{w}_{n_{0}+1})$ yields $\mathbf{H}_{n_{0}+1}\frac{\partial}{\partial\alpha}\phi_{\alpha}|_{Id}\left(\mathbf{x}_{n_{0}+1}\right)\allowbreak=\mathbf{0},$ and thus $\mathbf{H}_{n_{0}+1}\allowbreak\mathbf{F}_{n_{0}+1}\frac{\partial}{\partial\alpha}\phi_{\alpha}|_{Id}\left(\mathbf{x}_{n_{0}}\right)=\mathbf{0}$. A simple recursion proves $\frac{\partial}{\partial\alpha}\phi_{\alpha}|_{Id}\left(\mathbf{x}_{n_{0}}\right)\subset\operatorname{Ker}\mathcal{O}(\mathbf{x}_{n_{0}})$. ∎

Indeed, no matter the number of observations and moves, we are inherently unable to detect an initial (infinitesimal) transformation $\phi_{\alpha}(\cdot)$, the problem being invariant to it.

2.1 Observability Issues of the Standard EKF

Let $(\hat{\mathbf{x}}_{n})_{n\geq 0}$ be a sequence of state estimates given by an EKF. The linearized system “seen” by the EKF involves the estimated observability matrix

[TABLE]

where Jacobians are computed at the estimates. The directions spanned by $\phi_{\cdot}(\cdot)$ at $\hat{\mathbf{x}}_{n_{0}}$ necessarily lie in $\operatorname{Ker}\mathcal{O}(\hat{\mathbf{x}}_{n_{0}})$, as proved by Proposition 1, but there is a null probability that they lie in $\operatorname{Ker}\hat{\mathcal{O}}(\hat{\mathbf{x}}_{n_{0}})$, because of the noise and Kalman updates, see [28]. A major consequence is that the EKF gains spurious information along the unobservable directions.

In fact, this problem stems from the choice of estimation error $\mathbf{x}-\hat{\mathbf{x}}$ that does not match unobservability of system (1)-(2): changing the estimation error may resolve the problem.

2.2 EKF Based on a Nonlinear Error

In this section, we define an EKF based on a nonlinear function $\eta(\mathbf{x},\hat{\mathbf{x}})\in\mathcal{X}$ that provides an alternative to the usual linear estimation error $\mathbf{x}-\hat{\mathbf{x}}$. We prove consistency under compatibility assumptions of the group action and $\eta(\cdot,\cdot)$.

The methodology builds upon the alternative errors

[TABLE]

The filter is displayed in Algorithm 2. As the covariance matrix $\mathbf{P}^{e}$ is supposed to reflect the dispersion of $\mathbf{e}$, we need to define Jacobians w.r.t our alternative state error. At line 2 $\hat{\mathbf{F}}_{n}^{e}$, $\hat{\mathbf{G}}_{n}^{e}$ are Jacobians of the error propagation function, and at line 3, $\hat{\mathbf{H}}_{n}^{e}$, $\hat{\mathbf{J}}_{n}^{e}$ are Jacobians of the error measurement defined through the following first order approximations

[TABLE]

At line 4, $\mathbf{e}_{n}^{+}$ denotes the (best) error estimate according to the EKF. However, defining the (best) state corresponding to estimate $\hat{\mathbf{x}}_{n|n}$ is not straightforward as in the linear case where $\hat{\mathbf{x}}_{n|n}=\hat{\mathbf{x}}_{n|n-1}+\mathbf{e}_{n}^{+}$. At line 5 we use a retraction $\psi:\mathcal{X}\times\mathbb{R}^{q}\rightarrow\mathcal{X}$, that is, any function $\psi\left(\cdot\right)$ which is consistent with the error to the first order, i.e., $\mathbf{e}_{n}^{+}\approx\eta(\hat{\mathbf{x}}_{n|n},\hat{\mathbf{x}}_{n|n-1})$.

Note that, we recover the conventional EKF if we let $\mathbf{e}=\eta\left(\mathbf{x},\hat{\mathbf{x}}\right)=\mathbf{x}-\hat{\mathbf{x}}$ be the usual linear error.

2.3 Compatibility Assumptions and Main Consistency Result

The matrix222To fix ideas and help the reader understand the tools, let us pick an example. For instance we can let the state space be $\mathcal{X}=\mathbb{R}^{2}$ and $\phi_{\alpha}(\cdot)$ a rotation of angle $\alpha$ around the origin. The group $G=S^{1}$ is the circle, and the identity element is $Id=0+2k\pi$. In this case, using polar coordinates, i.e. $\mathbf{x}=\left[r\cos\theta,r\sin\theta\right]^{T}$ we have $\phi_{\alpha}(r\cos\theta,r\sin\theta)=\left[r\cos(\theta+\alpha),r\sin(\theta+\alpha)\right]^{T}$. The directions spanned by $\phi_{\cdot}(\cdot)$ at $\mathbf{x}$ are $\frac{\partial}{\partial\alpha}|_{Id}\phi(\mathbf{x})=\left[-r\sin\theta,r\cos\theta\right]^{T}$ which is a vector orthogonal to $\mathbf{x}$ in $\mathbb{R}^{2}$. Now, consider an error $\eta(\cdot,\cdot)$ between two elements of the state space $\mathcal{X}$. What we advocate in the preset paper is that nonlinear errors may be much better suited for EKF design, but to keep things simple at this stage assume merely that $\eta(\cdot,\cdot)$ denotes the linear error. In this case we have $\eta(\phi_{\alpha}(\mathbf{x}),\mathbf{x})=\phi_{\alpha}(\mathbf{x})-\mathbf{x}$. This is an element of the state space $\mathbb{R}^{2}$, and we can differentiate with respect to $\alpha$. We find $\frac{\partial}{\partial\alpha}|_{Id}\eta(\phi_{\alpha}(\mathbf{x}),\mathbf{x})=\left[-r\sin\theta,r\cos\theta\right]^{T}$. Of course, it would have been different if we had chosen a different error. If we choose the polar coordinates error, that is $\eta(\cdot,\cdot)$ is a two component vector with first component the difference of norms and second component the difference of angles in polar coordinates, then we find $\eta(\phi_{\alpha}(\mathbf{x}),\mathbf{x})=\left[0,\alpha\right]^{T}$ which is quite different, and thus $\frac{\partial}{\partial\alpha}|_{Id}\eta(\phi_{\alpha}(\mathbf{x}),\mathbf{x})=\left[0,1\right]^{T}$. We see to some extent this error is more “compatible” with the symmetry group we chose since it is the same at any $\mathbf{x}\in\mathbb{R}^{2}$. In this footnote, the group was of dimension 1, i.e. it was encoded by a one dimensional element $\alpha$. If $\alpha$ had another component, we would need to differentiate with respect to it also. As a result we would not obtain a vector $\frac{\partial}{\partial\alpha}|_{Id}\eta(\phi_{\alpha}(\mathbf{x}),\mathbf{x})$ but a matrix, each column being associated to a component of $\alpha$. $\frac{\partial}{\partial\alpha}|_{Id}\eta(\phi_{\alpha}(\mathbf{x}),\mathbf{x})$ reflects how infinitesimal transformations of the state produced by the action of $G$ affect the error variable $\mathbf{e}=\eta(\cdot,\cdot)$. In Assumption 1 below this matrix is used to define a kind of “compatibility” between an invariance group $G$ and a nonlinear error function $\eta(\cdot,\cdot)$, which leads to the main result of this paper (Theorem 1).

Assumption 1**.**

The image of the matrix $\frac{\partial}{\partial\alpha}|_{Id}\eta(\phi_{\alpha}(\mathbf{x}),\mathbf{x})$ is a fixed subspace $\mathcal{C}$ that does not depend on $\mathbf{x}$.

Proposition 1 proved that if the system (1)-(2) is totally invariant, then the directions infinitesimally spanned by $\phi_{\cdot}(\cdot)$ at any point $\mathbf{x}_{n_{0}}$ lie in $\operatorname{Ker}\mathcal{O}(\mathbf{x}_{n_{0}})$, with $\mathcal{O}(\mathbf{x}_{n_{0}})$ defined by (7), and thus they are unobservable. We have also recalled this is not true for the linearized system seen by the EKF, i.e. for $\hat{\mathcal{O}}(\hat{\mathbf{x}}_{n_{0}})$ [28]. We have the following powerful result:

Theorem 1**.**

If the system (1)-(2) is invariant, and under Assumption 1, the unobservable directions $\mathcal{C}$ spanned by $\phi_{\cdot}(\cdot)$ at any point $\hat{\mathbf{x}}_{n_{0}}$, measured using error $\eta(\cdot,\cdot)$ necessary lie in $\operatorname{Ker}\hat{\mathcal{O}}^{e}(\hat{\mathbf{x}}_{n_{0}})$, with $(\hat{\mathbf{F}}_{n}^{e},\hat{\mathbf{H}}_{n}^{e})_{n\geq n_{0}}$ defined by (13)-(14).

Proof.

Let $\hat{\mathbf{M}}_{n}:=\frac{\partial}{\partial\alpha}|_{Id}\eta(\phi_{\alpha}(\hat{\mathbf{x}}_{n}),\hat{\mathbf{x}}_{n})$. Recalling (14), we have $h(\phi_{\alpha}(\hat{\mathbf{x}}_{n}))-h(\hat{\mathbf{x}}_{n})\simeq\hat{\mathbf{H}}_{n}^{e}\eta(\phi_{\alpha}(\hat{\mathbf{x}}_{n}),\hat{\mathbf{x}}_{n})\simeq\hat{\mathbf{H}}_{n}^{e}\hat{\mathbf{M}}_{n}\delta\alpha$ with $\delta\alpha$ a linearized approximation to $\alpha\in G$. But also $h(\phi_{\alpha}(\hat{\mathbf{x}}_{n}))-h(\hat{\mathbf{x}}_{n})\simeq\frac{\partial h}{\partial\mathbf{x}}|_{(\hat{\mathbf{x}}_{n},\mathbf{0})}\frac{\partial}{\partial\alpha}\phi|_{Id}(\hat{\mathbf{x}}_{n})\delta\alpha$. From (9) the latter is $\mathbf{0}$. Thus $\hat{\mathbf{H}}_{n}^{e}u=\mathbf{0}$ for any $u\in\mathcal{C}$, i.e. $\hat{\mathbf{H}}_{n}^{e}\mathcal{C}=\mathbf{0}$.

Using (13), $\hat{\mathbf{F}}^{e}$ is defined as

[TABLE]

thus $\eta(f(\phi_{\alpha}(\hat{\mathbf{x}}_{n})),f(\hat{\mathbf{x}}_{n}))\simeq\hat{\mathbf{F}}_{n+1}^{e}\eta(\phi_{\alpha}(\hat{\mathbf{x}}_{n}),\hat{\mathbf{x}}_{n})$ $\simeq\hat{\mathbf{F}}_{n+1}^{e}\hat{\mathbf{M}}_{n}\delta\alpha$. Besides, using (5) yields

[TABLE]

applying Assumption 1 at $f(\hat{\mathbf{x}}_{n})$. Thus for any $\delta\alpha$ we have $\hat{\mathbf{F}}_{n+1}^{e}\hat{\mathbf{M}}_{n}\delta\alpha\subset\mathcal{C}$ and thus $\hat{\mathbf{F}}_{n+1}^{e}\mathcal{C}\subset\mathcal{C}$. We have thus proved

[TABLE]

This proves the result through an immediate recursion. ∎

We obtain the consistency property we pursue: the linearized model has the desirable property of the linear Kalman filter regarding the unobservabilities, when expressed in terms of error $\eta(\cdot,\cdot)$. As a byproduct, the unobservable subspace seen by the filter is automatically of appropriate dimension.

2.4 Consequences in Terms of Information

In the linear Gaussian case, the inverse of the covariance matrix output by the Kalman filter is the Fisher information available to the filter (as stated in [2] p. 304). Thus, the inverse of the covariance matrix $\mathbf{P}_{n|n}^{-1}$ output by any EKF should reflect an absence of information gain along unobservable directions. Otherwise, the output covariance matrix would be too optimistic, i.e., the filter inconsistent [2].

Note that, the covariance matrix $\mathbf{P}_{n|n}^{e}$ reflects the dispersion of the error $\mathbf{e}_{n|n}$ of (11)-(12), as emphasized by the superscript $e$. We have the following consistency result.

Proposition 2**.**

Let $u_{n_{0}}\in Im\leavevmode\nobreak\ \frac{\partial}{\partial\alpha}|_{Id}\eta(\phi_{\alpha}(\hat{\mathbf{x}}_{n_{0}}),\hat{\mathbf{x}}_{n_{0}})$ be an unobservable direction spanned by $\phi_{\cdot}(\cdot)$ at the estimate $\hat{\mathbf{x}}_{n_{0}}$, measured using alternative error $\eta(\cdot,\cdot)$. Let $(u_{n})_{n\geq n_{0}}$ with $u_{n}=\hat{\mathbf{F}}_{n}^{e}u_{n-1}$ be its propagation through the linearized model. Under Assumption 1 the Fisher information according to the filter about $(u_{n})_{n\geq n_{0}}$ is non-increasing , i.e.,

[TABLE]

Proof.

At propagation step we have

[TABLE]

since $\mathbf{Q}$ is positive semidefinite. As $(\hat{\mathbf{F}}_{n}^{e})^{-1}\hat{\mathbf{F}}_{n}^{e}=\mathbf{I}$ we have just proved $u_{n}^{T}(\mathbf{P}^{e}_{n|n-1})^{-1}u_{n}\leqslant u_{n-1}^{T}(\mathbf{P}^{e}_{n-1|n-1})^{-1}u_{n-1}^{T}$.

At update step (in information form) we have

[TABLE]

But using (18) we see $u_{i}\in\mathcal{C}$ $\forall i\geq n_{0}$ and thus $\mathbf{H}_{n}^{e}u_{n}=\mathbf{0}$ so $u_{n}^{T}(\mathbf{P}^{e}_{n|n})^{-1}u_{n}=u_{n}^{T}(\mathbf{P}^{e}_{n|n-1})^{-1}u_{n}\leqslant u_{n-1}^{T}(\mathbf{P}^{e}_{n-1|n-1})^{-1}u_{n-1}$. ∎

The theorem essentially ensures the linearized model of the filter has a structure which guarantees that the covariance matrix at all times reflects an absence of “spurious” (Bayesian Fisher) information gain over unobservable directions, ensuring strong consistency properties of our alternative EKF.

3 Application to Multi-Sensor Fusion for Navigation

In this section, we consider a navigating vehicle or a robot equipped with sensors which only measure quantities relative to the vehicle’s frame. Thus the vehicle cannot acquire information about its absolute position and orientation, which results in inevitable unobservability. The state space is $\mathcal{X}=SO(3)\times\mathbb{R}^{3l+3m+k}$ and the state $\mathbf{x}$ is defined as

[TABLE]

where $\mathbf{R}\in SO(3)$ represents the orientation of the vehicle, i.e., its columns are the axes of the vehicle’s frame, and where

$\{\mathbf{p}_{i}\in\mathbb{R}^{3}\}_{i=1,\ldots,l}$ are vectors of the global frame, such as the vehicle’s position, 2. 2.

$\{\mathbf{v}_{i}\in\mathbb{R}^{3}\}_{i=1,\ldots,m}$ are velocities in the global frame, and higher order derivatives of the $\mathbf{p}_{i}$’s. 3. 3.

$\{\mathbf{b}\}\in\mathbb{R}^{k}$, are quantities being invariant to global transformations, such as sensors’ biases or camera’s calibration parameters.

Without restriction, we consider in the following $l=m=1$ for convenience of notation.

Definition 3**.**

The Special Euclidean group $SE(3)$ describes rigid motions in 3D and is defined as $SE(3)=\{\alpha=(\mathbf{R}_{\alpha},\mathbf{p}_{\alpha}),\mathbf{R}_{\alpha}\in SO(3),\mathbf{p}_{\alpha}\in\mathbb{R}^{3}\}$. Given $\alpha,\beta\in SE(3)$, the group operation is $\alpha\beta=(\mathbf{R}_{\alpha}\mathbf{R}_{\beta},\mathbf{R}_{\alpha}\mathbf{p}_{\beta}+\mathbf{p}_{\alpha})$ and the inverse $\alpha^{-1}=(\mathbf{R}^{T}_{\alpha},-\mathbf{R}_{\alpha}^{T}\mathbf{p}_{\alpha})$. We denote $Id$ the identity.

Changes of global frame are encoded as the action $\phi_{\alpha}(\mathbf{x})$ of an element $\alpha\in SE(3)$ on $\mathcal{X}$. Thus, quantities expressed in the global frame (such as the vehicle position) are rotated and translated by the action $\phi_{\alpha}(\cdot)$, whereas quantities expressed in the vehicle’s frame, such as Inertial Measurement Unit (IMU) biases, are left unchanged. The action then writes (with $i=1,\ldots,l$ and $j=1,\ldots,m$)

[TABLE]

Assumption 2**.**

The vehicle’s dynamic does not depend on the choice of global frame, and the vehicle’s sensors only have access to relative observations, i.e. no absolute information is available.

As a result, the equations write (1)-(2) and are invariant to the action of $SE(3)$ in the sense of Definition 2.

To differentiate w.r.t. elements of $SE(3)$ we resort to its Lie algebra and do in detail what is sketched in Footnote 1.

Definition 4**.**

The Lie algebra $\mathfrak{se}(3)$ of $SE(3)$ encodes small rigid motions about the identity. It is defined as $\{\left((\delta\bm{\omega})_{\times},\delta\mathbf{p}\right);\leavevmode\nobreak\ \delta\bm{\omega},\delta\mathbf{p}\in\mathbb{R}^{3}\}$, where $(\bm{\omega})_{\times}$ is the skew symmetric matrix associated with cross product with $\bm{\omega}\in\mathbb{R}^{3}$. For any $\delta\alpha\in\mathfrak{se}(3)$, we have $\alpha:=\exp_{SE(3)}(\delta\alpha)\in SE(3)$ where $\exp_{SE(3)}(\cdot)$ denotes the exponential map of $SE(3)$ (for a definition see (39)-(43) below with $l=1,m=0,k=0$).

For more information about $SE(3)$ and its use in state estimation for robotics see the recent monographs [3, 18].

Writing $(\mathbf{R}_{\alpha},\mathbf{p}_{\alpha})=\exp_{SE(3)}\left(\left[(\delta\bm{\omega})_{\times},\delta\mathbf{p}\right]\right)$ in (27) we see the directions infinitesimally spanned by $\phi_{\cdot}(\cdot)$ at $\mathbf{x}$ in the direction $\delta\alpha=\left((\delta\bm{\omega})_{\times},\delta\mathbf{p}\right)$ write for the state (23):

[TABLE]

where $i=1,\ldots,l$ and $j=1,\ldots,m$.

Example 1**.**

[SLAM] Consider a simple SLAM system with one robot and one landmark [28]. Let $\mathbf{p}_{R}$ be the position of the robot, $\mathbf{R}$ the orientation of the robot, and $\mathbf{p}_{L}$ the landmark’s position. The state is

[TABLE]

The dynamics write $f(\mathbf{x},\mathbf{u},\mathbf{w})=(\mathbf{R}\bar{\mathbf{R}},\mathbf{p}_{R}+\mathbf{R}\bar{\mathbf{R}}\bar{\mathbf{p}},\mathbf{p}_{L})$ where $\bar{\mathbf{R}},\bar{\mathbf{p}}$ denote orientation and position increments typically measured through odometry. The observation of the landmark in the robot’s frame is of the form $\mathbf{y}=\tilde{h}(\mathbf{R}^{T}(\mathbf{p}_{L}-\mathbf{p}_{R}))$. Translations and rotations of the global frame correspond to actions of elements $\alpha=(\mathbf{R}_{\alpha},\mathbf{p}_{\alpha})\in SE(3)$ as

[TABLE]

The system is obviously invariant.

Referring to (25), as $l=2,m=0$ and $\mathbf{p}_{R}=\mathbf{p}_{1},\mathbf{p}_{L}=\mathbf{p}_{2}$, the directions spanned by $\phi_{\cdot}(\cdot)$ at $\mathbf{x}$ are as follows

[TABLE]

with $\left((\delta\bm{\omega})_{\times},\delta\mathbf{p}\right)\in\mathfrak{se}(3)$. As a direct consequence of Prop. 1, those directions are unobservable. The system continues to be invariant even if a sophisticated model is assumed: the motion equations do not depend a choice of global frame.

Example 2**.**

[VIO, or Visual Inertial Navigation System (VINS)] Consider a vehicle equipped with an IMU and a camera, as in [36]. Let $\mathbf{v}$ denote the vehicle velocity, $\mathbf{b}$ the IMU bias and/or scale factors. The state is

[TABLE]

and observations correspond to landmarks’ bearings in the vehicle frame, whereas inputs $\mathbf{u}_{n}\in\mathbb{R}^{6}$ are provided by an IMU. A change of global frame $\alpha=(\mathbf{R}_{\alpha},\mathbf{p}_{\alpha})$ writes

[TABLE]

where we restrict $\mathbf{R}_{\alpha}$ to be around the gravity axis, i.e. with $\mathbf{R}_{\alpha}\mathbf{g}=\mathbf{g}$, since the vertical is measured [36]. The action $\phi_{\cdot}(\cdot)$ is then also invariant.

3.1 EKF Based on a Nonlinear Error

For the general state $\mathbf{x}$ of (23) consider the nonlinear error

[TABLE]

where $i=1,\ldots,l$ and $j=1,\ldots,m$. We set $l=m=1$ for simplicity. To linearize, we have the following first order vector approximation of (55) that lives in $\mathbb{R}^{3(1+l+m)+k}$

[TABLE]

where $\mathbf{e}_{R}\in\mathbb{R}^{3}$.

Proposition 3**.**

The error (55) is compatible with the action (24) of $SE(3)$ in the sense of Assumption 1.

Proof.

For $\alpha\in SE(3)$, using (55) we obtain that

[TABLE]

such that $\eta(\phi_{\alpha}(\mathbf{x}),\mathbf{x})$ turns out to be independent of $\mathbf{x}$, and the result is readily obtained by differentiation at $\alpha=Id$. ∎

3.2 Choice of the Retraction

When the state space is a Lie group, and one uses errors that are invariant with respect to right multiplication, the theory of Invariant EKF (IEKF) [9, 5] suggests to use $\psi\left(\mathbf{x},\mathbf{e}\right)=\exp(\mathbf{e})\mathbf{x}$ where $\exp(\cdot)$ denotes the Lie group exponential. In [10], it was noticed a natural Lie group structure underlies the (odometry based) SLAM problem. In the preprint [4] of the current article, we formalized more elegantly this group, and called it $SE_{l+1}(3)$. The current paper is a generalization to more complete state (23), which may be endowed with the group structure $SE_{l+m}(3)\times\mathbb{R}^{k}$ (direct product of groups $SE_{l+m}(3)$ and $\mathbb{R}^{k}$). For state (23) we thus suggest $\mathbf{x}^{+}=\psi\left(\mathbf{x},\mathbf{e}\right):=\exp_{SE_{l+m}(3)\times\mathbb{R}^{k}}(\mathbf{e})\mathbf{x}$, i.e.

[TABLE]

with $i=1,\ldots,l,\leavevmode\nobreak\ j=1,\ldots,m$, and where

[TABLE]

[TABLE]

$\delta\mathbf{b}^{+}=\mathbf{e}_{\mathbf{b}}$, $\mathbf{0}=\mathbf{0}_{3+3m+3l\times 3}$ and $\mathbf{I}=\mathbf{I}_{3+3m+3l\times 3}$.

3.3 Extension to Problems Involving Multiple Robots

Consider a problem consisting of $M$ systems, see e.g. Section 5. We have $M$ global orientations, one for each system, that transform as the global frame’s orientation. For such problems, we define a collection of $\mathbf{x}_{i}$ of (23), and the alternative state error of the problem $\eta(\cdot,\cdot)$ merely writes

[TABLE]

where $\eta_{i}\left(\mathbf{x}_{i},\hat{\mathbf{x}}_{i}\right)$ is the error (55) for the $i$-th system.

4 Simulation Results

This section considers the 2D wheeled-robot SLAM problem and illustrates the performances of the proposed approach. We conduct similar numerical experiment as in the sound work [28] dedicated to EKF inconsistency and benefits of the OC-EKF, i.e., a robot makes 7 circular loops and 20 landmarks are disposed around the trajectory, see Figure 2. We refer the interested reader to the available Matlab code for the parameter setting and reproducing the present results.

We compare our approach to standard EKF which conveys an estimate of the linear error; OC-EKF [28] which linearizes the model in a nontrivial way to enforce the unobservable subspace of $\hat{\mathcal{O}}$ to have an appropriate dimension; robocentric EKF [15, 19, 37] which express the state in the robot’s frame and then devise an EKF; and iSAM [32, 33], a popular optimization technique both for SLAM and odometry estimation which finds the most likely state trajectory given all past measurements. The differences between the estimation errors used by the various EKF variants are recapped in Figure 1.

The results confirm the consistency guarantees of Theorem 1 and Proposition 2 are beneficial to the EKF in practice.

4.1 Monte-Carlo Based Numerical Results

Figure 3 displays the Normalized Estimation Error Squared (NEES), Root Mean Square Error (RMSE) and distance to Maximum-Likelihood estimate, over 1000 Monte-Carlo runs.

4.1.1 Consistency Evaluation

The NEES [2] provides information about the filter consistency, such that NEES $>$ 1 reveals an inconsistency issue: the actual uncertainty is higher than the computed uncertainty. As expected, the NEES of the robot pose estimates in Figure 3 indicates that the standard EKF is inconsistent, whereas the other approaches are more consistent. The proposed EKF and iSAM obtain the best NEES, whereas the NEES of OC-EKF and robocentric EKF slightly increase after the first turn, i.e. at the first loop closure.

4.1.2 Accuracy Performances

We evaluate accuracy through RMSE of the robot position error. This confirms that: “solving consistency issues improves the accuracy of the estimate as a byproduct, as wrong covariances yield wrong gains” [2]. Numerical results are displayed in Figure 1.

4.1.3 Distance to Maximum-Likelihood Estimate

We use as a third performance criterion distance to the estimates returned by iSAM [32], which are optimal in the sense that it returns the Maximum A Posteriori (MAP) estimate. To this respect, we see that the proposed EKF is the closest to iSAM.

4.1.4 Execution Time

We provide the execution time of the filters for the 100 Monte-Carlo runs in Table 1, which are implemented in Matlab and tested on Precision Tower 7910 armed with CPU E5-2630 v4 2.20 Hz. The iSAM’s execution time is not included since it cannot be compared: it is implemented using C++ and an optimized code, whereas we used Matlab based simulations. It is thus evidently lower. Regarding computational complexity, our proposed filter has similar complexity as the standard EKF and OC-EKF, since its EKF-based structure makes it quadratic in the state dimension, i.e., number of landmarks. The use of a retraction at the update step instead of mere addition may slightly increase the computational burden, but the impact in the execution time proves negligible. The robocentric filter is penalized because it moves the landmarks during propagation, which in turn impacts the propagation of the covariance matrix. In our solution landmarks remain fixed during propagation. Note that the proposed solution can be implemented using recent techniques [34, 30] to decrease computational load. To implement the robocentric and OC-EKF we report that we used the code of [28], see Acknowledgments.

These simulations confirm that regarding SLAM, the proposed filter is an alternative to the OC-EKF. Contrarily to OC-EKF the model is linearized at the (best) estimate, and is thus much closer to standard EKF methodology, and applies to a large class of problems without explicit computation of the unobservable directions.

5 Experimental Results

This section validates the proposed filter on multi-robot SLAM on the UTIAS dataset [35], to prove the feasibility and the benefits of the approach. Since both robocentric and OC-EKF are not straightforwardly applied to multi-robot SLAM, we compare our approach to iSAM and a standard EKF only, both filter using a centralized scheme, although both can be used in decentralized estimation [34] (an OC-EKF has been derived only for cooperative localization in [29]).

The 2D indoor UTIAS dataset [35] consists of a collection of 9 individual datasets of 20-70 min containing odometry and range and bearing measurements data from 5 robots, as well as ground-truth for all robot poses and 15 landmark positions, and has been especially realized for studding the multi-robot SLAM problem. The forward velocity and angular velocity commands are logged at $67\text{\,}\mathrm{H}\mathrm{z}$ as odometry data. The maximum forward velocity of a robot is $0.16\text{\,}\mathrm{m}\mathrm{/}\mathrm{s}$, and the maximum angular velocity is 0.35rad/s. Each robot and landmark has a unique identification barcode, that are detected in rectified images captured by the camera on each robot. The encoded identification barcode as well as the range and bearing to each barcode is then extracted, where the camera on each robot is placed to align with the robot body frame. The ground-truth pose for each robot and ground-truth position for each landmark is provided by a Vicon motion capture system at $100\text{\,}\mathrm{H}\mathrm{z}$ with accuracy on the order of ${10}^{-3}\text{\,}\mathrm{m}$.

5.1 Alternative Error Derivation

Let $M$ robots navigate in an unknown environment of $P$ landmarks, where $(\mathbf{p}_{R}^{i},\theta_{i})$ is the pose of the $i$-th robot. Inspired from Section 3.3, we suggest to treat each robot as a system with its own global orientation. Regarding landmarks, we chose to consider each landmark as a system (23), whose orientation is fixed and is associated with the orientation $\theta^{j}$ of the robot that observes this landmark for the first time (we have then $\theta^{j}_{n+1}=\theta^{j}_{n}$). Our error leads to an increase of the dimension of the covariance matrix $\mathbf{P}^{e}$, but clearly improves the filter accuracy, as shown below.

To recap the difference with EKF for the present problem:

• •

The standard EKF conveys an estimate of the dispersion of the linear error, which is $(\hat{\theta}_{i}-\theta_{i},\hat{\mathbf{p}}_{R,i}-\mathbf{p}_{R,i})$ for the $i$-th robot and $\hat{\mathbf{p}}_{L,j}-\mathbf{p}_{L,j}$ for the $j$-th landmark.

• •

The proposed EKF conveys an estimate of the dispersion of an alternative error defined as $(\hat{\theta}_{i}-\theta_{i},\hat{\mathbf{p}}_{R,i}-\hat{\mathbf{R}}_{i}\mathbf{R}_{i}^{T}\mathbf{p}_{R,i})$ for the $i$-th robot and $(\hat{\theta}^{j}-\theta^{j},\hat{\mathbf{p}}_{L,j}-\hat{\mathbf{R}}^{j}\mathbf{R}^{jT}\mathbf{p}_{L,j})$ for the $j$-th landmark.

This is an application of the method of Section 3.3, where each landmark position is associated to a fixed orientation $\theta^{j}$. Note that, deriving a robocentric EKF seems non-trivial.

5.2 Experimental Results

We conducted preliminary tests to calibrate the motion model and to characterize the noise properties of the motion and measurement models as follows: standard deviation on odometry as 20 % of the robot velocity, and standard deviation of range and bearing measurement as, respectively, $0.5\text{\,}\mathrm{m}$ and $3\text{\,}{}^{\circ}$. We then define a maximum observation range of $5\text{\,}\mathrm{m}$. This corresponds to the most favorable setting for standard EKF.

Since the NEES is a statistical indicator and is very sensitive to parameter tuning, we focus on the RMSE, which is plotted on Figure 5 for the robot positions on all the available experiments. The RMSE for the standard EKF is summarized over all datasets in Figure 2, with improvement for iSAM and the proposed EKF of as much as 50 % compared to the standard EKF in the more challenging dataset 9 (where visual barriers reduce the number of barcode detections).

6 Conclusion

This work evidences the EKF for robot navigation is not inherently inconsistent but the choice of the estimation error for linearization is pivotal: properly defining the error the EKF shall linearize yields consistency. For SLAM, Monte-Carlo simulations and real experiments have evidenced our alternative EKF outperforms the EKF and achieves similar performance as state of the art iSAM. It thus offers an alternative to OC-EKF based on a sound mathematical theory anchored in geometry. Moreover the general theory goes beyond basic SLAM. Future works concern the application of the method in various navigation problems and its derivation for both unscented Kalman filter and optimization techniques [32, 33].

Acknowledgments

We thank Guoquan Paul Huang of University of Delaware for sharing his code of OC-EKF.

Appendix A 2D Mono-Robot Wheeled-SLAM

We detail in this section the proposed filter for the 2D mono-robot wheeled-SLAM problem, which correspond to Section IV. This section starts by recalling the considering problem, details the proposed EKF and finishes with the standard EKF algorithm for the reader to compare and see what the differences are.

We consider a 2D SLAM system with one robot and $K$ landmarks. Let $\mathbf{p}_{R,n}\in\mathbb{R}^{2}$ be the position of the robot, $\mathbf{R}_{n}\in SO(2)$ the orientation of the robot, and $\mathbf{p}_{L,n}^{j}\in\mathbb{R}^{2}$ the position of the $j$-th landmark. The state is given as

[TABLE]

The dynamics write

[TABLE]

where

[TABLE]

denotes orientation and position increments typically measured through odometry, and $\mathbf{R}(\theta)$ is the rotation matrix of angle $\theta$. The noise in the propagation model is given as

[TABLE]

and contains noise on both the angular and position increments.

The observation of the landmarks in the robot’s frame writes

[TABLE]

where for any landmark we have

[TABLE]

which represents a range and bearing observation and $\mathbf{n}_{n}^{k}\sim\mathcal{N}\left(\mathbf{0},\mathbf{N}_{n}^{k}\right)$ is the noise in the measurement of the $k$-th landmark, letting

[TABLE]

be the covariance matrix for all the noise in the observation (51). Only a small fraction of the landmarks are observed at each step, i.e. only a subset of (51) is used. We now detail the proposed EKF for the considered SLAM problem. This EKF first appears in [4] and was then shown to remedy consistency issues in this context.

A.1 Proposed EKF Derivation

For the considered problem, the non-linear error is defined as, see (25)

[TABLE]

To linearize, we have the following first order vector approximation, see (26)-(27)

[TABLE]

The proposed filter operates in two steps: propagation and update, see Algorithm 2. We now detail these two steps.

A.2 Propagation

At this step, we first propagate the state with the noise free model to compute $\hat{\mathbf{x}}_{n|n-1}$. We then propagate the covariance, where Jacobian are obtained after conserving only the first order error term in $\eta(\mathbf{x}_{n|n-1},\mathbf{x}_{n})$. The Jacobian of the propagation are given as

[TABLE]

[TABLE]

A.3 Update

This step considers the observations of the landmarks. The Jacobian for for the measurements are given as

[TABLE]

where $\mathbb{M}$ is the matrix

[TABLE]

and $\hat{\mathbf{J}}^{e}_{n}=\mathbf{I}$. The non-zero parts of $\hat{\mathbf{H}}_{n}^{e,k}$ in (61) correspond to the error on the robot position and on the $k$-th landmark. Once the Kalman gain $\mathbf{K}_{n}$ is computed, we compute the innovation $\mathbf{e}_{n}^{+}$ and them update the state. The retraction required to update the state is given as the exponential of $SE_{1+K}(2)$, see Section IV-A and see also [4]. We have thus

[TABLE]

where

[TABLE]

We conclude this step by updating the matrix covariance, see step 6 of Algorithm 2.

A.4 Standard EKF Algorithm

We provide in this section the standard EKF algorithm. The standard EKF follows Algorithm 2 with a simple linear error that we denote using the superscript std.

In 3D, the difference between two rotation matrices does not make any sense. It is thus customary to use the difference in the sense of group multiplication on $SO(3)$, and this is what is referred to as “standard” EKF in the following. Thus, the state error on which the EKF is built is as follows:

[TABLE]

The following first order vector approximation writes

[TABLE]

The Jacobians and the retraction are given as

[TABLE]

[TABLE]

[TABLE]

[TABLE]

with $\mathbb{M}$ the matrix

[TABLE]

where we let

[TABLE]

and

[TABLE]

where

[TABLE]

and $\mathbf{e}_{n}^{+}$ has the same form (70) as for the proposed EKF, and $\hat{\mathbf{J}}^{\mathrm{\textsc{std}}}_{n}=\mathbf{I}$.

Appendix B 3D Mono-Robot Wheeled-SLAM

We detail in this section the proposed filter for the 3D mono-robot wheeled-SLAM problem. This section starts by recalling for the reader to compare. The similarities with the 2D problem (see Section A) are obvious.

We consider a 3D SLAM system with one robot and $K$ landmarks. Let $\mathbf{p}_{R,n}\in\mathbb{R}^{3}$ be the position of the robot, $\mathbf{R}_{n}\in SO(3)$ the orientation of the robot, and $\mathbf{p}_{L,n}^{j}\in\mathbb{R}^{3}$ the position of the $j$-th landmark. The state is given as

[TABLE]

The dynamics write

[TABLE]

where

[TABLE]

denotes orientation and position increments typically measured through odometry, and $\mathbf{R}(\bm{\omega})=\exp_{SO(3)}\left(\bm{\omega}\right)$. The noise in the propagation model is given as

[TABLE]

and contains noise on both the angular and position increments.

The observation of the landmarks in the robot’s frame is given as

[TABLE]

where the observation model for one landmark

[TABLE]

represents a perspective projection observation given e.g. by a monocular camera and $\mathbf{n}_{n}^{k}\sim\mathcal{N}\left(\mathbf{0},\mathbf{N}_{n}^{k}\right)$ is the noise in the measurement of the $k$-th landmark, letting

[TABLE]

be the covariance matrix for all the noise in the observation (93). Only a small fraction of the landmarks are observed at each step, i.e. only a subset of (93) is used. We now detail the proposed EKF for the considered SLAM problem.

B.1 Proposed EKF Derivation

For the considered problem, the non-linear error is defined as in (31)

[TABLE]

To linearize, we have the following first order vector approximation, see (32)-(33)

[TABLE]

The proposed filter operates in two steps: propagation and update, see Algorithm 2. We now detail these two steps.

B.2 Propagation

At this step, we first propagate the state with the noise free model to compute $\hat{\mathbf{x}}_{n|n-1}$. We then propagate the covariance, where Jacobian are obtained after conserving only the first order error term in $\eta(\mathbf{x}_{n|n-1},\mathbf{x}_{n})$. The Jacobians of the propagation are given as

[TABLE]

[TABLE]

B.3 Update

This step considers the observations of the landmarks. The Jacobian for for the measurements are given as

[TABLE]

and $\hat{\mathbf{J}}^{e}_{n}=\mathbf{I}$. Once the Kalman gain $\mathbf{K}_{n}$ is computed, we compute the innovation $\mathbf{e}_{n}^{+}$ and them update the state. The retraction required for updated the state is given as the exponential of $SE_{1+K}(3)$, see Section IV-A. We have thus

[TABLE]

where

[TABLE]

[TABLE]

[TABLE]

We conclude this step by updating the matrix covariance.

B.4 Standard EKF Algorithm

We provide in this section the standard EKF algorithm. The standard EKF follows Algorithm 2 with a different error as the proposed and where we use the superscript std.

The non-linear error is defined as

[TABLE]

The following first order vector approximation writes

[TABLE]

The Jacobians and the retraction are given as

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

where

[TABLE]

and $\mathbf{e}_{n}^{+}$ has the same form (70) as for the proposed EKF, and $\hat{\mathbf{J}}^{\mathrm{\textsc{std}}}_{n}=\mathbf{I}$.

Appendix C 2D Multi-Robot Wheeled-SLAM

We detail in this section the proposed filter for the 2D multi-robot wheeled-SLAM problem. This section starts by recalling the considering problem and details the proposed EKF. The similarities with the mono-robot problem (see Section A) are immediate.

We consider a 2D SLAM system with $M$ robots and $K$ landmarks. Let $\mathbf{p}_{R,n}^{m}\in\mathbb{R}^{2}$ be the position of the $m$-th robot, $\mathbf{R}_{n}^{m}\in SO(2)$ the orientation of the $m$-th robot, and $\mathbf{p}_{L,n}^{j}\in\mathbb{R}^{2}$ the position of the $j$-th landmark. The state is given as

[TABLE]

The dynamics writes

[TABLE]

where

[TABLE]

denotes orientation and position increments typically measured through odometry for each robot, and $\mathbf{R}(\theta)$ is the rotation matrix of angle $\theta$. The noise in the propagation model is given as

[TABLE]

and contains noise on both the angular and position increments.

The observations of the landmarks robots in the $M$ robot’s frames are given as

[TABLE]

where the observation model for one landmark or one robot

[TABLE]

represents a range and bearing observation and $\mathbf{n}_{n}^{k,m}\sim\mathcal{N}\left(\mathbf{0},\mathbf{N}_{n}^{k,m}\right)$ is the noise in the measurement, letting

[TABLE]

be the covariance matrix for all the noise in the observation (51). Only a small fraction of the landmarks are observed at each step, i.e. only a subset of (51) is used. We now detail the proposed EKF for the considered SLAM problem.

C.1 Proposed EKF Derivation

For the considered problem, the non-linear error is defined as, (25)

[TABLE]

To linearize, we have the following first order vector approximation, see (26)-(27)

[TABLE]

The proposed filter operates in two steps: propagation and update, see Algorithm 2. We now detail these two steps.

C.2 Propagation

TODO During these state, we first propagate the state with the noise free model to compute $\hat{\mathbf{x}}_{n|n-1}$. We then propagate the covariance, where Jacobian are obtained after conserving only the first order error term in $\eta(\mathbf{x}_{n|n-1},\mathbf{x}_{n})$. The Jacobian of the propagation are given as

[TABLE]

[TABLE]

C.3 Update

This step considers the observations of the landmarks. The Jacobian for for the measurements are given as

[TABLE]

and $\hat{\mathbf{J}}^{e}_{n}=\mathbf{I}$. Once the Kalman gain $\mathbf{K}_{n}$ is computed, we compute the innovation $\mathbf{e}_{n}^{+}$ and them update the state. The retraction required for updated the state is given as the exponential of $SE_{1+K}(2)$, see Section IV-A of [11]. We have thus

[TABLE]

where

[TABLE]

We conclude this step by updating the matrix covariance.