Autonomous Navigation Based on SEIF with Consistency Constraint for C-Ranger AUV

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 large-scalemaps in the SLAMproblem.However, there exists the inconsistency in the SEIF since the rank of the observabilitymatrix of linearized error-state model in SLAM system is higher than that of the nonlinear SLAM system. By analyzing the consistency of the SEIF-based SLAM from the perspective of observability, a SLAM based on SEIF with consistency constraint (SEIF-CC SLAM) is developed to improve the estimator’s consistency.The proposed algorithm uses the first-ever available estimates to calculate SEIF Jacobians for each of the state variables, called the First Estimates Jacobian (FEJ). Then, the linearized error-state 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 C-Ranger AUV. Experimental results show that the proposed SEIF-CC SLAM algorithm yields more consistent and accurate estimates compared with the SEIF-based SLAM.


Introduction
The inconsistency problem in linearization-based autonomous navigation system has attracted significant concern.The reason of inconsistency is analyzed from the perspective of observability.A consistency-constrained 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 driver-assistance-systems in cars [5].The SLAM problem involves an unknown and uncertain environment description and sensors noise [6][7][8][9][10][11][12][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.
EKF-based 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 EKF-based SLAM in large-scale maps [14][15][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 off-diagonal elements are close to zero [18].So a sparse approximated representation of information matrix is obtained by ignoring the conditional 2 Mathematical Problems in Engineering 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 real-world 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 single-point linearization as well as EKF, which may cause the accumulation of errors and lead to the estimated inconsistency [23][24][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 two-dimensional 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 error-state system is higher than that of the nonlinear SLAM system.The filter Jacobians are calculated using the first-ever available estimates for each state variable, called the First Estimates Jacobian (FEJ), which ensures that the observability of error-state model is the same as the underlying nonlinear SLAM systems.Simulations have also been demonstrated that the FEJ-based 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 EKF-based SLAM algorithm in the large-scale environments, a novel SLAM algorithm based on SEIF with the consistency constraint (SEIF-CC 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 error-state system and the nonlinear SLAM system.And the SEIF-CC SLAM will be compared with the SEIF-based SLAM through Monte Carlo simulations in terms of accuracy and consistency.
Moreover, autonomous navigation experiment of the C-Ranger AUV is employed to validate the reliability of the proposed SEIF-CC SLAM algorithm in this paper.The C-Ranger 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][35][36].The experimental results will show that the performance of the proposed algorithm is superior to the SEIF-based algorithm in terms of the consistency and accuracy based on the C-Ranger AUV.Furthermore, SEIF-CC SLAM also preserves the SEIF's low computational cost and storage requirements.
The remainder of the paper is organized as follows.The SEIF-based SLAM algorithm is presented briefly in Section 2. And Section 3 will analyze the consistency of SEIFbased SLAM.In Section 4, the proposed SEIF-CC SLAM algorithm is described and its performance is compared with SEIF-based SLAM through Monte Carlo simulations.Section 5 introduces the basic framework of the C-Ranger AUV platform as well as the onboard sensors.Then, results of sea trials will be presented to validate the performance of the proposed SEIF-CC SLAM algorithm.Finally, we summarize the results and give the future work.

The SEIF-Based SLAM
2.1.Information Form.The system model of SLAM includes the robot poses   and a series of features' or landmarks' locations  = (  1 ,   2 , . . .,    )  in the global coordinate system.The robot pose is determined by its position    = ( V ,  V ) and heading  V , 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   = { 1 ,  2 , . . .,   } and control inputs   = { 1 ,  2 , . . .,   } 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:

The Motion and Observation
Model.According to the system state at time , the motion model of the robot at time  + 1 is as follows: where   is the independent noise variable that obeyed the white Gaussian distribution,   ∼ (0, ).So the first-order 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 V  and the attitude information  V , which is provided by the velocity sensor and attitude sensor individually.The Jacobian matrix   can be presented (in this paper, the subscripts  |  and  + 1 |  refer to the state's update and prediction step at time , resp.): where (⋅) denotes the 2 × 2 rotation matrix and Δ  +1 is the velocity-based increment of the robot's position estimate between time steps  and  + 1: The observation model of the mobile robot is analyzed in the following.The approximation of the nonlinear observation function is derived from its first-order 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,   ∼ (0, ), the linearized observation model is given by The linearized observation model describes the direct measurement model of relative-position between features and robot.The relative-position is obtained by imaging sonar.

Some Related
Steps of SEIF-Based 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.

Motion
Step.Motion step is to predict the robot's state at time +1 according to one at time .Given information vector   and information matrix Λ  at time , the information vector  +1| and information matrix Λ +1| can be predicted: where Mathematical Problems in Engineering

Update
Step.The state estimation will be updated when a feature is repeatedly observed.The updated information matrix Λ +1 and information vector  +1 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   .

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 first-order Taylor expansion form are as follows: where   is the white Gaussian noise   ∼ (0, ) 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:

Consistency Analysis of SEIF-Based SLAM
In general, the system model of the real SLAM is higher dimension, highly nonlinear, and coupled.SEIF-based 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 first-order 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 (,  + 1] is given by where V  and   are the linear velocity and rotational velocity, respectively.They are the control inputs.
Considering the robot-to-landmark measurement as the relative-position 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 as So the first-order Lie derivative of ℎ 1 and ℎ 2 along  1 and  2 can be calculated: Then, the second-order Lie derivatives are as follows: The second-order Lie derivatives of other functions are equal to zeros.Obviously, the second-order Lie derivatives are the linear forms of the first-order ones.It is easy to find that the higher-order Lie derivatives can also express the linear form of the first-order ones; so the space spanned G can be denoted: Therefore, the observability matrix G with one landmark in the underlying nonlinear SLAM system is got as where Δ  =   −  V and Δ  =   −  V , 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 2 in the nonlinear SLAM system.

Local Observability of SEIF-Based SLAM. SEIF and EIF
are the information form of EKF based on single-point 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 Diag(⋅) expresses the block-diagonal matrix.It is nonsingular; so the rank of  is equivalent to the rank of , rank() = rank().

Ideal SEIF-Based SLAM.
As known, the evaluated filter Jacobian is derived by using the linearized error-state 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),  +1   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): Mathematical Problems in Engineering Now, the matrix Ȗ can be derived: The rank of the above matrix is 2, rank( Ȗ) = 2, which is equivalent to the rank of the actual nonlinear SLAM system.Therefore, the ideal SEIF-based SLAM and the actual nonlinear SLAM system have the similar observability.

Actual SEIF-Based 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 error-state 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 error-state model's observability matrix is equal to 3, rank() = 3.But the nonlinear observability matrix is two-dimensional according to formula (30).So the SEIF-based 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.

A SLAM Algorithm Based on SEIF with Consistency Constraint
4.1.The Consistency Constraint.The FEJ-based EKF estimator can improve the estimator's consistency [33].Its main idea is to choose the first-ever 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 P | is replaced by the position estimate P |−1 in the Jacobian matrix of the state.So formula (7) can be written as The mean will be recovered after motion step for P |−1 .
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 (SEIF-CC SLAM) with pseudocode is provided in Algorithm 1.And main differences are the motion and measurement step compared with SEIFbased SLAM.

Observability of SEIF-CC 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, rank(  ) = 2. Thus, the dimension of the linearized error-state system model is the same as that of the nonlinear observable system by means of the SEIF-CC SLAM algorithm.Similarly, it can be shown that the observability matrix   is of rank 2 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.

Simulation Experiment.
The performance of SEIF-CC SLAM is compared with SEIF-based SLAM through Monte Carlo simulations.50 Monte Carlo simulations with both SEIF and SEIF-CC 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, x and  are the estimated state and covariance matrix,   represents the dimension of , and  MC 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 nearest-neighbour (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 Chi-square 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 SEIF-CC.
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 SEIF-CC'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 SEIF-CC's error-state 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 SEIF-CC'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 SEIF-CC SLAM performs more accurate estimations than the SEIF-based SLAM.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 C-Ranger 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 C-Ranger travels at the speed of 1 knot and is fully charged.

C-Ranger AUV and Sea Trial
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 inner-hull parameter monitor module, the communication module, and peripherals for data processing.The bottom cabin is the battery cabin which contains li-ion battery packs, battery management module, and motion driven module.C-Ranger 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 C-Ranger.The two horizontal thrusters are equipped in the belly of C-Ranger 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.

Onboard Sensors.
There are some internal sensors installed on C-Ranger, 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 C-Ranger, working at the frequency of 720 kHz.It is the principal sensor of C-Ranger to provide active sensing of environment features for its real-time, crisp imagery of the underwater scene ahead.
In the navigation course of C-Ranger 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 high-precision GPS is installed at the top postmedian of C-Ranger 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.

SLAM for C-Ranger.
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,   ∼ (0,   ),   ∼ (0,   ).

Sea Trial.
The real-world 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 C-Ranger measured by GPS, and the starting point is marked by a yellow five-pointed star.The trajectory is regarded as ground truth used to compare the effectiveness of algorithms.The C-Ranger's sailing speed was about 1 knot in the experiment.
The imaging sonar was configured to 120 ∘ 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 real-world application.For the C-Ranger 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 SEIF-CC (the blue line).It can be seen that the path deviation of the proposed SEIF-CC SLAM relative to GPS is smaller than that of the SEIF-based SLAM.
Figures 8 and 9 present the average NEES and RMS errors of SEIF and SEIF-CC relative to GPS data, respectively.It can be seen that the NEES error of SEIF-CC is almost low of 10, but the SEIF's NEES is far higher than the SEIF-CC's NEES.So the consistency of the SEIF-CC SLAM algorithm is superior to the SEIF-based SLAM in sea trial.According to Figure 9, the RMS errors of SEIF-CC and SEIF are rising at the beginning, but the general trend of RMS error of SEIF is also upward while the SEIF-CC's error curve is dropt after a period of time.Therefore, the SEIF-CC SLAM algorithm is an efficient one with better consistency and accuracy in realworld environments.

Conclusion and Future Work
SEIF-based SLAM has been proposed to build large-scale 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.SEIF-CC SLAM performs better in terms of consistency and accuracy, because the observable matrix of this error-state system has the same rank as that of the nonlinear system.The NEES and RMS errors of SEIF-CC are compared with those of SEIF through Monte Carlo simulations and sea trials of C-Ranger AUV navigation in Tuandao Bay.The experimental results demonstrated that the SEIF-CC SLAM applied to AUV navigation can improve the consistency and accuracy compared with SEIF-based SLAM.
More work is required to further improve the consistency of SEIF in underwater large-scale 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, information-based 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 C-Ranger AUV.

Figure 1 :
Figure 1: The simulation path and environment.

Figure 2 :
Figure 2: Average NEES of the robot position errors.

Figure 6 :
Figure 6: The true environment of Tuandao Bay and GPS trajectory with the satellite image.