An Integrated GNSS/INS/LiDAR-SLAM Positioning Method for Highly Accurate Forest Stem Mapping

Forest mapping, one of the main components of performing a forest inventory, is an important driving force in the development of laser scanning. Mobile laser scanning (MLS), in which laser scanners are installed on moving platforms, has been studied as a convenient measurement method for forest mapping in the past several years. Positioning and attitude accuracies are important for forest mapping using MLS systems. Inertial Navigation Systems (INSs) and Global Navigation Satellite Systems (GNSSs) are typical and popular positioning and attitude sensors used in MLS systems. In forest environments, because of the loss of signal due to occlusion and severe multipath effects, the positioning accuracy of GNSS is severely degraded, and even that of GNSS/INS decreases considerably. Light Detection and Ranging (LiDAR)-based Simultaneous Localization and Mapping (SLAM) can achieve higher positioning accuracy in environments containing many features and is commonly implemented in GNSS-denied indoor environments. Forests are different from an indoor environment in that the GNSS signal is available to some extent in a forest. Although the positioning accuracy of GNSS/INS is reduced, estimates of heading angle and velocity can maintain high accurate even with fewer satellites. GNSS/INS and the LiDAR-based SLAM technique can be effectively integrated to form a sustainable, highly accurate positioning and mapping solution for use in forests without additional hardware costs. In this study, information such as heading angles and velocities extracted from a GNSS/INS is utilized to improve the positioning accuracy of the SLAM solution, and two information-aided SLAM methods are proposed. First, a heading angle-aided SLAM (H-aided SLAM) method is proposed that supplies the heading angle from GNSS/INS to SLAM. Field test results show that the horizontal positioning accuracy of an entire trajectory of 800 m is 0.13 m and is significantly improved (by 70%) compared to that of a traditional GNSS/INS; second, a more complex information added SLAM solution that utilizes both heading angle and velocity information simultaneously (HV-aided SLAM) is investigated. Experimental results show that the horizontal positioning accuracy can reach a level of six centimetres with the HV-aided SLAM, which is a significant improvement (by 86%). Thus, a more accurate forest map is obtained by the proposed integrated method.


Introduction
The accurate spatial distribution and attributes (such as height, diameter at breast height (DBH), species, etc.) of trees are basic and important elements of forest inventories. With the rapid development of sensors, laser scanning techniques are more and more widely adopted to obtain such information for forest inventory, such as the aerial laser scanning (ALS) [1], terrestrial laser scanning (TLS) and mobile laser scanning (MLS) [2,3]. In particular, MLS systems can improve the efficiency of field data collection. In addition, there are many recent studies on MLS systems and their accuracy [4][5][6][7].
The quality of the final data (the registered point cloud) based on MLS is related to the accuracy of position and attitude. Typically, position and attitude are provided by a positioning and orientation system (POS), which consists of an Inertial Navigation System (INS) and a Global Navigation Satellite System (GNSS), and the range is provided with a laser scanning sensor or MLS system [8].
However, in a forest, the satellites are commonly blocked by the canopy, resulting in the loss of the GNSS signals, and serious multipath effects also result in degraded positioning accuracy. This degraded accuracy is one of the main limitations of using MLS in a forest. There are extensive experiments that evaluate GNSS or GNSS/INS positioning accuracy in forests. In general, with standalone GNSS, whether PPP (Precise Point Positioning) mode or RTK (Real-Time Kinematic) mode is used, positioning accuracies are at approximately the metre level in mature and dense forests [9][10][11][12][13][14]. With integrated GNSS/INS navigation systems, the positioning accuracy can improve to sub-meter-levels [15]. However, an automatic high accuracy positioning solution that uses GNSS/INS is still not available for use in mature and dense forest environments.
SLAM constructs an environment map and determines the position of the mobile platform simultaneously. It was first proposed and investigated in the field of robotics in the 1980s. Most studies investigating SLAM are mainly for indoor environments, where GNSS is denied and which are full of features. Similarly, the application of SLAM to forests has been investigated in several studies [16][17][18][19][20][21][22][23][24]. In addition, many of them have studied the feasibility of using SLAM technology for MLS in forests [17][18][19]. However, forest environments are very different from indoor environments. There are rich and obvious features like lines and corners in indoor environments, and SLAM can acquire satisfactory solutions using feature matching methods. The features in forests are not continuous or stable, due to the density changes and the uneven terrain found in forests; moreover, GNSS satellites can be observed occasionally, particularly in open forest areas where SLAM cannot succeed in scan matching, leading to drift and gross errors [16]. The main cause of these errors is incorrect estimation of the heading angle, because there are not enough features for scan matching. In addition, drift errors accumulate rapidly over time. At the same time, gross errors are introduced in the relative positions between adjacent epochs. In forests, although the positioning accuracy of traditional GNSS/INS is restricted, some valuable navigation information can be provided by GNSS/INS, because some of the satellites can still be observed. For example, the attitude and velocity can be estimated with high accuracy from fewer satellites. The accurate heading angle provided by GNSS/INS can correct the problems with drift in feature-poor environments, and the gross errors in relative positions can be controlled using the precise velocity estimates. Integrating these pieces of navigation information from GNSS/INS with SLAM can eliminate drift and gross errors and enhance positioning accuracy in forests.
Summarizing the above discussion, the navigation information used by an MLS system is provided by the GNSS/INS navigation system separately. In fact, an MLS system normally includes both a LiDAR sensor and a GNSS/INS system, so the two methods should be integrated to provide navigation information for the MLS system. The main contributions of this paper are as follows: (1) navigation information from GNSS/INS, more specifically the heading and the velocity parameters, are utilized in LiDAR-based SLAM positioning solutions to improve their accuracy and stability; (2) a heading angle aided (H-aided) SLAM method is proposed and succeeds in improving the SLAM positioning accuracy in a test carried out in a forest; and (3) a heading angle and velocity aided (HV-aided) SLAM method is proposed and utilized in a forest to improve the positioning accuracy further. The remainder of this paper is organized as follows: Section 2 describes the materials and methods utilized in this research; Section 3 introduces the field tests; the experimental results are discussed in Section 4; and Section 5 draws conclusions.

GNSS/INS in GNSS-Challenged Environments
The tightly coupled integration passes pseudo-ranges, Doppler shifts, and other original carrier phases received by the GNSS receiver to a Kalman filter. When the number of visible satellites is between 0 and 4, Kalman filter measurement updating can limit the accumulation of INS errors. Therefore, this tightly coupled integration is suitable for urban canyons and other GNSS-degraded areas with fewer visible satellites. Past studies also showed that the tightly coupled integration has a stronger ability to resist gross errors [25,26]. The forest environment is similar to urban canyons, in that the number of visible satellites decreases because several satellites are blocked by tree crowns and serious errors are caused by the multipath effect. The tightly coupled integration may overcome these problems and obtain a guaranteed navigation solution in forests.
In this paper, GNSS/INS data are collected by a NovAtel SPAN system and processed using the Waypoint Inertial Explorer (IE) software package to obtain navigation solutions. The SPAN system is a GNSS/INS solution for obtaining continuous position, velocity and attitude information. By using INS data, in addition to GNSS, SPAN provides better position, velocity and attitude solutions which seamlessly bridge GNSS outages [27]. The IE software package supports both loosely and tightly coupled integration models, which combine GNSS data from the base and the rover with INS data to conduct differential processing. In this process, bidirectional filtering and a variety of quality control methods are adopted to obtain high accuracy results. According to field tests by NovAtel, the root mean square error (RMSE) for velocity is at the centimetre/second level and the RMSE for attitude is less than 0.1 degree in all 3 axes in urban downtown areas [27]. Similarly, our field test also yields velocity accuracy at the centimetre level in forest environments. As shown in Figure 1, the standard deviation (STD) of velocity is less than 5 cm/s during this field test. (HV-aided) SLAM method is proposed and utilized in a forest to improve the positioning accuracy further. The remainder of this paper is organized as follows: Section 2 describes the materials and methods utilized in this research; Section 3 introduces the field tests; the experimental results are discussed in Section 4; and Section 5 draws conclusions.

GNSS/INS in GNSS-Challenged Environments
The tightly coupled integration passes pseudo-ranges, Doppler shifts, and other original carrier phases received by the GNSS receiver to a Kalman filter. When the number of visible satellites is between 0 and 4, Kalman filter measurement updating can limit the accumulation of INS errors. Therefore, this tightly coupled integration is suitable for urban canyons and other GNSS-degraded areas with fewer visible satellites. Past studies also showed that the tightly coupled integration has a stronger ability to resist gross errors [25,26]. The forest environment is similar to urban canyons, in that the number of visible satellites decreases because several satellites are blocked by tree crowns and serious errors are caused by the multipath effect. The tightly coupled integration may overcome these problems and obtain a guaranteed navigation solution in forests.
In this paper, GNSS/INS data are collected by a NovAtel SPAN system and processed using the Waypoint Inertial Explorer (IE) software package to obtain navigation solutions. The SPAN system is a GNSS/INS solution for obtaining continuous position, velocity and attitude information. By using INS data, in addition to GNSS, SPAN provides better position, velocity and attitude solutions which seamlessly bridge GNSS outages [27]. The IE software package supports both loosely and tightly coupled integration models, which combine GNSS data from the base and the rover with INS data to conduct differential processing. In this process, bidirectional filtering and a variety of quality control methods are adopted to obtain high accuracy results. According to field tests by NovAtel, the root mean square error (RMSE) for velocity is at the centimetre/second level and the RMSE for attitude is less than 0.1 degree in all 3 axes in urban downtown areas [27]. Similarly, our field test also yields velocity accuracy at the centimetre level in forest environments. As shown in Figure 1, the standard deviation (STD) of velocity is less than 5 cm/s during this field test.

SLAM in Forest Areas
SLAM is a process of building and updating a map of an unknown environment based on data collected by range sensors [28]. Of the current SLAM methods, LiDAR-based SLAM is widely used in positioning and environment-recognizing fields and has made a number of successful trials [29].
In general, the classical method for laser scan matching is the Iterative Closest Point (ICP) algorithm [30], which performs matching by minimizing the sum of distances between two clouds of

SLAM in Forest Areas
SLAM is a process of building and updating a map of an unknown environment based on data collected by range sensors [28]. Of the current SLAM methods, LiDAR-based SLAM is widely used in positioning and environment-recognizing fields and has made a number of successful trials [29].
In general, the classical method for laser scan matching is the Iterative Closest Point (ICP) algorithm [30], which performs matching by minimizing the sum of distances between two clouds of points. There are many improvements on the classical ICP algorithm, such as Polar Scan Matching Remote Sens. 2017, 9, 3 4 of 16 (PSM) [31], the Iterative Closed Line (ICL) method [32], etc. These methods mainly use consecutive pairs of scans for matching without using historical scans. Thus, the matching error will accumulate over time. Feature-based SLAM is an effective method. For example, Guivant and Nebot utilize Compressed Extended Kalman Filter (CEKF) SLAM in forested areas and compare the result with GPS ground truth [23]. However, feature extraction and association increase the complexity. To improve this issue, the grid-map based method is proposed to process the laser scan matching. There is no feature extraction and feature data association process; all the previous scans are fully used in the grid-map based method. Generally, a grid-based occupancy likelihood map will be built based on all previous scans, and then the rigid-body transformation will be calculated by scan-to-map matching [30]. One excellent grid-map based method is Maximum Likelihood Estimation (MLE) [33]. Tang et al. [16] employ the Improved Maximum Likelihood Estimation (IMLE) method for positioning in forests and obtain promising results in mature forests. In applying this method, a grid map is built based on probabilistic and feature uncertainty models, and then a brute-force search matching method is applied for scan-to-map matching to find the optimal rigid-body transformation. Because of the complexity of natural conditions in forests, features such as undergrowth trees and irregular rocks introduce noise into the point cloud data. More accurate initial values and search ranges will significantly improve the robustness of the brute-force search matching method, which indicates that integration with GNSS/INS navigation information may result in substantially improved positioning accuracy in forests. Therefore, in this paper, an algorithm for integrating GNSS/INS and IMLE-SLAM is studied to improve positioning accuracy in forests.
The IMLE-SLAM searches for the best body transformation to acquire the maximum likelihood value: where T * is the optimal body transformation; M is the likelihood map, which stores likelihood values built by previous scans; and S t is the current scan. The detailed process and performance of IMLE-SLAM can be found in [33]. Moreover, some premises that are the same as in [16] have been taken into account, which are that multiple segmental laser scans from 3D space are projected onto 2D stem profiles to generate the stem map according to the accurate roll and pitch estimates from GNSS/INS by assuming that most mature stems have straight and circular stems. Because the main type of tree in the field test is upright pine trees, the 2D projection assumption is reasonable in our experiment.

Instrumentation
The research is based on the self-developed FGI ROAMER R2 mobile mapping system installed on an all-terrain-vehicle (ATV) and shown in Figure 2. The GNSS/INS part of this MLS system is a NovAtel Synchronized Position Attitude Navigation (SPAN) navigation system, which consists of a GNSS receiver (NovAtel Flexpak6) and an INS system (NovAtel UIMU-LCI). The LiDAR sensor adopted for SLAM research is a horizontally mounted FARO Focus3D X330, which can generate approximately 1 million points per second. All sensors are mounted on a rigid platform made of hard aluminium. The beam divergence of the FARO Focus3D X330 is 0.011 • , and the beam diameter at exit is 2.25 mm. The maximum valid range measurement utilized in this research is 25 m, resulting in a maximum footprint size of 7.3 mm, compared to 10-20 cm for models with large beam divergence at the same distance. The synchronized pulse of the LiDAR is utilized to trigger an event pin of the GNSS/INS device to obtain synchronization [16]. The detailed technical specifications of the GNSS/INS navigation system and the laser scanner can be found in [15]. The raw laser point cloud data in a scanner coordinate system are transformed to real world coordinates using the vehicle position and attitude fetched from the GNSS/INS. Therefore, the accuracy of the point cloud is dependent on the accuracy of the position and attitude measurements from the GNSS/INS.

Integration Method in Forests
As mentioned above, the forest environment is similar to the urban canyon environment. The satellite signal is blocked. The number of visible satellites is unstable, as shown in Figure 3, but not all the satellites are completely lost. This fact implies that a tightly coupled integration of GNSS/INS can effectively conduct measurement updates to suppress error accumulation over time, although the positioning accuracy will be affected and degraded, due to the reduced number of visible satellites and serious multipath effects. However, velocity and attitude estimates can still be obtained at a high level of accuracy. The IMLE-SLAM method is based on the maximum posterior probability criterion. The brute-force search method is applied to calculate the posterior probability of the current scan based on the grid map. The map is built up of previous scans with different body transformations. Because IMLE-SLAM finds the maximum posterior probability corresponding to the optimal body transformation, it is able to obtain centimetre-level positioning accuracy for long-term operation in indoor environments [33] and produce a more accurate and clearer tree trunk map, which is beneficial for extracting tree trunk information such as DBH [16]. However, IMLE-SLAM also has

Integration Method in Forests
As mentioned above, the forest environment is similar to the urban canyon environment. The satellite signal is blocked. The number of visible satellites is unstable, as shown in Figure 3, but not all the satellites are completely lost. This fact implies that a tightly coupled integration of GNSS/INS can effectively conduct measurement updates to suppress error accumulation over time, although the positioning accuracy will be affected and degraded, due to the reduced number of visible satellites and serious multipath effects. However, velocity and attitude estimates can still be obtained at a high level of accuracy.

Integration Method in Forests
As mentioned above, the forest environment is similar to the urban canyon environment. The satellite signal is blocked. The number of visible satellites is unstable, as shown in Figure 3, but not all the satellites are completely lost. This fact implies that a tightly coupled integration of GNSS/INS can effectively conduct measurement updates to suppress error accumulation over time, although the positioning accuracy will be affected and degraded, due to the reduced number of visible satellites and serious multipath effects. However, velocity and attitude estimates can still be obtained at a high level of accuracy. The IMLE-SLAM method is based on the maximum posterior probability criterion. The brute-force search method is applied to calculate the posterior probability of the current scan based on the grid map. The map is built up of previous scans with different body transformations. Because IMLE-SLAM finds the maximum posterior probability corresponding to the optimal body transformation, it is able to obtain centimetre-level positioning accuracy for long-term operation in indoor environments [33] and produce a more accurate and clearer tree trunk map, which is beneficial for extracting tree trunk information such as DBH [16]. However, IMLE-SLAM also has The IMLE-SLAM method is based on the maximum posterior probability criterion. The brute-force search method is applied to calculate the posterior probability of the current scan based on the grid map. The map is built up of previous scans with different body transformations. Because IMLE-SLAM finds the maximum posterior probability corresponding to the optimal body transformation, it is able to obtain centimetre-level positioning accuracy for long-term operation in indoor environments [33] and produce a more accurate and clearer tree trunk map, which is beneficial for extracting tree trunk information such as DBH [16]. However, IMLE-SLAM also has drawbacks. It is strongly dependent on the features in the environment. In a two-dimensional coordinate system, a body transformation includes two position parameters and one heading angle parameter, which means a three-dimensional search scope. The high-dimensional search scope also reduces the robustness of the algorithm. If the posterior probability is regarded as a function of position and heading angle, the brute-force search method searches for the function extremum within a certain region of three-dimensional space. However, due to the complexity of the environment and noise effects, there may be a plurality of extreme points in an inappropriate search scope. The influence of these disadvantages will be enlarged in the forest. If the dimension of the search scope can be reduced, the robustness and computational efficiency will be improved greatly. As a result, a more precise search scope will reduce the effects of noise in the environment.
It can be seen that strong complementarity between GNSS/INS and IMLE-SLAM exists. Although the accuracy of GNSS/INS positioning in forests is only sub-metre or even metre, the heading angle is accurate enough for IMLE-SLAM, and the precise velocity estimates can provide accurate relative displacements over short time intervals, as reported in [34].
IMLE-SLAM can use the heading angle from GNSS/INS to reduce the dimension of the search scope, and obtains a more restricted search scope using precise relative displacements obtained with a high data sampling rate. The workflow of the data processing in integrating GNSS/INS and IMLE-SLAM is shown in Figure 4. First, GNSS and INS data are processed to obtain velocity and heading angle estimates over time. When matching laser scan data during one epoch, the heading angle is regarded as a known parameter and input into the brute-force IMLE search process, and the velocity is integrated over the epoch interval. The displacement is then added to the estimated position from the last epoch to obtain the initial position of the current epoch, which constitutes the search scope with some position uncertainty. The uncertainty is mainly determined by the maximum resolution of the grid map. Supposing the size of the grid cells where the grid map's resolution is highest is σ, the search scope ranges from negative 3σ to 3σ on each component, centred on the initial position. The search scope of T in Equation (1) can then be written as follows: where ϕ 0 is the heading angle estimated by GNSS/INS, and X 0 and Y 0 represent the initial position as estimated using the velocity from GNSS/INS. drawbacks. It is strongly dependent on the features in the environment. In a two-dimensional coordinate system, a body transformation includes two position parameters and one heading angle parameter, which means a three-dimensional search scope. The high-dimensional search scope also reduces the robustness of the algorithm. If the posterior probability is regarded as a function of position and heading angle, the brute-force search method searches for the function extremum within a certain region of three-dimensional space. However, due to the complexity of the environment and noise effects, there may be a plurality of extreme points in an inappropriate search scope. The influence of these disadvantages will be enlarged in the forest. If the dimension of the search scope can be reduced, the robustness and computational efficiency will be improved greatly.
As a result, a more precise search scope will reduce the effects of noise in the environment. It can be seen that strong complementarity between GNSS/INS and IMLE-SLAM exists. Although the accuracy of GNSS/INS positioning in forests is only sub-metre or even metre, the heading angle is accurate enough for IMLE-SLAM, and the precise velocity estimates can provide accurate relative displacements over short time intervals, as reported in [34].
IMLE-SLAM can use the heading angle from GNSS/INS to reduce the dimension of the search scope, and obtains a more restricted search scope using precise relative displacements obtained with a high data sampling rate. The workflow of the data processing in integrating GNSS/INS and IMLE-SLAM is shown in Figure 4. First, GNSS and INS data are processed to obtain velocity and heading angle estimates over time. When matching laser scan data during one epoch, the heading angle is regarded as a known parameter and input into the brute-force IMLE search process, and the velocity is integrated over the epoch interval. The displacement is then added to the estimated position from the last epoch to obtain the initial position of the current epoch, which constitutes the search scope with some position uncertainty. The uncertainty is mainly determined by the maximum resolution of the grid map. Supposing the size of the grid cells where the grid map's resolution is highest is , the search scope ranges from negative 3 to 3 on each component, centred on the initial position. The search scope of in Equation (1) can then be written as follows: where is the heading angle estimated by GNSS/INS, and and represent the initial position as estimated using the velocity from GNSS/INS.

Field Test
In this paper, field tests were carried out in Evo, Southern Finland (61 • 19 N, 25 • 11 E). The area lies within the southern boreal forest zone. The dominant tree species are Scots pine and Norway spruce. Detailed information on this test field can be found in [16].
In the field area, there are 224 trees that were measured to the centre of the stem at breast height by a total station and RTK GNSS. The accuracies of the reference trees' positions are better than 10 cm. The positions of these trees in a 2D coordinate system are precisely estimated as the reference targets to evaluate the positioning accuracy. When the ROAMER R2 mobile mapping system passed through this field, GNSS/INS observations and laser scan measurements were recorded. Then, the laser point cloud was calculated based on the position and attitude of MLS, from which the positions of the reference trees could be extracted. The accuracy of the position and attitude estimates from MLS could be evaluated by comparing the positions of these trees with the reference positions directly. In the field tests, the average driving speed of the ATV was approximately 4 km/h and the length of the trajectory was approximately 800 m. The reference trees and the vehicle trajectory are shown in Figure 5.

Field Test
In this paper, field tests were carried out in Evo, Southern Finland (61°19′N, 25°11′E). The area lies within the southern boreal forest zone. The dominant tree species are Scots pine and Norway spruce. Detailed information on this test field can be found in [16].
In the field area, there are 224 trees that were measured to the centre of the stem at breast height by a total station and RTK GNSS. The accuracies of the reference trees' positions are better than 10 cm. The positions of these trees in a 2D coordinate system are precisely estimated as the reference targets to evaluate the positioning accuracy. When the ROAMER R2 mobile mapping system passed through this field, GNSS/INS observations and laser scan measurements were recorded. Then, the laser point cloud was calculated based on the position and attitude of MLS, from which the positions of the reference trees could be extracted. The accuracy of the position and attitude estimates from MLS could be evaluated by comparing the positions of these trees with the reference positions directly. In the field tests, the average driving speed of the ATV was approximately 4 km/h and the length of the trajectory was approximately 800 m. The reference trees and the vehicle trajectory are shown in Figure 5.  and has a radius of LiDAR's effective range (25 m in this experiment), as shown in Figure 6. The RMSE of the position error is calculated from the reference tree network as where d i is the offset of the centre of the i-th reference tree within the effective LiDAR range. as shown in Figure 6. The RMSE of the position error is calculated from the reference tree network as where is the offset of the centre of the -th reference tree within the effective LiDAR range.
Effective Distance Figure 6. Schematic diagram for evaluating the positioning accuracy using reference trees.

Separate Evaluation of GNSS/INS and IMLE-SLAM Methods
The trajectories and stem positions estimated using the GNSS/INS and IMLE-SLAM methods are compared in Figure 7a for the entire test. The RMSEs of the trajectory position errors from the GNSS/INS and IMLE-SLAM methods for the entire test are presented in Figure 7b and c, respectively.
As shown in Figure Table 1.

Separate Evaluation of GNSS/INS and IMLE-SLAM Methods
The trajectories and stem positions estimated using the GNSS/INS and IMLE-SLAM methods are compared in Figure 7a for the entire test. The RMSEs of the trajectory position errors from the GNSS/INS and IMLE-SLAM methods for the entire test are presented in Figure 7b,c, respectively.
As shown in Figure Table 1.  As shown in Figure 8, the heading angle estimated by IMLE-SLAM is unstable compared with that estimated by GNSS/INS. It can be observed that there are some jump errors between the epochs As shown in Figure 8, the heading angle estimated by IMLE-SLAM is unstable compared with that estimated by GNSS/INS. It can be observed that there are some jump errors between the epochs at 100 s and 200 s, when the ATV drove out of the mature forest and into the open forest. Heading angle errors leads to large positioning errors. Therefore, when both heading angle and position are estimated in IMLE-SLAM, the dependence on the environmental features will be much higher. In addition, once the heading angle is estimated incorrectly, enormous errors will be introduced into the estimation of position, as illustrated in Figure 7b and Table 1. Heading angle errors leads to large positioning errors. Therefore, when both heading angle and position are estimated in IMLE-SLAM, the dependence on the environmental features will be much higher. In addition, once the heading angle is estimated incorrectly, enormous errors will be introduced into the estimation of position, as illustrated in Figure 7b and Table 1. The heading angle error of IMLE-SLAM accumulates quickly due to the smaller number of features in the open forest, leading to position errors. The estimation of heading angle is a weakness of the SLAM algorithm, which depends heavily on the number and conspicuousness of features and is sensitive to noise [35]. If searching for the heading angle, the dimension of the search scope will be higher, this will greatly increase the instability of the algorithm, especially in complex environments. Moreover, it reduces the accuracy of the eventual position estimates. However, the performance is degraded in open forest with IMLE-SLAM, compared to its moderate performance in the mature forest. The statistics of position error of IMLE-SLAM and GNSS/INS method in the first dense forest area is shown in Figure 9a, the heading angle difference in the first dense forest area is shown in Figure 9b, and the RMSEs of the GNSS/INS and IMLE-SLAM position trajectories in the dense forest are given in Table 2. As shown in Figure 9a, that the positioning accuracy of IMLE-SLAM is higher than GNSS/INS in the dense forest. At the same time, the heading angles of IMLE-SLAM and GNSS/INS are in good agreement, as shown in Figure 9b. However, the difference in the IMLE-SLAM heading angle compared with GNSS/INS is still larger than 0.1 degrees in some periods. The reason might be that, the angular resolution of LiDAR limits the accuracy of the heading angle. The accurate heading angles estimated by GNSS/INS can be integrated with IMLE-SLAM to improve the positioning accuracy. The heading angle error of IMLE-SLAM accumulates quickly due to the smaller number of features in the open forest, leading to position errors. The estimation of heading angle is a weakness of the SLAM algorithm, which depends heavily on the number and conspicuousness of features and is sensitive to noise [35]. If searching for the heading angle, the dimension of the search scope will be higher, this will greatly increase the instability of the algorithm, especially in complex environments. Moreover, it reduces the accuracy of the eventual position estimates. However, the performance is degraded in open forest with IMLE-SLAM, compared to its moderate performance in the mature forest. The statistics of position error of IMLE-SLAM and GNSS/INS method in the first dense forest area is shown in Figure 9a, the heading angle difference in the first dense forest area is shown in Figure 9b, and the RMSEs of the GNSS/INS and IMLE-SLAM position trajectories in the dense forest are given in Table 2.

Results from the IMLE-SLAM Method Aided by Heading Angle
After supplying heading angle information from GNSS/INS to IMLE-SLAM, also called H-aided IMLE-SLAM, the dimensionality of the search scope decreases from three dimensions to two. The trajectories and tree stem positions of the entire test are shown in Figure 10a. In addition, the RMSE of position trajectory for the entire test is presented in Figure 10b. (a)

Results from the IMLE-SLAM Method Aided by Heading Angle
After supplying heading angle information from GNSS/INS to IMLE-SLAM, also called H-aided IMLE-SLAM, the dimensionality of the search scope decreases from three dimensions to two. The trajectories and tree stem positions of the entire test are shown in Figure 10a. In addition, the RMSE of position trajectory for the entire test is presented in Figure 10b.

Results from the IMLE-SLAM Method Aided by Heading Angle
After supplying heading angle information from GNSS/INS to IMLE-SLAM, also called H-aided IMLE-SLAM, the dimensionality of the search scope decreases from three dimensions to two. The trajectories and tree stem positions of the entire test are shown in Figure 10a. In addition, the RMSE of position trajectory for the entire test is presented in Figure 10b.

Result of IMLE-SLAM Method Aided by Heading Angle and Velocity
By integrating heading angle information, the IMLE-SLAM algorithm has been greatly improved. However, it can still be observed that the position error in the open forest increased. As shown in Figure 10b, though the accuracy recovers after re-entering the dense forest, the stem position in the open forest is inaccurate due to the position error. The reason is that the real position cannot be searched out, even after the dimension is reduced using the heading angle in the feature-poor environment. Under normal circumstances, the optimal position is identified within a certain search scope range. The range is an empirical value based on the position from the previous epoch, which is generally coarse. When processing a feature-poor data frame with many noisy points, such search scopes can easily lead to false optimum positions, as described in Section 2.4.
By supplying the velocity estimated by GNSS/INS, a narrower search scope range that contains the true position can be calculated from the velocity and the position of last epoch, as described in Section 2.4. The brute-force search method can easily find the optimal position corresponding to a maximum likelihood score. The trajectories and the stem positions of the heading angle and velocity-aided (HV-aided) IMLE-SLAM methods are shown in Figure 11a. In addition, the trajectory RMSE of HV-aided IMLE-SLAM is presented in Figure 11b.
As seen from Figure

Result of IMLE-SLAM Method Aided by Heading Angle and Velocity
By integrating heading angle information, the IMLE-SLAM algorithm has been greatly improved. However, it can still be observed that the position error in the open forest increased. As shown in Figure 10b, though the accuracy recovers after re-entering the dense forest, the stem position in the open forest is inaccurate due to the position error. The reason is that the real position cannot be searched out, even after the dimension is reduced using the heading angle in the feature-poor environment. Under normal circumstances, the optimal position is identified within a certain search scope range. The range is an empirical value based on the position from the previous epoch, which is generally coarse. When processing a feature-poor data frame with many noisy points, such search scopes can easily lead to false optimum positions, as described in Section 2.4.
By supplying the velocity estimated by GNSS/INS, a narrower search scope range that contains the true position can be calculated from the velocity and the position of last epoch, as described in Section 2.4. The brute-force search method can easily find the optimal position corresponding to a maximum likelihood score. The trajectories and the stem positions of the heading angle and velocity-aided (HV-aided) IMLE-SLAM methods are shown in Figure 11a. In addition, the trajectory RMSE of HV-aided IMLE-SLAM is presented in Figure 11b.
As seen from Figure

Conclusions
In summary, position and attitude play an important role in the use of MLS for forest mapping. Traditionally, position and attitude determinations have mainly relied on GNSS/INS systems, which can only achieve sub-meter positioning accuracy in forests. The SLAM method has recently been examined for its potential in providing navigation information and has achieved some promising results. Most SLAM algorithms are utilized in indoor environments and are integrated only with INS. GNSS/INS navigation information is rarely utilized in SLAM methods. In contrast to the indoor environment, the forest environment is much more complex, and only some satellites can be used for observation. However, highly precise attitude and velocity information can be obtained using GNSS/INS. In addition, the SLAM method can only provide good results in feature-rich forests, and does not perform well in feature-poor open forests. The highly precise attitude and velocity information from GNSS/INS can be integrated with SLAM to improve its robustness and accuracy for high-precision positioning in forests.
Field test results showed that when standalone IMLE-SLAM is used in dense forest, heading angles can be estimated correctly, and the horizontal accuracy of position is decimetre-level and better than the standalone GNSS/INS method, but navigation fails in open forest areas because heading angle error leads to large positioning error. When integrated with high-precision heading angle estimates from GNSS/INS, the H-aided IMLE-SLAM can successfully obtain guaranteed

Conclusions
In summary, position and attitude play an important role in the use of MLS for forest mapping. Traditionally, position and attitude determinations have mainly relied on GNSS/INS systems, which can only achieve sub-meter positioning accuracy in forests. The SLAM method has recently been examined for its potential in providing navigation information and has achieved some promising results. Most SLAM algorithms are utilized in indoor environments and are integrated only with INS. GNSS/INS navigation information is rarely utilized in SLAM methods. In contrast to the indoor environment, the forest environment is much more complex, and only some satellites can be used for observation. However, highly precise attitude and velocity information can be obtained using GNSS/INS. In addition, the SLAM method can only provide good results in feature-rich forests, and does not perform well in feature-poor open forests. The highly precise attitude and velocity information from GNSS/INS can be integrated with SLAM to improve its robustness and accuracy for high-precision positioning in forests.
Field test results showed that when standalone IMLE-SLAM is used in dense forest, heading angles can be estimated correctly, and the horizontal accuracy of position is decimetre-level and better than the standalone GNSS/INS method, but navigation fails in open forest areas because heading angle error leads to large positioning error. When integrated with high-precision heading angle estimates from GNSS/INS, the H-aided IMLE-SLAM can successfully obtain guaranteed positioning results for the entire test; the horizontal accuracy of the entire trajectory is 0.13 m and significantly improved (by 70%) compared to that of GNSS/INS. However, positioning error in the open forest is still greater than that in dense forest. When high-precision velocity data are additionally included, the positioning accuracy of HV-aided IMLE-SLAM in open forest is greatly improved. The horizontal accuracy of the entire trajectory reaches 0.06 m, which is improved by 86% relative to GNSS/INS. The results show that the integration of GNSS/INS with SLAM yields a high-precision navigation method in forest that can obtain more accurate stem maps.
Although the devices used herein are relatively expensive and therefore display better performance, the integration methods may also be applicable to low-cost equipment. Therefore, our future work will study the possibility of applying this integration method to low-cost equipment.