Abstract
An autonomous underwater vehicle (AUV) has to solve two essential problems in underwater environment, namely, localization and mapping. SLAM is one novel solution to estimate locations and maps simultaneously based on motion models and sensor measurements. Sparse extended information filter (SEIF) is an effective algorithm to reduce storage and computational costs of largescale maps in the SLAM problem. However, there exists the inconsistency in the SEIF since the rank of the observability matrix of linearized errorstate model in SLAM system is higher than that of the nonlinear SLAM system. By analyzing the consistency of the SEIFbased SLAM from the perspective of observability, a SLAM based on SEIF with consistency constraint (SEIFCC SLAM) is developed to improve the estimator’s consistency. The proposed algorithm uses the firstever available estimates to calculate SEIF Jacobians for each of the state variables, called the First Estimates Jacobian (FEJ). Then, the linearized errorstate model can keep the same observability as the underlying nonlinear SLAM system. The capability of autonomous navigation with the proposed algorithm is validated through simulations experiments and sea trials for a CRanger AUV. Experimental results show that the proposed SEIFCC SLAM algorithm yields more consistent and accurate estimates compared with the SEIFbased SLAM.
1. Introduction
The inconsistency problem in linearizationbased autonomous navigation system has attracted significant concern. The reason of inconsistency is analyzed from the perspective of observability. A consistencyconstrained method is adopted to guarantee the consistent observability and to minimize the linear errors caused by the filter.
As a key technique of truly autonomous navigation, simultaneous localization and mapping (SLAM) is the process by which a mobile robot can incrementally build consistent maps in an unknown environment and use the maps to determine its own location at the same time [1]. It has played an important role in different domains, such as autonomous underwater hull inspection [2], autonomous underwater vehicle (AUV) in deep sea regions [3], unmanned aerial vehicle (UAV), and miniature aerial vehicle (MAV) in the sky [4], as well as autonomous driverassistancesystems in cars [5]. The SLAM problem involves an unknown and uncertain environment description and sensors noise [6–13]. There are many algorithms developed to address this SLAM problem in various forms, such as extended Kalman filter (EKF) [9, 10], particle filter [11], and extended information filter (EIF) [12, 13] based SLAM algorithms.
EKFbased SLAM has been proved to be widely popular as a standard nonlinear state estimator for its simplicity [6, 7]. This algorithm uses the observable features to update robot’s pose and estimate map. However, the quadratic complexity of computational capacity is one shortcoming of EKFbased SLAM in largescale maps [14–16]. Extended information filter (EIF) [17] is the dual form of EKF and is parameterized by information matrix and information vector. It is found that the information matrix is dominated by a small number of diagonal elements while most offdiagonal elements are close to zero [18]. So a sparse approximated representation of information matrix is obtained by ignoring the conditional dependence relationships between the mobile robot and a subset of the map, which is developed into a sparse variant of the EIF, called the sparse extended information filter (SEIF) [12, 19]. Due to the sparsification of SLAM information matrix, SEIF can avoid EKF’s quadratic cost in computation and memory. Therefore, SEIF has been successfully implemented with realworld datasets and has been demonstrated to be more scalable [20, 21].
However, the estimate of SEIF is prone to inconsistency for two reasons. First and foremost, SEIF may easily become overconfident due to the sparsification strategy [22]. Secondly, SEIF is based on singlepoint linearization as well as EKF, which may cause the accumulation of errors and lead to the estimated inconsistency [23–25]. Then, some approaches have been developed to improve the inconsistency of SEIFbased SLAM through the theoretical analysis. One method is exactly sparse extended information filter (ESEIF) proposed by Walter et al. [26], which is a typical approach referred to as full SLAM [27, 28]. ESEIF keeps the exactly sparse information matrix through marginalizing out the robot pose and relocating the robot, which is both locally and globally consistent [29]. But ESEIF is no longer computationally tractable and also faces the same nonlinear problems as EKF. Another method to improve the inconsistency is Huang’s sparse local submap joining filter (SLSJF) [30], where a sequence of small sized local submaps is represented in the global coordinate frame and EIF is used to ensure that the information matrix is exactly sparse. However, small inconsistency in lots of local maps may result in large inconsistency in the global map. Additional work in the SLSJF is required to complete the local maps joining. In short, ESEIF and SLSJF cannot concurrently have advantages of low computational cost and consistency.
The observability of the nonlinear SLAM system was analyzed by Lee et al. [31]. The nonlinear SLAM system has three degrees of freedom, while the linearized errorstate system is a twodimensional subspace. So the obtained spurious information of linear SLAM system is the primary cause of inconsistency. Huang et al. [32] proved that the rank of the observation model’s Jacobians in the linearized errorstate system is higher than that of the nonlinear SLAM system. The filter Jacobians are calculated using the firstever available estimates for each state variable, called the First Estimates Jacobian (FEJ), which ensures that the observability of errorstate model is the same as the underlying nonlinear SLAM systems. Simulations have also been demonstrated that the FEJbased EKF algorithm is superior to the standard EKF in terms of accuracy and consistency [33].
In view of the quadratic complexity and inconsistency of the EKFbased SLAM algorithm in the largescale environments, a novel SLAM algorithm based on SEIF with the consistency constraint (SEIFCC SLAM) is proposed to improve the autonomous navigation capability of AUV in this paper. SEIF is adopted to maintain the sparsification of information matrix according to the low computational cost. Meanwhile, the estimator’s consistency is constrained by the FEJ algorithm so as to obtain the same dimension between the linearized errorstate system and the nonlinear SLAM system. And the SEIFCC SLAM will be compared with the SEIFbased SLAM through Monte Carlo simulations in terms of accuracy and consistency.
Moreover, autonomous navigation experiment of the CRanger AUV is employed to validate the reliability of the proposed SEIFCC SLAM algorithm in this paper. The CRanger AUV is developed in Underwater Vehicle Laboratory at Ocean University of China, which is equipped with a number of sensors, such as DVL, gyro, digital compass, AHRS, and GPS, as well as multibeam imaging sonar for active sensing in underwater environment [34–36]. The experimental results will show that the performance of the proposed algorithm is superior to the SEIFbased algorithm in terms of the consistency and accuracy based on the CRanger AUV. Furthermore, SEIFCC SLAM also preserves the SEIF’s low computational cost and storage requirements.
The remainder of the paper is organized as follows. The SEIFbased SLAM algorithm is presented briefly in Section 2. And Section 3 will analyze the consistency of SEIFbased SLAM. In Section 4, the proposed SEIFCC SLAM algorithm is described and its performance is compared with SEIFbased SLAM through Monte Carlo simulations. Section 5 introduces the basic framework of the CRanger AUV platform as well as the onboard sensors. Then, results of sea trials will be presented to validate the performance of the proposed SEIFCC SLAM algorithm. Finally, we summarize the results and give the future work.
2. The SEIFBased SLAM
2.1. Information Form
The system model of SLAM includes the robot poses and a series of features’ or landmarks’ locations in the global coordinate system. The robot pose is determined by its position and heading , while the location of a feature is expressed as . So at time , the state vector can be expressed as follows:where the superscript “” refers to the transpose of a vector.
The robot aims to find a probabilistic estimate of the state vector in SLAM. The estimation problem transforms to calculate a posterior distribution based on the Bayesian rule, where and are past sensor measurements and control inputs at time , respectively. Reviewing the EKF solution to SLAM problem, the posterior is obeying a multivariate Gaussian distribution:where and are the mean vector and covariance matrix and denotes the Gaussian distribution.
Fundamental representation of SEIF is the information vector and information matrix which substitute the state vector and covariance matrix of EKF. According to the dual representation for the Gaussian distribution [12], the posterior probability distribution of is also described by the information matrix and information vector:In particular, the transformation between the state representations in EKF and the information form in EIF is got via the following formulas:
2.2. The Motion and Observation Model
According to the system state at time , the motion model of the robot at time is as follows:where is the independent noise variable that obeyed the white Gaussian distribution, . So the firstorder Taylor expansion of the motion model is as follows:where is the Jacobian matrix of the motion function with respect to .
The system state of the robot is estimated according to the velocity and the attitude information , which is provided by the velocity sensor and attitude sensor individually. The Jacobian matrix can be presented (in this paper, the subscripts and refer to the state’s update and prediction step at time , resp.):where denotes the 2 × 2 rotation matrix and is the velocitybased increment of the robot’s position estimate between time steps and :
The observation model of the mobile robot is analyzed in the following. The approximation of the nonlinear observation function is derived from its firstorder Taylor expansion. Assuming that the global position of the th landmark is , is the Jacobian matrix of the observation function with respect to , and is also subject to the white Gaussian noise, , the linearized observation model is given by
The linearized observation model describes the direct measurement model of relativeposition between features and robot. The relativeposition is obtained by imaging sonar.
2.3. Some Related Steps of SEIFBased SLAM
This paper mainly aims to improve the consistency of SEIF from the perspective of observability without changing the sparsification strategy; so the motion, update, and augmentation step related to the consistency analysis are introduced briefly.
2.3.1. Motion Step
Motion step is to predict the robot’s state at time according to one at time . Given information vector and information matrix at time , the information vector and information matrix can be predicted:where
2.3.2. Update Step
The state estimation will be updated when a feature is repeatedly observed. The updated information matrix and information vector can be expressed:where expresses the Jacobian of corresponding to the landmark position relative to robot and elements of are nonzero only at positions associated with and .
2.3.3. Augmentation Step
The state augmentation step will be implemented if a feature is observed for the first time. In the process of state augmentation, the robot’s observation model and its firstorder Taylor expansion form are as follows:where is the white Gaussian noise and is Jacobian matrix of the observation function with respect to . The augmented information matrix and information vector are as follows:with
The mean of state is employed in update and sparsification steps; so the mean is recovered after update step by information vector and information matrix: .
3. Consistency Analysis of SEIFBased SLAM
In general, the system model of the real SLAM is higher dimension, highly nonlinear, and coupled. SEIFbased SLAM algorithm is significantly inconsistent because of the pruning strategy and linearization. Consistency problem of the system model should be reanalyzed from the observable perspective. Comparing the observability of the SEIF system with the nonlinear system, it is evident to discern a fundamental deficiency caused by the firstorder Taylor expansion of the motion and observation model. The observability of the real and linearized SLAM system is analyzed based on the observability rank condition [33]. They do not possess the same observable rank so as to lead to the inconsistency.
3.1. Nonlinear Observability Analysis for SLAM
The Lie derivative is employed to deduce the observability matrix with a single landmark in order to analyze the observability of the nonlinear SLAM system.
Based on the kinematic model of the robot, the increment of system state in time interval is given bywhere and are the linear velocity and rotational velocity, respectively. They are the control inputs.
Considering the robottolandmark measurement as the relativeposition measurement, the position of the landmark relative to the robot at time is
The Lie derivative of a function along an analytic vector field is defined asSo the firstorder Lie derivative of and along and can be calculated:Then, the secondorder Lie derivatives are as follows:
The secondorder Lie derivatives of other functions are equal to zeros. Obviously, the secondorder Lie derivatives are the linear forms of the firstorder ones. It is easy to find that the higherorder Lie derivatives can also express the linear form of the firstorder ones; so the space spanned can be denoted:
Therefore, the observability matrix with one landmark in the underlying nonlinear SLAM system is got aswhere and , respectively. The rank of the observability matrix of the nonlinear SLAM system is 2. So the rank of the observability matrix with dimensional landmarks is in the nonlinear SLAM system.
3.2. Local Observability of SEIFBased SLAM
SEIF and EIF are the information form of EKF based on singlepoint linearization in the SLAM framework. So they also have the same observability matrix. Assuming one landmark is observed between time steps and , the local observability matrix during time intervals can be defined: Here, and have been given in the formulas (7), (16), and (17), respectively. Formula (29) can be expanded:where expresses the blockdiagonal matrix. It is nonsingular; so the rank of is equivalent to the rank of , .
3.2.1. Ideal SEIFBased SLAM
As known, the evaluated filter Jacobian is derived by using the linearized errorstate model. In studying the observability of ideal SEIF, the filter Jacobian is evaluated using the true state variables to substitute the mean .
According to formula (7), is got as follows: So the following formula (32) can be induced:
Formula (33) can be deduced by means of formulas (17) and (32) (the superscript “” is used to denote the true state values):Now, the matrix can be derived:
The rank of the above matrix is 2, , which is equivalent to the rank of the actual nonlinear SLAM system. Therefore, the ideal SEIFbased SLAM and the actual nonlinear SLAM system have the similar observability.
3.2.2. Actual SEIFBased SLAM
The actual state variables are usually unknown in the observability analysis of SEIFbased SLAM; so the filter Jacobians are calculated by means of linearized errorstate model; that is, Jacobians are evaluated at the mean of the state after update. Similar to formulas (32) and (33), the expression can be derived:
Now, the matrix can be written:
The rank of the linearized errorstate model’s observability matrix is equal to 3, . But the nonlinear observability matrix is twodimensional according to formula (30). So the SEIFbased SLAM whose Jacobians are evaluated at the mean has one spurious information, which leads to the inconsistency. It is necessary to select special linearization points in order to guarantee the correct rank of observability matrix for each state variable.
4. A SLAM Algorithm Based on SEIF with Consistency Constraint
4.1. The Consistency Constraint
The FEJbased EKF estimator can improve the estimator’s consistency [33]. Its main idea is to choose the firstever available estimates as the linearization points in terms of all the state variables. The modified measure in EKF can analogously be used in SEIF since SEIF is an information filter form of the EKF. The consistency of SEIF is constrained by FEJ so as to obtain the same dimensionality between the state and observation model.
The updated estimate is replaced by the position estimate in the Jacobian matrix of the state. So formula (7) can be written asThe mean will be recovered after motion step for .
Assuming that a landmark was first observed at time , the measurement Jacobian can be written as
4.2. Algorithm Process
The proposed SLAM based on SEIF with consistency constraint (SEIFCC SLAM) with pseudocode is provided in Algorithm 1. And main differences are the motion and measurement step compared with SEIFbased SLAM.

4.3. Observability of SEIFCC SLAM
To verify the consistency of the proposed algorithm, the rank of the observability matrix is calculated. FEJ is employed to estimate the robot position in the filter Jacobians. And the landmark estimate observed at the first time is utilized to compute the measurement Jacobian matrix at the evaluated point. As a result, the observability matrix of this new filter can be presented:
It can be easily proven that the rank of is 2, . Thus, the dimension of the linearized errorstate system model is the same as that of the nonlinear observable system by means of the SEIFCC SLAM algorithm. Similarly, it can be shown that the observability matrix is of rank when there are landmarks included in the state system. FEJ is used in SEIF to ensure the consistency without changing the sparse formation of links, which plays the role of consistency constraint.
4.4. Simulation Experiment
The performance of SEIFCC SLAM is compared with SEIFbased SLAM through Monte Carlo simulations. 50 Monte Carlo simulations with both SEIF and SEIFCC are conducted, respectively, under the same environments and the same basic parameters. The normalized estimation error squared (NEES) and the root mean square (RMS) are used to evaluate the filters consistency and accuracy, respectively [37], which is defined as follows:where is the ground truth for the state vector, and are the estimated state and covariance matrix, represents the dimension of , and is the times of Monte Carlo simulations.
The simulation path and environment are shown in the Figure 1, where 134 point features are randomly distributed in the environment. The robot moves at a constant velocity of 4 m/s and is able to observe the limited number of neighboring features. All simulations apply with individual compatibility nearestneighbour (ICNN) data association. The rejected gate is 5 and the augmented gate is 30. And the number of active features in SEIF is limited to 8. Both control inputs and sensor measurements are added by additive white Gaussian noise.
The performance of each algorithm is evaluated by the average NEES and RMS errors of robot position, as well as the heading’s RMS errors, respectively. The RMS errors show the accuracy of a given estimator, and NEES is used to characterize the filter consistency. Specifically, NEES follows a Chisquare distribution with degrees of freedom [37], where is the dimension of . So the average NEES for robot position must have an upper bound indicated by the horizontal line 2.6 and a lower bound by 1.5 if a filter is consistent with the 95% probability density in 50 Monte Carlo runs [37]. All filters are used to process the same data and set the same parameters. The selected filters in the simulation are (1) the ideal SEIF, (2) the SEIF, and (3) the SEIFCC.
From Figure 2, it can be seen that the average NEES error of robot position for the ideal SEIF is always between the intervals [2.4, 2.6], and SEIFCC’s NEES is a little higher than the ideal SEIF’s NEES and much lower than the SEIF’s NEES. This result indicates that the observability of the SEIFCC’s errorstate system in SLAM has a significant effect on consistency.
Figures 3 and 4 present the RMS errors of robot position and heading, respectively. The RMS errors position and heading in ideal SEIF are always low of 8.0 and 0.1, while their RMS errors in SEIF are high of 8.0 and 0.095, respectively. But the SEIFCC’s RMS errors are superior to their SEIF’s RMS and inferior to ideal SEIF’s RMS errors. The comparative results of RMS indicate that the proposed SEIFCC SLAM performs more accurate estimations than the SEIFbased SLAM.
5. CRanger AUV and Sea Trial
5.1. CRanger AUV
As Figure 5 shown, the CRanger is an openframestructure vehicle with the size of 1.6 m long, 1.3 m wide, and 1.1 m high, the gross drainage tonnage of 208 L, and the maximum work depth of 100 m. The total weight of CRanger is about 206 kg when it works at full load. The maximal speed is close to 3 knots (1.5 m/s). The continuous running time can reach 8 hours when CRanger travels at the speed of 1 knot and is fully charged.
The AUV system is composed of two electronic cabins and five underwater propeller thrusters. The top cabin is referred to as instrument cabin to host sensors, including two PC104 industrial computer units, the innerhull parameter monitor module, the communication module, and peripherals for data processing. The bottom cabin is the battery cabin which contains liion battery packs, battery management module, and motion driven module. CRanger has five degrees of freedom (DOF), including yaw, pitch, roll, heave, and surge, which ensures the good maneuverability. In addition, there are five propeller thrusters in the CRanger. The two horizontal thrusters are equipped in the belly of CRanger and parallel to the bow direction, which are used to supply the horizontal propulsion and dominate the freedoms of yaw and surge. Meanwhile, the other three vertical ones are applied for offering the vertical propulsion and regulating the freedom of roll, pitch, and heave.
5.2. Onboard Sensors
There are some internal sensors installed on CRanger, such as gyroscope, digital compass, pressure sensor, and AHRS (Attitude and Heading Reference System). In addition, there are also some external sensors, including sonar, altimeter, GPS, CCD camera, and DVL (Doppler Velocity Log).
A multibeam imaging sonar (Gemini 720i) is installed at the front top of CRanger, working at the frequency of 720 kHz. It is the principal sensor of CRanger to provide active sensing of environment features for its realtime, crisp imagery of the underwater scene ahead.
In the navigation course of CRanger AUV, the vehicle velocity relative to the seabed is acquired by the NavQuest600 DVL installed at the rear bottom of vehicle; the DVL’s work frequency and maximum velocity are 600 kHz and 20 knots, respectively. The attitude and angular velocity information are acquired from the AHRS and gyroscope. In particular, the AHRS’s ranges of heading angle and acceleration are 0°~360° and ±2 g, respectively.
A highprecision GPS is installed at the top postmedian of CRanger with 1.1 m (CEP) accuracy and 20 Hz maximum data update rate. The GPS system is regarded as a perfect benchmark for the purpose of algorithm comparison.
5.3. SLAM for CRanger
The vehicle position can be expressed by its position and heading at time , and the feature is described by its and coordinate in the global coordinate system.
At time , the control inputs and observations of AUV are as follows:Assuming that the variables , , , denote vehicle length, vehicle velocity, rotation angle, and sampling interval, respectively, the motion model of AUV is as follows:
Assuming that the global position of the th feature is for the measurements of sonar, the observation model of AUV can be formed by the range and bearing from the vehicle to feature:where and are the observation Gaussian noise, , .
5.4. Sea Trial
The realworld experiment was performed at Tuandao Bay (Qingdao, China). The onboard sensors provided speed and heading angle information to predict the AUV position. At the same time, the multibeam imaging sonar perceived the environment, so that the appropriate environment feature points can be extracted and used for building an environmental global map. AUV traveled underwater with a certain depth; so the bidimensional vehiclelandmark model is adopted in underwater environment.
Figure 6 shows the trajectory (the red line) of CRanger measured by GPS, and the starting point is marked by a yellow fivepointed star. The trajectory is regarded as ground truth used to compare the effectiveness of algorithms. The CRanger’s sailing speed was about 1 knot in the experiment.
The imaging sonar was configured to field of view with scanning range of 100 meters. The pretreatment of the raw data measured by sonar includes denoising and sparsifying redundancy features; so the remaining features were not affecting the positioning accuracy. At the same time, the image distorted problem caused by the vehicle’s motion was also considered in the realworld application. For the CRanger AUV, an effectual position feedback technique was adopted to correct the distorted image [36].
Figure 7 shows clear path comparison results for GPS (the red line), SEIF (the green line), and the SEIFCC (the blue line). It can be seen that the path deviation of the proposed SEIFCC SLAM relative to GPS is smaller than that of the SEIFbased SLAM.
Figures 8 and 9 present the average NEES and RMS errors of SEIF and SEIFCC relative to GPS data, respectively. It can be seen that the NEES error of SEIFCC is almost low of 10, but the SEIF’s NEES is far higher than the SEIFCC’s NEES. So the consistency of the SEIFCC SLAM algorithm is superior to the SEIFbased SLAM in sea trial. According to Figure 9, the RMS errors of SEIFCC and SEIF are rising at the beginning, but the general trend of RMS error of SEIF is also upward while the SEIFCC’s error curve is dropt after a period of time. Therefore, the SEIFCC SLAM algorithm is an efficient one with better consistency and accuracy in realworld environments.
6. Conclusion and Future Work
SEIFbased SLAM has been proposed to build largescale maps. However, the state estimation will suffer from global inconsistency due to SEIF’s pruning strategy. What is more, the SEIF is also subject to the linearized error. So a SLAM algorithm based on SEIF with consistency constraint is proposed to improve the autonomous navigation capability of AUV in this paper. The “First Estimates Jacobian” is adopted to constrain the SEIF’s consistency. SEIFCC SLAM performs better in terms of consistency and accuracy, because the observable matrix of this errorstate system has the same rank as that of the nonlinear system. The NEES and RMS errors of SEIFCC are compared with those of SEIF through Monte Carlo simulations and sea trials of CRanger AUV navigation in Tuandao Bay. The experimental results demonstrated that the SEIFCC SLAM applied to AUV navigation can improve the consistency and accuracy compared with SEIFbased SLAM.
More work is required to further improve the consistency of SEIF in underwater largescale environment implementations. There are two sources that caused estimation errors. One is that the approximation is linearized; the other is that the Jacobians are evaluated at estimates, which are not at the true values. Moreover, informationbased compact pose SLAM is one method to address the first error source [38]. This method can reduce computational cost and delay inconsistency by means of keeping nonredundant poses and highly informative links. In addition, square root cubature information filter (SRCIF) was proposed for nonlinear state estimation [39], which provided a more accurate filtering to solve a wider range of nonlinear systems. Further methods will be used to improve the navigation accuracy of the CRanger AUV.
Conflict of Interests
The authors declared that they have no conflict of interests regarding this work.
Acknowledgments
This work is partially supported by the National Science Foundation of China (41176076, 51075377, and 51379198) and The High Technology Research and Development Program of China (2014AA093410).