Mathematical Problems in Engineering

Volume 2015, Article ID 752360, 12 pages

http://dx.doi.org/10.1155/2015/752360

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

^{1}Information Science and Engineering College, Ocean University of China, Qingdao 266100, China^{2}Institute of Oceanographic Instrumentation of Shandong Academy of Sciences, Qingdao 266001, China^{3}Department of Mechanical and Electrical Engineering, China Jiliang University, Hangzhou 310018, China

Received 21 April 2015; Revised 6 August 2015; Accepted 16 August 2015

Academic Editor: Oscar Reinoso

Copyright © 2015 Yue Shen et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

#### 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 large-scale maps in the SLAM problem. However, there exists the inconsistency in the SEIF since the rank of the observability matrix 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.

#### 1. 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–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–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 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–25]. Then, some approaches have been developed to improve the inconsistency of SEIF-based 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 error-state 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–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 SEIF-based 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.

#### 2. 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 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 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 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 velocity-based 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 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, , 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.

##### 2.3. 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.

###### 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 first-order 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 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 is given bywhere 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 asSo the first-order Lie derivative of and along and 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 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 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 expresses the block-diagonal matrix. It is nonsingular; so the rank of is equivalent to the rank of , .

###### 3.2.1. 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), 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 SEIF-based SLAM and the actual nonlinear SLAM system have the similar observability.

###### 3.2.2. Actual SEIF-Based SLAM

The actual state variables are usually unknown in the observability analysis of SEIF-based 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, . 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.

#### 4. 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 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 (SEIF-CC SLAM) with pseudocode is provided in Algorithm 1. And main differences are the motion and measurement step compared with SEIF-based SLAM.