LiDAR-Stabilised GNSS-IMU Platform Pose Tracking

Abstract

The requirement to estimate the six degree-of-freedom pose of a moving platform frequently arises in automation applications. It is common to estimate platform pose by the fusion of global navigation satellite systems (GNSS) measurements and translational acceleration and rotational rate measurements from an inertial measurement unit (IMU). This paper considers a specific situation where two GNSS receivers and one IMU are used and gives the full formulation of a Kalman filter-based estimator to do this. A limitation in using this sensor set is the difficulty of obtaining accurate estimates of the degree of freedom corresponding to rotation about the line passing through the two GNSS receiver antenna centres. The GNSS-aided IMU formulation is extended to incorporate LiDAR measurements in both known and unknown environments to stabilise this degree of freedom. The performance of the pose estimator is established by comparing expected LiDAR range measurements with actual range measurements. Distributions of the terrain point-to-model error are shown to improve from $0.27$ $\mathrm{m}$ mean error to $0.06$ $\mathrm{m}$ when the GNSS-aided IMU estimator is augmented with LiDAR measurements. This precision is marginally degraded to $0.14$ $\mathrm{m}$ when the pose estimator is operated in an a prior unknown environment.

1. Introduction

An inertial measurement unit (IMU) aided by global navigation satellite system (GNSS) information is often used to track the six degree of freedom (DOF) pose (here, pose refers to the six degree of freedom position (x, y, z) and orientation ($\theta$, $\varphi$, $\psi$) of the platform) of a moving platform. Solutions come in many forms but are broadly categorised as being tightly or loosely coupled depending on the level of sensor integration. The rationale for using the two sensor types is that GNSS receivers provide low-frequency information while the IMU provides information about higher frequency motion, noting that when stationary, the IMU measures gravity which can be considered as zero-frequency (DC) information. In a typical implementation, measurements from the two sensors are combined using optimal estimation methods, vis-à-vis Kalman filtering, to maintain an estimate of the pose of the platform frame P as it moves relative to a global frame G.

Where three GNSS receivers with non-collinear antenna are used, the low-frequency pose of the platform can be fully determined to the precision and bandwidth of the receivers. Each GNSS receiver provides positional knowledge of its antenna centres to a precision of approximately $0.05$ $\mathrm{m}$ at a bandwidth from 0 $\mathrm{Hz}$ to 2–5 $\mathrm{Hz}$ (with measurement updates typically provided at 10 $\mathrm{Hz}$). Higher frequency motion is then determined from information provided by the IMU, which will typically provide acceleration and angular rate information from 0 Hz to 50 Hz, at update rates of 50 Hz to 100 Hz. Such an arrangement provides a nice division of labour, with the Kalman filter-based pose estimator serving to combine the disparate frequency information provided by the two sensor types.

However, where two GNSS receivers are used, only five of the six degrees of freedom of the platform can be established using the information provided by the GNSS receivers. The platform’s rotational orientation about the line drawn through the antenna centres is not defined. In theory, gravity can be used to resolve this, provided the placement of the IMU frame does not result in the vector emanating from the sensor origin and directed by gravity, intersecting the antenna centre line. In practice, however, for a similarly structured estimator, start-up bias and the conflation of gravity measurement with accelerations due to platform motion limits the accuracy of pose estimates.

This paper develops and evaluates a method for using three-dimensional LiDAR measurements to stabilise pose estimates for platforms having two GNSS receivers. We consider two situations: (i) where the environment around the platform is a priori known, and (ii) where it is not. LiDAR sensors are often present on robotic platforms as components of a perception system. Their use for stabilising pose estimates becomes relevant where three GNSS receivers are impractical or cost-prohibitive.

Pose estimation is clearly ground for fertile research and has been for some time. Google Scholar identifies no fewer than 4650 publications that contain all of the terms “GNSS”, “IMU”, and “loosely coupled”. A total of 851 of these were published after 2020; the first was published in 1968 [1]. If the term “pose” is included, 1050 papers are identified; 279 of these were published after 2020, while the earliest is [2]. Adding the term “LiDAR” reduces the set to 452, of which 167 have been published since 2020, with the earliest again dating from 1981 [3]. These, and the many other papers that use alternative but equivalent terminology, span a variety of approaches and applications, from terrestrial [4,5,6], to aeronautical [7,8,9,10,11], space [12,13,14,15]. In some instances, the search terms appear in reference to the technology being used for a specific application, e.g., precision agriculture [16,17,18,19] and construction [20,21]. In others, reference is made to implementation and algorithms. These are described at various levels of detail, but often with ambiguity.

Methods for stabilising pose estimates with LiDAR measurements exist and generally fall into two categories. The first are feature-based methods which seek to identify features, sometimes known as landmarks, in LiDAR point clouds and maintain an estimate of their location. Features can be lines or planes [22,23,24], curves [25,26], or corners [27]. Feature-based methods can also match sequential LiDAR scans, solving for the incremental change in pose between scans [28,29,30,31,32].

An alternative approach uses model-based methods. These generally compare measurements from a ranging sensor with a model of the environment, searching for a pose solution that provides an optimal solution.

Ref. [33] propose an airborne solution for aiding a navigation system through the use of a large-scale terrain model and LiDAR sensors. Similarly, Ref. [34] augment pose estimates by fitting LiDAR measurements against a known two-dimensional environment model. Refs. [35,36] show how pose estimates of known objects can be obtained from only LiDAR scans by finding the pose that is most likely among the set of all possible poses.

Other work has focused on the augmentation of pose when GNSS measurements are degraded or unavailable, such as loss of real-time kinematic (RTK) corrections. Ref. [37] develop a solution that uses a known geometric model of the world and that augments GNSS measurements with a pseudo-measurement of the expected height of the receiver. Ref. [38] use a conceptually similar approach, but apply it to aerial vehicles.

Work on model-based methods has also focused on building an environment model simultaneously with localisation within this model. One solution, presented by [39], develops an algorithm for a LiDAR- and GNSS-aided IMU navigation system for use in an airborne platform that only maps the terrain when GNSS and IMU measurements are available and reliable. When the integrity of the GNSS-aided IMU navigation system is compromised, LiDAR observations are fitted to the generated terrain model through an ICP-based method. The authors find that the solution is robust and improves the accuracy of the pose when the GNSS system is in a degraded state.

Ref. [40] present a method that augments a GNSS-aided IMU and odometry navigation system with a scanning LiDAR sensor. The authors estimate the environment using the LiDAR scanner as they move through an urban scene, and remove dynamic obstacles by compressing this model to a ground plane, coloured based on infrared reflectivity from LiDAR return intensities. They identify that their novel navigation system is up to an order of magnitude more accurate than a GNSS-aided IMU and odometry navigation system.

Our rationale for writing this paper is twofold. First, we believe the approach we propose for the integration of LiDAR measurement into a GNSS-aided IMU solution is novel. Second, we have repeatedly sought clear descriptions of the algorithms others have used, only to find missing details or gaps that have frustrated our implementation efforts. Here, we attempt to give a full and complete description of a loosely coupled GNSS-aided IMU navigation system in the expectation that it will be useful to others.

2. Experimental Platform

Figure 1 shows the experimental platform considered in this paper. The machine is called a track-type tractor or bulldozer and has applications in agriculture, construction, and mining. In all these domains, tracking spatial pose is needed for technology applications that include production reporting, guidance, and automation.

Figure 1 shows the coordinate frames in which variables relevant to the development of the pose estimation system are represented. The global frame is denoted as G. Frame P describes the pose of the tractor in Frame G by a transformation $T_P\left(·,·\right)$. Sensors are described relative to Frame P: Frame I describes the position and orientation of the IMU; Frame L describes the position and orientation of a LiDAR; and $G_1$ and $G_2$ are the centres of the antennas of two GNSS receivers mounted on the tractor cabin.

The pose of the tractor is defined by the state vector,

$$\mathit{x}={\left[\begin{array}{ccccccc}{\mathit{p}}_P& {\dot{\mathit{p}}}_P& {\ddot{\mathit{p}}}_P& {\mathsf{\Phi }}_P& {\mathbf{\omega }}_P& {\mathit{b}}_a& {\mathit{b}}_r\end{array}\right]}^T.$$

(1)

The 21 state parameters of the state vector are intended to accommodate a constant acceleration model for translation and a constant rate model for attitude. Here,

• ${\mathit{p}}_P={\left[x,y,z\right]}^T$ is the coordinate of the origin of P in G;
• ${\dot{\mathit{p}}}_P={\left[\dot{x},\dot{y},\dot{z}\right]}^T$ is the velocity of point ${\mathit{p}}_P$ in G;
• ${\ddot{\mathit{p}}}_P={\left[\ddot{x},\ddot{y},\ddot{z}\right]}^T$ is the acceleration of point ${\mathit{p}}_P$ in G;
• ${\mathsf{\Phi }}_P={\left[\theta ,\varphi ,\psi \right]}^T$ describes the attitude of Frame P in G as $roll$, $pitch$ and $yaw$;
• ${\mathbf{\omega }}_P={\left[{\omega }_x,{\omega }_y,{\omega }_z\right]}^T$ describes the rotational velocity of Frame P in G;
• ${\mathit{b}}_a={\left[b_{\ddot{x}},b_{\ddot{y}},b_{\ddot{z}}\right]}^T$ describes the acceleration bias of the IMU in Frame I;
• ${\mathit{b}}_r={\left[b_{{\omega }_x},b_{{\omega }_y},b_{{\omega }_z}\right]}^T$ is the rotational rate bias of the IMU in Frame I.

The transformation matrix $T_P\left({\mathsf{\Phi }}_P,{\mathit{p}}_P\right)$ is given by

$$T_P\left({\mathsf{\Phi }}_P,{\mathit{p}}_P\right)=\left[\begin{array}{cc}{R}_P\left({\mathsf{\Phi }}_P\right)& {\mathit{p}}_P\\ 0_{1\times 3}& 1\end{array}\right],$$

(2)

where

$$R_P\left({\mathsf{\Phi }}_P\right)=\left[\begin{array}{ccc}\mathrm{c}\left(\psi \right)\mathrm{c}\left(\varphi \right)& \mathrm{c}\left(\psi \right)\mathrm{s}\left(\varphi \right)\mathrm{s}\left(\theta \right)-\mathrm{s}\left(\psi \right)\mathrm{c}\left(\theta \right)& \mathrm{c}\left(\psi \right)\mathrm{s}\left(\varphi \right)\mathrm{c}\left(\theta \right)+\mathrm{s}\left(\psi \right)\mathrm{s}\left(\theta \right)\\ \mathrm{s}\left(\psi \right)\mathrm{c}\left(\varphi \right)& \mathrm{s}\left(\psi \right)\mathrm{s}\left(\varphi \right)\mathrm{s}\left(\theta \right)+\mathrm{c}\left(\psi \right)\mathrm{c}\left(\theta \right)& \mathrm{s}\left(\psi \right)\mathrm{s}\left(\varphi \right)\mathrm{s}\left(\theta \right)-\mathrm{c}\left(\psi \right)\mathrm{s}\left(\theta \right)\\ -\mathrm{s}\left(\varphi \right)& \mathrm{c}\left(\varphi \right)\mathrm{s}\left(\theta \right)& \mathrm{c}\left(\varphi \right)\mathrm{c}\left(\theta \right)\end{array}\right].$$

(3)

Here, $c\left(·\right)$ is the cosine function and $s\left(·\right)$ is the sine function. Note, $T_P\left(·,·\right)$ transforms points in Frame P into Frame G; equally, it describes the location of Frame P in Frame G. The top left $3\times 3$ block of the $T_P\left(·,·\right)$ matrix is the rotation matrix which aligns Frame P in Frame G.

The IMU provides measurements at 50 $\mathrm{Hz}$; the GNSS receivers are RTK-enabled and provide observations at 10 $\mathrm{Hz}$. The LiDAR is a Velodyne VLP-16 and provides range measurements from 16 beams, equally spaced across a field of view of 30${}^{\circ }$ with 360${}^{\circ }$ and rotation at 10 $\mathrm{Hz}$. Other details of the sensors are given in

Table 1. Specifications of the LiDAR, IMU, and GNSS sensors used as inputs to the navigation system of this paper.

Sensors
Velodyne VLP-16 LiDAR Sensor [41]		IMU		GNSS Receivers
		Accelerometers
Number of rays:	16	Axes:	3	x uncertainty ($1\sigma$):	$0.03$$\mathrm{m}$
Range uncertainty ($1\sigma$):	$0.03$$\mathrm{m}$	Data Rate:	50 $\mathrm{Hz}$	y uncertainty ($1\sigma$):	$0.03$$\mathrm{m}$
Horizontal field of view:	360${}^{\circ }$			z uncertainty ($1\sigma$):	$0.055$$\mathrm{m}$
Vertical field of view:	30${}^{\circ }$ (± 15${}^{\circ }$)			Signals tracked:	L1, L2, L5
Horizontal resolution:	$0.1$${}^{\circ }$– $0.4$${}^{\circ }$	Gyroscopes		Data rate:	10 $\mathrm{Hz}$
Vertical resolution:	2${}^{\circ }$	Axes:	3	Corrections:	RTK
Scan rate:	5–20 Hz	Data Rate:	50 Hz
Points per second:	300,000

.

3. Pose Estimation Using GNSS, IMU, and LiDAR Measurements

The pose estimator has the structure given in Figure 2. Each block corresponds to a Kalman filter update. On receipt of a sensor measurement, a filter update occurs that depends on the type of the measurement (IMU, GNSS, LiDAR) with the structure designed to accommodate different data rates, including, prospectively, sequences of lost measurements. The structure takes advantage of the high-frequency IMU measurements, updated through an indirect Kalman filter, and the low-frequency GNSS and LiDAR measurements, which are updated through Kalman and Information filters, respectively. The structure depicted in Figure 2 is generally known as an indirect, or error-state, Kalman filter. It is the common method for implementing IMU-aided navigation solutions [42,43,44,45,46,47,48,49,50]. The formulation below assumes that IMU data arrive at a higher rate than either GNSS or LiDAR measurements.

Algorithm 1 gives the algorithm based on updates from the IMU, GNSS, and LiDAR measurements.

Algorithm 1: LiDAR- and GNSS aided IMU navigation solution algorithm.

4. Process Model

The three filter updates of Figure 2 share the same process model which is assumed linear having the discrete-time form

$${\mathit{x}}_{k+1}=F\left(t_{k+1}-t_k\right)·{\mathit{x}}_k+\left[{\int }_{t_k}^{t_{k+1}}F\left(t_{k+1}-\tau \right)\mathit{d}\mathsf{\beta }\left(\tau \right)\right].$$

(4)

The state transition model $F\left(\mathsf{\Delta }{t}_{k+1}\right)=F\left(t_{k+1}-t_k\right)$ predicts the platform’s state vector assuming a constant acceleration for translations and a constant rate for rotations. The state transition model also predicts the IMU acceleration and rotational rate biases through use of constants determined from experimental observations (see [51]). The $21\times 21$ matrix $F\left(·\right)$ has the form

$$F\left(\mathsf{\Delta }{t}_k\right)=\left[\begin{array}{ccccccc}{I}_{3\times 3}& \mathsf{\Delta }{t}_k·I_{3\times 3}& {\displaystyle \frac{\mathsf{\Delta }{t}_k^2}{2}}·I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& I_{3\times 3}& \mathsf{\Delta }{t}_k·I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& \mathsf{\Delta }{t}_k·I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& {\displaystyle \frac{T_{b_{\ddot{x},\ddot{y},\ddot{z}}}}{T_{b_{\ddot{x},\ddot{y},\ddot{z}}}+\mathsf{\Delta }{t}_k}}·I_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& {\displaystyle \frac{T_{b_{{\omega }_x,{\omega }_y,{\omega }_z}}}{T_{b_{{\omega }_x,{\omega }_y,{\omega }_z}}+\mathsf{\Delta }{t}_k}}·I_{3\times 3}\end{array}\right].$$

(5)

Here, $T_{b_{\ddot{x},\ddot{y},\ddot{z}}}$ describes the empirically determined accelerometer bias parameters, and $T_{b_{{\omega }_x,{\omega }_y,{\omega }_z}}$ the empirically determined gyroscope bias parameters. Observe that

$$\underset{\mathsf{\Delta }{t}_k\to 0}{lim}F\left(\mathsf{\Delta }t\right)=I_{21\times 21}.$$

(6)

The second term of the right-hand side of Equation (4) describes the process noise which accounts for modelling approximations and model integration errors. We define

$$\mathit{w}\left(t_k\right)=\left[{\int }_{t_k}^{t_{k+1}}F\left(t_{k+1}-\tau \right)\mathit{d}\mathsf{\beta }\left(\tau \right)\right],$$

(7)

where $\mathit{w}\left(·\right)$ is a white Gaussian discrete-time process and $\mathsf{\beta }\left(·\right)$ is Brownian motion with diffusion ${\mathsf{\sigma }}_Q^2\left(t\right)$ such that for consecutive measurement times $t_k$ and $t_{k+1}$,

$$\begin{array}{cc}\hfill E\left[\mathsf{\beta }\left(t_k\right)-\mathsf{\beta }\left(t_{k+1}\right)\right]& =0,\hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill E\left[\left\{\mathsf{\beta }\left(t_k\right)-\mathsf{\beta }\left(t_{k+1}\right)\right\}{\left\{\mathsf{\beta }\left(t_k\right)-\mathsf{\beta }\left(t_{k+1}\right)\right\}}^T\right]& ={\int }_{t_k}^{t_{k+1}}{\mathsf{\sigma }}_Q^2\left(t\right)dt.\hfill \end{array}$$

(9)

If the sample period is assumed to be small compared to the auto-correlation times of the process model modes,

$${\mathsf{\sigma }}_{Q,k+1}^2=E\left[\mathit{w}\left(t_k\right){\mathit{w}}^T\left(t_k\right)\right]\approx {\mathsf{\sigma }}_Q^2\left(t_k\right)·\left[t_{k+1}-t_k\right].$$

(10)

Observe that if the diffusion ${\mathsf{\sigma }}_Q^2\left(t\right)$ is time-invariant, the process noise covariance, ${\mathsf{\sigma }}^2$, increases linearly from the time since the last measurement. As this time increases, the process noise increases, resulting in the process model being weighted less in the Kalman filter update and measurements being weighted more.

5. Pose State Updates from IMU Measurements

5.1. IMU Measurement Model

The IMU is installed on the platform frame close to the platform’s centre of gravity and is defined by Frame I—see Figure 3. IMU measurements are made in an inertial frame instantaneously aligned with Frame I. The IMU measurement model follows from Equation (3) and takes the form

$${\mathit{z}}_I={\mathit{h}}_I\left(\mathit{x}\right)+{\mathit{v}}_{\mathit{z},I},$$

(11)

where

$${\mathit{z}}_I={\left[\begin{array}{cc}{\mathit{a}}_I& {\mathbf{\omega }}_I\end{array}\right]}^T={\left[\begin{array}{cccccc}{\ddot{x}}_I& {\ddot{y}}_I& {\ddot{z}}_I& {\dot{\theta }}_I& {\dot{\varphi }}_I& {\dot{\psi }}_I\end{array}\right]}^T$$

(12)

and measurement noise ${\mathit{v}}_{\mathit{z},I}$ is assumed to be white and Gaussian with covariance ${\mathsf{\sigma }}_{\mathit{z},I}^2$—see

Table 2. The diagonal elements of the IMU measurement covariance matrix. The cross-terms are several orders of magnitude smaller than the diagonal element. These values were determined experimentally from statistical analysis of a stationary data segment of 930 s.

IMU Covariance, ${\mathit{\sigma }}_{\mathit{z},\mathit{I}}^2$
${\ddot{x}}_I{\left[\frac{\mathrm{m}}{{\mathrm{s}}^2}\right]}^2$	${\ddot{y}}_I{\left[\frac{\mathrm{m}}{{\mathrm{s}}^2}\right]}^2$	${\ddot{z}}_I{\left[\frac{\mathrm{m}}{{\mathrm{s}}^2}\right]}^2$	${\dot{\theta }}_I{\left[\frac{{}^{\circ }}{{\mathrm{s}}^2}\right]}^2$	${\dot{\varphi }}_I{\left[\frac{{}^{\circ }}{{\mathrm{s}}^2}\right]}^2$	${\dot{\psi }}_I{\left[\frac{{}^{\circ }}{{\mathrm{s}}^2}\right]}^2$
$7.8\times {10}^{-5}$	$3.9\times {10}^{-5}$	$4.0\times {10}^{-4}$	$7.6\times {10}^{-3}$	$7.6\times {10}^{-3}$	$1.4\times {10}^{-2}$

.

To determine the function ${\mathit{h}}_I\left(·\right)$, note that the position of the origin of the IMU in the global frame is given by

$$\left[\begin{array}{c}{\mathit{p}}_I\left(t\right)\\ 1\end{array}\right]=\left[\begin{array}{cc}{R}_P\left(t\right)& {\mathit{p}}_P\left(t\right)\\ 0_{1\times 3}& 1\end{array}\right]\left[\begin{array}{c}{\mathit{p}}_i\\ 1\end{array}\right],$$

(13)

where ${\mathit{p}}_i$ is the (fixed and known) position of the origin of Frame I in Frame P. The velocity of this point at time t is

$$\left[\begin{array}{c}{\dot{\mathit{p}}}_I\left(t\right)\\ 0\end{array}\right]=\left[\begin{array}{cc}{\dot{R}}_P\left(t\right)& {\dot{\mathit{p}}}_P\left(t\right)\\ 0_{1\times 3}& 0\end{array}\right]\left[\begin{array}{c}{\mathit{p}}_i\\ 1\end{array}\right]=\left[\begin{array}{cc}{\mathsf{\Omega }}_P\left(t\right)& {\mathit{v}}_P\left(t\right)\\ 0_{1\times 3}& 1\end{array}\right]\left[\begin{array}{c}{\mathit{p}}_I\left(t\right)\\ 1\end{array}\right],$$

(14)

where ${\mathsf{\Omega }}_P\left(t\right)$ is the anti-symmetric matrix representing the angular velocity of Frame P in Frame G. The matrix ${\mathsf{\Omega }}_P\left(t\right)$ is

$${\mathsf{\Omega }}_P=\mathrm{sk}\left({\mathbf{\omega }}_P\right)=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right].$$

(15)

The acceleration of the origin of Frame I in Frame G is found by differentiating Equation (14), i.e.,

$$\left[\begin{array}{c}{\ddot{\mathit{p}}}_I\\ 0\end{array}\right]=\left[\begin{array}{cc}{\ddot{R}}_P\left(t\right)& {\ddot{\mathit{p}}}_P\left(t\right)\\ 0_{1\times 3}& 0\end{array}\right]\left[\begin{array}{c}{\mathit{p}}_i\\ 1\end{array}\right].$$

(16)

Noting that if a constant angular rate is assumed (i.e., ${\dot{\mathsf{\Omega }}}_P=0$),

$${\ddot{R}}_P\left(t\right)={\dot{\mathsf{\Omega }}}_P\left(t\right)·R_P\left(t\right)+{\mathsf{\Omega }}_P\left(t\right)·{\dot{R}}_P\left(t\right)={\dot{\mathsf{\Omega }}}_P\left(t\right)·R_P\left(t\right)+{\mathsf{\Omega }}_P^2\left(t\right)·R_P\left(t\right)={\mathsf{\Omega }}_P^2\left(t\right)·R_P\left(t\right),$$

(17)

which gives,

$$\left[\begin{array}{c}{\ddot{\mathit{p}}}_I\left(t\right)\\ 0\end{array}\right]=\left[\begin{array}{cc}{\mathsf{\Omega }}_P^2\left(t\right)·R_P\left(t\right)& {\ddot{\mathit{p}}}_P\left(t\right)\\ 0_{1\times 3}& 0\end{array}\right]\left[\begin{array}{c}{\mathit{p}}_i\left(t\right)\\ 1\end{array}\right].$$

(18)

The measurement equations follow from Equation (18) and can be expressed in terms of the pose state vector:

$${\mathit{z}}_I={\mathit{h}}_I\left(\mathit{x}\right)=\left[\begin{array}{c}{\mathit{a}}_I\\ {\mathbf{\omega }}_I\end{array}\right]=R_I^T·R_P^T\left({\mathsf{\Phi }}_P\right)·\left[\begin{array}{c}{\mathsf{\Omega }}_P^2\left({\mathbf{\omega }}_P\right)·R_P\left({\mathsf{\Phi }}_P\right)·{\mathit{p}}_i+{\ddot{\mathit{p}}}_P+\mathit{g}\\ {\mathbf{\omega }}_P\end{array}\right]+\left[\begin{array}{c}{\mathit{b}}_a\\ {\mathit{b}}_r\end{array}\right].$$

(19)

Note that the rotation matrix $R_I^T·R_P^T\left(·\right)$ serves to align the acceleration and rate vectors to Frame I in which measurements are made. The vector ${\mathit{b}}_r$ is the bias of rate gyro, ${\mathit{b}}_a$ is the accelerometer bias, and $\mathit{g}$ denotes gravitational acceleration.

The Jacobian of $\mathit{h}\left(·\right)$ can be found analytically from

$$\nabla {\mathit{h}}_I\left(\mathit{x}\right)=\left[\begin{array}{ccccccc}{0}_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& M_1& M_2& I_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& R_I^T·{\displaystyle \frac{\partial R_P\left({\mathsf{\Phi }}_P\right)}{\partial {\mathsf{\Phi }}_P}}·{\mathbf{\omega }}_P& R_I^T·R_P^T\left({\mathsf{\Phi }}_P\right)& 0_{3\times 3}& I_{3\times 3}\end{array}\right],$$

(20)

where

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill M_1& =R_I^T·{\displaystyle \frac{\partial R_P^T\left({\mathsf{\Phi }}_P\right)}{\partial {\mathsf{\Phi }}_P}}·\left({\mathsf{\Omega }}_P^2\left({\mathbf{\omega }}_P\right)·R_P\left({\mathsf{\Phi }}_P\right)·{\mathit{p}}_i+{\ddot{\mathit{p}}}_P+\mathit{g}\right)\hfill \\ \hfill & +R_I^T·R_P^T\left({\mathsf{\Phi }}_P\right)·{\mathsf{\Omega }}_P^2\left({\mathbf{\omega }}_P\right)·{\displaystyle \frac{\partial R_P\left({\mathsf{\Phi }}_P\right)}{\partial {\mathsf{\Phi }}_P}}·{\mathit{p}}_i,\hfill \end{array}\end{array}$$

(21)

$$\begin{array}{cc}\hfill M_2& =R_I^T·R_P^T\left({\mathsf{\Phi }}_P\right)·{\displaystyle \frac{\partial {\mathsf{\Omega }}_P^2\left({\mathbf{\omega }}_P\right)}{\partial {\mathbf{\omega }}_P}}·R_P\left({\mathsf{\Phi }}_P\right)·{\mathit{p}}_i.\hfill \end{array}$$

(22)

5.2. IMU Updates

The approach to updating measurements from the IMU involves estimating the error on the state estimate, $\delta {\widehat{\mathit{x}}}_{k\mid k}^{\prime }$, at time $t_k$ given measurements to time $t_k$, and combining this with the propagated state estimate, ${\widehat{\mathit{x}}}_{k\mid k}^{\prime }$, from the most recent GNSS or LiDAR update. The state estimate update, ${\widehat{\mathit{x}}}_{k\mid k}$, is then computed as the sum,

$${\widehat{\mathit{x}}}_{k\mid k}={\widehat{\mathit{x}}}_{k\mid k}^{\prime }+\delta {\widehat{\mathit{x}}}_{k\mid k}^{\prime }.$$

(23)

The IMU filter is reset after each GNSS or LiDAR update: state estimate and state estimate covariance are set from the error-corrected state estimate and error-corrected state estimate covariance,

$$\begin{array}{cc}\hfill {\widehat{\mathit{x}}}_{k-1\mid k-1}^{\prime }& ={\widehat{\mathit{x}}}_{k-1\mid k-1},\hfill \end{array}$$

(24)

$$\begin{array}{cc}\hfill {\mathsf{\sigma }}_{{\widehat{\mathit{x}}}^{\prime },k-1\mid k-1}^2& ={\mathsf{\sigma }}_{\widehat{\mathit{x}},k-1\mid k-1}^2,\hfill \end{array}$$

(25)

and the error state estimate and error state estimate covariance are initialised to zero:

$$\begin{array}{cc}\hfill \delta {\widehat{\mathit{x}}}_{k-1\mid k-1}^{\prime }& ={\mathbf{0}}_{21\times 1},\hfill \end{array}$$

(26)

$$\begin{array}{cc}\hfill {\mathsf{\sigma }}_{\delta {\widehat{\mathit{x}}}^{\prime },k-1\mid k-1}^2& =0_{21\times 21}.\hfill \end{array}$$

(27)

On receipt of an IMU measurement, the calculation of $\delta {\widehat{\mathit{x}}}_{k\mid k}$ starts with the prediction of the state estimate,

$${\widehat{\mathit{x}}}_{k\mid k-1}^{\prime }=F\left(\mathsf{\Delta }{t}_k\right)·{\widehat{\mathit{x}}}_{k-1\mid k-1}^{\prime },$$

(28)

the error state,

$$\delta {\widehat{\mathit{x}}}_{k\mid k-1}^{\prime }=F\left(\mathsf{\Delta }{t}_k\right)·\delta {\widehat{\mathit{x}}}_{k-1\mid k-1}^{\prime },$$

(29)

state covariance,

$${\mathsf{\sigma }}_{{\widehat{\mathit{x}}}^{\prime },k\mid k-1}^2=F\left(\mathsf{\Delta }{t}_k\right)·{\mathsf{\sigma }}_{{\widehat{\mathit{x}}}^{\prime },k-1\mid k-1}^2·F{\left(\mathsf{\Delta }{t}_k\right)}^T.$$

(30)

and error state covariance,

$${\mathsf{\sigma }}_{\delta {\widehat{\mathit{x}}}^{\prime },k\mid k-1}^2=F\left(\mathsf{\Delta }{t}_k\right)·{\mathsf{\sigma }}_{\delta {\widehat{\mathit{x}}}^{\prime },k-1\mid k-1}^2·F{\left(\mathsf{\Delta }{t}_k\right)}^T+{\mathsf{\sigma }}_{Q,k}^2.$$

(31)

The innovation is computed from

$${\mathit{\nu }}_{I,k}={\mathit{z}}_{I,k}-\left({\mathit{h}}_I\left({\widehat{\mathit{x}}}_{k\mid k-1}^{\prime }\right)+\nabla {\mathit{h}}_I\left({\widehat{\mathit{x}}}_{k\mid k-1}^{\prime }\right)·\delta {\widehat{\mathit{x}}}_{k\mid k-1}^{\prime }\right)={\mathit{z}}_{I,k}-\left({\widehat{\mathit{z}}}_{I,{\widehat{\mathit{x}}}_{k\mid k-1}^{\prime }}+{\widehat{\mathit{z}}}_{I,\delta {\widehat{\mathit{x}}}_{k\mid k-1}^{\prime }}\right),$$

(32)

and the innovation covariance, ${\mathsf{\sigma }}_{\mathit{\nu },k}^2$, and Kalman gain, $W_{I,k}$, are found from

$$\begin{array}{cc}\hfill {\mathsf{\sigma }}_{\mathit{\nu },I,k}^2& =\nabla {\mathit{h}}_I\left({\widehat{\mathit{x}}}_{k\mid k-1}^{\prime }\right)·{\mathsf{\sigma }}_{\delta {\widehat{\mathit{x}}}^{\prime },k\mid k-1}^2·\nabla {\mathit{h}}_I{\left({\widehat{\mathit{x}}}_{k\mid k-1}^{\prime }\right)}^T+{\mathsf{\sigma }}_{\mathit{z},I}^2,\hfill \end{array}$$

(33)

$$\begin{array}{cc}\hfill W_{I,k}& ={\mathsf{\sigma }}_{\delta {\widehat{\mathit{x}}}^{\prime },k\mid k-1}^2·\nabla {\mathit{h}}_I{\left({\widehat{\mathit{x}}}_{k\mid k-1}^{\prime }\right)}^T·{\left({\mathsf{\sigma }}_{\mathit{\nu },I,k}^2\right)}^{-1}.\hfill \end{array}$$

(34)

The corrected error state and the corrected error state covariance are computed from

$$\begin{array}{cc}\hfill \delta {\widehat{\mathit{x}}}_{k\mid k}^{\prime }& =\delta {\widehat{\mathit{x}}}_{k\mid k-1}^{\prime }+W_{I,k}·{\mathit{\nu }}_{I,k},\hfill \end{array}$$

(35)

$$\begin{array}{cc}\hfill {\mathsf{\sigma }}_{\delta {\widehat{\mathit{x}}}^{\prime },k\mid k}^2& ={\mathsf{\sigma }}_{\delta {\widehat{\mathit{x}}}^{\prime },k\mid k-1}^2-W_{I,k}·{\mathsf{\sigma }}_{\mathit{\nu },I,k}^2·W_{I,k}^T.\hfill \end{array}$$

(36)

The predicted but uncorrected state estimate, ${\widehat{\mathit{x}}}_{k\mid k-1}$, and state estimate covariance, ${\mathsf{\sigma }}_{\widehat{\mathit{x}},k\mid k-1}^2$, are used in the next iteration of the filter,

$$\begin{array}{cc}\hfill {\widehat{\mathit{x}}}_{k\mid k}^{\prime }& ={\widehat{\mathit{x}}}_{k\mid k-1}^{\prime },\hfill \end{array}$$

(37)

$$\begin{array}{cc}\hfill {\mathsf{\sigma }}_{{\widehat{\mathit{x}}}^{\prime },k\mid k}^2& ={\mathsf{\sigma }}_{{\widehat{\mathit{x}}}^{\prime },k\mid k-1}^2.\hfill \end{array}$$

(38)

The error-corrected state estimate, ${\widehat{\mathit{x}}}_{k\mid k}$, which incorporates the newest IMU observation, is computed through the addition with the error state estimate,

$$\begin{array}{cc}\hfill {\widehat{\mathit{x}}}_{k\mid k}& ={\widehat{\mathit{x}}}_{k\mid k}^{\prime }+\delta {\widehat{\mathit{x}}}_{k\mid k}^{\prime },\hfill \end{array}$$

(39)

$$\begin{array}{cc}\hfill {\mathsf{\sigma }}_{\widehat{\mathit{x}},k\mid k}^2& ={\mathsf{\sigma }}_{{\widehat{\mathit{x}}}^{\prime },k\mid k}^2+{\mathsf{\sigma }}_{\delta {\widehat{\mathit{x}}}^{\prime },k\mid k}^2.\hfill \end{array}$$

(40)

The indirect extended Kalman filter (IEKF) algorithm for IMU updates is detailed in Algorithm 2.    

Algorithm 2: IMUUpdate - An Indirect Extended Kalman Filter (IEKF) algorithm for state estimation using IMU observations.

6. Pose State Updates from GNSS Measurements

6.1. GNSS Measurement Model

The GNSS measurement model gives the locations of the GNSS receivers in the global frame and takes the form

$${\mathit{z}}_G={\mathit{h}}_G\left(\mathit{x}\right)+{\mathit{v}}_{\mathit{z},G},$$

(41)

Here, ${\mathit{h}}_G\left(·\right)$ gives the locations of the GNSS receivers as a function of the state,

$${\mathit{z}}_G={\mathit{h}}_G\left(\mathit{x}\right)=\left[\begin{array}{c}{R}_P\left({\mathsf{\Phi }}_P\right)·{\mathit{p}}_{G1}+{\mathit{p}}_P\\ R_P\left({\mathsf{\Phi }}_P\right)·{\mathit{p}}_{G2}+{\mathit{p}}_P\end{array}\right],$$

(42)

where ${\mathit{p}}_{G1}$ and ${\mathit{p}}_{G2}$ are the coordinates of the GNSS antennas in the platform frame (see Figure 4). The measurement vector ${\mathit{z}}_G$ is the stacked vectors of the antennas’ Cartesian coordinates in Frame G. Equation (42) can be readily extended if a third GNSS receiver were mounted to the platform. The measurement noise, ${\mathit{v}}_{\mathit{z},G}$, is assumed to be white and Gaussian with covariance ${\mathsf{\sigma }}_{\mathit{z},G}^2$ (see

Table 3. The diagonal elements of the GNSS measurement covariance matrix. The off-diagonal terms are several orders of magnitude smaller than the diagonal elements. These values were determined from statistical analysis of a 930 s measurement sequence where the platform was stationary.

GNSS Covariance, ${\mathit{\sigma }}_{\mathit{z},\mathit{G}}^2$
$x_G{\left[\mathrm{m}\right]}^2$	$y_G{\left[\mathrm{m}\right]}^2$	$z_G{\left[\mathrm{m}\right]}^2$
$1.9\times {10}^{-6}$	$2.4\times {10}^{-6}$	$2.2\times {10}^{-5}$

).

The GNSS measurement Jacobian is given by

$$\nabla {\mathit{h}}_G\left(\mathit{x}\right)=\left[\begin{array}{ccccccc}{I}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& {\displaystyle \frac{\partial R_P^T\left({\mathsf{\Phi }}_P\right)}{\partial {\mathsf{\Phi }}_P}}·{\mathit{p}}_{G1}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& {\displaystyle \frac{\partial R_P^T\left({\mathsf{\Phi }}_P\right)}{\partial {\mathsf{\Phi }}_P}}·{\mathit{p}}_{G2}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\end{array}\right].$$

(43)

6.2. GNSS Updates

The estimation of the error-corrected state estimate, ${\widehat{\mathit{x}}}_{k\mid k}$, begins with a prediction of state using a state transition model, $F\left(\mathsf{\Delta }{t}_k\right)$, and the previous error-corrected state estimate, ${\widehat{\mathit{x}}}_{k-1\mid k-1}$,

$${\widehat{\mathit{x}}}_{k\mid k-1}=F\left(\mathsf{\Delta }{t}_k\right)·\left({\widehat{\mathit{x}}}_{k-1\mid k-1}\right),$$

(44)

and the prediction of the state covariance,

$${\mathsf{\sigma }}_{\widehat{\mathit{x}},k\mid k-1}^2=F\left(\mathsf{\Delta }{t}_k\right)·{\mathsf{\sigma }}_{\widehat{\mathit{x}},k-1\mid k-1}^2·F{\left(\mathsf{\Delta }{t}_k\right)}^T+{\mathsf{\sigma }}_{Q,k}^2.$$

(45)

The GNSS measurement innovation and innovation covariance are computed from

$$\begin{array}{cc}\hfill {\mathit{\nu }}_{G,k}& ={\mathit{z}}_{G,k}-{\mathit{h}}_G\left({\widehat{\mathit{x}}}_{k\mid k-1}\right)={\mathit{z}}_{G,k}-{\widehat{\mathit{z}}}_{G,k},\hfill \end{array}$$

(46)

$$\begin{array}{cc}\hfill {\mathsf{\sigma }}_{\mathit{\nu },G,k}^2& =\nabla {\mathit{h}}_G\left({\widehat{\mathit{x}}}_{k\mid k-1}\right)·{\mathsf{\sigma }}_{\widehat{\mathit{x}},k\mid k-1}^2·\nabla {\mathit{h}}_G{\left({\widehat{\mathit{x}}}_{k\mid k-1}\right)}^T+{\mathsf{\sigma }}_{\mathit{z},G}^2,\hfill \end{array}$$

(47)

where ${\mathit{z}}_{G,k}$ is the GNSS measurement.

The Kalman gain, $W_{G,k}$, is then computed using

$$W_{G,k}={\mathsf{\sigma }}_{\widehat{\mathit{x}},k\mid k-1}^2·\nabla {\mathit{h}}_G{\left({\widehat{\mathit{x}}}_{k\mid k-1}\right)}^T·{\left({\mathsf{\sigma }}_{\mathit{\nu },G,k}^2\right)}^{-1}.$$

(48)

The error-corrected state estimate, ${\widehat{\mathit{x}}}_{k\mid k}$, and error-corrected state estimate covariance, ${\mathsf{\sigma }}_{\widehat{\mathit{x}},k\mid k}^2$, are computed using

$$\begin{array}{cc}\hfill {\widehat{\mathit{x}}}_{k\mid k}& ={\widehat{\mathit{x}}}_{k\mid k-1}+W_{G,k}·{\mathit{\nu }}_{G,k},\hfill \end{array}$$

(49)

$$\begin{array}{cc}\hfill {\mathsf{\sigma }}_{\widehat{\mathit{x}},k\mid k}^2& ={\mathsf{\sigma }}_{\widehat{\mathit{x}},k\mid k-1}^2-W_{G,k}·{\mathsf{\sigma }}_{\mathit{\nu },G,k}^2·W_{G,k}^T.\hfill \end{array}$$

(50)

The GNSS update is given as Algorithm 3.    

Algorithm 3: GNSSUpdate—An extended Kalman filter (EKF) algorithm for state estimation using GNSS observations.

7. Pose State Updates Using LiDAR Measurements

7.1. LiDAR Measurement Model in a Known Environment

The LiDAR measurement model developed in this section computes the expected range measurements based on the state vector and a triangulated model of the environment, denoted X. The LiDAR measurement model ray-casts from the LiDAR to the environment model X to determine the expected measurement ranges from the LiDAR sensor. In this section, the terrain model X is assumed known; in Section 9, the approach is generalised to unknown terrain.

The LiDAR measurement function is denoted

$${\mathit{z}}_L={\mathit{h}}_L\left(\mathit{x},X\right)+{\mathit{v}}_{\mathit{z},L},$$

(51)

where the LiDAR measurement vector, ${\mathit{z}}_L$ is a set of ranges corresponding to each ray from the LiDAR sensor. For n ranges,

$${\mathit{z}}_L={\left[\begin{array}{cccc}{z}_0& z_1& \cdots & z_n\end{array}\right]}^T.$$

(52)

The measurement noise ${\mathit{v}}_{\mathit{z},L}$ is assumed to be white and Gaussian with covariance ${\mathsf{\sigma }}_{\mathit{z},L}^2$—see Section 7.2.

The LiDAR measurement function, ${\mathit{h}}_L(·,·)$, determines the expected sensor observations by ray-casting against a known terrain model given a pose state estimate, X (see Figure 5). Each ray emanates from a point ${\mathit{s}}_0$ in Frame L known as the sensor origin. The direction of each ray or beam of the LiDAR can be defined by a second point ${\mathit{s}}_j$, $j=1\dots n$ with $\left|\right|{\mathit{s}}_j-{\mathit{s}}_0\left|\right|=1$. The location of these points in Frame G can be found with transformations

$$\begin{array}{cc}\hfill {\mathit{q}}_0& =T_P\left({\mathsf{\Phi }}_P,{\mathit{p}}_P\right)·T_L·{\mathit{s}}_0,\hfill \end{array}$$

(53)

$$\begin{array}{cc}\hfill {\mathit{q}}_j& =T_P\left({\mathsf{\Phi }}_P,{\mathit{p}}_P\right)·T_L·{\mathit{s}}_j.\hfill \end{array}$$

(54)

Here, $T_L$ describes the location of Frame L in Frame P. This transform is assumed known. See [52] for approaches to finding it.

Each ray is parametrised by a range $r_j$ and can be described by

$$\mathsf{\Gamma }\left(r_j\right)={\mathit{q}}_0+r_j·\left({\mathit{q}}_j-{\mathit{q}}_0\right).$$

(55)

To find $r_j$, it is necessary to first determine the triangle in X which it intersects. Denote the vertices of this triangle, ${\mathsf{\Delta }}_j=\left({\mathbf{\alpha }}_j,{\mathsf{\beta }}_j,{\mathbf{\gamma }}_j\right)$. Then,

$$r_j={\displaystyle \frac{{\mathit{n}}_j·\left({\mathbf{\alpha }}_j-{\mathit{q}}_0\right)}{{\mathit{n}}_j·\left({\mathit{q}}_0-{\mathit{q}}_j\right)}},$$

(56)

where

$${\mathit{n}}_j=\left({\mathsf{\beta }}_j-{\mathbf{\alpha }}_j\right)\times \left({\mathbf{\gamma }}_j-{\mathbf{\alpha }}_j\right).$$

(57)

The ray-triangle intersection is depicted in Figure 6.

The measurement model for n ray intersections can be constructed from

$${\mathit{z}}_L={\left[r_1,r_2,\dots ,r_n\right]}^T,$$

(58)

and the LiDAR measurement Jacobian from

$$\nabla {\mathit{h}}_L\left(\mathit{x},X\right)=\left[\begin{array}{cccc}{\displaystyle \frac{\partial r_1}{\partial {\mathit{p}}_P}}& 0_{1\times 6}& {\displaystyle \frac{\partial r_1}{\partial {\mathsf{\Phi }}_P}}& 0_{1\times 9}\\ {\displaystyle \frac{\partial r_2}{\partial {\mathit{p}}_P}}& 0_{1\times 6}& {\displaystyle \frac{\partial r_2}{\partial {\mathsf{\Phi }}_P}}& 0_{1\times 9}\\ ⋮& ⋮& ⋮& ⋮\\ {\displaystyle \frac{\partial r_n}{\partial {\mathit{p}}_P}}& 0_{1\times 6}& {\displaystyle \frac{\partial r_n}{\partial {\mathsf{\Phi }}_P}}& 0_{1\times 9}\end{array}\right].$$

(59)

7.2. LiDAR Measurement Noise Model

Observations from the LiDAR sensor present in this application are transformed from the LiDAR frame, L, into the global frame, G. However, each of the transforms are required to achieve this contain uncertainty. For sensors such as the GNSS receivers and IMU, the uncertainty introduced by these transforms is small, as observations are obtained at the sensor. However, for perception sensors such as LiDAR, which perceive the environment at potentially significant ranges, small transform errors can lead to quite significant observation errors. For example, a registration error of 1${}^{\circ }$ at 50 $\mathrm{m}$ results in ∼ $0.7$ $\mathrm{m}$ of endpoint error.

The uncertainty of frame locations and measurements may be propagated from their origin to the point of measurement to determine the effective uncertainty. We consider four sources of uncertainty which we propagate from their origin to the LiDAR ray endpoint (shown in Figure 7). The sources of uncertainty considered are (i) the uncertainty in the global frame location, ${\mathsf{\sigma }}_G^2$; (ii) the estimated location of the platform relative to the global frame, ${\mathsf{\sigma }}_P^2$; (iii) the uncertainty in the LiDAR registration, ${\mathsf{\sigma }}_L^2$; and (iv) the LiDAR’s measurement uncertainty, ${\mathsf{\sigma }}_{\mathit{z},L}^2$. The Jacobian of each of the relevant transforms is used to transform the uncertainty through the system.

The uncertainty from each of these sources propagates through to the endpoint of the ray as follows:

$$\begin{array}{cc}\hfill {\mathsf{\sigma }}_{P,k}^2& =\nabla T_P\left({\mathsf{\Phi }}_P,{\mathit{p}}_P\right)·{\mathsf{\sigma }}_G^2·\nabla T_P{\left({\mathsf{\Phi }}_P,{\mathit{p}}_P\right)}^T+{\mathsf{\sigma }}_P^2,\hfill \end{array}$$

(60)

$$\begin{array}{cc}\hfill {\mathsf{\sigma }}_{L,k}^2& =\nabla T_L·{\mathsf{\sigma }}_{P,k}^2·\nabla T_L^T+{\mathsf{\sigma }}_L^2,\hfill \end{array}$$

(61)

$$\begin{array}{cc}\hfill {\mathsf{\sigma }}_{z,L}^2& =\nabla T_R·{\mathsf{\sigma }}_{L,k}^2·\nabla T_R^T+{\mathsf{\sigma }}_{\mathit{z},L}^2.\hfill \end{array}$$

(62)

Here, the notation $\nabla T_L$ is used to denote the Jacobian of the homogeneous transform—in this case, the transform from the platform frame to the LiDAR frame.

Figure 7 provides a visual representation of the propagation of uncertainty from the global frame through to the endpoint of the ray. The covariances shown in Figure 7 have been exaggerated for visual clarity.

7.3. LiDAR Updates

The number of LiDAR measurements typically exceeds the dimension of the state vector so it is computationally more efficient to use the information form of the extended Kalman filter to perform the pose update from LiDAR measurements [53].

The current state and covariance are transformed in the information space

$$\begin{array}{cc}\hfill Y_{k-1\mid k-1}& ={\left({\mathsf{\sigma }}_{\widehat{\mathit{x}},k-1\mid k-1}^2\right)}^{-1},\hfill \end{array}$$

(63)

$$\begin{array}{cc}\hfill {\widehat{\mathit{y}}}_{k-1\mid k-1}& =Y_{k-1\mid k-1}·{\widehat{\mathit{x}}}_{k-1\mid k-1}.\hfill \end{array}$$

(64)

The Fisher information matrix (FIM) and Fisher information vector (FIV) are predicted to the current time step using

$$\begin{array}{cc}\hfill Y_{k\mid k-1}& ={\left[F\left(\mathsf{\Delta }{t}_k\right)·{\left(Y_{k-1\mid k-1}\right)}^{-1}·F{\left(\mathsf{\Delta }{t}_k\right)}^T+{\mathsf{\sigma }}_{Q,k}^2\right]}^{-1},\hfill \end{array}$$

(65)

$$\begin{array}{cc}\hfill {\widehat{\mathit{y}}}_{k\mid k-1}& =Y_{k\mid k-1}·F\left(\mathsf{\Delta }{t}_k\right)·{\widehat{\mathit{x}}}_{k-1\mid k-1}.\hfill \end{array}$$

(66)

The predicted state estimate is obtained from

$$\begin{array}{c}\hfill {\widehat{\mathit{x}}}_{k\mid k-1}={\left(Y_{k-1\mid k-1}\right)}^{-1}·{\widehat{\mathit{y}}}_{k\mid k-1}\end{array}$$

(67)

and the innovation is obtained using the LiDAR observation function, the current predicted state estimate, and the terrain model, X:

$$\begin{array}{c}\hfill {\mathit{\nu }}_{L,k}={\mathit{z}}_{L,k}-{\mathit{h}}_L\left({\widehat{\mathit{x}}}_{k\mid k-1},X\right).\end{array}$$

(68)

The predicted FIV and FIM are corrected through use of the innovation and the LiDAR range measurements using

$$\begin{array}{cc}\hfill {\widehat{\mathit{y}}}_{k\mid k}& ={\widehat{\mathit{y}}}_{k\mid k-1}+\nabla {\mathit{h}}_L{\left({\widehat{\mathit{x}}}_{k\mid k-1},X\right)}^T·{\left({\mathsf{\sigma }}_{\mathit{z},L}^2\right)}^{-1}·\left[{\mathit{\nu }}_{L,k}+\nabla {\mathit{h}}_L\left({\widehat{\mathit{x}}}_{k\mid k-1},X\right)·{\widehat{\mathit{x}}}_{k\mid k-1}\right],\hfill \end{array}$$

(69)

$$\begin{array}{cc}\hfill Y_{k\mid k}& =Y_{k\mid k-1}+\nabla {\mathit{h}}_L{\left({\widehat{\mathit{x}}}_{k\mid k-1},X\right)}^T·{\left({\mathsf{\sigma }}_{\mathit{z},L}^2\right)}^{-1}·\nabla {\mathit{h}}_L\left({\widehat{\mathit{x}}}_{k\mid k-1},X\right).\hfill \end{array}$$

(70)

The FIM and FIV are now transformed back into the state space,

$$\begin{array}{cc}\hfill {\widehat{\mathit{x}}}_{k\mid k}& ={\left(Y_{k\mid k}\right)}^{-1}·{\widehat{\mathit{y}}}_{k\mid k},\hfill \end{array}$$

(71)

$$\begin{array}{cc}\hfill {\mathsf{\sigma }}_{\widehat{x},k\mid k}^2& ={\left(Y_{k\mid k}\right)}^{-1}.\hfill \end{array}$$

(72)

Algorithm 4 shows the LiDAR pose estimation extended information filter (EIF) algorithmically.    

Algorithm 4: LiDARUpdate—An extended information filter (EIF) algorithm for state estimation using LiDAR observations in a known environment.

8. Evaluation

It is necessary to use indirect methods for the evaluation of the pose estimator as there no is ground truth pose trajectory. The approach chosen is to survey the environment to obtain a ground truth model and compare LiDAR measurements with the predicted ranges from ray-casting against this scanned ground truth model. These differences are representative of the error in the pose. The survey to obtain the ground truth model was created with a Faro Focus^3D terrestrial LiDAR scanner [54]. This instrument claims a one standard deviation range precision of $0.0011$ $\mathrm{m}$. Figure 8 shows the environment model obtained by scanning.

The scene used for evaluation is shown in Figure 9. It comprises a generally flat terrain with piled dirt, forming mounds. The bulldozer was manually moved around in this environment following the path shown in Figure 8. During the execution of this motion, at various points, the platform was pitched and rolled using the bulldozer blade.

Figure 10 shows the point-to-model errors for the GNSS-aided IMU solution and the LiDAR- and GNSS-aided solution in a known environment. A ray having the length of the measurement is cast along the corresponding LiDAR beam at the estimated pose. The distance from the end point of the ray to the nearest point of the model is determined as the point-to-model error. The smaller the point-to-model error, the closer the real pose to the estimated pose. For perfect pose, a perfect model, and zero measurement error, the point-to-model error is zero.

The distribution of point-to-model errors given in Figure 10 evidences that the LiDAR- and GNSS-aided IMU solution is more accurate than the GNSS aiding. The mean point-to-model error is reduced from $0.25$ $\mathrm{m}$ to $0.06$ $\mathrm{m}$. Significantly, the highest number of point-to-model errors for the LiDAR- and GNSS-aided solution corresponds to zero error. This is not unexpected: the positioning of the GNSS antennas provides information that supports good spatial positioning along with roll and yaw estimation. However, no information is available from GNSS measurements about the orientation of the axis that passes through the two antenna centres. This rotation heavily contributes to the pitch of the platform. The provision of measurements of the forward-facing 3D LiDAR provides information that stabilises the pitch estimate, this accounting for the improved accuracy. The mean value for point-to-model error for the GNSS-aided solution is prospectively due to start-up bias on the centre. It is noted that the GNSS-aided solution has a broader distribution of error.

The time estimates for the six degrees of freedom of the platform’s pose are shown in Figure 11. Significantly both estimators give similar results for x, y, z, and yaw. Most of the difference is in estimates for the pitch and roll of the platform. The bias in the pitch estimate for the GNSS-aided solution is attributed to start-up bias in the IMU sensor, which the estimator has been unable to account for; it is present at both the start and end of the motion sequence when the platform is stationary.

The results in this paper were produced on a computer running a Linux-based operating system (Intel Xeon W-2125 CPU, 32 GB of memory). The GNSS-aided IMU filter could update up to ∼ $10.000$ $\mathrm{Hz}$, much faster than the measurement rate. The LiDAR- and GNSS-aided IMU filter required more computing per time step and was capable of ∼ 500 $\mathrm{Hz}$.

9. LiDAR Measurement Model in an Unknown Environment

The capability to estimate pose when the environment is known helps to establish the potential performance of the LiDAR- and GNSS-aiding estimator, but is of limited practical usefulness. Most environments of interest in which this platform operates have unknown geometry. The estimator is extended to achieve the benefits of pitch stabilisation provided by the LiDAR measurement by concurrently estimating the pose and the local terrain.

For this purpose, the terrain is modelled as a height grid that divides the $(x,y)$ plane into a regular grid. Each grid element, known as a cell, is assigned a height. The points defined by each cell in the height grid can be triangulated to develop a representation of the terrain as a surface. A representation of the triangulated height grid with states $h_{0,0}$ to $h_{3,3}$ is shown in Figure 12.

Each of the $o·p=q$ cell heights in the grid can be considered additional states for which estimates are sought. These states, collectively denoted ${\mathit{x}}_t$, augment the state vector defined by Equation (1), shown here as ${\mathit{x}}_P$. Here, ${\mathit{x}}_t$ takes the form

$${\mathit{x}}_t={\left[h_{0,0},\cdots h_{i,j},\cdots ,h_{n,m}\right]}^T.$$

(73)

The subscript t indicates terrain. The additional terrain state estimates are appended to the state vector as follows:

$$\mathit{x}=\left[\begin{array}{c}{\mathit{x}}_p\\ {\mathit{x}}_t\end{array}\right].$$

(74)

As the terrain model is now part of the state vector, the LiDAR observation function is of the form

$${\mathit{z}}_L={\mathit{h}}_L\left(\mathit{x}\right)+{\mathit{v}}_{\mathit{z},L}.$$

(75)

The measurement function used to estimate the terrain and pose is identical to that of the previous section. However, expected range measurements are now determined through ray-casting onto the triangulated height grid model constructed from the ${\mathit{x}}_t$ vector, not a provided static model. The z-value or height of the vertices of each triangle of the grid, ${\mathbf{\alpha }}_j$, ${\mathsf{\beta }}_j$ and ${\mathbf{\gamma }}_j$, are now defined by the elements of ${\mathit{x}}_t$. The measurement Jacobian for the terrain states is determined analytically from the partial derivative of the LiDAR range measurement with respect to the z-value of the points of the intercepted triangle (${\mathbf{\alpha }}_j$, ${\mathsf{\beta }}_j$ and ${\mathbf{\gamma }}_j$).

Figure 13 shows that the concurrent terrain and pose estimation system performs slightly worse than the LiDAR- and GNSS-aided IMU solution and significantly better than the GNSS-aided IMU solution. The mean point-to-model error for the concurrent terrain and pose estimation system is $0.14$ $\mathrm{m}$. The zero point-to-model error bin is the largest bin, with the error distribution extending to higher point-to-model error values.

Figure 14 shows the environment model generated by the concurrent terrain and pose estimation system. The terrain model is coloured based on the difference between the estimated terrain and the surveyed ground truth model. The terrain estimate shows close agreement with the truth model: generally, state errors are less than $0.1$ $\mathrm{m}$ but extend out to $0.25$ $\mathrm{m}$. The terrain estimate has an RMS error of $0.126$ $\mathrm{m}$, which was achieved using a 1 $\mathrm{m}$ height grid terrain representation. Generally, areas of large error are present in high gradient locations of the terrain model. These can be difficult to capture using a regular grid terrain representation.

The estimates of the six degrees of freedom of the platform’s pose are shown in Figure 15. The solution generated without a priori knowledge of this environment shows some deviation in both roll and pitch when compared to that obtained using a known environment. The forward-looking orientation of the LiDAR sensor clearly stabilises the pitch estimate; the roll estimate is similar to the GNSS-aided IMU solution. A wider three-dimensional scan is expected to yield better stabilisation of roll. Equally, better roll estimates could be achieved by increasing the space between the GNSS antennas. The yaw, x, y, and z degrees of freedom are similar to those estimated when the environment is known. Overall, the results demonstrate the effectiveness of the approach.

The concurrent terrain and pose estimation filter, again, required more computing per timestep. It was demonstrated to run at ∼ 50 $\mathrm{Hz}$ on the target hardware (see Section 8).

10. Conclusions

The main result from this work is to show how LiDAR can be used to stabilise two-antenna GNSS-aided IMU pose estimators in environments with known and unknown geometry. The formulation is structured to accommodate multi-rate and non-synchronous measurements. It enables continuity through short-term periods of lost measurements from one of the sensors. Lost measurements are a characteristic observed on some brands of RTK-GNSS receivers, and the solution described in this paper adapts well to this situation.

We have not explored the possibility of using LiDAR stabilisation in GNSS-denied environments; however, the approach described here could be readily adapted to such situations.

In the paper introduction, we identified that LiDAR- and GNSS-aided IMU solutions can be effective when three-dimensional LiDAR measurements are available, but that three GNSS receivers are impractical or cost-prohibitive. Our recommendation, based on experience, is that if three receivers are possible, they should be used, particularly if an accurate pose solution to high frequencies is required.