esikf-lio-core

Iterated Error-State Kalman Filter (ESIKF) LiDAR-inertial localization — implemented from the equations up.

No filter framework, no SLAM library. The IMU propagation, the on-manifold iterated update, and every Jacobian in between are derived by hand and written directly in C++ (~700 lines for the filter itself). This repo is the localization core extracted from my handheld 3D mapping stack; the sensor drivers, mapping backend, and web GUI live in a private repo.

Verified on a 10 km handheld Livox Mid-360 dataset. A real cross-session example is included for download: a handheld walk localized against a prior map built from a different session an hour earlier, tracking at 5–9 ms per frame (single thread) with 3.5 cm ATE RMSE against the localization session's own SLAM trajectory aligned into the map frame:

recording (chunk_XXXX/*.bin)          prior map (.ply/.pcd/.xyz)
        │                                      │
   LidarReplay                              KnnMap
   frames + IMU stream                 nanoflann kd-tree
        │                                      │
        └────────────► EsikfLio ◄──────────────┘
                       │
         1. IMU propagation  (18-dim error-state covariance)
         2. motion deskewing (per-point SE(3) interpolation)
         3. point-to-plane matching (5-NN + PCA plane fit)
         4. iterated on-manifold Kalman update
                       │
                trajectory.txt

The filter

State

The nominal state lives on the manifold, the error state in its tangent space:

$$ x = (R, ; p, ; v, ; b_\omega, ; b_a, ; g), \qquad \delta x = [\delta\phi, ; \delta p, ; \delta v, ; \delta b_\omega, ; \delta b_a, ; \delta g] \in \mathbb{R}^{18} $$

with $R \in SO(3)$ and the rotation error defined right-multiplicatively, $R = \hat{R},\mathrm{Exp}(\delta\phi)$.

Prediction

For each IMU sample with bias-corrected measurements $a_k = a_m - b_a$, $\omega_k = \omega_m - b_\omega$ and interval $\Delta t$, the nominal state propagates as

$$ p \mathrel{+}= v\Delta t + \tfrac{1}{2}(R a_k + g)\Delta t^2, \qquad v \mathrel{+}= (R a_k + g)\Delta t, \qquad R \mathrel{{=}} R,\mathrm{Exp}(\omega_k \Delta t) $$

and the error state propagates as $\delta x_{k+1} = F_x \delta x_k + G_x n$ with noise $n = [n_\omega, n_a, n_{b\omega}, n_{ba}]$. The non-trivial blocks of $F_x$ (all derived in EsikfLio::esikf_loop, matching the code layout):

$$ \frac{\partial \delta\phi_{k+1}}{\partial \delta\phi_k} = \mathrm{Exp}(-\omega_k \Delta t), \qquad \frac{\partial \delta\phi_{k+1}}{\partial \delta b_{\omega}} = -\Delta t , J_r(\omega_k \Delta t) $$

$$ \frac{\partial \delta p_{k+1}}{\partial \delta\phi_k} = -\tfrac{1}{2} R [a_k]_\times \Delta t^2, \qquad \frac{\partial \delta p_{k+1}}{\partial \delta b_a} = -\tfrac{1}{2} R \Delta t^2, \qquad \frac{\partial \delta p_{k+1}}{\partial \delta g} = \tfrac{1}{2} \Delta t^2 I $$

$$ \frac{\partial \delta v_{k+1}}{\partial \delta\phi_k} = -R [a_k]_\times \Delta t, \qquad \frac{\partial \delta v_{k+1}}{\partial \delta b_a} = -R \Delta t, \qquad \frac{\partial \delta v_{k+1}}{\partial \delta g} = \Delta t , I $$

where $J_r$ is the right Jacobian of $SO(3)$, $J_r(\phi) = I - \frac{1-\cos\theta}{\theta^2}[\phi]\times + \frac{\theta-\sin\theta}{\theta^3}[\phi]\times^2$. The covariance follows $P \leftarrow F_x P F_x^\top + G_x Q G_x^\top$, symmetrized every step.

Deskewing

Each LiDAR point carries its own timestamp inside the sweep. The IMU propagation above is stored per-sample as an $SE(3)$ trajectory; each point is transformed with the pose interpolated at its timestamp (Sophus::interpolate), then pulled back into the body frame at sweep end. So the update below sees a rigid, motion-compensated scan.

Update — iterated, on-manifold

For each of up to 2,000 randomly sampled points: find the 5 nearest map points (kd-tree), fit a local plane by PCA (normal = eigenvector of the smallest eigenvalue, with curvature and distance gating), and form the point-to-plane residual

$$ r = n^\top (R,p_b + p - q), \qquad H_{\delta\phi} = -n^\top R [p_b]_\times, \qquad H_{\delta p} = n^\top . $$

Because the update iterates while the linearization point moves away from the prior, the prior covariance must be pulled back through the retraction. With $\eta_\phi = \mathrm{Log}(\hat{R}^\top R)$:

$$ P_{\text{eff}} = J^{-1} P J^{-\top}, \qquad J^{-1} = \mathrm{diag}\big(J_r(\eta_\phi), I, \dots\big) $$

$$ K = \left(H^\top R_m^{-1} H + P_{\text{eff}}^{-1}\right)^{-1} H^\top R_m^{-1} $$

$$ \delta x = -K r - (I - KH) , J^{-1} \eta $$

The correction is injected back onto the manifold ($R \leftarrow R,\mathrm{Exp}(\delta\phi)$, vectors additively) and the loop repeats until $\lVert\delta p\rVert, \lVert\delta\phi\rVert < 10^{-3}$ (max 10 iterations). The posterior covariance uses the Joseph form for numerical symmetry:

$$ P \leftarrow (I - KH) P_{\text{eff}} (I - KH)^\top + K R_m K^\top . $$

On startup, roll and pitch are initialized by gravity alignment over the first ~200 stationary IMU samples; yaw and position come from the user-supplied initial pose.

Build

Dependencies: Eigen3, Sophus, TBB, spdlog, nlohmann-json. (nanoflann is vendored, BSD license.)

cmake -B build -DCMAKE_BUILD_TYPE=Release
cmake --build build -j

Real-data example

Two separate handheld Mid-360 sessions in the same building, recorded ~70 minutes apart. The first session built the prior map (with my mapping stack); the second session's recording (~65 s indoor walk) is what gets localized — so the map has never seen this data. Both ship as a release asset (~110 MB):

tools/get_example_data.sh                     # downloads + writes example_data/params.json
./build/esikf_lio_core example_data/params.json
python3 tools/plot_trajectory.py trajectory.txt \
    --map example_data/esikf-lio-example/map.ply \
    --gt  example_data/esikf-lio-example/ref_keyframes.txt   # optional, needs matplotlib

ref_keyframes.txt is the reference trajectory: the localization session's own SLAM keyframe poses, brought into the map frame by map-to-map ICP alignment. It is SLAM output rather than survey-grade ground truth, so read the ATE as an upper bound — it contains both sessions' SLAM error and the alignment error, not just the filter's. Both files are TUM format, so evo works directly (evo_ape tum ref_keyframes.txt trajectory.txt); the plot tool above reports the same interpolated ATE.

The image above is exactly this run: 648 frames, 82–96 % of the 2 000 sampled points matching per frame, converging in ~2 iterations, 4–7 cm point-to-plane residual RMS against the prior map, ATE 3.5 cm RMSE / 7.2 cm max vs the reference.

Quick demo (no hardware needed)

The recording format is produced by my own handheld mapper, so the repo ships a generator that synthesizes a dataset — a 10 m × 10 m room as the prior map and a 10-second stationary recording (IMU @ 200 Hz, LiDAR @ 10 Hz) inside it:

python3 tools/make_demo_data.py          # writes demo_data/
./build/esikf_lio_core demo_data/params.json

The sensor sits at (0, 0, 1); the estimated trajectory should stay within ~1 mm of it. On this synthetic run the filter converges in 1–2 iterations per frame with sub-millimetre residual RMS — a sanity check that the propagation, deskewing, and update chain are consistent end to end.

To see the result (requires matplotlib):

python3 tools/plot_trajectory.py trajectory.txt --map demo_data/map.xyz

which renders a top-down view of the trajectory over the map plus a height profile to trajectory.png.

Run

./build/esikf_lio_core config/params.json

params.json (see config/params.example.json):

key	meaning
recording_path	directory containing chunk_XXXX/{lidar_imu.bin, lidar_frames.bin}
map_path	prior map — .ply (ascii/binary), .pcd (ascii/binary) or .xyz
lidar_imu_tf	LiDAR→IMU extrinsic, "tx,ty,tz,rx,ry,rz" (m, deg, ZYX)
init_pose	initial pose guess in the map frame, same format
replay_speed	0 = as fast as possible, 1 = wall-clock

Output: trajectory.txt — TUM format (timestamp tx ty tz qx qy qz qw), the LiDAR sensor pose in the map frame per LiDAR frame — plus per-second console stats: processing time, iterations used, valid-match ratio, residual RMS, and position/rotation σ from the covariance.

Recording format

Recordings are 120-second chunks written by my handheld mapper. Per chunk:

• lidar_imu.bin — packed ImuRecord (80 bytes: t, acc[3], gyr[3], so3[3], all doubles)
• lidar_frames.bin — repeated [t: double][n: uint32][n × LidarPointRecord], where LidarPointRecord is 21 packed bytes: delta_t, x, y, z, reflect (floats) + tag (uint8, Livox confidence — low-confidence points are filtered on load)

License

MIT — see LICENSE. src/map/nanoflann.hpp is © its authors, BSD-licensed.