PaperlayerView exact revision on arXiv

Source-linked AI summary

Quaternion kinematics for the error-state Kalman filter

Joan Solà

arXiv:1711.02508v1cs.RO

TL;DR

The paper addresses the need for precise quaternion and rotation formulations in estimation systems using IMU integration. It develops quaternion and rotation-matrix representations, Lie-group tools, perturbations, derivatives, and integrals, then applies them to error-state Kalman-filter kinematics while identifying practical scope boundaries.

  • Problem

    IMU integration supports dead-reckoning but drifts over time, motivating precise rotation formulations and fusion with absolute position measurements.

  • Method

    The paper develops quaternion and rotation-matrix formulations of SO(3), including exponential-map, interpolation, Jacobian, isoclinic-rotation, perturbation, derivative, and integration tools for estimation.

  • Results

    The paper derives formulations and geometric interpretations for quaternion-based rotation handling and applies them to precise error-state Kalman-filter kinematics using IMU signals.

  • Takeaways & Limitations

    Quaternion and rotation-group tools provide a framework for handling rotations, perturbations, derivatives, and integrations in practical IMU-based estimation.

  • Takeaways & Limitations

    Neglecting Earth rotation is usually acceptable, but high-end IMUs may measure ωE = 15°/h ≈ 7.3·10^-5 rad/s, requiring it to remain in the error model.

Abstract

from arXiv · show

This article is an exhaustive revision of concepts and formulas related to quaternions and rotations in 3D space, and their proper use in estimation engines such as the error-state Kalman filter. The paper includes an in-depth study of the rotation group and its Lie structure, with formulations using both quaternions and rotation matrices. It makes special attention in the definition of rotation perturbations, derivatives and integrals. It provides numerous intuitions and geometrical interpretations to help the reader grasp the inner mechanisms of 3D rotation. The whole material is used to devise precise formulations for error-state Kalman filters suited for real applications using integration of signals from an inertial measurement unit (IMU).

1 Quaternion definition and properties

Quaternions extend complex-number algebra with a scalar–vector representation and noncommutative multiplication, while supporting conjugation, inverses, powers, and exponentials for rotation calculations.

  • Quaternion construction and conventions: Quaternions are constructed as Q = A + Cj from two complex numbers, with imaginary units satisfying ij = −ji = k, jk = −kj = i, and ki = −ik = j.The paper uses the Hamilton convention and notes that alternative handedness, component ordering, and rotation interpretations change formulation details.
  • Rotations: Unit quaternions encode three-dimensional rotations through x′ = q ⊗ x ⊗ q∗, extending the complex-number representation of planar rotations.The paper emphasizes that quaternion conventions must be fixed because vector-versus-frame rotation interpretations correspond to opposite operations.
  • Scalar–vector representation: A quaternion decomposes as Q = qw + qv, where qw is the scalar part and qv is the three-dimensional vector part, and it can be represented as a 4-vector.Pure quaternions have zero scalar part and form the three-dimensional imaginary subspace Hp = Im(H).
  • Quaternion algebra: Quaternion multiplication is bilinear, associative, and distributive, but generally noncommutative because its vector-part expression contains a cross product.Multiplication becomes commutative when one quaternion is real or the vector parts are parallel.
  • Group properties: Unit quaternions form a noncommutative group whose identity is 1 and whose inverse coincides with the conjugate.For unit quaternions interpreted as rotations, conjugation implements the inverse rotation.
  • Powers and exponential: The quaternion exponential is defined by an absolutely convergent power series; for a pure quaternion, grouping even and odd powers yields cosine and sine terms and a unit quaternion.Small-angle approximations avoid division by zero in u = v/∥v∥ by truncating the Taylor series of sine and cosine.

2 Rotations and cross-relations

The section develops 3D vector rotations, defines SO(3), and compares rotation-matrix and quaternion representations. It then introduces exponential-map and quaternion formulations relevant to rotational estimation.

  • 2.1 The 3D vector rotation formula: A vector rotation decomposes x into an axis-parallel component that remains fixed and an orthogonal component that rotates in the plane normal to u.The resulting formula is x′ = x|| + x⊥cos φ + (u × x) sin φ.
  • 2.2 The rotation group SO(3): SO(3) consists of origin-centered rotations that preserve vector norms, angles, and relative orientations, thereby representing rigid-body rotations in 3D.Its operator definition requires norm preservation and preservation of the cross-product structure.
  • 2.3 The rotation group and the rotation matrix: Rotation matrices and unit quaternions both represent 3D rotations with three degrees of freedom, but unit quaternions form a double cover of SO(3).The matrix and quaternion representations support corresponding inverse and composition operations, while quaternion interpolation must account for the double cover.
  • 2.3.1 The exponential map: The rotation-matrix exponential map identifies skew-symmetric matrices in so(3) with vectors in R3 and enables rotational derivatives, perturbations, and velocities.Continuous trajectories in SO(3) yield skew-symmetric RᵀṘ matrices with three degrees of freedom.
  • 2.4.2 The capitalized exponential map: The quaternion exponential map uses half-angle parameters, with q = Exp(φ) converting a rotation vector into a quaternion representation.The quaternion construction captures the half-angle effect, while the associated 3D rotation uses a double quaternion product.
  • 2.7 Spherical linear interpolation (SLERP): Quaternion interpolation requires selecting the appropriate cover because only quaternion pairs with acute separation follow the shortest rotational path directly.The paper states that shortest-path interpolation corresponds to rotation angles φ ≤ π and quaternion separation Δθ ≤ π/2.

3 Quaternion conventions. My choice.

Quaternion conventions differ in component order, algebra, handedness, and operator interpretation, producing formulas that are not generally compatible. The paper adopts the right-handed Hamilton convention and emphasizes careful notation when converting or composing representations.

  • Convention choices: Four binary choices define quaternion conventions: component order, multiplication formula, operator function, and passive-operation direction.These choices produce 12 combinations, including Hamilton and JPL variants.
  • Compatibility: Convention differences make rotation and composition formulas incompatible, so the convention must be selected clearly from the beginning.The paper identifies Hamilton and JPL as the two most commonly used documented conventions.
  • The paper's choice: The paper chooses the Hamilton convention because it is right-handed and aligns with widely used robotics libraries and IMU attitude-estimation literature.The cited libraries include Eigen, ROS, and Google Ceres.
  • Component notation: Changing component order requires care with full 4 × 4 and 3×4 quaternion matrices because rows or columns may need to be swapped.The resulting errors can be difficult to detect.
  • Algebra and handedness: Hamilton uses ij = k and is right-handed, whereas JPL uses ji = k and is left-handed.The handedness changes the sign of the cross-product term and therefore the rotation rule.
  • Operator interpretation: Active operators rotate vectors, while passive operators represent frame transformations, and the two interpretations use inverse operators.Both Hamilton and JPL use the passive convention in this document.

4 Perturbations, derivatives and integrals

The paper defines tangent-space perturbation operators for SO(3), develops derivatives and Jacobians across representations, and analyzes quaternion-based rotation integration.

  • Perturbations: SO(3) replaces ordinary vector addition and subtraction with ⊕ and ⊖ operators for composing rotations and measuring angular differences.The plus operator composes a reference rotation with Exp(θ), while the minus operator recovers the tangent-space vector Log(R^-1◦S).
  • Perturbations: The same perturbation calculus applies to quaternion and rotation-matrix representations through quaternion products and matrix composition.For quaternions, qS = qR ⊗ Exp(θ); for matrices, RS = RR·Exp(θ).
  • Derivatives: The right Jacobian maps parameter variations δθ to tangent-space variations δφ for the exponential map from R3 to SO(3).It has a closed-form expression and approximates Exp(θ + δθ) through Exp(θ) Exp(Jr(θ)δθ).
  • Perturbations: Local perturbations provide a 3×3 covariance representation, while global perturbations are defined at the manifold origin.Local perturbations appear on the right of the nominal orientation; global perturbations appear on the left.
  • Derivatives: Quaternion time derivatives relate local angular rates to quaternion and rotation-matrix evolution through q̇ = 1/2 q ⊗ωL and Ṙ = R[ωL]×.The local perturbation over an integration interval corresponds to the angular-rate vector in the local frame.
  • Integrals: The second-order integration correction vanishes for collinear consecutive angular rates and is typically small for usual IMU sampling intervals.For Δt ≤ 0.01s and near-collinear rates, the correction is of order 10^-6∥ω∥^2 or smaller.

5 Error-state kinematics for IMU-driven systems

This section formulates IMU-driven true, nominal, and error-state kinematics for an ESKF, emphasizing minimal local orientation errors and linearized dynamics.

  • ESKF formulation: IMU integration supports dead-reckoning but produces position drift, so absolute measurements such as GPS or vision are needed to avoid drift.The ESKF estimates accumulated errors while the nominal state integrates high-frequency IMU data.
  • ESKF formulation: The ESKF uses a minimal orientation error-state, remains near the origin, simplifies Jacobians, and permits slower correction updates than prediction updates.These properties reduce over-parameterization, singularity risks, and computational complexity.
  • ESKF formulation: The true state is composed from nominal and error states, with nonlinear large-signal dynamics assigned to the nominal state and small-signal dynamics assigned to the error state.The error-state is therefore suitable for linear-Gaussian filtering.
  • Conventions: The formulation defines angular rates and angular errors locally with respect to the nominal quaternion and orientation.The local rate convention allows direct use of body-referenced gyrometer measurements, while globally defined errors are also considered.
  • True-state kinematics: The true kinematics isolate acceleration and angular rate from noisy accelerometer and gyrometer measurements by removing biases and noise.The resulting system is driven by noisy IMU readings and perturbed by white Gaussian noise, while gravity is estimated with constant evolution.
  • True-state kinematics: Earth rotation is usually neglected, but high-end IMUs may measure ωE = 15°/h ≈ 7.3·10^-5 rad/s, requiring its inclusion for model validity.The scope boundary concerns sensors with very small noise and bias.
  • Error-state dynamics: Linearized error dynamics are derived by composing true and nominal equations and discarding second-order infinitesimals.Position, bias, and gravity errors are straightforward, whereas velocity and orientation errors require nonlinear manipulations.

6 Fusing IMU with complementary sensory data

The ESKF fuses complementary measurements with IMU predictions by correcting, injecting, and resetting the error state, including covariance adjustments for orientation resets.

  • Measurement fusion: GPS, vision, and other state-dependent measurements provide complementary information used to correct IMU-driven ESKF predictions and observe bias errors.Common combinations include GPS + IMU, monocular vision + IMU, and stereo vision + IMU.
  • Correction: The ESKF correction consists of observing the error state, injecting the observed errors into the nominal state, and resetting the error state.These are the three stated stages of the correction procedure.
  • Filter correction: The filter correction uses a measurement Jacobian defined with respect to the error state and evaluated at the best true-state estimate.The Jacobian can be obtained by applying the chain rule to the measurement function and the true-state composition.
  • Filter correction: The true-state Jacobian is identity in most 3×3 blocks, except for the quaternion orientation term Qδθ.This exception arises from differentiating the quaternion composition with respect to the orientation error.
  • ESKF reset: After error injection, the orientation error is expressed relative to the new nominal orientation, so its covariance must be transformed during reset.The reset Jacobian is identity on diagonal blocks except for the orientation-error block.
  • ESKF reset: In most cases, neglecting the estimated orientation error gives G = I18 and a trivial reset, while the full expression may improve long-term odometry drift.The precise reset is presented as potentially useful when reducing long-term error drift.

7 The ESKF using global angular errors

The global angular-error formulation revises the ESKF dynamics and algorithmic steps while retaining local angular-rate measurements because gyrometers provide body-frame rates.

  • Global angular-error formulation: The global angular-error formulation retraces the local-error development and changes the orientation-error composition to the left-hand side.True and nominal kinematics remain unchanged.
  • Angular-rate convention: Local angular rates are retained regardless of the global error definition because gyrometers measure body-frame rates.The discrete quaternion update therefore remains q ← q ⊗ q{ω∆t}.
  • Error-state kinematics: The global formulation linearizes the orientation and velocity error dynamics by expanding true and nominal equations and neglecting second-order terms.The orientation-error dynamics are obtained after deriving true quaternion evolution through left and right developments.
  • Error-state kinematics: The resulting Euler error-state updates propagate position, velocity, orientation, accelerometer bias, gyroscope bias, and gravity errors.The orientation update includes −Rδωb∆t, while the velocity update includes orientation, acceleration-bias, gravity, and process-noise terms.
  • Transition and perturbation matrices: Three changes distinguish the global-error transition Jacobian from the local-error case, while perturbation Jacobians and matrices remain unchanged.These changes are summarized in Table 4.
  • ESKF fusion: Global angular errors slightly modify ESKF fusion, affecting the observation Jacobian orientation block, quaternion injection, and reset-related operations.Only the quaternion equation changes in the nominal-state composition, and the error mean is reset after injection.

A Runge-Kutta numerical integration methods

Runge-Kutta methods integrate nonlinear differential equations over a finite interval by estimating derivatives through multiple iterations before producing a difference equation.

  • General setup: The section converts nonlinear differential equations into discrete difference equations over a limited interval ∆t.It assumes sampling times tn = n∆t and defines xn as x(tn).
  • Runge-Kutta methods: Runge-Kutta methods estimate the derivative over an interval using several iterations and then integrate that estimate over the step ∆t.The section presents RK methods from the simplest to the most general forms.
  • Source: The section’s RK material is taken from the English Wikipedia Runge-Kutta method entry.This identifies the stated source of the presented material.

A.1 The Euler method

The Euler method uses a single derivative evaluation at the initial point and assumes that derivative remains constant across the interval.

  • Euler method: Euler integration assumes the derivative f(·) is constant over the interval.This assumption underlies its single-stage formulation.
  • Euler method: As a general Runge-Kutta method, Euler integration is a single-stage method that computes the derivative at the initial point.The initial-point derivative is then used for the step.
  • Euler method: The initial derivative is used to compute the integrated value at the endpoint.Thus the method advances the state from the initial point using one derivative estimate.

A.2 The midpoint method

The midpoint method estimates the derivative at the interval midpoint by first integrating there with Euler, then using the midpoint derivative for the full-step update.

  • Midpoint method: The midpoint method assumes the relevant derivative is evaluated at the interval midpoint.It performs one iteration to compute the state at that midpoint.
  • Two-step procedure: The first step uses Euler integration and k1 to advance the state from the initial point to the midpoint.This produces the midpoint state needed for the second derivative evaluation.
  • Two-step procedure: The second step evaluates the derivative k2 at the midpoint and uses it to integrate to the endpoint.The endpoint update is xn+1 = xn + ∆t·k2.

A.3 The RK4 method

RK4 computes a fourth-order integration estimate from four sequential slope evaluations at the interval’s start, midpoint, and end, then combines them by weighted averaging.

  • RK4 uses four sequential stages, k1 through k4, to evaluate slopes across one integration interval.The evaluations occur at the start, midpoint, and end of the interval.
  • The four slopes are weight-averaged as k1 + 2k2 + 2k3 + k4 to produce the fourth-order interval estimate.
  • k1 is evaluated at the interval start using xn, providing the Euler-method slope.
  • k2 evaluates the midpoint using xn + 1 2∆t·k1, while k3 reevaluates the midpoint using xn + 1 2∆t·k2.
  • k4 evaluates the interval endpoint using xn + ∆t·k3.

A.4 General Runge-Kutta method

General Runge-Kutta methods organize integration through staged slope evaluations and weighting, while the paper derives closed-form transition-matrix expressions for linear and IMU-driven systems.

  • General Runge-Kutta formulation: A general RK method uses s stages, weights bi, evaluation times ci, and coefficients aij to determine the slopes ki.
  • Explicit and implicit methods: Explicit RK methods compute slopes sequentially from previous values, requiring a lower-triangular coefficient matrix with zero diagonal.
  • Explicit and implicit methods: Implicit RK methods use a full coefficient matrix and solve linear equations for all slopes, increasing computational cost while improving possible accuracy and stability.
  • Closed-form transition matrices: For first-order linear systems with constant dynamics over an interval, integration uses a transition matrix Φ obtained from the matrix exponential and its Taylor expansion.
  • Full IMU system: In the full IMU model, the dynamic matrix is block-sparse, its sparsity stabilizes after the third power, and the simplified model’s upper-left block retains its earlier closed form.
  • Closed-form transition matrices: The full IMU transition matrix is constructed from finite closed-form series Σn, whose terms remain finite and computable for finite n, with n ≤3 in the example.

B.1 Integration of the angular error

For angular error dynamics without bias and noise, the Taylor-series transition matrix can be regrouped into a closed-form rotation solution.

  • The angular error dynamics are analyzed in a cleaned IMU-driven system without bias and noise.
  • The transition matrix is first represented through a Taylor series and then reorganized using the rotation axis and angle defined by ω∆t ≜ u∆θ.
  • The regrouped expression yields a closed-form solution for the angular error transition.
  • The solution equals Φ = R{−u∆θ} = R{ω∆t}⊤, linking the transition matrix to Rodrigues’ rotation formula.
  • The closed-form development is motivated by uncertainty about the impact of high-order errors in real algorithms with frequent IMU-error corrections.

C.1.1 First order truncation: the finite differences method

Transition-matrix approximations arise by truncating Taylor expansions system-wise or block-wise; the paper relates finite differences, Euler integration, compact rotational forms, and Runge-Kutta integration.

  • First-order truncation: Euler, finite-differences, and first-order system-wise Taylor truncation are equivalent and produce an approximate transition matrix.
  • First-order truncation: For the simplified IMU example, the finite-differences method yields a first-order transition-matrix approximation whose rotational block can instead use the compact closed form R(ω∆t)⊤.
  • System-wise truncation: Truncating after the order where no new non-zero terms appear exploits matrix sparsity; this order is 2 for the simplified IMU example and 3 for the full example.
  • System-wise truncation: The full form of the third-order approximation is omitted for space reasons, with the relevant powers of A provided elsewhere in the paper.
  • Block-wise truncation: Block-wise truncation approximates each transition-matrix block at its first significant term, producing a more accurate yet still easy-to-compute approximation than system-wide first-order truncation.
  • Runge-Kutta integration: Runge-Kutta integration is proposed when the dynamic matrix A cannot be considered constant over the integration interval.

D.1 Error-state example

The section develops an error-state Kalman filter for IMU systems, integrating nominal dynamics, transition matrices, measurement noise, and random perturbations consistently across continuous and discrete time.

  • The true state combines a nominal state with an error state whose dynamics are linear and time-varying with the nominal state and control.
  • RK4 integration evaluates interpolated controls and nominal states at intermediate points to compute the transition matrix.The evaluation points are t_n, t_n + Δt/2, and t_n + Δt.
  • Continuous-time uncertainty is integrated differently for sampled measurement noise and unsampled perturbations.Sampled measurement noise behaves deterministically within the interval, whereas white Gaussian perturbations require stochastic integration.
  • The discrete error-state model uses Fx = Φ = eA∆t, Fu = B∆t, and Fw = C for transition, control, and perturbation effects.These matrices arise from the continuous-time linearized error-state dynamics.
  • The covariance update contains dynamic, measurement, and perturbation terms whose dependence on ∆t is exponential, quadratic, and linear, respectively.
  • Perturbation covariances can be represented through full-state impulses or smaller mapped impulse covariances, with equivalent formulations under the stated conditions.The reduced representation uses Fi to map each impulse to the state components it affects.
  • For IMUs, control noise models additive measurement noise while perturbations model bias random walks; isotropic noise yields diagonal covariance matrices.Non-isotropic IMUs require a proper Jacobian Fi and corresponding covariance specification.
Loading 1711.02508v1…