LiDAR-Stabilised GNSS-IMU Platform Pose Tracking

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 m mean error to 0.06 m when the GNSS-aided IMU estimator is augmented with LiDAR measurements. This precision is marginally degraded to 0.14 m when the pose estimator is operated in an a prior unknown environment.


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 (θ, φ, ψ) 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 m at a bandwidth from 0 Hz to 2-5 Hz (with measurement updates typically provided at 10 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 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. 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. The Caterpillar D11T bulldozer considered in this paper, showing the sensors installed on the platform. The platform is equipped with two GNSS receivers, one IMU, and a scanning LiDAR sensor that faces forward and scans around a horizontal axis. The frames referred to in this paper are labelled as the LiDAR frame, L, the IMU frame, I, the platform frame, P, and the global frame, G. 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 (·, ·). 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.

Experimental Platform
The pose of the tractor is defined by the state vector, 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, p P = [x, y, z] T is the coordinate of the origin of P in G; p P = [ẋ,ẏ,ż] T is the velocity of point p P in G; p P = [ẍ,ÿ,z] T is the acceleration of point p P in G; Φ P = [θ, φ, ψ] T describes the attitude of Frame P in G as roll, pitch and yaw; ω P = ω x , ω y , ω z T describes the rotational velocity of Frame P in G; b a = bẍ, bÿ, bz T describes the acceleration bias of the IMU in Frame I; is the rotational rate bias of the IMU in Frame I.
The transformation matrix T P (Φ P , p P ) is given by where Here, c(·) is the cosine function and s(·) is the sine function. Note, T P (·, ·) transforms points in Frame P into Frame G; equally, it describes the location of Frame P in Frame G. The top left 3 × 3 block of the T P (·, ·) matrix is the rotation matrix which aligns Frame P in Frame G.
The IMU provides measurements at 50 Hz; the GNSS receivers are RTK-enabled and provide observations at 10 Hz. The LiDAR is a Velodyne VLP-16 and provides range measurements from 16 beams, equally spaced across a field of view of 30°with 360°and rotation at 10 Hz. Other details of the sensors are given in Table 1.

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. Input: Initial state estimate,x 0|0 , and state estimate covariance, σ 2 x,0|0 . Initialise filter:

Process Model
The three filter updates of Figure 2 share the same process model which is assumed linear having the discrete-time form The state transition model F(∆t k+1 ) = F(t k+1 − t k ) 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 × 21 matrix F(·) has the form Here, T bẍ ,ÿ,z describes the empirically determined accelerometer bias parameters, and T b ωx ,ωy,ωz the empirically determined gyroscope bias parameters. Observe that 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 where w(·) is a white Gaussian discrete-time process and β(·) is Brownian motion with diffusion σ 2 Q (t) such that for consecutive measurement times t k and t k+1 , If the sample period is assumed to be small compared to the auto-correlation times of the process model modes, Observe that if the diffusion σ 2 Q (t) is time-invariant, the process noise covariance, σ 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.

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 where and measurement noise v z,I is assumed to be white and Gaussian with covariance σ 2 z,I -see Table 2.  To determine the function h I (·), note that the position of the origin of the IMU in the global frame is given by where 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 where Ω P (t) is the anti-symmetric matrix representing the angular velocity of Frame P in Frame G. The matrix Ω P (t) is The acceleration of the origin of Frame I in Frame G is found by differentiating Equation (14), i.e., Noting that if a constant angular rate is assumed (i.e.,Ω P = 0), which gives, The measurement equations follow from Equation (18) and can be expressed in terms of the pose state vector: Note that the rotation matrix R T I · R T P (·) serves to align the acceleration and rate vectors to Frame I in which measurements are made. The vector b r is the bias of rate gyro, b a is the accelerometer bias, and g denotes gravitational acceleration.
The Jacobian of h(·) can be found analytically from where

IMU Updates
The approach to updating measurements from the IMU involves estimating the error on the state estimate, δx k|k , at time t k given measurements to time t k , and combining this with the propagated state estimate,x k|k , from the most recent GNSS or LiDAR update. The state estimate update,x k|k , is then computed as the sum, 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,x and the error state estimate and error state estimate covariance are initialised to zero: On receipt of an IMU measurement, the calculation of δx k|k starts with the prediction of the state estimate,x the error state, state covariance, and error state covariance, The innovation is computed from and the innovation covariance, σ 2 ν,k , and Kalman gain, W I,k , are found from The corrected error state and the corrected error state covariance are computed from The predicted but uncorrected state estimate,x k|k−1 , and state estimate covariance, σ 2 x,k|k−1 , are used in the next iteration of the filter, The error-corrected state estimate,x k|k , which incorporates the newest IMU observation, is computed through the addition with the error state estimate, The indirect extended Kalman filter (IEKF) algorithm for IMU updates is detailed in Algorithm 2.
: if the previous measurement was from the GNSS or LiDAR then Initialise the filter with the following static variables: Predict the state estimate: Predict the error state estimate: Predict the state estimate covariance: Compute innovation covariance: Compute the Kalman gain: Update the error state estimate: δx k|k = δx k|k−1 + W I,k · ν I,k . Update the error state estimate covariance: σ 2 δx ,k|k = σ 2 δx ,k|k−1 − W I,k · σ 2 ν,I,k · W T I,k . Update the state estimate covariance: σ 2 x ,k|k = σ 2 x ,k|k + σ 2 δx ,k|k . The predicted but uncorrected state estimate and state estimate covariance are used in the next iteration of the filter: x k|k =x k|k−1 , Compute the error-corrected state estimate:x k|k =x k|k + δx k|k .

GNSS Measurement Model
The GNSS measurement model gives the locations of the GNSS receivers in the global frame and takes the form Here, h G (·) gives the locations of the GNSS receivers as a function of the state, where p G1 and p G2 are the coordinates of the GNSS antennas in the platform frame (see Figure 4). The measurement vector 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, v z,G , is assumed to be white and Gaussian with covariance σ 2 z,G (see Table 3). The GNSS measurement Jacobian is given by

GNSS Updates
The estimation of the error-corrected state estimate,x k|k , begins with a prediction of state using a state transition model, F(∆t k ), and the previous error-corrected state estimate, and the prediction of the state covariance, The GNSS measurement innovation and innovation covariance are computed from where z G,k is the GNSS measurement. The Kalman gain, W G,k , is then computed using The error-corrected state estimate,x k|k , and error-corrected state estimate covariance, The GNSS update is given as Algorithm 3.

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 where the LiDAR measurement vector, z L is a set of ranges corresponding to each ray from the LiDAR sensor. For n ranges, The measurement noise v z,L is assumed to be white and Gaussian with covariance σ 2 z,L -see Section 7.2. The LiDAR measurement function, 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 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 s j , j = 1 . . . n with ||s j − s 0 || = 1. The location of these points in Frame G can be found with transformations 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 To find r j , it is necessary to first determine the triangle in X which it intersects. Denote the vertices of this triangle, ∆ j = α j , β j , γ j . Then, where The ray-triangle intersection is depicted in Figure 6. The measurement model for n ray intersections can be constructed from and the LiDAR measurement Jacobian from  Figure 5. The LiDAR measurement model provides the measurement that the LiDAR sensor is expected to observe given the current state vector. The difference between the expected and observed measurements is termed the innovation.

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°at 50 m results in ∼0.7 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, σ 2 G ; (ii) the estimated location of the platform relative to the global frame, σ 2 P ; (iii) the uncertainty in the LiDAR registration, σ 2 L ; and (iv) the LiDAR's measurement uncertainty, σ 2 z,L . 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: Here, the notation ∇T L is used to denote the Jacobian of the homogeneous transformin 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.

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 The Fisher information matrix (FIM) and Fisher information vector (FIV) are predicted to the current time step using The predicted state estimate is obtained from and the innovation is obtained using the LiDAR observation function, the current predicted state estimate, and the terrain model, X: The predicted FIV and FIM are corrected through use of the innovation and the LiDAR range measurements usinĝ The FIM and FIV are now transformed back into the state space, 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. Function x k|k , σ 2 x,k|k = LiDARUpdate(x k−1|k−1 , σ 2 x,k−1|k−1 , z L,k , σ 2 z,L , ∆t k ): Transform the state estimate and state estimate covariance into the Information space: Predict the Fisher information matrix: Predict the Fisher information vector: Compute the predicted state estimate: if in an unknown environment then Extract the terrain state elements, x t , from the state vector and triangulate them: Compute the innovation: ν L,k = z L,k − h L x k|k−1 , X . Compute the uncertainty of the LiDAR range measurements using Section 7.2. Correct the predictions: Transform the updated Fisher Information Vector and Fisher Information Matrix back into the state space: returnx k|k and σ 2 x,k|k .

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 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.   The distribution of point-to-model errors given in Figure 10 evidences that the LiDARand GNSS-aided IMU solution is more accurate than the GNSS aiding. The mean point-tomodel error is reduced from 0.25 m to 0.06 m. Significantly, the highest number of point-tomodel 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 Hz, much faster than the measurement rate. The LiDARand GNSS-aided IMU filter required more computing per time step and was capable of ∼500 Hz.

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 x t , augment the state vector defined by Equation (1), shown here as x P . Here, x t takes the form The subscript t indicates terrain. The additional terrain state estimates are appended to the state vector as follows: As the terrain model is now part of the state vector, the LiDAR observation function is of the form 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 x t vector, not a provided static model. The z-value or height of the vertices of each triangle of the grid, α j , β j and γ j , are now defined by the elements of 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 (α j , β j and γ 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 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 m but extend out to 0.25 m. The terrain estimate has an RMS error of 0.126 m, which was achieved using a 1 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 Hz on the target hardware (see Section 8).

Conclusions
The main result from this work is to show how LiDAR can be used to stabilise twoantenna 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. Acknowledgments: The authors would like to thank Richard Hensel and Peter Beasley who conducted the collection of data used in this paper. Access to the Caterpillar D11T Bulldozer platform was facilitated by Caterpillar product distributor WesTrac Pty. Ltd. We acknowledge the outstanding support of Caterpillar Inc. who are a partner in ACARP project C27063.

Conflicts of Interest:
The authors declare no conflict of interest.

Abbreviations
The following abbreviations are used in this manuscript: