The invariant extended Kalman filter as a stable observer

Axel Barrau; Silvère Bonnabel

Source-linked AI summary

The invariant extended Kalman filter as a stable observer

Axel Barrau, Silvère Bonnabel

arXiv:1410.1465v4Oct 6, 2014eess.SY

TL;DR

The paper asks how to obtain stable nonlinear observers when EKF convergence guarantees are limited. It characterizes a broad Lie-group system class with autonomous error propagation, then uses the result to prove local IEKF stability around any trajectory; simulations show the IEKF converges where the EKF can diverge.

Problem
Nonlinear observer design lacks a general method, and the EKF is not known to provide nonlinear-observer stability guarantees.
Method
The paper characterizes systems with autonomous invariant-error propagation and applies this structure to a deterministic IEKF for continuous-time systems with discrete observations.
Results
The IEKF has theoretical local stability guarantees under standard linear-case conditions, while simulations show it outperforming the EKF, which can diverge in challenging navigation situations.
Takeaways & Limitations
The IEKF provides a stable-observer alternative to the EKF with similar tuning, implementation, and computational load.
Takeaways & Limitations
The stability analysis can become more complicated because sensor covariance matrices may require trajectory-dependent changes of frame.

Abstract

from arXiv · show

We analyze the convergence aspects of the invariant extended Kalman filter (IEKF), when the latter is used as a deterministic non-linear observer on Lie groups, for continuous-time systems with discrete observations. One of the main features of invariant observers for left-invariant systems on Lie groups is that the estimation error is autonomous. In this paper we first generalize this result by characterizing the (much broader) class of systems for which this property holds. Then, we leverage the result to prove for those systems the local stability of the IEKF around any trajectory, under the standard conditions of the linear case. One mobile robotics example and one inertial navigation example illustrate the interest of the approach. Simulations evidence the fact that the EKF is capable of diverging in some challenging situations, where the IEKF with identical tuning keeps converging.

1 Introduction

The paper addresses the lack of general nonlinear observer-design methods by characterizing systems with autonomous estimation-error dynamics and using that property to establish local IEKF convergence. Examples and simulations show advantages over the EKF, including cases where the EKF diverges.

Nonlinear observer design lacks a general method beyond a few system classes, while global convergence of state-estimation error remains ambitious.
The paper characterizes a class broader than left-invariant systems for which the estimation-error equation is autonomous.
A suitable nonlinear function of the nonlinear error satisfies a linear differential equation.
For this class, the IEKF has local convergence guarantees around any trajectory under standard linear-case convergence conditions.
Mobile robotics and navigation examples show the IEKF outperforming the EKF in challenging situations where the EKF can diverge.

2 A special class of multiplicative systems

The paper develops a Lie-group framework for state-trajectory-independent error propagation and shows that an appropriate logarithmic error evolves according to a linear differential equation. This generalizes familiar linear and invariant-system properties to a broader nonlinear class.

For linear systems, the discrepancy between two trajectories evolves independently of the reference trajectory, supporting convergent-observer design.
2.1 An introductory example: In the non-holonomic car, the standard error depends on both trajectories, whereas a nonlinear error can remove this dependence.
2.1 An introductory example: The car’s alternative nonlinear error obeys a linear autonomous equation despite the system and error being nonlinear.
2.2 Systems on Lie groups with state trajectory independent error propagation property: A geometric framework characterizes Lie-group systems whose invariant errors have state-trajectory-independent propagation.
2.2 Systems on Lie groups with state trajectory independent error propagation property: Theorem 1 makes left- and right-invariant error autonomy equivalent and identifies equivalent conditions for the dynamics.
2.3 Log-linear property of the error propagation: The log-linear property maps invariant errors between arbitrarily far trajectories to solutions of a linear differential equation.

3 Invariant Extended Kalman Filtering

The IEKF uses invariant error variables whose dynamics are independent of the true trajectory for a broad class of Lie-group systems. This autonomy enables local asymptotic stability under standard linear Kalman conditions.

3 Invariant Extended Kalman Filtering: For left-invariant observations, the invariant error remains independent of the true state during propagation and update.The corresponding right-invariant formulation has the same state-independent error property.
3 Invariant Extended Kalman Filtering: The invariant error can be represented exactly as the exponential of a linearized error trajectory during propagation, even for arbitrarily large initial errors.This exact propagation result follows from the autonomous error equation, while update errors are controlled through bounded second-order terms.
3 Invariant Extended Kalman Filtering: Trajectory-dependent covariance transformations may be required when sensors are attached to earth-fixed or body-fixed frames, complicating but not weakening the stability result.The transformed tuning matrices arise from changing sensor-frame coordinates in mobile robotics and navigation.
3 Invariant Extended Kalman Filtering: The IEKF is an asymptotically stable observer around any trajectory when the linearized system satisfies standard Kalman-filter stability conditions along the true trajectory.The result applies to continuous-time systems with discrete observations and uses covariance matrices as deterministic design parameters.

4 Simplified car example

The simplified car example embeds planar pose estimation in SE(2) and applies the IEKF to GPS or landmark measurements. Under stated motion or observability conditions, the IEKF is stable around any trajectory and outperforms the EKF in challenging simulations.

4 Simplified car example: The example models a non-holonomic car with planar position and heading, using odometry-driven dynamics and GPS or landmark range-and-bearing observations.The system is embedded in the matrix Lie group SE(2) for invariant filtering.
4 Simplified car example: The IEKF design associates a noisy system, embeds it on a matrix Lie group, linearizes the equations, and uses Kalman equations to tune the gain.This procedure supplies engineering-oriented design matrices for the deterministic observer.
4.3.1 Stability of the IEKF for the left-invariant output (39): The LIEKF is asymptotically stable around any trajectory when inter-observation displacement is at least vmin > 0 and input velocity is bounded by vmax.These assumptions preserve observability of heading and avoid unfeasibly high velocities.
4.3.2 Stability of the IEKF for the right-invariant output (40): The IEKF is asymptotically stable around any bounded trajectory when at least two distinct landmark points are observed.Two known landmark vectors make the observation matrix full-rank, allowing position and heading recovery.

5 Navigation on flat earth

This section applies the IEKF to flat-earth navigation models on a matrix Lie group, proving stability under observability and comparing it experimentally with the multiplicative EKF. The IEKF converges around any bounded trajectory and remains effective in challenging inertial-navigation simulations.

Model: The navigation model estimates attitude, velocity, and position from accelerometers, gyroscopes, and relative observations of known features.The vehicle dynamics evolve in three-dimensional space, while feature positions are known in an earth-fixed frame.
IEKF design: The IEKF construction introduces sensor noise, embeds the system in a matrix Lie group, linearizes the resulting equations, and uses Kalman equations to tune the gain.This methodology provides engineering-oriented design matrices while fitting the model into the invariant-filter framework.
IEKF design: The model is not left or right invariant, but it satisfies the broader relation required for the autonomous-error framework.The paper therefore extends invariant-observer analysis beyond systems possessing ordinary left- or right-invariance.
Stability: With three non-collinear observed points, the IEKF is an asymptotically stable observer about any bounded trajectory.The proof reduces to observability of the linearized pair (A,H), established using the observation and propagated-observation matrices.
Simulations: The simulation uses a vehicle driving a 10-meter-diameter circle for 30 seconds, with observations every second and inertial measurements at 100 Hz.Both filters use the same initial errors: 15 degrees for attitude and 1 meter for position standard deviations.
Simulations: With a deliberately small process-noise matrix Q1, the EKF diverges from nonlinear initialization errors, whereas the IEKF drives attitude and position errors to zero.No measurement or process noise was added in this experiment; the failure is attributed to nonlinear transient behavior.
Simulations: Inflating Q improves EKF convergence, but the EKF remains much slower than the IEKF and loses the physical interpretability of its covariance tuning.The inflated matrices are selected for a specific trajectory without guaranteed robustness and no longer faithfully represent sensor accuracy.

6 Conclusion

The conclusion presents the IEKF as a stable deterministic observer for a characterized class of Lie-group systems, with simulations supporting its practical advantage over the EKF.

Conclusion: The IEKF has theoretical stability guarantees under the simple and natural hypotheses used for the linear case.The paper states that the EKF has not been proved to share this feature as a nonlinear observer.
Conclusion: Simulations show the IEKF is superior to the EKF in the reported settings, including challenging situations.The filters remain similar in tuning, implementation, and computational load.

A Matrix Lie groups useful formulas

This appendix introduces matrix Lie groups, their Lie algebras, and exponential coordinates used to represent navigation states and calculations.

A Matrix Lie groups useful formulas: A matrix Lie group is a set of square invertible matrices satisfying the group properties.The appendix uses these groups as the matrix framework for the observer formulation.
A Matrix Lie groups useful formulas: The Lie algebra contains derivatives at the identity of curves evolving in the matrix Lie group.It is a vector subspace of the space of square matrices.
A Matrix Lie groups useful formulas: The paper identifies the Lie algebra with Euclidean coordinates through a linear map Lg.This lets algebra elements be handled as vectors in R^dimg.
A Matrix Lie groups useful formulas: The matrix exponential maps Lie-algebra elements to the associated matrix Lie group.The appendix also introduces an exponential-coordinate mapping from Euclidean coordinates to group elements.

B Further explanation and proof of the log-linear property

This section explains why the flow’s local behavior near the identity determines the global error-flow structure through exponentiation.

Local flow representation: The linear mapping At is defined through the first-order expansion of the system vector field near the identity.This provides a practical representation of the local dynamics on the Lie algebra.
Proof structure: The proof of the log-linear property is built from a sequence of lemmas concerning particular solutions, error functions, and the system flow.The section explicitly introduces these lemmas before proving the main theorem.
Error decompositions: Right- and left-error functions generate corresponding solution decompositions relative to a particular trajectory.The two constructions are defined by subtracting the system vector field in different group-relative ways.
Error decompositions: The error-propagation functions possess a special flow-related property that underlies the log-linear result.The paper describes this property as the key behavior governing error propagation.
Log-linear property: The flow’s behavior infinitely close to the identity determines its behavior arbitrarily far from the identity because the flow commutes with exponentiation.This supplies the conceptual bridge from infinitesimal algebraic dynamics to group-level dynamics.
Log-linear property: Theorem 7 connects the nonlinear flow with a matrix solution governed by the linear equation d/dt Ft = AtFt.The matrix begins at the identity, F0 = Id.
Proof structure: The derivation retains first-order terms while identifying the remaining contribution as quadratic.This is the local approximation step used in establishing the log-linear relationship.

C.1 Proof rationale

The proof controls nonlinear error growth by comparing estimated- and true-trajectory Riccati equations, bounding second-order update terms, and combining these bounds with linear decay. This yields first- and second-order controls over successive observation intervals under local covariance and error assumptions.

Error decomposition: The linear error flow provides exponential decay, while the nonlinear remainder introduced at updates is uniformly second order in the estimation error.The BCH formula gives a remainder bounded by a function of order O(||ξ||^2), and the proof uses decay of the linear flow to compensate it.
First-order control: Over 2M observation steps, successive propagation and update bounds keep the estimation error controlled by a first-order function l1(||ξt0||).The resulting bound satisfies l1(x) = O(x).
Covariance perturbation: The proof rewrites the estimated-trajectory Riccati equation as a perturbation of the true-trajectory equation and controls the perturbation using continuity near zero.The covariance bounds are transferred from the true trajectory to a nearby estimated trajectory under sufficiently small error.
Lyapunov control: The Lyapunov function increases by at most a second-order term l2(||ξt0||) over the controlled interval, with l2(x) = O(x^2).This estimate is obtained by comparing the nonlinear error trajectory with the linear flow and iterating the update bounds.
Blockwise iteration: The final growth estimate organizes the error evolution across successive blocks of M updates and combines the preceding first- and second-order bounds.The decomposition tracks the last update, the number J of complete M-update blocks, and the remaining updates.
Locality assumption: The argument assumes the initial error is sufficiently small so that the trajectory remains inside the local radius where the covariance and remainder estimates apply.The proof uses a continuation argument after establishing that the error cannot leave the prescribed neighborhood in finite time.

C.4 Proof of theorem 4

The theorem is completed by showing that sufficiently small initial error remains within the local neighborhood for all time. A contradiction argument then establishes global-in-time validity of the preceding local estimates.

Radius selection: For sufficiently small x, the second-order bound satisfies l2(x) < βk, allowing the local radius to be reduced while preserving the growth estimate.The proof chooses ε′ no larger than ε so that the second-order term remains controlled.
Invariant neighborhood: The continuation argument shows that ||ξt0+s|| < ε′ implies ||ξt0+s|| ≤ ε′/2 for sufficiently small initial error.This creates a strict interior bound needed to prevent finite-time exit from the local neighborhood.
Global-in-time conclusion: Assuming a finite exit time leads to a contradiction, so t = +∞ and all preceding estimates hold for sufficiently small ||ξt0||.The conclusion is local in the initial error but valid over an unbounded time horizon.

D Proof of proposition 3

The proposition verifies the required linear-system conditions for the inertial-navigation example by bounding the state-transition flow and establishing a lower bound involving landmark covariance.

Condition verification: The proof identifies conditions (i) and (v) as the only non-trivial conditions requiring verification for the inertial-navigation system.The remaining conditions follow directly from the system-specific setup described in the proposition.
Flow bound: The orthogonality of the dynamics flow yields a lower bound on the state-transition matrix, thereby verifying condition (i).The bound uses (Φ_tn+1^tn)^TΦ_tn+1^tn = I3 and the maximum angular-rate parameter vmax.
Covariance lower bound: Condition (v) is addressed by seeking a lower bound on the quadratic form induced by the landmark-noise covariance matrix N.The proof introduces the N-weighted scalar product to simplify this lower-bound argument.
Proposition conclusion: The resulting covariance argument establishes the proposition’s required bound for the inertial-navigation example.The final displayed result follows after introducing the covariance-weighted quadratic form and its associated scalar product.