Error-state Kalman filter

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 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 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 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 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 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 2102.12400 §IV-E, §IV-F, eq. 8-11.

LiDAR measurements enter as point-to-plane residuals on planar features 2102.12400 §IV-G, eq. 12. Camera measurements enter as reprojection residuals on tracked corners 2102.12400 §IV-H, eq. 16. Both feed the same gain, which weights each residual by its noise covariance 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 2102.12400 §III, §V. Card: R2LIVE.

Iterated error-state filter with photometric residuals in R3LIVE

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

1. A frame-to-frame update on the reprojection residuals of tracked points, iterated "until convergence" 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 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 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 2109.07982 §III-A. The paper assumes the LiDAR is synchronised with the IMU 2109.07982 §III-A. Card: R3LIVE.

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.

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 2102.12400 §IV-I 2109.07982 §V-A2.

# 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

# 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 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 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 2109.07982 §V-B1. The paper gives no fixed value for the observed-colour variance, and it leaves the random-walk parameter unstated 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 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 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 2102.12400 §IV-I. R3LIVE iterates "until convergence" with no threshold in the paper 2109.07982 §V-A2, §V-B2. Neither paper gives a cap or threshold, so implementations must choose one.

Papers

• R2LIVE (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 (arXiv:2109.07982): LiDAR-inertial-visual mapping that adds photometric residuals and map colouring on top of an iterated error-state filter.