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

Authors: Yue Shen; Hao Zhang; Bo He; Tianhong Yan; Yang Liu
Journal: Mathematical Problems in Engineering
 (2015)

---

## 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 aC-Ranger AUV. Experimental results show that the proposed SEIF-CC SLAM algorithm yields more consistent and accurate estimates compared with the SEIF-based SLAM.


---

## Body

## 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 Section2. 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 posesx
t and a series of features’ or landmarks’ locations L
=
(
P
L
1
,
P
L
2
,
…
,
P
L
t
)
T in the global coordinate system. The robot pose is determined by its position P
R
t
=
(
x
v
t
,
y
v
t
) and heading θ
v
t, while the location of a feature is expressed as P
L
i
=
(
x
L
i
,
y
L
i
). So at time t, the state vector ξ
t can be expressed as follows:(1)
ξ
t
=
x
t
L
=
x
v
t
,
y
v
t
,
θ
v
t
,
x
L
1
,
y
L
1
,
…
,
x
L
t
,
y
L
t
T
=
P
R
t
,
θ
v
t
,
P
L
1
,
…
,
P
L
t
T
,where the superscript “T” refers to the transpose of a vector.The robot aims to find a probabilistic estimate of the state vectorξ
t in SLAM. The estimation problem transforms to calculate a posterior distribution p
x
t
,
L
∣
Z
t
,
U
t based on the Bayesian rule, where Z
t and U
t are past sensor measurements Z
t
=
z
1
,
z
2
,
…
,
z
t and control inputs U
t
=
u
1
,
u
2
,
…
,
u
t at time t, respectively. Reviewing the EKF solution to SLAM problem, the posterior p
(
x
t
,
L
∣
Z
t
,
U
t
) is obeying a multivariate Gaussian distribution:(2)
p
x
t
,
L
∣
Z
t
,
U
t
=
N
μ
x
t
μ
L
,
Σ
x
t
x
t
Σ
x
t
L
Σ
L
x
t
Σ
L
L
=
N
μ
t
,
Σ
t
,where μ
t and Σ
t are the mean vector and covariance matrix and N denotes the Gaussian distribution.Fundamental representation of SEIF is the information vectorη
t and information matrix Λ
t 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 ξ
t is also described by the information matrix and information vector:(3)
p
x
t
,
L
∣
Z
t
,
U
t
=
N
-
1
η
x
t
η
L
,
Λ
x
t
x
t
Λ
x
t
L
Λ
L
x
t
Λ
L
L
=
N
-
1
η
t
,
Λ
t
.In particular, the transformation between the state representations in EKF and the information form in EIF is got via the following formulas:(4)
Λ
t
=
Σ
t
-
1
,
η
t
=
Σ
t
-
1
μ
t
,
Σ
t
=
Λ
t
-
1
,
μ
t
=
Λ
t
-
1
η
t
.


### 2.2. The Motion and Observation Model

According to the system state at timet, the motion model of the robot at time t
+
1 is as follows:(5)
x
t
+
1
=
f
x
t
,
u
t
+
1
+
q
t
,where q
t is the independent noise variable that obeyed the white Gaussian distribution, q
t
~
N
(
0
,
Q
). So the first-order Taylor expansion of the motion model is as follows:(6)
x
t
+
1
≈
f
μ
t
,
u
t
+
1
+
F
t
x
t
-
μ
t
+
q
t
,where F
t is the Jacobian matrix of the motion function f with respect to μ
t.The system state of the robot is estimated according to the velocityv
t and the attitude information θ
v
t, which is provided by the velocity sensor and attitude sensor individually. The Jacobian matrix F
t can be presented (in this paper, the subscripts t
∣
t and t
+
1
∣
t refer to the state’s update and prediction step at time t, resp.):(7)
F
t
=
1
0
-
v
t
δ
t
sin
⁡
θ
^
v
t
0
1
v
t
δ
t
cos
⁡
θ
^
v
t
0
0
1
=
I
2
J
C
θ
^
v
t
Δ
P
R
t
+
1
0
1
×
2
1
≡
I
2
J
P
R
t
+
1
∣
t
-
P
R
t
∣
t
0
1
×
2
1
,where C
(
·
) denotes the 2 × 2 rotation matrix and Δ
P
R
t
+
1 is the velocity-based increment of the robot’s position estimate between time steps t and t
+
1:(8)
C
θ
^
v
t
=
cos
⁡
θ
^
v
t
-
sin
⁡
θ
^
v
t
sin
⁡
θ
^
v
t
cos
⁡
θ
^
v
t
,
Δ
P
R
t
+
1
=
v
t
δ
t
0
,
J
=
0
-
1
1
0
.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 theith landmark is (
x
L
i
,
y
L
i
)
T, H is the Jacobian matrix of the observation function h with respect to μ
t, and w
t is also subject to the white Gaussian noise, w
t
~
N
(
0
,
R
), the linearized observation model is given by(9)
z
t
=
h
ξ
t
+
w
t
≈
h
μ
t
+
H
t
ξ
t
-
μ
t
+
w
t
≈
h
C
T
θ
^
v
t
P
L
i
-
P
R
t
+
w
t
.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 timet
+
1 according to one at time t. Given information vector η
t and information matrix Λ
t at time t, the information vector η
t
+
1
∣
t and information matrix Λ
t
+
1
∣
t can be predicted:(10)
Λ
t
+
1
∣
t
=
Ψ
Q
-
1
F
t
Ω
-
1
Λ
x
t
L
Λ
L
x
t
Ω
-
1
F
t
T
Q
-
1
Λ
L
L
-
Λ
L
x
t
Ω
-
1
Λ
x
t
L
,
(11)
η
t
+
1
∣
t
=
Q
-
1
F
t
Ω
-
1
η
x
t
+
Ψ
u
η
L
-
Λ
L
x
t
Ω
-
1
F
t
T
Q
-
1
η
x
t
-
F
t
T
Q
-
1
u
,where(12)
Ψ
=
Q
+
F
t
Λ
x
t
x
t
F
t
T
-
1
,
Ω
=
Λ
x
t
x
t
+
F
t
T
Q
-
1
F
t
,
u
t
=
f
μ
x
t
,
u
t
+
1
-
F
t
μ
x
t
.


#### 2.3.2. Update Step

The state estimation will be updated when a feature is repeatedly observed. The updated information matrixΛ
t
+
1 and information vector η
t
+
1 can be expressed:(13)
Λ
t
+
1
=
Λ
t
+
1
∣
t
+
H
t
T
R
t
-
1
H
t
,
(14)
η
t
+
1
=
η
t
+
1
∣
t
+
H
t
T
R
t
-
1
u
,
(15)
u
=
z
t
-
h
μ
t
+
H
t
μ
t
,
(16)
H
t
=
∂
h
∂
x
t
0
⋯
0
∂
h
∂
L
t
0
⋯
0
T
,
(17)
∂
h
∂
x
t
=
H
R
t
=
∇
h
t
C
T
θ
^
v
t
-
I
2
-
J
P
L
t
-
P
R
t
,
∂
h
∂
L
t
=
H
L
t
=
∇
h
t
C
T
θ
^
v
t
,where ∇
h
t expresses the Jacobian of h corresponding to the landmark position relative to robot and elements of H
t are nonzero only at positions associated with x
t and L
t.


#### 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:(18)
l
t
=
g
x
t
,
z
t
+
ζ
t
≈
g
μ
t
+
G
x
t
-
μ
t
+
ζ
t
,where ζ
t is the white Gaussian noise ζ
t
~
N
(
0
,
W
) and G is Jacobian matrix of the observation function g with respect to μ
t. The augmented information matrix and information vector are as follows:(19)
Λ
t
+
1
=
Λ
x
t
x
t
+
G
T
W
-
1
G
Λ
x
t
L
-
G
T
W
-
1
Λ
L
x
t
Λ
L
L
0
-
W
-
1
G
0
W
-
1
,
(20)
η
t
+
1
=
η
x
t
-
G
T
W
-
1
u
η
L
W
-
1
uwith(21)
u
=
g
-
1
μ
t
,
z
t
-
G
μ
t
.The mean of stateμ
t is employed in update and sparsification steps; so the mean is recovered after update step by information vector and information matrix: μ
t
=
Λ
t
-
1
η
t.


## 2.1. Information Form

The system model of SLAM includes the robot posesx
t and a series of features’ or landmarks’ locations L
=
(
P
L
1
,
P
L
2
,
…
,
P
L
t
)
T in the global coordinate system. The robot pose is determined by its position P
R
t
=
(
x
v
t
,
y
v
t
) and heading θ
v
t, while the location of a feature is expressed as P
L
i
=
(
x
L
i
,
y
L
i
). So at time t, the state vector ξ
t can be expressed as follows:(1)
ξ
t
=
x
t
L
=
x
v
t
,
y
v
t
,
θ
v
t
,
x
L
1
,
y
L
1
,
…
,
x
L
t
,
y
L
t
T
=
P
R
t
,
θ
v
t
,
P
L
1
,
…
,
P
L
t
T
,where the superscript “T” refers to the transpose of a vector.The robot aims to find a probabilistic estimate of the state vectorξ
t in SLAM. The estimation problem transforms to calculate a posterior distribution p
x
t
,
L
∣
Z
t
,
U
t based on the Bayesian rule, where Z
t and U
t are past sensor measurements Z
t
=
z
1
,
z
2
,
…
,
z
t and control inputs U
t
=
u
1
,
u
2
,
…
,
u
t at time t, respectively. Reviewing the EKF solution to SLAM problem, the posterior p
(
x
t
,
L
∣
Z
t
,
U
t
) is obeying a multivariate Gaussian distribution:(2)
p
x
t
,
L
∣
Z
t
,
U
t
=
N
μ
x
t
μ
L
,
Σ
x
t
x
t
Σ
x
t
L
Σ
L
x
t
Σ
L
L
=
N
μ
t
,
Σ
t
,where μ
t and Σ
t are the mean vector and covariance matrix and N denotes the Gaussian distribution.Fundamental representation of SEIF is the information vectorη
t and information matrix Λ
t 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 ξ
t is also described by the information matrix and information vector:(3)
p
x
t
,
L
∣
Z
t
,
U
t
=
N
-
1
η
x
t
η
L
,
Λ
x
t
x
t
Λ
x
t
L
Λ
L
x
t
Λ
L
L
=
N
-
1
η
t
,
Λ
t
.In particular, the transformation between the state representations in EKF and the information form in EIF is got via the following formulas:(4)
Λ
t
=
Σ
t
-
1
,
η
t
=
Σ
t
-
1
μ
t
,
Σ
t
=
Λ
t
-
1
,
μ
t
=
Λ
t
-
1
η
t
.


## 2.2. The Motion and Observation Model

According to the system state at timet, the motion model of the robot at time t
+
1 is as follows:(5)
x
t
+
1
=
f
x
t
,
u
t
+
1
+
q
t
,where q
t is the independent noise variable that obeyed the white Gaussian distribution, q
t
~
N
(
0
,
Q
). So the first-order Taylor expansion of the motion model is as follows:(6)
x
t
+
1
≈
f
μ
t
,
u
t
+
1
+
F
t
x
t
-
μ
t
+
q
t
,where F
t is the Jacobian matrix of the motion function f with respect to μ
t.The system state of the robot is estimated according to the velocityv
t and the attitude information θ
v
t, which is provided by the velocity sensor and attitude sensor individually. The Jacobian matrix F
t can be presented (in this paper, the subscripts t
∣
t and t
+
1
∣
t refer to the state’s update and prediction step at time t, resp.):(7)
F
t
=
1
0
-
v
t
δ
t
sin
⁡
θ
^
v
t
0
1
v
t
δ
t
cos
⁡
θ
^
v
t
0
0
1
=
I
2
J
C
θ
^
v
t
Δ
P
R
t
+
1
0
1
×
2
1
≡
I
2
J
P
R
t
+
1
∣
t
-
P
R
t
∣
t
0
1
×
2
1
,where C
(
·
) denotes the 2 × 2 rotation matrix and Δ
P
R
t
+
1 is the velocity-based increment of the robot’s position estimate between time steps t and t
+
1:(8)
C
θ
^
v
t
=
cos
⁡
θ
^
v
t
-
sin
⁡
θ
^
v
t
sin
⁡
θ
^
v
t
cos
⁡
θ
^
v
t
,
Δ
P
R
t
+
1
=
v
t
δ
t
0
,
J
=
0
-
1
1
0
.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 theith landmark is (
x
L
i
,
y
L
i
)
T, H is the Jacobian matrix of the observation function h with respect to μ
t, and w
t is also subject to the white Gaussian noise, w
t
~
N
(
0
,
R
), the linearized observation model is given by(9)
z
t
=
h
ξ
t
+
w
t
≈
h
μ
t
+
H
t
ξ
t
-
μ
t
+
w
t
≈
h
C
T
θ
^
v
t
P
L
i
-
P
R
t
+
w
t
.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 timet
+
1 according to one at time t. Given information vector η
t and information matrix Λ
t at time t, the information vector η
t
+
1
∣
t and information matrix Λ
t
+
1
∣
t can be predicted:(10)
Λ
t
+
1
∣
t
=
Ψ
Q
-
1
F
t
Ω
-
1
Λ
x
t
L
Λ
L
x
t
Ω
-
1
F
t
T
Q
-
1
Λ
L
L
-
Λ
L
x
t
Ω
-
1
Λ
x
t
L
,
(11)
η
t
+
1
∣
t
=
Q
-
1
F
t
Ω
-
1
η
x
t
+
Ψ
u
η
L
-
Λ
L
x
t
Ω
-
1
F
t
T
Q
-
1
η
x
t
-
F
t
T
Q
-
1
u
,where(12)
Ψ
=
Q
+
F
t
Λ
x
t
x
t
F
t
T
-
1
,
Ω
=
Λ
x
t
x
t
+
F
t
T
Q
-
1
F
t
,
u
t
=
f
μ
x
t
,
u
t
+
1
-
F
t
μ
x
t
.


### 2.3.2. Update Step

The state estimation will be updated when a feature is repeatedly observed. The updated information matrixΛ
t
+
1 and information vector η
t
+
1 can be expressed:(13)
Λ
t
+
1
=
Λ
t
+
1
∣
t
+
H
t
T
R
t
-
1
H
t
,
(14)
η
t
+
1
=
η
t
+
1
∣
t
+
H
t
T
R
t
-
1
u
,
(15)
u
=
z
t
-
h
μ
t
+
H
t
μ
t
,
(16)
H
t
=
∂
h
∂
x
t
0
⋯
0
∂
h
∂
L
t
0
⋯
0
T
,
(17)
∂
h
∂
x
t
=
H
R
t
=
∇
h
t
C
T
θ
^
v
t
-
I
2
-
J
P
L
t
-
P
R
t
,
∂
h
∂
L
t
=
H
L
t
=
∇
h
t
C
T
θ
^
v
t
,where ∇
h
t expresses the Jacobian of h corresponding to the landmark position relative to robot and elements of H
t are nonzero only at positions associated with x
t and L
t.


### 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:(18)
l
t
=
g
x
t
,
z
t
+
ζ
t
≈
g
μ
t
+
G
x
t
-
μ
t
+
ζ
t
,where ζ
t is the white Gaussian noise ζ
t
~
N
(
0
,
W
) and G is Jacobian matrix of the observation function g with respect to μ
t. The augmented information matrix and information vector are as follows:(19)
Λ
t
+
1
=
Λ
x
t
x
t
+
G
T
W
-
1
G
Λ
x
t
L
-
G
T
W
-
1
Λ
L
x
t
Λ
L
L
0
-
W
-
1
G
0
W
-
1
,
(20)
η
t
+
1
=
η
x
t
-
G
T
W
-
1
u
η
L
W
-
1
uwith(21)
u
=
g
-
1
μ
t
,
z
t
-
G
μ
t
.The mean of stateμ
t is employed in update and sparsification steps; so the mean is recovered after update step by information vector and information matrix: μ
t
=
Λ
t
-
1
η
t.


## 2.3.1. Motion Step

Motion step is to predict the robot’s state at timet
+
1 according to one at time t. Given information vector η
t and information matrix Λ
t at time t, the information vector η
t
+
1
∣
t and information matrix Λ
t
+
1
∣
t can be predicted:(10)
Λ
t
+
1
∣
t
=
Ψ
Q
-
1
F
t
Ω
-
1
Λ
x
t
L
Λ
L
x
t
Ω
-
1
F
t
T
Q
-
1
Λ
L
L
-
Λ
L
x
t
Ω
-
1
Λ
x
t
L
,
(11)
η
t
+
1
∣
t
=
Q
-
1
F
t
Ω
-
1
η
x
t
+
Ψ
u
η
L
-
Λ
L
x
t
Ω
-
1
F
t
T
Q
-
1
η
x
t
-
F
t
T
Q
-
1
u
,where(12)
Ψ
=
Q
+
F
t
Λ
x
t
x
t
F
t
T
-
1
,
Ω
=
Λ
x
t
x
t
+
F
t
T
Q
-
1
F
t
,
u
t
=
f
μ
x
t
,
u
t
+
1
-
F
t
μ
x
t
.


## 2.3.2. Update Step

The state estimation will be updated when a feature is repeatedly observed. The updated information matrixΛ
t
+
1 and information vector η
t
+
1 can be expressed:(13)
Λ
t
+
1
=
Λ
t
+
1
∣
t
+
H
t
T
R
t
-
1
H
t
,
(14)
η
t
+
1
=
η
t
+
1
∣
t
+
H
t
T
R
t
-
1
u
,
(15)
u
=
z
t
-
h
μ
t
+
H
t
μ
t
,
(16)
H
t
=
∂
h
∂
x
t
0
⋯
0
∂
h
∂
L
t
0
⋯
0
T
,
(17)
∂
h
∂
x
t
=
H
R
t
=
∇
h
t
C
T
θ
^
v
t
-
I
2
-
J
P
L
t
-
P
R
t
,
∂
h
∂
L
t
=
H
L
t
=
∇
h
t
C
T
θ
^
v
t
,where ∇
h
t expresses the Jacobian of h corresponding to the landmark position relative to robot and elements of H
t are nonzero only at positions associated with x
t and L
t.


## 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:(18)
l
t
=
g
x
t
,
z
t
+
ζ
t
≈
g
μ
t
+
G
x
t
-
μ
t
+
ζ
t
,where ζ
t is the white Gaussian noise ζ
t
~
N
(
0
,
W
) and G is Jacobian matrix of the observation function g with respect to μ
t. The augmented information matrix and information vector are as follows:(19)
Λ
t
+
1
=
Λ
x
t
x
t
+
G
T
W
-
1
G
Λ
x
t
L
-
G
T
W
-
1
Λ
L
x
t
Λ
L
L
0
-
W
-
1
G
0
W
-
1
,
(20)
η
t
+
1
=
η
x
t
-
G
T
W
-
1
u
η
L
W
-
1
uwith(21)
u
=
g
-
1
μ
t
,
z
t
-
G
μ
t
.The mean of stateμ
t is employed in update and sparsification steps; so the mean is recovered after update step by information vector and information matrix: μ
t
=
Λ
t
-
1
η
t.


## 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(
t
,
t
+
1
] is given by(22)
Δ
x
v
t
Δ
y
v
t
Δ
θ
v
t
Δ
x
L
Δ
y
L
=
cos
⁡
θ
v
t
sin
⁡
θ
v
t
0
0
0
v
t
+
0
0
1
0
0
ω
t
⟹
Δ
x
t
=
f
1
v
t
+
f
2
ω
t
,where v
t and ω
t 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 timet is(23)
z
t
=
C
T
θ
v
t
P
L
-
P
v
t
=
cos
⁡
θ
v
t
x
L
-
x
v
t
+
sin
⁡
θ
v
t
y
L
-
y
v
t
-
sin
⁡
θ
v
t
x
L
-
x
v
t
+
cos
⁡
θ
v
t
y
L
-
y
v
t
⟹
z
t
=
h
1
h
2
.The Lie derivative of a functionh along an analytic vector field f is defined as(24)
L
f
h
=
d
h
T
f
.So the first-order Lie derivative of h
1 and h
2 along f
1 and f
2 can be calculated:(25)
L
f
1
h
1
=
-
1
,
L
f
1
h
2
=
0
,
L
f
2
h
1
=
-
sin
⁡
θ
v
t
x
L
-
x
v
t
+
cos
⁡
θ
v
t
y
L
-
y
v
t
,
L
f
2
h
2
=
-
cos
⁡
θ
v
t
x
L
-
x
v
t
-
sin
⁡
θ
v
t
y
L
-
y
v
t
.Then, the second-order Lie derivatives are as follows:(26)
L
f
2
L
f
2
h
1
=
-
cos
⁡
θ
v
t
x
L
-
x
v
t
-
sin
⁡
θ
v
t
y
L
-
y
v
t
,
L
f
1
L
f
2
h
2
=
1
,
L
f
2
L
f
2
h
2
=
-
sin
⁡
θ
v
t
x
L
-
x
v
t
+
cos
⁡
θ
v
t
y
L
-
y
v
t
.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 spannedG can be denoted:(27)
G
=
s
p
a
n
L
f
1
h
1
L
f
1
h
2
L
f
2
h
1
L
f
2
h
2
.Therefore, the observability matrixd
G with one landmark in the underlying nonlinear SLAM system is got as(28)
d
G
=
sin
⁡
θ
v
t
cos
⁡
θ
v
t
-
cos
⁡
θ
v
t
sin
⁡
θ
v
t
-
Δ
x
L
cos
⁡
θ
v
t
-
Δ
y
L
sin
⁡
θ
v
t
Δ
x
L
sin
⁡
θ
v
t
-
Δ
y
L
cos
⁡
θ
v
t
-
sin
⁡
θ
v
t
cos
⁡
θ
v
t
-
cos
⁡
θ
v
t
-
sin
⁡
θ
v
t
T
,where Δ
x
L
=
x
L
-
x
v
t and Δ
y
L
=
y
L
-
y
v
t, respectively. The rank of the observability matrix of the nonlinear SLAM system is 2. So the rank of the observability matrix with M-dimensional landmarks is 2
M 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 stepst and t
+
m, the local observability matrix during m time intervals can be defined: (29)
D
=
H
t
H
t
+
1
F
t
⋮
H
t
+
m
F
t
+
m
-
1
⋯
F
t
.Here, F
i and H
i have been given in the formulas (7), (16), and (17), respectively. Formula (29) can be expanded:(30)
D
=
H
R
t
H
L
t
H
R
t
+
1
F
t
H
L
t
+
1
⋮
⋮
H
R
t
+
m
F
t
+
m
-
1
⋯
F
t
H
L
t
+
m
=
D
i
a
g
H
L
t
,
…
,
H
L
t
+
m
H
L
t
-
1
H
R
t
I
2
H
L
t
+
1
-
1
H
R
t
+
1
F
t
I
2
⋮
⋮
H
L
t
+
m
-
1
H
R
t
+
m
F
t
+
m
-
1
⋯
F
t
I
2
︸
U
,where D
i
a
g
(
·
) expresses the block-diagonal matrix. It is nonsingular; so the rank of U is equivalent to the rank of D, r
a
n
k
(
D
)
=
r
a
n
k
(
U
).


#### 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μ
t.According to formula (7), F
t
+
1
F
t is got as follows: (31)
F
t
+
1
F
t
=
I
2
J
P
R
t
+
2
-
P
R
t
+
1
0
1
×
2
1
I
2
J
P
R
t
+
1
-
P
R
t
0
1
×
2
1
=
I
2
J
P
R
t
+
2
-
P
R
t
0
1
×
2
1
.So the following formula (32) can be induced:(32)
F
t
+
i
-
1
F
t
+
i
-
2
⋯
F
t
=
I
2
J
P
R
t
+
i
-
P
R
t
0
1
×
2
1
.Formula (33) can be deduced by means of formulas (17) and (32) (the superscript “˘” is used to denote the true state values):(33)
H
˘
L
t
+
i
-
1
H
˘
R
t
+
i
F
˘
t
+
i
-
1
⋯
F
˘
t
=
C
-
T
θ
v
t
+
i
∇
h
˘
t
+
i
-
1
∇
h
˘
t
+
i
C
T
θ
v
t
+
i
-
I
2
-
J
P
L
-
P
R
t
+
1
I
2
J
P
R
t
+
i
-
P
R
t
0
1
×
2
1
=
C
θ
v
t
+
i
-
C
T
θ
v
t
+
i
I
2
J
P
L
-
P
R
t
=
-
I
2
J
P
L
-
P
R
t
.Now, the matrix U
˘ can be derived:(34)
U
˘
=
-
I
2
-
J
P
L
-
P
R
t
I
2
-
I
2
-
J
P
L
-
P
R
t
I
2
⋮
⋮
⋮
-
I
2
-
J
P
L
-
P
R
t
I
2
.The rank of the above matrix is 2,r
a
n
k
(
U
˘
)
=
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μ
t of the state after update. Similar to formulas (32) and (33), the expression can be derived:(35)
F
t
+
i
-
1
F
t
+
i
-
2
⋯
F
t
=
I
2
J
P
^
R
t
+
2
∣
t
+
1
-
P
^
R
t
+
1
∣
t
+
1
0
1
×
2
1
I
2
J
P
^
R
t
+
1
∣
t
-
P
^
R
t
∣
t
0
1
×
2
1
=
I
2
J
P
^
R
t
+
i
∣
t
+
i
-
1
-
P
^
R
t
∣
t
+
∑
j
=
t
t
+
i
-
2
P
^
R
j
+
1
∣
j
-
P
^
R
j
+
1
∣
j
+
1
0
1
×
2
1
H
L
t
+
i
-
1
H
R
t
+
i
F
t
+
i
-
1
⋯
F
t
=
-
C
-
T
θ
v
t
+
i
∣
t
+
i
-
1
∇
h
t
+
i
-
1
∇
h
t
+
i
C
T
θ
v
t
+
i
∣
t
+
i
-
1
I
2
J
P
L
t
+
i
∣
t
+
i
-
1
-
P
R
t
+
i
∣
t
+
i
-
1
I
2
J
P
^
R
t
+
i
∣
t
+
i
-
1
-
P
^
R
t
∣
t
+
∑
j
=
t
t
+
i
-
2
P
^
R
j
+
1
∣
j
-
P
^
R
j
+
1
∣
j
+
1
0
1
×
2
1
=
I
2
J
P
L
t
+
i
∣
t
+
i
-
1
-
P
^
R
t
∣
t
+
∑
j
=
t
t
+
i
-
2
P
^
R
j
+
1
∣
j
-
P
^
R
j
+
1
∣
j
+
1
.Now, the matrixU can be written:(36)
U
=
-
I
2
-
J
P
L
t
∣
t
-
1
-
P
^
R
t
∣
t
-
1
I
2
-
I
2
-
J
P
L
t
+
1
∣
t
-
P
^
R
t
∣
t
I
2
-
I
2
-
J
P
L
t
+
2
∣
t
+
1
-
P
^
R
t
∣
t
+
P
^
R
t
+
1
∣
t
-
P
^
R
t
+
1
∣
t
+
1
I
2
⋮
⋮
⋮
-
I
2
-
J
P
L
t
+
i
∣
t
+
i
-
1
-
P
^
R
t
∣
t
+
∑
j
=
t
t
+
i
-
2
P
^
R
j
+
1
∣
j
-
P
^
R
j
+
1
∣
j
+
1
I
2
.The rank of the linearized error-state model’s observability matrix is equal to 3,r
a
n
k
(
U
)
=
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 μ
t 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.


## 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(
t
,
t
+
1
] is given by(22)
Δ
x
v
t
Δ
y
v
t
Δ
θ
v
t
Δ
x
L
Δ
y
L
=
cos
⁡
θ
v
t
sin
⁡
θ
v
t
0
0
0
v
t
+
0
0
1
0
0
ω
t
⟹
Δ
x
t
=
f
1
v
t
+
f
2
ω
t
,where v
t and ω
t 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 timet is(23)
z
t
=
C
T
θ
v
t
P
L
-
P
v
t
=
cos
⁡
θ
v
t
x
L
-
x
v
t
+
sin
⁡
θ
v
t
y
L
-
y
v
t
-
sin
⁡
θ
v
t
x
L
-
x
v
t
+
cos
⁡
θ
v
t
y
L
-
y
v
t
⟹
z
t
=
h
1
h
2
.The Lie derivative of a functionh along an analytic vector field f is defined as(24)
L
f
h
=
d
h
T
f
.So the first-order Lie derivative of h
1 and h
2 along f
1 and f
2 can be calculated:(25)
L
f
1
h
1
=
-
1
,
L
f
1
h
2
=
0
,
L
f
2
h
1
=
-
sin
⁡
θ
v
t
x
L
-
x
v
t
+
cos
⁡
θ
v
t
y
L
-
y
v
t
,
L
f
2
h
2
=
-
cos
⁡
θ
v
t
x
L
-
x
v
t
-
sin
⁡
θ
v
t
y
L
-
y
v
t
.Then, the second-order Lie derivatives are as follows:(26)
L
f
2
L
f
2
h
1
=
-
cos
⁡
θ
v
t
x
L
-
x
v
t
-
sin
⁡
θ
v
t
y
L
-
y
v
t
,
L
f
1
L
f
2
h
2
=
1
,
L
f
2
L
f
2
h
2
=
-
sin
⁡
θ
v
t
x
L
-
x
v
t
+
cos
⁡
θ
v
t
y
L
-
y
v
t
.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 spannedG can be denoted:(27)
G
=
s
p
a
n
L
f
1
h
1
L
f
1
h
2
L
f
2
h
1
L
f
2
h
2
.Therefore, the observability matrixd
G with one landmark in the underlying nonlinear SLAM system is got as(28)
d
G
=
sin
⁡
θ
v
t
cos
⁡
θ
v
t
-
cos
⁡
θ
v
t
sin
⁡
θ
v
t
-
Δ
x
L
cos
⁡
θ
v
t
-
Δ
y
L
sin
⁡
θ
v
t
Δ
x
L
sin
⁡
θ
v
t
-
Δ
y
L
cos
⁡
θ
v
t
-
sin
⁡
θ
v
t
cos
⁡
θ
v
t
-
cos
⁡
θ
v
t
-
sin
⁡
θ
v
t
T
,where Δ
x
L
=
x
L
-
x
v
t and Δ
y
L
=
y
L
-
y
v
t, respectively. The rank of the observability matrix of the nonlinear SLAM system is 2. So the rank of the observability matrix with M-dimensional landmarks is 2
M 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 stepst and t
+
m, the local observability matrix during m time intervals can be defined: (29)
D
=
H
t
H
t
+
1
F
t
⋮
H
t
+
m
F
t
+
m
-
1
⋯
F
t
.Here, F
i and H
i have been given in the formulas (7), (16), and (17), respectively. Formula (29) can be expanded:(30)
D
=
H
R
t
H
L
t
H
R
t
+
1
F
t
H
L
t
+
1
⋮
⋮
H
R
t
+
m
F
t
+
m
-
1
⋯
F
t
H
L
t
+
m
=
D
i
a
g
H
L
t
,
…
,
H
L
t
+
m
H
L
t
-
1
H
R
t
I
2
H
L
t
+
1
-
1
H
R
t
+
1
F
t
I
2
⋮
⋮
H
L
t
+
m
-
1
H
R
t
+
m
F
t
+
m
-
1
⋯
F
t
I
2
︸
U
,where D
i
a
g
(
·
) expresses the block-diagonal matrix. It is nonsingular; so the rank of U is equivalent to the rank of D, r
a
n
k
(
D
)
=
r
a
n
k
(
U
).


### 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μ
t.According to formula (7), F
t
+
1
F
t is got as follows: (31)
F
t
+
1
F
t
=
I
2
J
P
R
t
+
2
-
P
R
t
+
1
0
1
×
2
1
I
2
J
P
R
t
+
1
-
P
R
t
0
1
×
2
1
=
I
2
J
P
R
t
+
2
-
P
R
t
0
1
×
2
1
.So the following formula (32) can be induced:(32)
F
t
+
i
-
1
F
t
+
i
-
2
⋯
F
t
=
I
2
J
P
R
t
+
i
-
P
R
t
0
1
×
2
1
.Formula (33) can be deduced by means of formulas (17) and (32) (the superscript “˘” is used to denote the true state values):(33)
H
˘
L
t
+
i
-
1
H
˘
R
t
+
i
F
˘
t
+
i
-
1
⋯
F
˘
t
=
C
-
T
θ
v
t
+
i
∇
h
˘
t
+
i
-
1
∇
h
˘
t
+
i
C
T
θ
v
t
+
i
-
I
2
-
J
P
L
-
P
R
t
+
1
I
2
J
P
R
t
+
i
-
P
R
t
0
1
×
2
1
=
C
θ
v
t
+
i
-
C
T
θ
v
t
+
i
I
2
J
P
L
-
P
R
t
=
-
I
2
J
P
L
-
P
R
t
.Now, the matrix U
˘ can be derived:(34)
U
˘
=
-
I
2
-
J
P
L
-
P
R
t
I
2
-
I
2
-
J
P
L
-
P
R
t
I
2
⋮
⋮
⋮
-
I
2
-
J
P
L
-
P
R
t
I
2
.The rank of the above matrix is 2,r
a
n
k
(
U
˘
)
=
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μ
t of the state after update. Similar to formulas (32) and (33), the expression can be derived:(35)
F
t
+
i
-
1
F
t
+
i
-
2
⋯
F
t
=
I
2
J
P
^
R
t
+
2
∣
t
+
1
-
P
^
R
t
+
1
∣
t
+
1
0
1
×
2
1
I
2
J
P
^
R
t
+
1
∣
t
-
P
^
R
t
∣
t
0
1
×
2
1
=
I
2
J
P
^
R
t
+
i
∣
t
+
i
-
1
-
P
^
R
t
∣
t
+
∑
j
=
t
t
+
i
-
2
P
^
R
j
+
1
∣
j
-
P
^
R
j
+
1
∣
j
+
1
0
1
×
2
1
H
L
t
+
i
-
1
H
R
t
+
i
F
t
+
i
-
1
⋯
F
t
=
-
C
-
T
θ
v
t
+
i
∣
t
+
i
-
1
∇
h
t
+
i
-
1
∇
h
t
+
i
C
T
θ
v
t
+
i
∣
t
+
i
-
1
I
2
J
P
L
t
+
i
∣
t
+
i
-
1
-
P
R
t
+
i
∣
t
+
i
-
1
I
2
J
P
^
R
t
+
i
∣
t
+
i
-
1
-
P
^
R
t
∣
t
+
∑
j
=
t
t
+
i
-
2
P
^
R
j
+
1
∣
j
-
P
^
R
j
+
1
∣
j
+
1
0
1
×
2
1
=
I
2
J
P
L
t
+
i
∣
t
+
i
-
1
-
P
^
R
t
∣
t
+
∑
j
=
t
t
+
i
-
2
P
^
R
j
+
1
∣
j
-
P
^
R
j
+
1
∣
j
+
1
.Now, the matrixU can be written:(36)
U
=
-
I
2
-
J
P
L
t
∣
t
-
1
-
P
^
R
t
∣
t
-
1
I
2
-
I
2
-
J
P
L
t
+
1
∣
t
-
P
^
R
t
∣
t
I
2
-
I
2
-
J
P
L
t
+
2
∣
t
+
1
-
P
^
R
t
∣
t
+
P
^
R
t
+
1
∣
t
-
P
^
R
t
+
1
∣
t
+
1
I
2
⋮
⋮
⋮
-
I
2
-
J
P
L
t
+
i
∣
t
+
i
-
1
-
P
^
R
t
∣
t
+
∑
j
=
t
t
+
i
-
2
P
^
R
j
+
1
∣
j
-
P
^
R
j
+
1
∣
j
+
1
I
2
.The rank of the linearized error-state model’s observability matrix is equal to 3,r
a
n
k
(
U
)
=
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 μ
t 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.


## 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μ
t.According to formula (7), F
t
+
1
F
t is got as follows: (31)
F
t
+
1
F
t
=
I
2
J
P
R
t
+
2
-
P
R
t
+
1
0
1
×
2
1
I
2
J
P
R
t
+
1
-
P
R
t
0
1
×
2
1
=
I
2
J
P
R
t
+
2
-
P
R
t
0
1
×
2
1
.So the following formula (32) can be induced:(32)
F
t
+
i
-
1
F
t
+
i
-
2
⋯
F
t
=
I
2
J
P
R
t
+
i
-
P
R
t
0
1
×
2
1
.Formula (33) can be deduced by means of formulas (17) and (32) (the superscript “˘” is used to denote the true state values):(33)
H
˘
L
t
+
i
-
1
H
˘
R
t
+
i
F
˘
t
+
i
-
1
⋯
F
˘
t
=
C
-
T
θ
v
t
+
i
∇
h
˘
t
+
i
-
1
∇
h
˘
t
+
i
C
T
θ
v
t
+
i
-
I
2
-
J
P
L
-
P
R
t
+
1
I
2
J
P
R
t
+
i
-
P
R
t
0
1
×
2
1
=
C
θ
v
t
+
i
-
C
T
θ
v
t
+
i
I
2
J
P
L
-
P
R
t
=
-
I
2
J
P
L
-
P
R
t
.Now, the matrix U
˘ can be derived:(34)
U
˘
=
-
I
2
-
J
P
L
-
P
R
t
I
2
-
I
2
-
J
P
L
-
P
R
t
I
2
⋮
⋮
⋮
-
I
2
-
J
P
L
-
P
R
t
I
2
.The rank of the above matrix is 2,r
a
n
k
(
U
˘
)
=
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μ
t of the state after update. Similar to formulas (32) and (33), the expression can be derived:(35)
F
t
+
i
-
1
F
t
+
i
-
2
⋯
F
t
=
I
2
J
P
^
R
t
+
2
∣
t
+
1
-
P
^
R
t
+
1
∣
t
+
1
0
1
×
2
1
I
2
J
P
^
R
t
+
1
∣
t
-
P
^
R
t
∣
t
0
1
×
2
1
=
I
2
J
P
^
R
t
+
i
∣
t
+
i
-
1
-
P
^
R
t
∣
t
+
∑
j
=
t
t
+
i
-
2
P
^
R
j
+
1
∣
j
-
P
^
R
j
+
1
∣
j
+
1
0
1
×
2
1
H
L
t
+
i
-
1
H
R
t
+
i
F
t
+
i
-
1
⋯
F
t
=
-
C
-
T
θ
v
t
+
i
∣
t
+
i
-
1
∇
h
t
+
i
-
1
∇
h
t
+
i
C
T
θ
v
t
+
i
∣
t
+
i
-
1
I
2
J
P
L
t
+
i
∣
t
+
i
-
1
-
P
R
t
+
i
∣
t
+
i
-
1
I
2
J
P
^
R
t
+
i
∣
t
+
i
-
1
-
P
^
R
t
∣
t
+
∑
j
=
t
t
+
i
-
2
P
^
R
j
+
1
∣
j
-
P
^
R
j
+
1
∣
j
+
1
0
1
×
2
1
=
I
2
J
P
L
t
+
i
∣
t
+
i
-
1
-
P
^
R
t
∣
t
+
∑
j
=
t
t
+
i
-
2
P
^
R
j
+
1
∣
j
-
P
^
R
j
+
1
∣
j
+
1
.Now, the matrixU can be written:(36)
U
=
-
I
2
-
J
P
L
t
∣
t
-
1
-
P
^
R
t
∣
t
-
1
I
2
-
I
2
-
J
P
L
t
+
1
∣
t
-
P
^
R
t
∣
t
I
2
-
I
2
-
J
P
L
t
+
2
∣
t
+
1
-
P
^
R
t
∣
t
+
P
^
R
t
+
1
∣
t
-
P
^
R
t
+
1
∣
t
+
1
I
2
⋮
⋮
⋮
-
I
2
-
J
P
L
t
+
i
∣
t
+
i
-
1
-
P
^
R
t
∣
t
+
∑
j
=
t
t
+
i
-
2
P
^
R
j
+
1
∣
j
-
P
^
R
j
+
1
∣
j
+
1
I
2
.The rank of the linearized error-state model’s observability matrix is equal to 3,r
a
n
k
(
U
)
=
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 μ
t 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 estimateP
^
R
t
∣
t is replaced by the position estimate P
^
R
t
∣
t
-
1 in the Jacobian matrix of the state. So formula (7) can be written as(37)
F
t
′
=
I
2
J
P
^
R
t
+
1
∣
t
-
P
^
R
t
∣
t
-
1
0
1
×
2
1
.The mean will be recovered after motion step for P
^
R
t
∣
t
-
1.Assuming that a landmark was first observed at timet, the measurement Jacobian can be written as(38)
H
R
t
′
=
∇
h
t
C
T
θ
^
v
t
∣
t
-
1
-
I
2
-
J
P
^
L
t
∣
t
-
P
^
R
t
∣
t
-
1
.


### 4.2. Algorithm Process

The proposed SLAM based on SEIF with consistency constraint (SEIF-CC SLAM) with pseudocode is provided in Algorithm1. And main differences are the motion and measurement step compared with SEIF-based SLAM.Algorithm 1:The SEIF-CC SLAM algorithm.
Require: waypoints
GlobalIN
F
matrix  IN
F
vector  X
predict  Z
obs  //global variables
[
X
true
Z
obs
] = Initialization (0);  //initialize global variables
Threshold = 8;  //The limited number of active features
WHILE waypoints not 0DO       //main loop
X
true
≔
vehicle_model
(
X
true, v, g, d
t); //new vehicle pose
Add random noise to nominal control values:[
v
n
,
g
n
]
←
(
v
,
g
,
Q
)
//Motion model
Predict_FEJ(v
n, g
n, Q, d
t)      //prediction
Predict stateP
^
R
t
+
1
∣
t
ComputeF
t
′
← Equation (37)  //estimate at X
predict
Computeu
t  Ψ  Ω
← Equation (12) with F
t
′
Predict information matrixΛ
t
+
1
∣
t
← Equation (10)
Predict information vectorη
t
+
1
∣
t
← Equation (11)
Mean recovery:X
predict
←
Λ
t
-
1
η
t
Get observationsZ
obs: set of rang-bearing observation
Add random measurement noiseZ
obs
←
(
Z
obs
,
R
)
//Data association
//i
d
f: index tags for each landmark;
z
old
,
z
new
,
i
d
f
=
Data_associate
Z
obs
,
R
,
GA
T
rej
,
GA
T
aug;
//Update step
IFz
old not empty  THEN //z
old is not a new feature
Observe_Update(z
old, i
d
f, R)
FORi from 1 to size(Z
obs, 2) DO
Compute H
R
t
′
← Equation (38)
Compute u
← Equation (15)
Compute Λ
t
+
1
← Equation (13)
Compute η
t
+
1
← Equation (14)
END FOR
END IF
//Augmentation step
IFz
new not empty  THEN //new feature
SEIF_Augment(z
new
,
W)
ComputeG at X
predict
Computeu
← Equation (21)
ComputeΛ
t
+
!
← Equation (19)
Computeη
t
+
1
← Equation (20)
END IF
Recover meanX
predict
←
Λ
t
-
1
η
t
IF the number of active features > Threshold THEN
SEIF_Sparse();  //sparsification
END IF
END WHILE


### 4.3. 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 matrixH
R
t
′ at the evaluated point. As a result, the observability matrix of this new filter can be presented:(39)
U
′
=
-
I
2
-
J
P
^
L
t
∣
t
-
P
^
R
t
∣
t
-
1
I
2
-
I
2
-
J
P
^
L
t
∣
t
-
P
^
R
t
∣
t
-
1
I
2
⋮
⋮
⋮
-
I
2
-
J
P
^
L
t
∣
t
-
P
^
R
t
∣
t
-
1
I
2
.It can be easily proven that the rank ofU
′ is 2, r
a
n
k
(
U
′
)
=
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 U
′ is of rank 2
M when there are M 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 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:(40)
Average  NEES
=
1
N
M
C
∑
i
=
1
N
M
C
x
i
-
x
^
i
T
S
i
-
1
x
i
-
x
^
i
,
R
M
S
=
1
n
x
∑
i
=
1
n
x
x
i
-
x
^
i
2
n
,where x
i is the ground truth for the state vector, x
^
i and S are the estimated state and covariance matrix, n
x represents the dimension of x, and N
M
C is the times of Monte Carlo simulations.The simulation path and environment are shown in the Figure1, 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.Figure 1
The simulation path and environment.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 withn
x degrees of freedom [37], where n
x is the dimension of x. 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 Figure2, 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.Figure 2
Average NEES of the robot position errors.Figures3 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.Figure 3
RMS errors for the robot position.Figure 4
RMS errors for the robot heading.


## 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 estimateP
^
R
t
∣
t is replaced by the position estimate P
^
R
t
∣
t
-
1 in the Jacobian matrix of the state. So formula (7) can be written as(37)
F
t
′
=
I
2
J
P
^
R
t
+
1
∣
t
-
P
^
R
t
∣
t
-
1
0
1
×
2
1
.The mean will be recovered after motion step for P
^
R
t
∣
t
-
1.Assuming that a landmark was first observed at timet, the measurement Jacobian can be written as(38)
H
R
t
′
=
∇
h
t
C
T
θ
^
v
t
∣
t
-
1
-
I
2
-
J
P
^
L
t
∣
t
-
P
^
R
t
∣
t
-
1
.


## 4.2. Algorithm Process

The proposed SLAM based on SEIF with consistency constraint (SEIF-CC SLAM) with pseudocode is provided in Algorithm1. And main differences are the motion and measurement step compared with SEIF-based SLAM.Algorithm 1:The SEIF-CC SLAM algorithm.
Require: waypoints
GlobalIN
F
matrix  IN
F
vector  X
predict  Z
obs  //global variables
[
X
true
Z
obs
] = Initialization (0);  //initialize global variables
Threshold = 8;  //The limited number of active features
WHILE waypoints not 0DO       //main loop
X
true
≔
vehicle_model
(
X
true, v, g, d
t); //new vehicle pose
Add random noise to nominal control values:[
v
n
,
g
n
]
←
(
v
,
g
,
Q
)
//Motion model
Predict_FEJ(v
n, g
n, Q, d
t)      //prediction
Predict stateP
^
R
t
+
1
∣
t
ComputeF
t
′
← Equation (37)  //estimate at X
predict
Computeu
t  Ψ  Ω
← Equation (12) with F
t
′
Predict information matrixΛ
t
+
1
∣
t
← Equation (10)
Predict information vectorη
t
+
1
∣
t
← Equation (11)
Mean recovery:X
predict
←
Λ
t
-
1
η
t
Get observationsZ
obs: set of rang-bearing observation
Add random measurement noiseZ
obs
←
(
Z
obs
,
R
)
//Data association
//i
d
f: index tags for each landmark;
z
old
,
z
new
,
i
d
f
=
Data_associate
Z
obs
,
R
,
GA
T
rej
,
GA
T
aug;
//Update step
IFz
old not empty  THEN //z
old is not a new feature
Observe_Update(z
old, i
d
f, R)
FORi from 1 to size(Z
obs, 2) DO
Compute H
R
t
′
← Equation (38)
Compute u
← Equation (15)
Compute Λ
t
+
1
← Equation (13)
Compute η
t
+
1
← Equation (14)
END FOR
END IF
//Augmentation step
IFz
new not empty  THEN //new feature
SEIF_Augment(z
new
,
W)
ComputeG at X
predict
Computeu
← Equation (21)
ComputeΛ
t
+
!
← Equation (19)
Computeη
t
+
1
← Equation (20)
END IF
Recover meanX
predict
←
Λ
t
-
1
η
t
IF the number of active features > Threshold THEN
SEIF_Sparse();  //sparsification
END IF
END WHILE


## 4.3. 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 matrixH
R
t
′ at the evaluated point. As a result, the observability matrix of this new filter can be presented:(39)
U
′
=
-
I
2
-
J
P
^
L
t
∣
t
-
P
^
R
t
∣
t
-
1
I
2
-
I
2
-
J
P
^
L
t
∣
t
-
P
^
R
t
∣
t
-
1
I
2
⋮
⋮
⋮
-
I
2
-
J
P
^
L
t
∣
t
-
P
^
R
t
∣
t
-
1
I
2
.It can be easily proven that the rank ofU
′ is 2, r
a
n
k
(
U
′
)
=
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 U
′ is of rank 2
M when there are M 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 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:(40)
Average  NEES
=
1
N
M
C
∑
i
=
1
N
M
C
x
i
-
x
^
i
T
S
i
-
1
x
i
-
x
^
i
,
R
M
S
=
1
n
x
∑
i
=
1
n
x
x
i
-
x
^
i
2
n
,where x
i is the ground truth for the state vector, x
^
i and S are the estimated state and covariance matrix, n
x represents the dimension of x, and N
M
C is the times of Monte Carlo simulations.The simulation path and environment are shown in the Figure1, 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.Figure 1
The simulation path and environment.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 withn
x degrees of freedom [37], where n
x is the dimension of x. 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 Figure2, 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.Figure 2
Average NEES of the robot position errors.Figures3 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.Figure 3
RMS errors for the robot position.Figure 4
RMS errors for the robot heading.


## 5. C-Ranger AUV and Sea Trial

### 5.1. C-Ranger AUV

As Figure5 shown, theC-Ranger is an open-frame-structure 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 ofC-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 whenC-Ranger travels at the speed of 1 knot and is fully charged.Figure 5
C-Ranger in deployment.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 theC-Ranger. The two horizontal thrusters are equipped in the belly ofC-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.


### 5.2. Onboard Sensors

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


### 5.3. SLAM for C-Ranger

The vehicle position can be expressed by its position and heading at timet, and the feature is described by its x and y coordinate in the global coordinate system.At timet, the control inputs and observations of AUV are as follows:(41)
U
t
=
u
1
,
u
2
,
…
,
u
t
=
U
t
-
1
,
u
t
Z
t
=
z
1
,
z
2
,
…
,
z
t
=
Z
t
-
1
,
z
t
.Assuming that the variables L, V, θ, Δ
T denote vehicle length, vehicle velocity, rotation angle, and sampling interval, respectively, the motion model of AUV is as follows:(42)
x
t
+
1
=
x
v
t
+
Δ
T
V
cos
⁡
θ
+
θ
v
t
y
v
t
+
Δ
T
V
sin
⁡
θ
+
θ
v
t
θ
v
t
+
Δ
T
V
sin
⁡
θ
L
+
q
t
.Assuming that the global position of theith feature is (
x
i
,
y
i
)
T for the measurements of sonar, the observation model of AUV can be formed by the range and bearing from the vehicle to feature:(43)
z
t
=
z
γ
t
z
α
t
=
x
i
-
x
v
t
2
+
y
i
-
y
v
t
2
arctan
⁡
y
i
-
y
v
t
x
i
-
x
v
t
-
θ
v
t
+
w
γ
t
w
α
t
,where w
γ
t and w
α
t are the observation Gaussian noise, w
γ
t
~
N
(
0
,
R
γ
), w
α
t
~
N
(
0
,
R
α
).


### 5.4. Sea Trial

The real-world experiment was performed atTuandao 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 vehicle-landmark model is adopted in underwater environment.Figure6 shows the trajectory (the red line) ofC-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. TheC-Ranger’s sailing speed was about 1 knot in the experiment.Figure 6
The true environment ofTuandao Bay and GPS trajectory with the satellite image.The imaging sonar was configured to12
0
° 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 theC-Ranger AUV, an effectual position feedback technique was adopted to correct the distorted image [36].Figure7 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.Figure 7
Paths comparison for GPS, SEIF, and SEIF-CC.Figures8 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 real-world environments.Figure 8
The NEES error comparison between SEIF and SEIF-CC.Figure 9
The RMS error comparison between SEIF and SEIF-CC relative to GPS. The GPS data has been used as ground truth.


## 5.1. C-Ranger AUV

As Figure5 shown, theC-Ranger is an open-frame-structure 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 ofC-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 whenC-Ranger travels at the speed of 1 knot and is fully charged.Figure 5
C-Ranger in deployment.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 theC-Ranger. The two horizontal thrusters are equipped in the belly ofC-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.


## 5.2. Onboard Sensors

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


## 5.3. SLAM for C-Ranger

The vehicle position can be expressed by its position and heading at timet, and the feature is described by its x and y coordinate in the global coordinate system.At timet, the control inputs and observations of AUV are as follows:(41)
U
t
=
u
1
,
u
2
,
…
,
u
t
=
U
t
-
1
,
u
t
Z
t
=
z
1
,
z
2
,
…
,
z
t
=
Z
t
-
1
,
z
t
.Assuming that the variables L, V, θ, Δ
T denote vehicle length, vehicle velocity, rotation angle, and sampling interval, respectively, the motion model of AUV is as follows:(42)
x
t
+
1
=
x
v
t
+
Δ
T
V
cos
⁡
θ
+
θ
v
t
y
v
t
+
Δ
T
V
sin
⁡
θ
+
θ
v
t
θ
v
t
+
Δ
T
V
sin
⁡
θ
L
+
q
t
.Assuming that the global position of theith feature is (
x
i
,
y
i
)
T for the measurements of sonar, the observation model of AUV can be formed by the range and bearing from the vehicle to feature:(43)
z
t
=
z
γ
t
z
α
t
=
x
i
-
x
v
t
2
+
y
i
-
y
v
t
2
arctan
⁡
y
i
-
y
v
t
x
i
-
x
v
t
-
θ
v
t
+
w
γ
t
w
α
t
,where w
γ
t and w
α
t are the observation Gaussian noise, w
γ
t
~
N
(
0
,
R
γ
), w
α
t
~
N
(
0
,
R
α
).


## 5.4. Sea Trial

The real-world experiment was performed atTuandao 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 vehicle-landmark model is adopted in underwater environment.Figure6 shows the trajectory (the red line) ofC-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. TheC-Ranger’s sailing speed was about 1 knot in the experiment.Figure 6
The true environment ofTuandao Bay and GPS trajectory with the satellite image.The imaging sonar was configured to12
0
° 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 theC-Ranger AUV, an effectual position feedback technique was adopted to correct the distorted image [36].Figure7 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.Figure 7
Paths comparison for GPS, SEIF, and SEIF-CC.Figures8 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 real-world environments.Figure 8
The NEES error comparison between SEIF and SEIF-CC.Figure 9
The RMS error comparison between SEIF and SEIF-CC relative to GPS. The GPS data has been used as ground truth.


## 6. 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 ofC-Ranger AUV navigation inTuandao 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 theC-Ranger AUV.



---

*Source: 752360-2015-09-10.xml*