This reference guide explains the family of Kalman Filters — from the basic linear filter through the Error-State variants used in production SLAM systems — specifically as they apply to building a LiDAR-Inertial SLAM engine. The goal is to fuse data from a LiDAR sensor (accurate spatial measurements at ~10–20 Hz) with an IMU sensor (high-frequency orientation and acceleration at ~100–400 Hz) to produce real-time, accurate, drift-corrected 6-DOF pose estimation and 3D mapping.
Kalman filters sit at the convergence of four mathematical branches. Relative to the learning path from Algebra through Calculus and beyond:
Algebra → Trigonometry / Precalculus → Calculus 1 → then three parallel branches:
1. Linear Algebra — the biggest prerequisite. The filter is fundamentally a matrix algorithm: state vectors, covariance matrices, matrix multiplication and inversion.
2. Probability & Statistics — Gaussian distributions, covariance, conditional probability. Without this, the linear algebra looks like arbitrary matrix shuffling.
3. Differential Equations → Control Theory — the filter models system evolution using state-space equations $\dot{\mathbf{x}} = A\mathbf{x} + B\mathbf{u}$, which are systems of differential equations in matrix form.
These three branches converge at Kalman Filters. For the nonlinear variants (EKF, ESKF, UKF) used in SLAM, you additionally need Calculus 3 (multivariable calculus, Jacobians) and familiarity with 3D rotation representations (SO(3), quaternions).
Vectors and matrices, matrix multiplication, transpose, inverse. Eigenvalues and eigenvectors. Positive-definite matrices (covariance matrices are always symmetric positive-definite). The matrix identity $(\mathbf{A}\mathbf{B})^{-1} = \mathbf{B}^{-1}\mathbf{A}^{-1}$.
Random variables, mean (expectation), variance, covariance. The multivariate Gaussian distribution $\mathcal{N}(\boldsymbol{\mu}, \mathbf{P})$. Bayes' theorem. The concept that fusing two uncertain estimates produces a better estimate than either alone.
Derivatives and integrals (Calc 1). Partial derivatives and Jacobian matrices (Calc 3). The Jacobian $\mathbf{J} = \frac{\partial \mathbf{f}}{\partial \mathbf{x}}$ is how the EKF linearizes nonlinear functions — each entry is $J_{ij} = \frac{\partial f_i}{\partial x_j}$.
State-space representation $\dot{\mathbf{x}} = \mathbf{f}(\mathbf{x}, \mathbf{u})$. Discretization of continuous models. Numerical integration (Euler, Runge-Kutta) for IMU propagation.
Rotation matrices $\mathbf{R} \in SO(3)$, quaternions, Euler angles. The skew-symmetric matrix $[\mathbf{a}]_\times$ for cross products. Lie groups and Lie algebras (for the error-state formulation on manifolds). Coordinate frame transformations.
You don't need to master all of this before starting. Many engineers learn by implementing a simple 1D Kalman filter first, then gradually adding complexity. The math deepens as the problems demand it.
A LiDAR-IMU SLAM system must estimate the robot's state $\mathbf{x}$ — typically position, velocity, orientation, and IMU biases — by combining two complementary sensors:
| Sensor | Strengths | Weaknesses | Typical Rate |
|---|---|---|---|
| IMU | High frequency, captures fast motion, provides orientation | Drifts over time (integrating noisy accelerations), no absolute position | 100–400 Hz |
| LiDAR | Accurate 3D point clouds, absolute spatial measurements | Lower rate, can miss fast motion, struggles in feature-poor environments | 10–20 Hz |
The Kalman filter is the mathematical framework that optimally fuses these: the IMU drives the prediction step (fast, drifty) and the LiDAR drives the correction step (slow, accurate).
Between LiDAR scans, propagate the state forward using IMU readings (prediction).
When a LiDAR scan arrives, correct the prediction using the spatial measurement (update).
The Kalman gain $\mathbf{K}$ automatically determines how much to trust each source.
The foundation. Optimal for linear systems with Gaussian noise. Not directly usable for SLAM (which is nonlinear), but understanding it is essential before moving to the nonlinear variants.
$\mathbf{x}_k$ = state vector, $\mathbf{F}_k$ = state transition matrix, $\mathbf{B}_k$ = control input matrix, $\mathbf{u}_k$ = control input, $\mathbf{w}_k \sim \mathcal{N}(0, \mathbf{Q}_k)$ = process noise.
$\mathbf{z}_k$ = measurement vector, $\mathbf{H}_k$ = observation matrix, $\mathbf{v}_k \sim \mathcal{N}(0, \mathbf{R}_k)$ = measurement noise.
The Kalman Gain $\mathbf{K}$ is the key. When the prediction is uncertain ($\mathbf{P}$ is large), $\mathbf{K}$ is large and the filter trusts the measurement more. When the measurement is noisy ($\mathbf{R}$ is large), $\mathbf{K}$ is small and the filter trusts its prediction more. It's an optimal weighted average.
The linear KF requires $\mathbf{F}$ and $\mathbf{H}$ to be constant matrices — but in SLAM, the motion model involves 3D rotations (nonlinear), and the LiDAR measurement model maps 3D points through coordinate transforms (also nonlinear). This is why we need the extensions below.
The most widely used nonlinear filter. It handles nonlinear process and measurement models by linearizing them at each time step using Jacobian matrices.
$\mathbf{f}(\cdot)$ is now a nonlinear function (e.g., 3D rotation + translation from IMU integration).
$\mathbf{h}(\cdot)$ maps the state to expected measurements (e.g., projecting map points into the LiDAR frame).
The EKF approximates the nonlinear functions with first-order Taylor expansions around the current estimate:
$$\mathbf{F}_k = \frac{\partial \mathbf{f}}{\partial \mathbf{x}}\bigg|_{\hat{\mathbf{x}}_{k|k}} \qquad \mathbf{H}_k = \frac{\partial \mathbf{h}}{\partial \mathbf{x}}\bigg|_{\hat{\mathbf{x}}_{k+1|k}}$$The term $\mathbf{z}_{k+1} - \mathbf{h}(\hat{\mathbf{x}}_{k+1|k})$ is called the innovation or residual — the difference between what the LiDAR actually measured and what the filter predicted it should measure. This is where scan-matching (ICP, NDT, or feature matching) produces the LiDAR "measurement."
The preferred variant for IMU-centric fusion and the foundation of FAST-LIO, FAST-LIO2, and most modern LiDAR-inertial systems. Instead of estimating the full state, it estimates the error between the IMU's predicted state and the true state.
Three key advantages for SLAM:
The "big" state propagated purely from IMU readings, using the nonlinear kinematic equations. No covariance, no correction.
$$\bar{\mathbf{x}}_{k+1} = \mathbf{f}(\bar{\mathbf{x}}_k, \mathbf{u}_k)$$The small deviation $\delta\mathbf{x}$ between the nominal state and the true state:
$$\mathbf{x}_{\text{true}} = \bar{\mathbf{x}} \oplus \delta\mathbf{x}$$where $\oplus$ denotes the composition operation (simple addition for position/velocity, but a rotation composition for orientation).
where $\delta\mathbf{p}$ = position error, $\delta\mathbf{v}$ = velocity error, $\delta\boldsymbol{\theta}$ = orientation error (small angle), $\delta\mathbf{b}_a$ = accelerometer bias error, $\delta\mathbf{b}_\omega$ = gyroscope bias error, $\delta\mathbf{g}$ = gravity direction error.
$\mathbf{F}_\delta$ = Jacobian of the error-state dynamics, $\mathbf{G}$ = noise input matrix.
Then reset the error state to zero: $\delta\hat{\mathbf{x}} \leftarrow \mathbf{0}$.
Used in FAST-LIO and FAST-LIO2. The key insight: the standard EKF linearizes around the prediction, but if the correction moves the state significantly, that linearization point was wrong. The IEKF iterates — re-linearizing around the updated estimate — until convergence.
Starting from the predicted state $\hat{\mathbf{x}}^{(0)} = \hat{\mathbf{x}}_{k+1|k}$, iterate for $j = 0, 1, 2, \ldots$:
$$\mathbf{H}^{(j)} = \frac{\partial \mathbf{h}}{\partial \mathbf{x}}\bigg|_{\hat{\mathbf{x}}^{(j)}}$$ $$\mathbf{K}^{(j)} = \mathbf{P}_{k+1|k}(\mathbf{H}^{(j)})^T\left(\mathbf{H}^{(j)}\mathbf{P}_{k+1|k}(\mathbf{H}^{(j)})^T + \mathbf{R}\right)^{-1}$$ $$\hat{\mathbf{x}}^{(j+1)} = \hat{\mathbf{x}}_{k+1|k} + \mathbf{K}^{(j)}\left(\mathbf{z} - \mathbf{h}(\hat{\mathbf{x}}^{(j)}) - \mathbf{H}^{(j)}(\hat{\mathbf{x}}_{k+1|k} - \hat{\mathbf{x}}^{(j)})\right)$$Stop when $\|\hat{\mathbf{x}}^{(j+1)} - \hat{\mathbf{x}}^{(j)}\| < \varepsilon$ (convergence).
FAST-LIO reformulates the Kalman gain computation so that its complexity depends on the state dimension (~24) rather than the measurement dimension (thousands of LiDAR points). This is what makes iterating practical in real-time — each iteration costs microseconds instead of milliseconds.
An alternative to the EKF that avoids computing Jacobians entirely. Instead of linearizing, it passes carefully chosen sigma points through the nonlinear function and reconstructs the Gaussian statistics from the transformed points.
For state dimension $n$ with mean $\hat{\mathbf{x}}$ and covariance $\mathbf{P}$, generate $2n+1$ sigma points:
$$\boldsymbol{\chi}_0 = \hat{\mathbf{x}}$$ $$\boldsymbol{\chi}_i = \hat{\mathbf{x}} + \left(\sqrt{(n+\lambda)\mathbf{P}}\right)_i \quad \text{for } i = 1, \ldots, n$$ $$\boldsymbol{\chi}_{i+n} = \hat{\mathbf{x}} - \left(\sqrt{(n+\lambda)\mathbf{P}}\right)_i \quad \text{for } i = 1, \ldots, n$$where $\lambda = \alpha^2(n + \kappa) - n$ is a scaling parameter and $\left(\sqrt{(n+\lambda)\mathbf{P}}\right)_i$ is the $i$-th column of the matrix square root.
The UKF captures second-order nonlinear effects that the EKF misses (no linearization error), and requires no Jacobian derivation. However, it's computationally heavier — generating and propagating $2n+1$ sigma points for an $n$-dimensional state. For SLAM with $n \approx 18$–24, that's 37–49 sigma points per step. In practice, the ESKF/IEKF is preferred for LiDAR-IMU SLAM because the error-state is already nearly linear, making the EKF's linearization accurate enough.
The IMU measures angular velocity $\boldsymbol{\omega}_m$ and linear acceleration $\mathbf{a}_m$, both corrupted by bias and noise:
$$\boldsymbol{\omega}_m = \boldsymbol{\omega} + \mathbf{b}_\omega + \mathbf{n}_\omega$$ $$\mathbf{a}_m = \mathbf{R}^T(\mathbf{a} - {}^G\mathbf{g}) + \mathbf{b}_a + \mathbf{n}_a$$where $\mathbf{R}$ is the rotation from global to IMU frame, ${}^G\mathbf{g}$ is gravity in the global frame, and $\mathbf{b}_\omega, \mathbf{b}_a$ are slowly-varying biases modeled as random walks.
where $[\cdot]_\times$ denotes the skew-symmetric matrix for cross products.
Between IMU samples at period $\Delta t$:
$$\mathbf{R}_{k+1} = \mathbf{R}_k \cdot \text{Exp}\left((\boldsymbol{\omega}_m - \mathbf{b}_\omega)\Delta t\right)$$ $$\mathbf{v}_{k+1} = \mathbf{v}_k + \left(\mathbf{R}_k(\mathbf{a}_m - \mathbf{b}_a) + {}^G\mathbf{g}\right)\Delta t$$ $$\mathbf{p}_{k+1} = \mathbf{p}_k + \mathbf{v}_k \Delta t + \frac{1}{2}\left(\mathbf{R}_k(\mathbf{a}_m - \mathbf{b}_a) + {}^G\mathbf{g}\right)\Delta t^2$$The LiDAR provides 3D point clouds. In SLAM, these points are matched to a map (scan-to-map registration) to produce a pose "measurement" that corrects the IMU prediction.
For each LiDAR point ${}^L\mathbf{p}_j$ (in the LiDAR frame), transform it to the global frame using the current state estimate and compute its distance to the nearest plane in the map:
$$r_j = \mathbf{n}_j^T\left(\mathbf{R}\cdot{}^I\mathbf{R}_L \cdot {}^L\mathbf{p}_j + \mathbf{R}\cdot{}^I\mathbf{t}_L + \mathbf{p} - \mathbf{q}_j\right)$$where $\mathbf{n}_j$ is the plane normal at the nearest map point $\mathbf{q}_j$, and ${}^I\mathbf{R}_L, {}^I\mathbf{t}_L$ are the LiDAR-to-IMU extrinsic calibration.
Each LiDAR scan produces hundreds or thousands of residuals $r_j$. Stacking them gives the measurement vector $\mathbf{z}$ and the Jacobian $\mathbf{H}$ needed by the Kalman filter. This is why FAST-LIO's reformulated Kalman gain (which avoids inverting the huge $\mathbf{H}\mathbf{P}\mathbf{H}^T + \mathbf{R}$ matrix) is so important.
At each IMU sample (~100–400 Hz): integrate the kinematic equations to propagate the nominal state forward. Propagate the error-state covariance $\mathbf{P}$ using the error-state Jacobian $\mathbf{F}_\delta$.
LiDAR points within a scan are collected over ~100ms. During this time the robot moves. Use the IMU-propagated poses to "undo" the motion and align all points to the same reference time (backward propagation).
Match the de-skewed point cloud to the map using nearest-neighbor search (KD-tree or ikd-Tree). Compute point-to-plane residuals for each matched point.
Stack all residuals into the measurement vector. Compute the Kalman gain $\mathbf{K}$. Update the error state $\delta\hat{\mathbf{x}}$. Inject into the nominal state. Reset the error state to zero. Optionally iterate (IEKF).
Transform the corrected scan points to the global frame and insert them into the map data structure (e.g., ikd-Tree, voxel hash map).
| System | Filter Type | Key Innovation | Reference |
|---|---|---|---|
| FAST-LIO | Iterated ESKF | Reformulated Kalman gain (state-dim complexity instead of measurement-dim) | Xu & Zhang, 2021 |
| FAST-LIO2 | Iterated ESKF | Direct point registration (no feature extraction) + ikd-Tree | Xu et al., 2022 |
| LIO-SAM | Factor Graph + EKF | IMU preintegration on factor graph with loop closure support | Shan et al., 2020 |
| DLIO | ESKF | Coarse-to-fine continuous-time trajectory for motion correction | Chen et al., 2023 |
| Faster-LIO | Iterated ESKF | Parallel sparse incremental voxels (iVox) replacing ikd-Tree | Bai et al., 2022 |
| Point-LIO | ESKF | Point-by-point processing instead of scan-by-scan | He et al., 2023 |
| Filter | Complexity | Accuracy | Best For |
|---|---|---|---|
| Linear KF | Lowest | Optimal (linear systems only) | Learning, simple 1D/2D tracking |
| EKF | Low | Good (if nonlinearity is mild) | General sensor fusion, GPS/INS |
| ESKF | Low | Very good for IMU-centric fusion | LiDAR-IMU SLAM (recommended starting point) |
| IEKF / IESKF | Medium | Best for large corrections | Production SLAM (FAST-LIO2 approach) |
| UKF | Higher | Better for highly nonlinear models | When Jacobians are hard to derive, moderate state dimension |
Start with an Error-State Kalman Filter (ESKF) — it's the natural fit for IMU-centric fusion, handles 3D rotations cleanly, and is what most modern LiDAR-inertial systems use. If you need better accuracy under aggressive motion, upgrade to the Iterated ESKF (IESKF) as used in FAST-LIO2. The UKF is an alternative if you want to avoid deriving Jacobians, but it's computationally heavier and less common in production SLAM.
| Paper | Authors | Year | Link |
|---|---|---|---|
| A New Approach to Linear Filtering and Prediction Problems | R. E. Kalman | 1960 | cs.unc.edu (PDF) |
| New Results in Linear Filtering and Prediction Theory | R. E. Kalman, R. S. Bucy | 1961 | duke.edu (PDF) |
| Unscented Filtering and Nonlinear Estimation | S. J. Julier, J. K. Uhlmann | 2004 | Proc. IEEE, Vol. 92, No. 3 |
| Paper | Authors | Year | Link |
|---|---|---|---|
| FAST-LIO: A Fast, Robust LiDAR-Inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter | W. Xu, F. Zhang | 2021 | arXiv:2010.08196 |
| FAST-LIO2: Fast Direct LiDAR-Inertial Odometry | W. Xu, Y. Cai, D. He, J. Lin, F. Zhang | 2022 | GitHub + IEEE TRO |
| LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping | T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, D. Rus | 2020 | arXiv:2007.00258 |
| LOAM: Lidar Odometry and Mapping in Real-time | J. Zhang, S. Singh | 2014 | RSS 2014 |
| LINS: A Lidar-Inertial State Estimator for Robust and Efficient Navigation | C. Qin, H. Ye, C. Pranata, J. Han, S. Zhang, M. Liu | 2020 | arXiv:1907.02233 |
| Title | Authors | Notes |
|---|---|---|
| Kalman Filter from the Ground Up | A. Becker | Best starting point. Hands-on numerical examples with MATLAB code. Covers linear, EKF, and UKF. Available at kalmanfilter.net |
| Optimal State Estimation: Kalman, H-Infinity, and Nonlinear Approaches | D. Simon | Comprehensive graduate-level text covering all filter variants with exercises. Wiley, 2006. |
| Stochastic Models, Estimation, and Control (Vol. 1–3) | P. S. Maybeck | Classic. Ch. 1 freely available at cs.unc.edu. Outstanding intuitive introduction. |
| Kalman Filtering: Theory and Practice | M. S. Grewal, A. P. Andrews | Strong on both theory and practical implementation. Prentice Hall / Wiley. |
| Applied Optimal Estimation | A. Gelb (ed.) | The MIT/Draper Labs classic. Engineering-focused with practical examples. Often called "the Gelb book." |
| Introduction to Random Signals and Applied Kalman Filtering | R. G. Brown, P. Y. C. Hwang | Excellent introductory text. Wiley, 4th ed. |
| Probabilistic Robotics | S. Thrun, W. Burgard, D. Fox | The definitive SLAM textbook. Covers EKF-SLAM, particle filters, graph-based SLAM. MIT Press, 2005. |
| State Estimation for Robotics | T. D. Barfoot | Modern treatment of estimation on Lie groups (SO(3), SE(3)). Essential for error-state formulations. Cambridge, 2017. Free draft at utoronto.ca |
| Resource | Description | Link |
|---|---|---|
| kalmanfilter.net | Free step-by-step tutorial with numerical examples. Best online introduction. | kalmanfilter.net |
| Welch & Bishop KF Page | UNC Chapel Hill's canonical Kalman filter resource page — links to papers, books, software. | cs.unc.edu/~welch/kalman |
| Joan Solà — Quaternion Kinematics for the Error-State Kalman Filter | The essential reference for ESKF with quaternion rotations. Free technical report. | arXiv:1711.02508 |
| MATLAB Sensor Fusion Toolbox | MathWorks documentation on linear and extended Kalman filters for sensor fusion. | mathworks.com |
| Alan Zucconi — Kalman Filter Series | Visual, intuitive 5-part tutorial with interactive demos. | alanzucconi.com |
| The Kalman Filter (thekalmanfilter.com) | Practical tutorial focused on radar tracking with step-by-step equations. | thekalmanfilter.com |
| Repository | Description | Link |
|---|---|---|
| FAST-LIO / FAST-LIO2 | HKU-MARS lab. C++/ROS. Iterated error-state Kalman filter with ikd-Tree. The current gold standard for LiDAR-inertial odometry. | github.com/hku-mars/FAST_LIO |
| LIO-SAM | MIT. C++/ROS. Factor-graph-based LiDAR-inertial SLAM with loop closure. | github.com/TixiaoShan/LIO-SAM |
| DLIO | UCLA. Lightweight LiDAR-inertial odometry with continuous-time trajectories. | github.com/vectr-ucla/direct_lidar_inertial_odometry |
| Faster-LIO | Parallel sparse incremental voxels for faster map queries. | github.com/gaoxiang12/faster-lio |