PaperlayerView exact revision on arXiv

Source-linked AI summary

An EKF-SLAM algorithm with consistency properties

Axel Barrau, Silvere Bonnabel

arXiv:1510.06263v3cs.ROcs.SYeess.SY

TL;DR

EKF-SLAM can become inconsistent because it treats the global frame’s unobservable origin and orientation as estimable. This paper applies an Invariant EKF to SLAM and reports that its model preserves those unobservable directions, with simulations showing improved consistency over several EKF variants.

  • Problem

    EKF-SLAM can report information gain in the globally unobservable origin and orientation, yielding an overoptimistic covariance.

    Source: §1 · p. 1

  • Method

    The paper applies an Invariant EKF to SLAM, using an alternative error representation to preserve the considered unobservable directions.

    Source: §1 · p. 2, §5.1 · p. 14

  • Results

    IEKF-SLAM’s NEES remains close to 1, and the simulations report striking improvement over EKF, UKF, OC-EKF, and ideal EKF.

    Source: §7.3 · p. 23, §8 · p. 23

  • Takeaways & Limitations

    For the considered unobservable global-frame transformations, the paper shows that EKF-SLAM is not inherently inconsistent and that the choice of linearization coordinates is pivotal.

    Source: §8 · p. 23

    The analysis focuses, for simplicity, on the standard steered bicycle (or unicycle) model.

    Source: §2.2 · p. 5

Abstract

from arXiv · show

In this paper we address the inconsistency of the EKF-based SLAM algorithm that stems from non-observability of the origin and orientation of the global reference frame. We prove on the non-linear two-dimensional problem with point landmarks observed that this type of inconsistency is remedied using the Invariant EKF, a recently introduced variant ot the EKF meant to account for the symmetries of the state space. Extensive Monte-Carlo runs illustrate the theoretical results.

1 Introduction

EKF-SLAM can mistake the global frame’s unobservable origin and orientation for estimable quantities, producing inconsistent covariance. The paper proposes an Invariant EKF-SLAM that addresses this issue through a different choice of estimation error.

1 Introduction

  • Because SLAM measurements are relative, the global frame’s origin and orientation are unobservable, yet EKF-SLAM can report information gain in those directions.The resulting covariance is too optimistic and can lead to inaccurate estimates.Source: §1 · p. 1
  • The paper generalizes the IEKF to handle general observations and shows its SLAM variant remedies inconsistency from global-frame unobservability.The paper describes this as an alternative to treating EKF-like methods as inherently inconsistent.Source: §1 · p. 2

1.1 Links and differences with previous literature

  • Earlier analyses link inconsistency to unobservable global-frame transformations; OC-EKF addresses this by selecting a linearization point with the appropriate unobservable-subspace dimension.The IEKF instead changes the estimation-error variable and uses the latest state estimate as its linearization point.Source: §1.1 · pp. 2–3

1.2 Paper’s organization

  • The paper reviews EKF-SLAM, develops the IEKF-SLAM and its observability properties, presents simulations, and derives a 3D version in the appendix.It also discusses an output-covariance property in terms of Fisher information.Source: §1.2 · p. 4

2 The EKF-SLAM algorithm

This section recaps the standard EKF and applies it to a two-dimensional SLAM model with robot pose and point landmarks. The resulting EKF-SLAM uses first-order expansions and Kalman updates for feature observations.

2.1 Statement of the general standard EKF equations

  • The standard EKF propagates its estimate, linearizes the dynamics and observations, then uses Kalman equations to update the error estimate, covariance, and state.It approximates the error as Gaussian after each measurement; the linearizations may cause inconsistency or divergence.Source: §2.1 · pp. 4–5

2.2 The considered SLAM problem

  • The SLAM state includes robot heading and 2D position plus unknown landmark positions, with odometry motion and feature observations in the robot’s frame.The observation model allows a general function; range-and-bearing observations are included, and the exposition assumes all features are observed at each step for simplicity.Source: §2.2 · pp. 5–6

If we choose instead the one dimensional observation ˜h

  • The observability properties considered are insensitive to how noise enters the system because they depend on the deterministic part of the model.The paper also considers a one-dimensional observation form for the 2D monocular SLAM measurement.Source: If we choose instead the one… · p. 6

2.3 The EKF-SLAM algorithm

  • Applying the standard EKF first-order expansions to the SLAM dynamics and observations yields the EKF-SLAM algorithm, summarized in Algorithm 2.Source: §2.3 · p. 7

3 Observability issues and consistency of the EKF

The section characterizes unobservable shifts and explains how the EKF’s linearized error model can fail to preserve them. It traces that failure to the standard error definition and motivates changing the error variable.

3 Observability issues and consistency of the EKF

  • An unobservable shift is an initial-state perturbation whose propagated linearized shifts remain undetectable in the observations.Such directions represent changes the filter should not treat as accurately estimated.Source: §3 · p. 8

3.1 Considered unobservable shifts

  • Global-frame translations and rotations leave measurements unchanged; infinitesimal rotations are unobservable shifts of the system.Equal shifts of robot and landmark positions cannot be distinguished through the measurements.Source: §3.1 · pp. 8–9

3.2 Inconsistency of the EKF

  • For the EKF-linearized system, an infinitesimal rotation is unobservable only if a condition involving state updates holds, and noise makes that condition almost surely fail in practice.The condition involves the updates of the state.Source: §3.2 · pp. 9–10
  • The paper attributes the false observability problem to defining estimation error as the linear difference between true and estimated states, rather than to EKF-like SLAM methods inherently.It proposes that a different error variable can alter the condition governing observability.Source: §3.2 · p. 10

4 A novel EKF-SLAM algorithm

The paper introduces an IEKF for SLAM based on an alternative estimation error. It derives the error and observation linearizations used to update the state and covariance.

4 A novel EKF-SLAM algorithm

  • The proposed IEKF for SLAM applies the matrix-Lie-group IEKF framework, extended to allow general observation forms.The paper presents the SLAM example to explain the method without requiring the reader to study the general theory first.Source: §4 · p. 10
  • Instead of the usual state difference, the algorithm uses an alternative estimation error that is zero exactly when the estimate equals the true state.The resulting error system is propagated through a first-order approximation, and the odometer outputs vanish from the propagation expression.Source: §4 · pp. 10–11
  • The observation error is linearized in that alternative error, and the resulting matrices support Kalman-gain and covariance calculations.The state update follows the IEKF’s nonlinear update rule, which agrees with the direct alternative-error update to first order.Source: §4 · pp. 11–12

5 Remedying EKF SLAM consistency

The IEKF-SLAM linearized model treats global-frame rotations and translations as unobservable shifts, remedying the inconsistency associated with those directions. Its invariant error also explains how the filter avoids spurious covariance reduction over unobservable shifts.

5 Remedying EKF SLAM consistency

  • Unlike the usual restriction on linearization points, IEKF-SLAM retains the unobservable shifts regardless of the chosen linearization point.The paper contrasts this property with previous results.Source: §5 · pp. 12–13

5.1 Main result

  • A global-frame rotation appears in the linearized error as a rotation perturbation, while a translation appears as the same displacement of the robot and landmark.The translation perturbation has the form (0,δu,δu)T.Source: §5.1 · p. 14
  • IEKF-SLAM’s linearized model preserves global-frame rotations and translations as unobservable shifts for any sequence of true states and estimates.The paper identifies this as a consistency property of the algorithm.Source: §5.1 · p. 14
  • The linearized model correctly captures global rotations and translations, and the unobservable directions have the appropriate dimension.The paper describes the correct dimension as a byproduct of the consistency property.Source: §5.1 · p. 14

5.2 Interpretation and discussion

  • The IEKF interprets the updated covariance as the covariance of an error defined using the updated estimate, preventing spurious covariance reduction over unobservable shifts.The covariance is computed using the predicted estimate, then interpreted through an error whose definition depends on the updated estimate.Source: §5.2 · p. 15
  • The alternative error arises naturally from the Lie group structure because the SLAM model is invariant to global translations and rotations.The paper links this symmetry-aware error to the usefulness of the Invariant approach.Source: §5.2 · p. 15

6 IEKF consistency and information

The section connects IEKF observability to Bayesian Fisher information: global-frame shifts remain unobservable in the linearized model, and the output covariance reflects no information gain along them.

6 IEKF consistency and information

  • Theorem 1 shows infinitesimal global-frame rotations and translations remain unobservable in the IEKF linearized model, avoiding false observability along those directions.Source: §6 · p. 16
  • The covariance analysis frames unobservability as absent Fisher-information gain, relating the result to Fisher-information-based SLAM consistency approaches.Source: §6 · p. 16

6.1 The general Bayesian Fisher Information Matrix

  • The Bayesian Fisher Information Matrix is built from the log-likelihood gradient dyad and yields a lower bound on accuracy achievable by filtering estimators.Source: §6.1 · p. 16
  • In the linear-Gaussian case, the inverse Kalman covariance reflects the statistical information available about the state at time n.Source: §6.1 · pp. 16–17

6.2 Application to IEKF-SLAM consistency

  • Theorem 2 ensures the IEKF covariance reflects no spurious Bayesian Fisher information gain along global-frame translations and rotations, including in information form when covariance is noninvertible.The theorem states the result for invertible covariance; the proof also supports it in information form when covariance is not invertible.Source: §6.2 · p. 17

7 Simulation results

Simulations compare IEKF-SLAM with four filters and show improved pose consistency alongside RMS performance similar to OC-EKF and ideal EKF.

7.3 Numerical results

  • IEKF SLAM keeps pose NEES close to 1, while EKF and UKF show large inconsistencies; OC-EKF and ideal EKF range from 1 to 1.7.Source: Figure 2 · p. 19, §7.3 · p. 23

7.1 Simulation setting

  • The experiment repeats a 15 m loop ten times past 20 unknown features and compares IEKF with EKF, UKF, OC-EKF, and an ideal EKF.Source: §7 · p. 18, §7.1 · p. 19

7.3 Numerical results

7.2 The NEES indicator

  • NEES normalizes squared estimation error by filter covariance; for Gaussian errors it approaches 1 at large sample counts, while values above 1 indicate overconfidence.Source: §7.2 · p. 22

7.3 Numerical results

  • In a single run, EKF and UKF heading errors leave the 99% envelope and landmark errors miss ellipsoids, unlike IEKF, OC-EKF, and ideal EKF.Source: Figure 4 · p. 21, §7.3 · p. 23
  • Information along an infinitesimal rotation shift decreases for IEKF, ideal EKF, and OC-EKF, but not EKF; UKF is slightly better than EKF.Source: §7.3 · p. 23

8 Conclusion

The paper argues that EKF-SLAM inconsistency tied to unobservable global-frame transformations can be remedied by choosing suitable linearization coordinates. Monte-Carlo simulations illustrate the IEKF’s consistency and improvement over several alternatives.

8 Conclusion

  • Monte-Carlo simulations illustrate IEKF-SLAM consistency and striking improvement over EKF, UKF, OC-EKF, and the ideal EKF.The ideal EKF linearizes about the true trajectory and is impossible to implement.Source: §8 · p. 23
  • EKF-SLAM is not inherently inconsistent with respect to unobservable global-frame transformations; the choice of linearization coordinates is pivotal.Source: §8 · p. 23
  • The IEKF may also be relevant to autonomous navigation and motion-planning controllers, beyond SLAM.Source: §8 · pp. 23–24
  • IEKF retains EKF’s advantages while adding guaranteed properties, including recursive operation and suitability for online real-time applications.The paper notes that IEKF is similar to EKF in other respects.Source: §8 · p. 24
  • IEKF-SLAM may help initialize high-dimensional SLAM optimization methods, for which small initial estimation error is critical.Source: §8 · p. 24
  • The authors leave applying their symmetry-based approach to improve first-order SLAM optimization techniques for future work.Their proposed geometric gradient directions would follow unobservable directions; the discussion assumes noise-free sensors for simplicity.Source: §8 · p. 24

A IEKF theory, applications to 2D and 3D SLAM

This section details IEKF theory on matrix Lie groups, its use in the SLAM algorithm, and the IEKF equations for 3D SLAM.

A IEKF theory, applications to 2D and 3D SLAM

A.1 Primer on matrix Lie groups

The primer introduces matrix Lie groups and their associated algebraic tools, including the Lie algebra, exponential map, and adjoint matrix.

A.1 Primer on matrix Lie groups

  • A matrix Lie group is a subset of square invertible matrices satisfying specified properties.Source: §A.1 · p. 25
  • Derivatives at the identity define the Lie algebra, which can be identified with a vector space and mapped to the group by the Lie exponential.The exponential map is invertible for small ξ, and the BCH formula gives a local product expansion.Source: §A.1 · p. 25
  • The adjoint matrix is defined through conjugation of an exponential by a group element.Source: §A.1 · p. 25

A.1.1 Group of direct planar isometries SE(2)

The section introduces the group of direct planar isometries using homogeneous matrices.

A.1.1 Group of direct planar isometries SE(2)

  • The group of direct planar isometries is defined using homogeneous matrices.Source: §A.1.1 · p. 25

A.1.2 Group of multiple direct spatial isometries SEK+1(2)

The appendix constructs the Lie-group framework for IEKF-SLAM and derives its general filtering equations, then applies them to 2D and 3D SLAM. The 3D equations receive brief treatment because they are not the paper’s primary focus.

A.1.2 Group of multiple direct spatial isometries SEK+1(2)

  • The 2D SLAM state is embedded in the extended group SEK+1(2), which generalizes SE(2) and has a closed-form exponential and a derived adjoint.The adjoint is given by the matrix Gn in equation (21).Source: §A.1.2 · pp. 25–26

A.2 Statement of the general IEKF equations

  • The general IEKF uses right-invariant errors on a matrix Lie group, which equal identity at the true estimate and are invariant to right multiplication.The paper notes that left-invariant errors are less fruitful for SLAM.Source: §A.2 · p. 26

A.2.1 Linearized error equations over the group G

  • The IEKF linearizes the group error around identity; its error propagation omits Γ, and the derivation neglects second-order and cross terms.The output error is also linearized by a first-order expansion to define Hn.Source: §A.2.1 · p. 27

A.2.2 Computing the Kalman gain

  • The Kalman gain uses the linearized error system, and the state estimate is updated by mapping the estimated error back onto the Lie group.Algorithm 4 gathers the filter equations, including propagation and update.Source: §A.2.2 · p. 27, §A.2.3 · pp. 27–28

A.3 Lie group based derivation of the 2D IEKF-SLAM

  • For 2D SLAM, the state maps to SEK+1(2), and the derived error propagation matches the general IEKF theory’s adjoint-based propagation.The appendix presents Algorithm 3 as a strict application of the general IEKF algorithm.Source: §A.3 · p. 28, §A.3.1 · p. 28, §A.3.4 · p. 29

A.4 Equations of the IEKF-SLAM in 3D

  • The appendix extends the construction to 3D with SEK+1(3), a discretized motion model, and an observation model for landmarks in the car’s frame.The discretization is first-order in the time step; the 3D exposition is deliberately brief because it is not the paper’s primary object.Source: §A.4 · pp. 29–30, §A.4.1 · p. 30, §A.4.2 · p. 31

A.4.4 Right-invariant error (29)

  • In 3D, the mapped state yields a right-invariant error whose first-order form drives propagation through the group adjoint and defines the linearized output map.The rotation representation does not change the group equations; quaternions are recommended and Euler angles are not.Source: §A.4.3 · p. 31, §A.4.4 · p. 31, §A.4.5 · p. 32
Loading 1510.06263v3…