FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by

Tightly-Coupled Iterated Kalman Filter

Wei Xu1 , Fu Zhang1

Abstract— This paper presents a computationally efficient Camera

and robust LiDAR-inertial odometry framework. We fuse
LiDAR feature points with IMU data using a tightly-coupled it-
erated extended Kalman filter to allow robust navigation in fast-
motion, noisy or cluttered environments where degeneration LiDAR (Livox AVIA)
arXiv:2010.08196v3 [[Link]] 14 Apr 2021

occurs. To lower the computation load in the presence of a large

number of measurements, we present a new formula to compute
the Kalman gain. The new formula has computation load
depending on the state dimension instead of the measurement
dimension. The proposed method and its implementation are
tested in various indoor and outdoor environments. In all Airframe
tests, our method produces reliable navigation results in real- Onboard Computer (280mm WheelBase)
time: running on a quadrotor onboard computer, it fuses more (DJI Manifold 2-C)
than 1,200 effective feature points in a scan and completes all
iterations of an iEKF step within 25 ms. Our codes are open-
sourced on Github2 . Fig. 1. Our LiDAR-inertial navigation system runs on a Livox AVIA
LiDAR3 and a DJI Manifold 2-C onboard computer4 , all on a customized
I. I NTRODUCTION small-scale quadrotor UAV (280 mm wheelbase). The RGB camera is
not used in our algorithm, but only for visualization. Video is available
Simultaneous localization and mapping (SLAM) is a fun- at [Link]
damental prerequisite of mobile robots, such as unmanned
aerial vehicles (UAVs). Visual (-inertial) odometry (VO),
such as Stereo VO [1] and Monocular VO [2, 3] are edges and planes) in the environments. When the UAV is
commonly used on mobile robots due to their lightweight and operating in cluttered environments where no strong features
low-cost. Although providing rich RGB information, visual are present, the LiDAR-based solution easily degenerates.
solutions lack direct depth measurements and require much This problem is more obvious when the LiDAR has a
computation resources to reconstruct the 3D environment small FoV. 2) Due to the high-resolution along the scanning
for trajectory planning. Moreover, they are very sensitive direction, a LiDAR scan usually contains many feature points
to lighting conditions. Light detection and ranging (LiDAR) (e.g., a few thousand). While these feature points are not ad-
sensors could overcome all these difficulties but have been equate to reliably determine the pose in case of degeneration,
too costly (and bulky) for small-scale mobile robots. tightly fusing such a large number of feature points to IMU
Solid-state LiDARs recently emerge as main trends in measurements requires tremendous computational resources
LiDAR developments, such as those based on micro- that are not affordable by the UAV onboard computer. 3)
electro-mechanical-system (MEMS) scanning [4] and rotat- Since the LiDAR samples point sequentially with a few
ing prisms [5]. These LiDARs are very cost-effective (in a laser/receiver pairs, laser points in a scan are always sampled
cost range similar to global shutter cameras), lightweight at different times, resulting in motion distortion that will
(can be carried by a small-scale UAV), and of high per- significantly degrade a scan registration [6]. The constant
formance (producing active and direct 3D measurements of rotations of UAV propellers and motors also introduce sig-
long-range and high-accuracy). These features make such nificant noises to the IMU measurements.
LiDARs viable for UAVs, especially industrial UAVs, which To make the LiDAR navigation viable for small-scale
need to acquire accurate 3D maps of the environments (e.g., mobile robots such as UAVs, we propose the FAST-LIO, a
aerial mapping) or may operate in cluttered environments computationally efficient and robust LiDAR-inertial odome-
with severe illumination variations (e.g., post-disaster search try package. More specifically, our contributions are as fol-
and inspection). lows: 1) To cope with fast-motion, noisy or cluttered environ-
Despite the great potentiality, solid-state LiDARs bring ments where degeneration occurs, we adopt a tightly-coupled
new challenges to SLAM: 1) the feature points in LiDAR iterated Kalman filter to fuse LiDAR feature points with
measurements are usually the geometrical structures (e.g., IMU measurements. We propose a formal back-propagation
process to compensate for the motion distortion; 2) To lower
This work is funded by a DJI (project no.200009538).
1
All authors are with Mechatronics and Robotic Systems (MaRS) Lab- the computation load caused by a large number of LiDAR
oratory, Department of Mechanical Engineering, University of Hong Kong.
3
{xuweii, fuzhang}@[Link] [Link]
2 4
[Link] [Link]
feature points, we propose a new formula for computing the pose measurement with IMU measurements. The separation
Kalman gain and prove its equivalence to the conventional between scan registration and data fusion reduces the compu-
Kalman gain formula. The new formula has a computation tation load. However, it ignores the correlation between the
complexity depending on the state dimension instead of the system’s other states (e.g., velocity) and the pose of the new
measurement dimension. 3) We implement our formulations scan. Moreover, in the case of featureless environments, the
into a fast and robust LiDAR-inertial odometry software scan registration could degenerate in certain directions and
package. The system is able to run on a small-scale quadrotor causes unreliable fusion in later stages.
onboard computer. 4) We conduct experiments in various
indoor and outdoor environments and with actual UAV flight C. Tightly-coupled LiDAR-Inertial Odometry
tests (Fig. 1) to validate the system’s robustness when fast
Unlike the loosely-coupled methods, tightly-coupled
motion or intense vibration noise exists.
LiDAR-inertial odometry methods typically fuse the raw
The remaining paper is organized as follows: In Section.
feature points (instead of scan registration results) of LiDAR
II, we discuss relevant research works. We give an overview
with IMU data. There are two main approaches to tightly-
of the complete system pipeline and the details of each key
coupled LIO: optimization-based and filter-based. Geneva et
component in Section. III. The experiments are presented in
al. [14] use a graph optimization with IMU pre-integration
Section. IV, followed by conclusions in Section. V.
constrains [15] and plane constraints [16] from LiDAR
II. R ELATED W ORKS feature points. Recently, Ye et al. [17] propose the LIOM
package which uses a similar graph optimization but is
Existing works on LiDAR SLAM are extensive. Here we
based on edge and plane features. For filter-based methods,
limit our review to the most relevant works: LiDAR-only
Bry [18] uses a Gaussian Particle Filter (GPF) to fuse the
odometry and mapping, loosely-coupled and tightly-coupled
data of IMU and a planar 2D LiDAR. This method has also
LiDAR-Inertial fusion methods.
been used in the Boston Dynamics Atlas humanoid robot.
A. LiDAR Odometry and Mapping Since the computation complexity of particle filter grows
quickly with the number of feature points and the system
Besl et al. [6] propose an iterated closest points (ICP)
dimension, Kalman filter and its variants are usually more
method for scan registration, which builds the basis for
preferred, such as extended Kalman filter [19], unscented
LiDAR odometry. ICP performs well for dense 3D scans.
Kalman filter [20], and iterated Kalman filter [21].
However, for sparse point clouds of LiDAR measurements,
Our method falls into the tightly-coupled approach. We
the exact point matching required by ICP rarely exists.
adopt an iterated extended Kalman filter similar to [21] to
To cope with this problem, Segal et al. [7] propose a
mitigate linearization errors. Kalman filter (and its variants)
generalized-ICP based on the point-to-plane distance. Then
has a time complexity O m2 where m is the measure-


Zhang et al. [8] combine this ICP method with a point-
ment dimension [22], this may lead to remarkably high
to-edge distance and developed a LiDAR odometry and
computation load when dealing with a large number of
mapping (LOAM) framework. Thereafter, many variants of
LiDAR measurements. Naive down-sampling would reduce
LOAM have been developed, such as LeGO-LOAM [9]
the number of measurements but at the cost of information
and LOAM-Livox [10]. While these methods work well for
loss. [21] reduces the number of measurements by extracting
structured environments and LiDARs of large FoV, they are
and fitting ground planes similar to [9]. This, however, does
very vulnerable to featureless environments or small FoV
not apply to aerial applications where the ground plane may
LiDARs [10].
not always present.
B. Loosely-coupled LiDAR-Inertial Odometry
III. M ETHODOLOGY
IMU measurements are commonly used to mitigate the
problem of LiDAR degeneration in featureless environments. A. Framework Overview
Loosely-coupled LiDAR-inertial odometry (LIO) methods
This paper will use the notations shown in Table I. The
typically process the LiDAR and IMU measurements sep-
overview of our workflow is shown in Fig. 2. The LiDAR
arately and fuse their results later. For example, IMU-aided
inputs are fed into the feature extraction module to obtain
LOAM [8] takes the pose integrated from IMU measure-
planar and edge features. Then the extracted features and
ments as the initial estimate for LiDAR scan registration.
IMU measurements are fed into our state estimation module
Zhen et al. [11] fuse the IMU measurements and the Gaus-
for state estimation at 10Hz−50Hz. The estimated pose then
sian Particle Filter output of LiDAR measurements using the
registers the feature points to the global frame and merges
error-state EKF. Balazadegan et al [12] add the IMU-gravity
them with the feature points map built so far. The updated
model to estimate the 6-DOF ego-motion to aid the LiDAR
map is finally used to register further new points in the next
scan registration. Zuo et al. [13] use a Multi-State Constraint
step.
Kalman Filter (MSCKF) to fuse the scan registration results
with IMU and visual measurements. A common procedure
B. System Description
of the loosely-coupled approach is obtaining a pose mea-
surement by registering a new scan and then fusing the 1)
/ operator:
 LiDAR Inputs IMU Inputs A LiDAR Scan
Map Add Updated Feature Points Framework 𝑡2 𝑡𝑖 𝑡𝑘
(History Feature Points)
overview Time
Retrieve Forward Propagation:

100k-500kHz 100-250Hz
Sub-Map IMU inputs: IMU
Inputs:
... ...
Pre-Processing
Points Accumulation Forward Propagation
(Section III-C-1)
Residual Computation
(Section III-C-3)
Iterated Kalman Filter
Feature
Feature
points:Points: × ×
...
Backward Propagation:

× × ×
...
= −𝐟(𝐱, 𝐮,0)
× × × ×
N
Feature Extraction 10-50Hz
Backward Propagation State Update Converged? Odometry
Planar
&Motion Compensation
(Section III-C-4) Y
Edge (Section III-C-2) Projection of points
State Estimation (10-50Hz)

(a) (b)
Fig. 2. System overview of FAST-LIO. (a): the overall pipeline; (b): the forward and backward propagation.

TABLE I G
g is the unknown gravity vector in the global frame, am
S OME I MPORTANT N OTATIONS
and ωm are IMU measurements, na and nω are the white
Symbols Meaning noise of IMU measurements, ba and bω are the IMU bias
modelled as the random walk process with Gaussian noises
tk The scan-end time of the k-th LiDAR scan.
τi The i-th IMU sample time in a LiDAR scan. nba and nbω , and the notation bac∧ denotes the skew-
ρj The j-th feature point’s sample time in a LiDAR scan. symmetric matrix of vector a ∈ R3 that maps the cross
Ii , Ij , Ik The IMU body frame at the time τi , ρj and tk . product operation.
Lj , Lk The LiDAR body frame at the time ρj and tk .
x, xb, x̄ The ground-true, propagated, and updated value of x. 3) Discrete model:
x
e The error between ground-true x and its estimation x̄. Based on the
operation defined above, we can discretize
bκ
x The κ-th update of x in the iterated Kalman filter. the continuous model in (1) at the IMU sampling period ∆t
xi , xj , xk The vector (e.g.,state) x at time τi , ρj and tk .
x̌j Estimate of xj relative to xk in the back propagation. using a zero-order holder. The resultant discrete model is
xi+1 = xi
(∆tf (xi , ui , wi )) (2)

Let M be the manifold of dimension n in consideration where i is the index of IMU measurements, the function f ,
(e.g., M = SO(3)). Since manifolds are locally homeomor- state x, input u and noise w are defined as below:
phic to Rn , we can establish a bijective mapping from a
M = SO(3) × R15 , dim(M) = 18
local neighborhood on M to its tangent space Rn via two
.  T
encapsulation operators
and [23]: x = G RTI G pTI G vIT bTω bTa G gT ∈ M
.  T T .  T

: M×Rn→M; : M×M → Rn u = ωm aTm , w = nTω nTa nTbω nTba
R1 R2 = Log(RT
 
M= SO(3) : R
r = RExp(r); 2 R1) ωmi − bωi − nωi
G
vIi  (3)
M= Rn : a
b = a+b; a b = a−b

G 
 RI (am − ba − na ) + G gi 
r r 2 f (xi , ui , wi ) =  i i i i 
where Exp (r) = I+ krk sin (krk)+ krk 2 (1−cos (krk)) is the

 nbωi 

exponential map [23] and Log(·) is its inverse map. For a  nbai 
compound manifold M = SO(3) × Rn we have: 03×1

            4) Preprocessing of LiDAR measurements:

R r R
r R1 R2 R1 R2 LiDAR measurements are point coordinates in its local

= ; =
a b a+b a b a−b body frame. Since the raw LiDAR points are sampled at
From the above definition, it is easy to verify that a very high rate (e.g., 200kHz), it is usually not possible to
process each new point once being received. A more practical
(x
u) x = u; x
(y x) = y; ∀x, y ∈ M, ∀u ∈ Rn . approach is to accumulate these points for a certain time
and process them all at once. In FAST-LIO, the minimum
2) Continuous model:
accumulation interval is set to 20 ms, leading to up to 50 Hz
Assuming an IMU is rigidly attached
to the LiDAR with
full state estimation (i.e., odometry output) and map update
a known extrinsic I TL = I RL , I pL . Taking the IMU
as shown in Fig. 2 (a). Such an accumulated set of points is
frame (denoted as I) as the body frame of reference leads
called a scan, and the time for processing it is denoted as tk
to a kinematic model:
(see Fig. 2 (b)). From the raw points, we extract planar points
G
ṗI = G vI , G
v̇I = G RI (am − ba − na ) + G g, G
ġ = 0 with high local smoothness [8] and edge points with low
G G local smoothness as in [10]. Assume the number of feature
ṘI = RI bωm − bω − nω c∧ , ḃω = nbω , ḃa = nba
(1) points is m, each is sampled at time ρj ∈ (tk−1 , tk ] and is
where G pI , G RI are the position and attitude of IMU in denoted as Lj pfj , where Lj is the LiDAR local frame at the
the global frame (i.e., the first IMU frame, denoted as G), time ρj . During a LiDAR scan, there are also multiple IMU
  T   T 
Exp (−b ωi ∆t) 0 0 −A(b ωi ∆t) ∆t 0 0 −A (b
ωi ∆t) ∆t 0 0 0

 0 I I∆t 0 0 0 


 0 0 0 0 

−G R
b I bba c ∆t 0 I 0 − Gb
R ∆t I∆t  0 −G R
b I ∆t 0 0 
Fxe=
 i i ∧ I i , Fw= i  (7)
 0 0 0 I 0 0 


 0 0 I∆t 0 

 0 0 0 0 I 0  0 0 0 I∆t
0 0 0 0 0 I 0 0 0 0

measurements, each sampled at time τi ∈ [tk−1 , tk ] with Denoting the covariance of white noises w as Q, then
the respective state xi as in (2). Notice that the last LiDAR the propagated covariance Pb i can be computed iteratively
feature point is the end of a scan, i.e., ρm = tk , while the following the below equation.
IMU measurements may not necessarily be aligned with the
start or end of the scan. P b i FT + Fw QFT ; P
b i+1 = Fxe P b 0 = P̄k−1 . (8)
x
e w

C. State Estimation The propagation continues until reaching the end time of
To estimate the states in the state formulation (2), we use a new scan at tk where the propagated state and covariance
an iterated extended Kalman filter. Moreover, we characterize are denoted as x bk , P
b k . Then Pb k represents the covariance
the estimation covariance in the tangent space of the state of the error between the ground-truth state xk and the state
estimate as in [23, 24]. Assume the optimal state estimate of propagation x bk (i.e., xk x bk ).
the last LiDAR scan at tk−1 is x̄k−1 with covariance matrix 2) Backward Propagation and Motion Compensation:
P̄k−1 . Then P̄k−1 represents the covariance of the random When the points accumulation time interval is reached at
error state vector defined below: time tk , the new scan of feature points should be fused with
iT the propagated state x bk and covariance P b k to produce an
.
h
ek−1 = xk−1 x̄k−1 = δθ T G p
x e TI G veIT b
eT b
ω
eT Gg
a e T
optimal state update. However, although the new scan is at
time tk , the feature points are measured at their respective
where δθ = Log(G R̄TI G RI ) is the attitude error and the rests sampling time ρj ≤ tk (see Section. III-B.4 and Fig. 2 (b)),
are standard additive errors (i.e., the error in the estimate x̄ causing a mismatch in the body frame of reference.
of a quantity x is x e = x − x̄). Intuitively, the attitude error To compensate the relative motion (i.e., motion distortion)
δθ describes the (small) deviation between the true and the between time ρj and time tk , we propagate (2) backward as
estimated attitude. The main advantage of this error definition x̌j−1 = x̌j
(−∆tf (x̌j , uj , 0)), starting from zero pose and
is that it allows us to represent
 the attitude uncertainty by the rests states (e.g., velocity and bias) from x bk . The backward
3 × 3 covariance matrix E δθδθ T . Since the attitude has 3 propagation is performed at the frequency of feature point,
degree of freedom (DOF), this is a minimal representation. which is usually much higher than the IMU rate. For all
1) Forward Propagation: the feature points sampled between two IMU measurements,
The forward propagation is performed once receiving we use the left IMU measurement as the input in the back
an IMU input (see Fig. 2). More specifically, the state is propagation. Furthermore, noticing that the last three block
propagated following (2) by setting the process noise wi to elements (corresponding to the gyro bias, accelerometer bias,
zero: and extrinsic) of f (xj , uj , 0) (see (3)) are zeros, the back
propagation can be reduced to:
x
bi+1 = x
bi
(∆tf (b
xi , ui , 0)) ; x
b0 = x̄k−1 . (4)

where ∆t = τi+1 − τi . To propagate the covariance, we use Ik

p̌Ij−1 = Ik p̌Ij − Ik v̌Ij ∆t, s.f. Ik
p̌Im = 0;
the error state dynamic model obtained below: Ik Ik
v̌Ij−1 = v̌Ij − ŘIj (ami−1 − bak )∆t − Ik g
Ik b bk ∆t,
x
ei+1 = xi+1 x
bi+1 Ik G bT G Ik GbT G
s.f. v̌I = R m
v
bI , g
Ik bk = R
k
g
bk ; Ik
= (xi
∆tf (xi , ui , wi )) (b
xi
∆tf (b
xi , ui , 0)) (5) Ik
ŘIj−1 = Ik ŘIj Exp((b
b ω −ωm )∆t), s.f.
k i−1 RIm = I. Ik
(23)
' Fxe x
e i + Fw wi . (9)
where ρj−1 ∈ [τi−1 , τi ), ∆t = ρj − ρj−1 , and s.f. means
The matrix Fxe and Fw in (5) is computed following “starting from”.
the Appendix. A. The result is shown in (7), where ω bi = The backward propagation will produce a relative pose
−1
ωmi − b bω , a
i
b i = ami − b
b ai and A (u) follows the same between time
ρj and the scan-end time tk : Ik ŤIj =
definition in [25] as below: Ik
ŘIj , Ik p̌Ij . This relative pose enables us to project the
−1 buc2∧ local measurement Lj pfj to scan-end measurement Lk pfj as
A (u) = I − 21 buc∧ + (1 − α (kuk)) kuk2

m cos(m/2) (6) follows (see Fig. 2):
α (m) = m m
2 cot 2 = 2 sin(m/2)
Lk
pfj = I T−1
L
Ik
ŤIj I TL Lj pfj . (10)
where I TL is the known extrinsic (see Section. III-B.2). where x eκk = xk x bκk (or equivalently xk = bκk
x

x eκk ), Hκj
Then the projected point Lk pfj is used to construct a residual is the Jacobin matrix of hj x κ
bk
x κ Lj
ek , nfj with respect to
in the following section. eκk , evaluated at zero, and vj ∈ N (0, Rj ) comes from the
x
3) Residual computation: raw measurement noise Lj nfj .
With the motion compensation in (10), we can view the Notice that the prior distribution of xk obtained from the
scan of feature points {Lk pfj } all sampled at the same forward propagation in Section. III-C.1 is for
time tk and use it to construct the residual. Assume the xk x xκk
x
bk = (b eκk ) x bκk
bk = x b k + Jκ x
x eκk (15)
current iteration of the iterated Kalman filter is κ, and the
corresponding state estimate is xbκk . When κ = 0, x bκk = xbk , where Jκ is the partial differentiation of (b
xκk
x
eκk ) x
bk with
κ
the predicted state from the propagation in (4). Then, the respect to x
ek evaluated at zero:
feature points {Lk pfj } can be transformed to the global " 
G bκ Gb
−T #
frame as below: κ
J = A R Ik R Ik 0 3×15 (16)
015×3 I15×15
G κ
p
b fj = GT
b κ I TL Lk pf ; j = 1, · · · , m.
Ik j
(11)
where A(·)−1 is defined in (6). For the first iteration (i.e.,
For each LiDAR feature point, the closest plane or edge the case of extended Kalman filter), xbκk = x
bk , then Jκ = I.
defined by its nearby feature points in the map is assumed to Combining the prior in (15) with the posteriori distribution
be where the point truly belongs to. That is, the residual is from (14) yields the maximum a-posteriori estimate (MAP):
defined as the distance between the feature point’s estimated 
2
Xm
κ κ κ 2

global frame coordinate G p b κfj and the nearest plane (or min kx k x
b k kP −1 + kz j + H x
j k R
e k −1 (17)
eκ
x j=1
b
k j
k
edge) in the map. Denoting uj the normal vector (or edge
orientation) of the corresponding plane (or edge), on which where kxk2M = xT Mx. Substituting the linearization of
lying a point G qj , then the residual zκj is computed as: the prior in (15) into (17) and optimizing the resultant
  quadratic cost leads to the standard iterated Kalman fil-
zκj = Gj G p b κfj − G qj (12) ter [21], which can be computed below (to simplify the nota-
T T
tion, let H = [Hκ1 , · · · , Hκm ]T , R = diag (R1 , · · · Rm ),P =
where Gj = uTj for planar features and Gj = buj c∧ for −1 b κ −T
h T T T
i
(Jκ ) P k (J ) , and zκ = zκ , · · · , zκ
k 1 m ):
edge features. The computation of the uj and the search of
nearby points in the map, which define the corresponding K = PHT (HPHT +R)−1 ,
plane or edge, is achieved by building a KD-tree of the
(18)
bκ+1
x k bκk

=x −Kzκk − (I − KH)(Jκ )−1 (b
xκk x
bk ) .
points in the most recent map [10]. Moreover, we only
consider residuals whose norm is below certain threshold The updated estimate x bκ+1
k is then used to compute the
(e.g., 0.5m). Residuals exceeding this threshold are either residual in Section. III-C.3 and repeat the process until
outliers or newly observed points. xκ+1
convergence (i.e., kb k bκk k < ). After convergence, the
x
4) Iterated state update: optimal state estimation and covariance is:
To fuse the residual zκj computed in (12) with the state
bκ+1
x̄k = x k , P̄k = (I − KH) P (19)
prediction xbk and covariance P
b k propagated from the IMU
data, we need to linearize the measurement model that A problem with the commonly used Kalman gain form in
relates the residual zκj to the ground-truth state xk and (18) is that it requires to invert the matrix HPHT +R which
measurement noise. The measurement noise originates from is in the dimension of the measurements. In practice, the
the LiDAR ranging and beam-directing noise Lj nfj when number of LiDAR feature points are very large in number,
measuring the point Lj pfj . Removing this noise from the inverting a matrix of this size is prohibitive. As such, existing
point measurement Lj pfj leads to the true point location works [21, 26] only use a small number of measurements.
In this paper, we show that this limitation can be avoided.
Lj
pgt
fj =
Lj
pfj − Lj nfj . (13) The intuition originates from (17) where the cost function
This true point, after projecting to the frame Lk via (10) and is over the state, hence the solution should be calculated
then to the global frame with the ground-truth state xk (i.e, with complexity depending on the state dimension. In fact,
pose), should lie exactly on the plane (or edge) in the map. if directly solving (17), we can obtain the same solution in
That is, plugging (13) into (10), then into (11), and further (18) but with a new form of Kalman gain shown below:
into (12) should result in zero. i.e.,
−1 T −1
K= HT R−1 H+P−1 H R . (20)
0 = hj xk ,Lj nfj = Gj G TIkIk ŤIjI TL Lj pfj − Lj nfj −G qj




We prove in Appendix B that the two forms of Kalman
Approximating the above equation by its first order ap- gains are indeed equivalent based on the matrix inverse
proximation made at xbκk leads to lemma [27]. Since the LiDAR measurements are indepen-
dent, the covariance matrix R is (block) diagonal and hence
0 = hj xk , Lj nfj ' hj (b xκk , 0) + Hκj x
eκk + vj


(14) the new formula only requires to invert two matrices both
κ κ κ
= zj + H j x
ek + vj in the dimension of state instead of measurements. The new
 TABLE II
formula greatly saves the computation as the state dimension
T HE RUNNING TIMES OF TWO K ALMAN GAIN FORMULAS
is usually much lower than measurements in LIO (e.g., more
than 1,000 effective feature points in a scan for 10 Hz scan Feature Num. 307 717 998 1243 1453 1802
rate while the state dimension is only 18). Old Formula (ms) 7.1 23.4 109.3 251 1219 1621
New Formula (ms) 0.07 0.11 0.25 0.37 0.59 1.16
5) The algorithm:
Our state estimation is summarized in Algorithm 1.
Algorithm 1: State Estimation
Input : Last optimal estimation x̄k−1 and P̄k−1 ,
IMU inputs (am , ωm ) in current scan;
LiDAR feature points Lj pfj in current scan.
1 Forward propagation to obtain state prediction x bk via
(4) and covariance prediction P
b k via (8);
2 Backward propagation to obtain Lk pfj via (9), (10);
3 κ = −1, x bκ=0
k =x bk ;
4 repeat Fig. 3. During the flight experiment, the UAV is automatically flying
5 κ = κ + 1; in a circle path with 1.8 m radius and 1.4 m height. The circle path is
conducted repeatedly for 4 times with different periods (6-10 s). The yaw
6 Compute Jκ via (16) and P = (Jκ )−1 P b k (Jκ )−T ;
command of the UAV maintains constant during the flight. In the end, the
7 Compute residual zj (12) and Jocobin Hκj (14);
κ
UAV is manually controlled to land at the take-off point, which enables us
8 Compute the state update xbκ+1
k via (18) with the to measure the drift.
Kalman gain K from (20);
9 until kbxκ+1
k bκk k < ;
x
κ+1 B. UAV Flight Experiments
10 x̄k = xbk ; P̄k = (I − KH) P.
Output: Current optimal estimation x̄k and P̄k . In order to validate the robustness and computational
efficiency of FAST-LIO in actual mobile robots, we build
a small-scale quadrotor that can carry a Livox Avia LiDAR
D. Map Update with 70◦ FoV and a DJI Manifold 2-C onboard computer
With the state update x̄k (hence G T̄Ik = (G R̄Ik , G p̄Ik )), with a 1.8 GHz quad-core Intel i7-8550U CPU and 8
each feature point (Lk pfj ) projected to the body frame Lk GB RAM, as shown in Fig. 1. The UAV has only 280
(see (10)) is then transformed to the global frame via: mm wheelbase, and the LiDAR is directly installed on
the airframe. The LiDAR-inertial odometry is sent to the
G
p̄fj = G T̄Ik I TL Lk pfj ; j = 1, · · · , m. (21) flight controller tracking a circle trajectory (Fig. 3). The
actual flight experiments show that FAST-LIO can achieve
These features points are finally appended to the existing real-time and stable odometry output and mapping in a
map containing feature points from all previous steps. maximum of 50 Hz for the indoor environment. The flight
E. Initialization trajectory and mapping results of 50 Hz frame rate indoor
To obtain a good initial estimate of the system state (e.g., experiment are shown in Fig. 3. The average number of
gravity vector G g, bias, and noise covariance) so to speedup effective feature points and running time is 270 and 6.7
the state estimator, initialization is required. In FAST-LIO, ms, respectively , the drift is smaller than 0.3% (0.08 m
the initialization is simple: keeping the LiDAR static for drift over 32 m trajectory). The flight video can be found at
several seconds (2 seconds for all the experiments in this [Link]
paper), the collected data is then used to initialize the IMU C. Indoor Experiments
bias and the gravity vector. If non-repetitive scanning is
Then we test FAST-LIO in a challenging indoor envi-
supported by the LiDAR (e.g., Livox AVIA), keeping static
ronment with large rotation speeds. In order to generate
also allows the LiDAR to capture an initial high-resolution
large rotation, the sensor suite is held on hands and un-
map that is beneficial for the subsequent navigation.
dergoes quickly shaking. Fig. 4 shows the angular velocity
IV. E XPERIMENT R ESULTS and acceleration during the experiment. It is seen that the
A. Computational Complexity Experiments angular velocity often exceeds 100 deg/s. A state of the
art implementation of LOAM on Livox LiDARs5 [10] and
In order to validate the computational efficiency of the
LOAM with IMU6 [8] are also tested as comparisons when
proposed new formula for computing Kalman gains. We
the feature extraction are replaced with the one of FAST-LIO.
intentionally replace the computation of Kalman gains with
The results show that FAST-LIO can output odometry faster
the old formula in our system and compare their computation
and more stable than others, as shown in Fig. 5 and Table. III.
time under the same system pipeline and number of feature
It should be noted that the LOAM+IMU is a loosely-coupled
points. The results are shown in Table. II. It is obvious that
the complexity of the new formula is much lower than the 5
[Link]
6
old one. [Link]
 TABLE III
C OMPARISON OF PROCESSING TIME FOR A L I DAR SCAN AT 10Hz

Packages Num. of effective features Running time

LOAM 1107 59 ms
LOAM+IMU 1107 44 ms
FAST-LIO 1430 23 ms

Fig. 4. The angular velocity and acceleration in the indoor experiments.

method, hence results in inconsistent mapping. To further

verify the mapping result, we perform a second experiment Fig. 5. The Mapping results of different LIO packages in an indoor
in the same environment but with a much slower motion. environment with large rotation speed.
The map built by FAST-LIO is shown in the lower-right
figure of Fig. 4. Since the two experiments have non-identical
movements, it leads to slight visual differences at places
occlusions occur. The rest mapping results are very close.

D. Outdoor Experiments
Here we show the performance of FAST-LIO in outdoor
environments. Fig. 6 shows the mapping results (displaying
all raw points) of the Main Building in the University of
Hong Kong. The sensor suite is handheld during the data
collection and returned to the starting position after traveling Fig. 6. Mapping results of the Main Building, University of Hong Kong.
The LiDAR platform, the same one in Fig. 1, is handheld randomly walking
around 140m. The drift in this experiment is smaller than to scan the building. In order to show the drift, the experiment are started
0.05% (0.07 m drift over 140 m trajectory). The scan rate is and ended at the same place.
set to 10 Hz in this experiment, and the average processing
time of a scan is 25 ms with average 1497 effective feature
V. C ONCLUSION
points.
Further, we compare FAST-LIO with LINS7 [21]. To make This paper proposed FAST-LIO, a computationally effi-
a fair comparison, we use the dataset from LINS [21], which cient and robust LiDAR-inertial odometry framework by a
is a seaport area data collected by a Velodyne VLP-16 and tightly-coupled iterated Kalman filter. We used the forward
an Xsens MTiG-710 IMU7 . The results show that the FAST- and backward propagation to predict the states and compen-
LIO can achieve better mapping accuracy (see Fig. 7) and sate for the motion in a LiDAR scan. Besides, we proved and
only consumes 7.3 ms processing time in average while implemented an equivalent formula that can achieve much
LINS takes 34.5 ms in average, both running at 10Hz. It lower complexity for the Kalman gain computation. FAST-
should be noted that since the EKF formula in LINS package LIO was tested in the UAV flight experiment, challenging
has high computational complexity (see Section. III-C.4)), it indoor environment with large rotation speed and outdoor
down samples the feature points to average 147 points in environment. In all tests, our method produced precise, real-
a scan (while 784 in a scan for FAST-LIO). This leads to time, and reliable navigation results.
degraded mapping accuracy for LINS. The result in Fig. 7
shows all the feature points (before down-sample) of FAST-
A PPENDIX
LIO and LINS. All the experiments are conducted on the
DJI Manifold2 onboard computer. A. Computation of Fxe and Fw
7
[Link] Recall xi = x bi
x ei , denote g (e x i , wi ) =
LINS---LiDAR-inertial-SLAM f (xi , ui , wi )∆t = f (b
xi
x
ei , ui , wi )∆t. Then the error state
 [6] P. J. Besl and N. D. McKay, “Method for registration of 3-d shapes,”
in Sensor fusion IV: control paradigms and data structures, vol. 1611.
International Society for Optics and Photonics, 1992, pp. 586–606.
[7] A. Segal, D. Haehnel, and S. Thrun, “Generalized-icp.” in Robotics:
science and systems, vol. 2, no. 4. Seattle, WA, 2009, p. 435.
[8] J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in real-
time.” in Robotics: Science and Systems, vol. 2, no. 9, 2014.
[9] T. Shan and B. Englot, “Lego-loam: Lightweight and ground-
optimized lidar odometry and mapping on variable terrain,” in 2018
IEEE/RSJ International Conference on Intelligent Robots and Systems
(IROS). IEEE, 2018, pp. 4758–4765.
[10] J. Lin and F. Zhang, “Loam livox: A fast, robust, high-precision lidar
odometry and mapping package for lidars of small fov,” in 2020 IEEE
International Conference on Robotics and Automation (ICRA). IEEE,
2020, pp. 3126–3131.
[11] W. Zhen, S. Zeng, and S. Soberer, “Robust localization and local-
izability estimation with a rotating laser scanner,” in 2017 IEEE
International Conference on Robotics and Automation (ICRA). IEEE,
2017, pp. 6240–6245.
Fig. 7. Comparison between LINS [21] and FAST-LIO. The data is from [12] Y. Balazadegan Sarvrood, S. Hosseinyalamdary, and Y. Gao, “Visual-
[21] and is collected by a Velodyne VLP-16 LiDAR and an Xsens MTiG- lidar odometry aided by reduced imu,” ISPRS international journal of
710 IMU, the green straight line in the center is the odometry output. geo-information, vol. 5, no. 1, p. 3, 2016.
[13] X. Zuo, P. Geneva, W. Lee, Y. Liu, and G. Huang, “Lic-fusion: Lidar-
model (5) is rewriten as: inertial-camera odometry,” in 2019 IEEE/RSJ International Confer-
ence on Intelligent Robots and Systems (IROS), 2019, pp. 5848–5854.
x
ei+1 = ((b
xi
e
xi )
g (e
xi , wi )) (b
xi
g (0, 0)) [14] P. Geneva, K. Eckenhoff, Y. Yang, and G. Huang, “Lips: Lidar-
| {z } (22) inertial 3d plane slam,” in 2018 IEEE/RSJ International Conference
G(e
xi ,g(e
xi ,wi )) on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 123–130.
[15] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold
Following the chain rule of partial differention, the matrix preintegration for real-time visual–inertial odometry,” IEEE Transac-
Fxe and Fw in (5) are computed as below. tions on Robotics, vol. 33, no. 1, pp. 1–21, 2016.
  [16] M. Hsiao, E. Westman, and M. Kaess, “Dense planar-inertial slam
Fxe = ∂G(ex∂e
i ,g(0,0))
xi + ∂G(0,g(e
∂g(e
xi ,0)) ∂g(e
xi ,0)
xi ,0)
∂e
xi
with structural constraints,” in 2018 IEEE International Conference
x
ei =0 on Robotics and Automation (ICRA). IEEE, 2018, pp. 6521–6528.
  (23) [17] H. Ye, Y. Chen, and M. Liu, “Tightly coupled 3d lidar inertial
∂G(0,g(0,wi )) ∂g(0,wi )
Fw = ∂g(0,wi ) ∂wi odometry and mapping,” in 2019 International Conference on Robotics
wi =0 and Automation (ICRA). IEEE, 2019, pp. 3144–3150.
B. Equivalent Kalman Gain formula [18] A. Bry, A. Bachrach, and N. Roy, “State estimation for aggressive
flight in gps-denied environments using onboard sensing,” in 2012
Based on the matrix inverse lemma [27], we can get: IEEE International Conference on Robotics and Automation. IEEE,
2012, pp. 1–8.

−1
−1 [19] J. A. Hesch, F. M. Mirzaei, G. L. Mariottini, and S. I. Roumeliotis, “A
P−1 +HT R−1 H = P − PHT HPHT + R HP laser-aided inertial navigation system (l-ins) for human localization in
unknown indoor environments,” in 2010 IEEE International Confer-
Substituting above into (20), we can get: ence on Robotics and Automation. IEEE, 2010, pp. 5376–5382.

−1 T −1 [20] Z. Cheng, D. Liu, Y. Yang, T. Ling, X. Chen, L. Zhang, J. Bai,
K = HT R−1 H+P−1 H R Y. Shen, L. Miao, and W. Huang, “Practical phase unwrapping of

−1 interferometric fringes based on unscented kalman filter technique,”
T −1
= PH R −PH HPHT +R T
HPHT R−1 Optics express, vol. 23, no. 25, pp. 32 337–32 349, 2015.
[21] C. Qin, H. Ye, C. E. Pranata, J. Han, S. Zhang, and M. Liu, “Lins:
Now note that HPHT R−1 = HPHT + R R−1 − I.


A lidar-inertial state estimator for robust and efficient navigation,”
in 2020 IEEE International Conference on Robotics and Automation
Substituting it into above, we can get the standard Kalman (ICRA). IEEE, 2020, pp. 8899–8906.
gain formula in (18), as shown below. [22] M. Raitoharju and R. Piché, “On computational complexity reduction

−1 methods for kalman filter extensions,” IEEE Aerospace and Electronic
K = PHT R−1 − PHT R−1 + PHT HPHT + R Systems Magazine, vol. 34, no. 10, pp. 2–19, 2019.

−1 [23] C. Hertzberg, R. Wagner, U. Frese, and L. Schröder, “Integrating
= PHT HPHT + R .  generic sensor fusion algorithms with sound state representations
through encapsulation of manifolds,” Information Fusion, vol. 14,
R EFERENCES no. 1, pp. 57–77, 2013.
[24] W. Xu, D. He, Y. Cai, and F. Zhang, “Robots state estimation and
[1] K. Sun, K. Mohta, B. Pfrommer, M. Watterson, S. Liu, Y. Mulgaonkar, observability analysis based on statistical motion models,” 2020.
C. J. Taylor, and V. Kumar, “Robust stereo visual inertial odometry [25] F. Bullo and R. M. Murray, “Proportional derivative (pd) control on
for fast autonomous flight,” IEEE Robotics and Automation Letters, the euclidean group,” in European control conference, vol. 2, 1995,
vol. 3, no. 2, pp. 965–972, 2018. pp. 1091–1097.
[2] T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monoc- [26] M. Bloesch, M. Burri, S. Omari, M. Hutter, and R. Siegwart, “Iterated
ular visual-inertial state estimator,” IEEE Transactions on Robotics, extended kalman filter based visual-inertial odometry using direct pho-
vol. 34, no. 4, pp. 1004–1020, 2018. tometric feedback,” The International Journal of Robotics Research,
[3] C. Forster, M. Pizzoli, and D. Scaramuzza, “Svo: Fast semi-direct vol. 36, no. 10, pp. 1053–1072, 2017.
monocular visual odometry,” in 2014 IEEE international conference [27] N. J. Higham, Accuracy and stability of numerical algorithms. SIAM,
on robotics and automation (ICRA). IEEE, 2014, pp. 15–22. 2002.
[4] D. Wang, C. Watkins, and H. Xie, “Mems mirrors for lidar: A review,”
Micromachines, vol. 11, no. 5, p. 456, 2020.
[5] Z. Liu, F. Zhang, and X. Hong, “Low-cost retina-like robotic lidars
based on incommensurable scanning,” IEEE/ASME Transactions on
Mechatronics, pp. 1–1, 2021.