---
kind: concept
slug: error-state-kalman-filter
title: Error-state Kalman filter
summary: >-
  An error-state Kalman filter propagates a nominal state with IMU readings and estimates a small error state on a manifold, so rotations stay singularity-free. Two LiDAR-inertial-visual systems use it: R2LIVE fuses LiDAR and camera residuals, R3LIVE adds photometric colour residuals. Accuracy figures are author-reported.
tags: [state-estimation, sensor-fusion, kalman-filter, slam]
papers: ["2102.12400", "2109.07982"]
related: [unit-quaternion-rotation, state-space-model, lidar-inertial-odometry, visual-inertial-odometry, simultaneous-localization-and-mapping]
status: draft
updated_at: 2026-10-31
---

## Overview

A filter that tracks a pose, a velocity and sensor biases must handle rotations, which do not form a vector space. The error-state formulation keeps two things apart. A nominal state is propagated forward with the inertial measurements. A small error state, expressed on the tangent space of the manifold, is what the Kalman filter estimates and corrects [arxiv:2102.12400 §IV-A, eq. 1]. The filter's uncertainty is kept in the error state, while the nominal state carries the full pose and velocity. Each measurement update linearizes the residuals at the current estimate, and the iteration refines that linearization [arxiv:2102.12400 §IV-F, §IV-I]. The iterated form is called ESIKF in the R3LIVE paper, which applies it to both its LiDAR and its camera updates [arxiv:2109.07982 §II, §IV, §V-A2].

The two papers below use the same filter core for different measurements. They differ in what the camera contributes and in how the map is built. Read the variant sections before choosing.

## Variants

### Iterated error-state filter with geometric residuals in R2LIVE

The state holds the IMU pose, velocity, the camera-IMU extrinsic, and the gyroscope and accelerometer biases. Its error state has 21 dimensions [arxiv:2102.12400 §IV-C]. Between sensor events the nominal state moves with the IMU readings held constant over each sample period, and the error covariance grows with the state and noise Jacobians [arxiv:2102.12400 §IV-C, §IV-D, eq. 5, eq. 7]. At each LiDAR or camera time, the propagated prior is fused with the measurements in an iterated maximum-a-posteriori update [arxiv:2102.12400 §IV-E, §IV-F, eq. 8-11].

LiDAR measurements enter as point-to-plane residuals on planar features [arxiv:2102.12400 §IV-G, eq. 12]. Camera measurements enter as reprojection residuals on tracked corners [arxiv:2102.12400 §IV-H, eq. 16]. Both feed the same gain, which weights each residual by its noise covariance [arxiv:2102.12400 §IV-I, eq. 23]. The paper reports that this filter runs odometry at IMU rate and that a separate factor graph refines keyframe poses and landmarks [arxiv:2102.12400 §III, §V]. Card: [R2LIVE](/papers/2102.12400).

### Iterated error-state filter with photometric residuals in R3LIVE

R3LIVE splits the work into two subsystems that share one global map [arxiv:2109.07982 §II]. The LiDAR-inertial part is an ESIKF on point-to-plane residuals, run once per LiDAR scan [arxiv:2109.07982 §IV]. The visual-inertial part runs two more ESIKF updates per image [arxiv:2109.07982 §V-A, §V-B]:

1. A frame-to-frame update on the reprojection residuals of tracked points, iterated "until convergence" [arxiv:2109.07982 §V-A1, §V-A2].
2. A frame-to-map update on photometric residuals. Each residual is the stored colour of a map point minus the colour interpolated at its predicted pixel [arxiv:2109.07982 §V-B1, §V-B2].

The map is coloured afterwards. Each map point's colour is fused with the observed colour in a Bayesian update [arxiv:2109.07982 §V-C]. The filter state is 29-dimensional. It includes the IMU-camera time offset and the camera intrinsics, and both are estimated online [arxiv:2109.07982 §III-A]. The paper assumes the LiDAR is synchronised with the IMU [arxiv:2109.07982 §III-A]. Card: [R3LIVE](/papers/2109.07982).

## When to use which

The table gives constraints that favour one variant. It does not rank the two, because the papers test different sensor rigs and report different metrics.

| Situation | Use | Why |
|---|---|---|
| Fast rotation and a LiDAR with a narrow field of view that loses features in bare corridors | R2LIVE: iterated filter with LiDAR and camera residuals | The paper targets this case and reports usable tracking at high angular speed in its own tests [arxiv:2102.12400 §I, §VI-B] |
| Walls or corridors with little geometric or visual texture, and a need for a coloured point map | R3LIVE: photometric residuals on the map | The paper tracks without visual features and reports tests on bare white walls [arxiv:2109.07982 §I, §VI-B] |
| You need a dense map with per-point colour, not only a trajectory | R3LIVE: map colour update after tracking | The paper's visual part colours the map points, and the result is a coloured point cloud [arxiv:2109.07982 §I, §V-C] |
| Onboard compute of the class the papers tested (an embedded i7-8550U with 8 GB) | R2LIVE front end runs in about 14 to 20 ms for visual-inertial updates and 16 to 31 ms for LiDAR-inertial updates, averaged per experiment | The paper reports these onboard times and notes that the back end may run at a lower rate [arxiv:2102.12400 §VI-F, tab. I] |
| Same hardware, camera at high resolution and dense map points | R3LIVE with reduced resolution or coarser point spacing | The paper reports 166 ms for its visual-inertial part onboard at 1280 x 1024 and 0.01 m spacing, and its cost grows with image size and with smaller spacing [arxiv:2109.07982 tab. IV] |
| Sensor time offsets not calibrated in advance | Estimate the offset online, or measure it before the run | R2LIVE estimates the camera-LiDAR offset in the back end, but gives no initial value [arxiv:2102.12400 §V]. R3LIVE estimates the IMU-camera offset in its filter state [arxiv:2109.07982 §III-A] |
| Visual-inertial odometry without a LiDAR | Neither paper covers this case | R3LIVE projects points from a map that its LiDAR part builds, and the paper does not say whether the visual part runs without it [arxiv:2109.07982 §II, §V] |
| Noise densities for the IMU are unknown | Calibrate them from your own sensor first | R2LIVE uses an undefined noise term in its covariance propagation and gives no values [arxiv:2102.12400 §IV-D, eq. 7] |
| Sensor setup different from the paper's | Check before use | Both papers test the authors' own rigs. R2LIVE tests only its handheld rig [arxiv:2102.12400 §VI-A], and R3LIVE tests its own rig and campus and seaport data [arxiv:2109.07982 §VI-A] |
| Reuse of the authors' code in a closed or permissively licensed project | Implement from the descriptions above | Both authors' repositories are GPL-2.0, see [R2LIVE](/papers/2102.12400) and [R3LIVE](/papers/2109.07982) |

## Implementation sketch

Language-agnostic outline of the common core, with the paper's section for each step. Both papers leave the threshold, the iteration cap and the noise values to the implementer, so the values in the sketch are placeholders [arxiv:2102.12400 §IV-I] [arxiv:2109.07982 §V-A2].

```pseudocode
# Propagate the nominal state on each IMU sample  [arxiv:2102.12400 §IV-C]
def propagate(state, cov, imu_samples, dt):
    for gyro, accel in imu_samples:
        state = state_plus(state, dt * dynamics(state, gyro, accel))
        # Covariance grows with the state and noise Jacobians  [§IV-D, eq. 7]
        F = state_jacobian(state, gyro, accel)
        G = noise_jacobian(state, gyro)
        cov = F @ cov @ F.T + G @ noise_q @ G.T   # noise_q: from calibration
    return state, cov
```

```pseudocode
# Iterated update at one LiDAR or camera time  [arxiv:2102.12400 §IV-I, eq. 21]
def iterated_update(prior_state, prior_cov, residual_fn, max_steps):
    iterate = prior_state
    for _ in range(max_steps):       # cap not stated in the paper
        # Linearize every residual at the current iterate  [§IV-G, §IV-H]
        residuals, H, R_inv = residual_fn(iterate)
        # Kalman gain  [§IV-I, eq. 23]
        gain = solve(H.T @ R_inv @ H + inv(prior_cov), H.T @ R_inv)
        step = -gain @ residuals - (eye - gain @ H) @ prior_term(iterate, prior_state)
        iterate = boxplus(iterate, step)
        if norm(step) < tol:         # tolerance not stated in the paper
            break
    # Posterior covariance  [§IV-I]
    return iterate, (eye - gain @ H) @ prior_cov
```

Where the two variants differ is in `residual_fn`. R2LIVE supplies point-to-plane and reprojection residuals [arxiv:2102.12400 §IV-G, §IV-H]. R3LIVE supplies reprojection residuals for its frame-to-frame step and photometric residuals for its frame-to-map step [arxiv:2109.07982 §V-A1, §V-B1]. The R3LIVE photometric residual is a difference of colours, so its noise term comes from the colour variance that the paper derives from the interpolation [arxiv:2109.07982 §V-B1]. The paper gives no fixed value for the observed-colour variance, and it leaves the random-walk parameter unstated [arxiv:2109.07982 §V-B1].

## Contradictions

Both papers report accuracy on their own data, so their numbers do not compare directly.

- R2LIVE reports a median relative pose error of 0.21 % translation and 0.27 deg rotation on one 300 m sequence against RTK, using a handheld rig [arxiv:2102.12400 §VI-E, fig. 11].
- R3LIVE reports, for the same 300 m distance on one seaport sequence, 0.34 deg / 0.21 % for R2LIVE and 0.21 deg / 0.17 % for its own high-resolution setting. Its table lists FAST-LIO2 at 0.21 deg / 0.20 % on that sequence [arxiv:2109.07982 tab. III, §VI-D].

The two R2LIVE figures come from different sequences in different experiments. Neither paper repeats the other's test, so the two sets of numbers cannot be ranked against each other.

The papers also differ on how the iteration ends. R2LIVE iterates "until the update is small" with no threshold [arxiv:2102.12400 §IV-I]. R3LIVE iterates "until convergence" with no threshold in the paper [arxiv:2109.07982 §V-A2, §V-B2]. Neither paper gives a cap or threshold, so implementations must choose one.

## Papers

- [R2LIVE](/papers/2102.12400) (arXiv:2102.12400): tightly coupled LiDAR, inertial and camera estimator with an iterated error-state filter at the front end and a sliding-window factor graph at the back end.
- [R3LIVE](/papers/2109.07982) (arXiv:2109.07982): LiDAR-inertial-visual mapping that adds photometric residuals and map colouring on top of an iterated error-state filter.