Autonomous Navigation with Constrained Consistency for C-Ranger

Abstract

Autonomous underwater vehicles (AUVs) have become the most widely used tools for undertaking complex exploration tasks in marine environments. Their synthetic ability to carry out localization autonomously and build an environmental map concurrently, in other words, simultaneous localization and mapping (SLAM), are considered to be pivotal requirements for AUVs to have truly autonomous navigation. However, the consistency problem of the SLAM system has been greatly ignored during the past decades. In this paper, a consistency constrained extended Kalman filter (EKF) SLAM algorithm, applying the idea of local consistency, is proposed and applied to the autonomous navigation of the C-Ranger AUV, which is developed as our experimental platform. The concept of local consistency (LC) is introduced after an explicit theoretical derivation of the EKF-SLAM system. Then, we present a locally consistency-constrained EKF-SLAM design, LC-EKF, in which the landmark estimates used for linearization are fixed at the beginning of each local time period, rather than evaluated at the latest landmark estimates. Finally, our proposed LC-EKF algorithm is experimentally verified, both in simulations and sea trials. The experimental results show that the LC-EKF performs well with regard to consistency, accuracy and computational efficiency.

Keywords

AUV SLAM EKF Filter Consistency

1. Introduction

Since the first relevant equation was put forward, simultaneous localization and mapping (SLAM), which is one of the key issues that need to be addressed for truly autonomous vehicles probing an unknown environment, has drawn tremendous attention in the realm of mobile robotics. Roughly speaking, SLAM can be categorized according to the means of map representation and the algorithms of its estimation technique. The well-known approaches of describing environmental maps are the feature-based method, the grid-based method, and the topological method. In addition, substantial estimation algorithms have been introduced to solve the SLAM issue, for instance, the extended Kalman filter (EKF) [1], the Extended Information Filter (EIF) [2], the unscented Kalman filter (UKF) [3], FastSLAM [4] and many other variants. However, EKF is the most common choice among the aforementioned algorithms, because it can be easily implemented, especially with lower computational complexity. Essentially speaking, EKF is a state filter or estimator. Therefore, consistency evaluation is essential for checking a filter design and evaluating its optimality.

Ever more appealing developments have been made in addressing the SLAM problem during the past decades. However, a large proportion of the attention has been devoted to handling the problems of computational complexity [5, 6], loop closing [7] and data association [8]. Another fundamental problem, of crucial importance, the consistency issue, has been greatly ignored and has only started to receive much attention, recently. The work by Julier and Uhlmann [9] pointed out for the first time that the SLAM algorithm inevitably obtains an inconsistent environment map by analysing a special case (namely, an immobile vehicle without process noise), as well as the general case of a mobile vehicle with process noise. Castellanos et al. [10] demonstrated that the EKF-SLAM state filter becomes inconsistent because of the linearization error and proposed a consistency-improved robocentric mapping approach. Bailey et al. [11] checked a large amount of inconsistent situations for EKF-SLAM and reached the conclusion that the uncertainty of the vehicle's orientation is the core reason for filter inconsistency. Later, theoretical analyses and experimental results presented by Huang and Dissanayake [12] also validated the corresponding conclusions reached by Bailey et al. and showed that the constrained Jacobians (i.e., Lemma 3.8 in reference [12]) could satisfy the prerequisite of filter consistency.

Technically speaking, the referenced works presented numerous inconsistent symptoms in EKF-SLAM, but did not reveal the deeper reasons for the filter inconsistency. Recently, the consistency problem of the EKF-SLAM was researched, based on an observability analysis, by Guoquan Huang et al. [13, 14] and a novel EKF-SLAM algorithm, based on the first estimates Jacobian (FEJ) was put forward to ameliorate the filter consistency. FEJ-EKF has been proved to have better consistency, compared with the standard EKF. However, the landmark estimates used for the linearization in FEJ-EKF are fixed after the first time within the global observation time and never updated. The weakness of FEJ-EKF is obvious: if the landmark estimates are inaccurate (this is a common situation), the resulting linearization errors will increase with them. To overcome this problem, we try to explore the consistency issue of EKF-SLAM in the local time period, delimited by the system order, rather than in the global time like the FEJ-EKF estimator. In this paper, a consistency constrained EKF-SLAM model based on the idea of local consistency is proposed and applied for the autonomous navigation of the C-Ranger AUV, which is developed as our experimental platform. The main contributions of this paper are as follows:

1)
On the theoretical basis of local observability, we introduce the concept of local consistency, which is useful for the consistency analysis of the EKF-SLAM system in the local period;
2)
A novel LC-EKF estimator is proposed to improve the filter consistency of the EKF-based SLAM system efficiently by applying the methodology of local consistency.
3)
Sea trials on our own test platform, C-Ranger AUV are carried out to evaluate the performance of the LC-EKF. We also adopt the process of sensor data updating into the LC-EKF estimator for the purpose of improving performance.

The remainder of this work is structured as follows: section 2 briefly restates the basic theory of the standard EKF-SLAM. In section 3, we introduce the concept of local consistency and then propose the LC-EKF estimator to improve the filter consistency in EKF-SLAM systems. Section 4 presents the general framework of our test platform, the C-Ranger AUV, as well as the on-board sensors installed on the C-Ranger. In Section 5, a set of experimental comparisons using simulations and our own datasets are carried out, while we also discuss the corresponding experimental results. Lastly, section 6 sketches out the crucial conclusions of this work.
2. Restatement of standard EKF-SLAM algorithm

As a recursive solution to the SLAM problem, the EKF model estimates the system state by minimizing the mean of the square error [15]. In this section, we briefly restate the state vector, the process model and prediction stage, the measurement model and update stage of the standard EKF-SLAM algorithm.

2.1 State vector in EKF-SLAM

In stochastic mapping, we can represent the system state at time step k by an augmented vector x_k including the vehicle pose vector x_{R
_k} and the position vector p_{L
_k} of a set of N_k landmarks in a known global reference frame and its covariance matrix P_k by correlations between the vehicle and landmarks:

\begin{array}{l} x_{k} = {[\begin{matrix} x_{R_{k}}^{T} & p_{L_{k}}^{T} \end{matrix}]}^{T} \\ = {[\begin{matrix} \begin{matrix} p_{R_{k}}^{T} & ϕ_{R_{k}} \end{matrix} & \begin{matrix} {(p_{L_{k}}^{1})}^{T} & \dots & {(p_{L_{k}}^{N_{k}})}^{T} \end{matrix} \end{matrix}]}^{T} \\ = {[\begin{matrix} \begin{matrix} x_{R_{k}} & y_{R_{k}} & ϕ_{R_{k}} \end{matrix} & \begin{matrix} \begin{matrix} x_{L_{k}}^{1} & y_{L_{k}}^{1} \end{matrix} & \dots & \begin{matrix} x_{L_{k}}^{N_{k}} & y_{L_{k}}^{N_{k}} \end{matrix} \end{matrix} \end{matrix}]}^{T} \end{array}

(1)

P_{k} = [\begin{matrix} P_{R R} & P_{R L} \\ P_{L R} & P_{L L} \end{matrix}]

(2)

where the vehicle pose $X_{R_{k}} = {[p_{R_{k}}^{T} ϕ_{R_{k}}]}^{T}$ includes the position $p_{R_{k}}^{}$ and orientation $φ_{R_{k}}$ of the mobile robot. If we suppose that the vehicle keeps running or moving in a plane, the vehicle position can be represented by the coordinates ${[x_{R_{k}} y_{R_{k}}]}^{T}$ in the EN (east-north) reference frame, and the vehicle orientation is normally referred to as the heading, referenced to the north. $p_{L_{k}}^{j} = {[x_{L_{k}}^{j} y_{L_{k}}^{j}]}^{T}$ denotes the position of the j^th landmark.

2.2 Process model and prediction stage for EKF-SLAM

The process model for the state prediction of the EKF-SLAM system is usually written as a first order non-linear formulation.

x_{k + 1} = f (x_{k}, u_{k}) + w_{k}

(3)

where u_k is the known control input, w_k∼N(0, Q_k) denotes the zero mean white Gaussian noise. The prediction formulation above Equation (3) can also be expressed by Equations 4 –(6) in detail as:

{\hat{p}}_{R_{k + 1 | k}} = {\hat{p}}_{R_{k | k}} + C({\hat{ϕ}}_{R_{k | k}} {) Δ \hat{p}}_{R_{k + 1 | k}}

(4)

{\hat{ϕ}}_{R_{k + 1 | k}} = {\hat{ϕ}}_{R_{k | k}} + Δ {\hat{ϕ}}_{R_{k + 1 | k}}

(5)

{\hat{p}}_{L_{k + 1 | k}} = {\hat{p}}_{L_{k | k}}

(6)

where C(·) presents the rotation matrix, the displacement ${Δ \hat{x}}_{R_{k + 1 | k}} = {[{Δ \hat{p}}_{R_{k + 1 | k}}^{T} Δ {\hat{φ}}_{R_{k + 1 | k}}]}^{T}$ between time step k and k+1 is estimated by odometry.

Aside from the frequently used expression above, we can also express the EKF-SLAM system in its indirect form, i.e., the error-state space equation, where the state error $\tilde{x}$ is the difference between the true state x and the estimated state $\hat{x}$ .

{\tilde{x}}_{k + 1 | k} ≃ [\begin{matrix} F_{R_{k}} & 0_{3 \times 2 N_{k}} \\ 0_{2 N_{k} \times 3} & I_{2 N_{k}} \end{matrix}] [\begin{matrix} {\tilde{x}}_{R_{k | k}} \\ {\tilde{p}}_{L_{k | k}} \end{matrix}] + [\begin{matrix} G_{R_{k}} \\ 0_{2 N_{k} \times 3} \end{matrix}] w_{k} ≜ F_{k} {\tilde{x}}_{k | k} + G_{k} w_{k}

(7)

That is,

F_{k} = [\begin{matrix} F_{R_{k}} & 0_{3 \times 2 N_{k}} \\ 0_{2 N_{k} \times 3} & I_{2 N_{k}} \end{matrix}]

(8)

G_{k} = [\begin{matrix} G_{R_{k}} \\ 0_{2 N_{k} \times 3} \end{matrix}]

(9)

where the transition matrices $F_{R_{k}}$ and $G_{R_{k}}$ are as follows:

F_{R_{k}} = [\begin{matrix} I_{2} & JC({\hat{ϕ}}_{R_{k | k}} {) Δ \hat{p}}_{R_{k + 1 | k}} \\ 0_{1 \times 2} & 1 \end{matrix}] = [\begin{matrix} I_{2} & {J (\hat{p}}_{R_{k + 1 | k}} - {\hat{p}}_{R_{k | k}}) \\ 0_{1 \times 2} & 1 \end{matrix}]

(10)

G_{R_{k}} = [\begin{matrix} C({\hat{ϕ}}_{R_{k | k}}) & 0_{2 \times 1} \\ 0_{1 \times 2} & 1 \end{matrix}]

(11)

with $J = [\begin{matrix} 0 & - 1 \\ 1 & 0 \end{matrix}]$ . Then the prediction covariance matrix will be:

P_{k + 1 | k} = F_{k} P_{k | k} F_{k}^{T} + G_{k} Q_{k} G_{k}^{T}

(12)

2.3 Measurement model and update stage for EKF-SLAM

The measurement model for the on-board sensor (e.g., sonar), observing the environmental features, can be expressed by the following nonlinear function h(·):

z_{k} = {h(x}_{k}) + v_{k}

(13)

where v_k ∼N(0, R_k) is the zero mean white Gaussian noise. The general relative position measurement can be written, in detail, as:

z_{k} = [\begin{matrix} h_{1} {(x}_{k}) \\ h_{2} {(x}_{k}) \end{matrix}] = [\begin{matrix} \cos (ϕ_{R_{k}} {)Δx}_{k}^{j} + \sin (ϕ_{R_{k}} {)Δy}_{k}^{j} \\ - \sin (ϕ_{R_{k}} {)Δx}_{k}^{j} + \cos (ϕ_{R_{k}} {)Δy}_{k}^{j} \end{matrix}]

(14)

where ${Δx}_{k}^{j} = x_{L_{k}}^{j} - x_{R_{k}}$ , ${Δy}_{k}^{j} = y_{L_{k}}^{j} - y_{R_{k}}$ , $j = 0, 1, …, M_{k}$ . Similarly, the linearized error-state equation for measurement is:

{\tilde{z}}_{k} ≃ [\begin{matrix} H_{R_{k}} & H_{L_{k}} \end{matrix}] [\begin{matrix} {\tilde{x}}_{R_{k | k - 1}} \\ {\tilde{p}}_{L_{k | k - 1}} \end{matrix}] + v_{k} ≜ H_{k} {\tilde{x}}_{k | k - 1} + v_{k}

(15)

That is,

H_{k} = [\begin{matrix} H_{R_{k}} & H_{L_{k}} \end{matrix}]

(16)

where $H_{R_{k}}$ and $H_{L_{k}}$ denote the Jacobian matrixes of h(·) for the vehicle pose and the landmark position:

H_{R_{k}} = (\nabla h_{k} {)C}^{T} ({\hat{ϕ}}_{R_{k | k - 1}}) [\begin{matrix} - I_{2} & - {J (\hat{p}}_{L_{k | k - 1}} - {\hat{p}}_{R_{k | k - 1}}) \end{matrix}]

(17)

H_{L_{k}} = (\nabla h_{k} {)C}^{T} ({\hat{ϕ}}_{R_{k | k - 1}})

(18)

where ∇h_k is the Jacobian matrix of h(·) calculated by the landmark's local position in the vehicle's coordinate system. Then, the innovation covariance matrix and the Kaiman gain can be evaluated as:

S_{k} = H_{k} P_{k | k - 1} H_{k}^{T} + R_{k}

(19)

W_{k} = P_{k | k - 1} H_{k} {(S_{k})}^{- 1}

(20)

Finally, the system state vector and its covariance matrix are updated as:

{\hat{x}}_{k | k} = {\hat{x}}_{k | k - 1} + W_{k} υ_{k}

(21)

P_{k | k} = P_{k | k - 1} - W_{k} S_{k} W_{k}^{T}

(22)

where v_k presents the innovation sequence (also called the measurement residual).

3. Local consistency and LC-EKF estimator

Inevitably, the nonlinear functions in Equation (3) and (13) will cause errors, and the corresponding estimate results of the process and measurement model are not always accurate. In addition, the Jacobians in Equation (8), (9), (17) and (18) are evaluated using the latest estimated value of the system state vector, rather than its true value. Therefore, it is inevitable that the EKF system will confront the inconsistency problem. In this section, the consistency issue will be discussed in a local period for the classical EKF-SLAM system.

3.1 Local consistency based on observability analysis

Simply speaking, the typical EKF-SLAM system can be roughly classified into two sub-systems: the nonlinear SLAM sub-system and the linearized EKF sub-system.

3.1.1 Observability analysis for the nonlinear SLAM sub-system

Through the derivations of previous works [13, 14, 16], based on the important theory of observability rank condition [17], the observability matrix of the nonlinear SLAM sub-system with M_k observed landmarks at time step k can be given by¹

O_{k}^{N} =span [\begin{matrix} \begin{matrix} \begin{matrix} s ϕ \\ c ϕ \end{matrix} \\ \begin{matrix} s ϕ \\ c ϕ \\ ⋮ \end{matrix} \\ \begin{matrix} s ϕ \\ c ϕ \end{matrix} \end{matrix} & \begin{matrix} \begin{matrix} - c ϕ \\ s ϕ \end{matrix} \\ \begin{matrix} - c ϕ \\ s ϕ \\ ⋮ \end{matrix} \\ \begin{matrix} - c ϕ \\ s ϕ \end{matrix} \end{matrix} & \begin{matrix} \begin{matrix} - c ϕ {Δx}_{k}^{1} - s ϕ {Δy}_{k}^{1}_{1} \\ s ϕ {Δx}_{k}^{1} - c ϕ {Δy}_{k}^{1} \end{matrix} \\ \begin{matrix} - c ϕ {Δx}_{k}^{2} - s ϕ {Δy}_{k}^{2} \\ s ϕ {Δx}_{k}^{2} - c ϕ {Δy}_{k}^{2} \\ ⋮ \end{matrix} \\ \begin{matrix} - c ϕ {Δx}_{k}^{M_{k}} - s ϕ {Δy}_{k}^{M_{k}} \\ s ϕ {Δx}_{k}^{M_{k}} - c ϕ {Δy}_{k}^{M_{k}} \end{matrix} \end{matrix} & \begin{matrix} \begin{matrix} \begin{matrix} - s ϕ \\ - c ϕ \end{matrix} \\ \begin{matrix} 0 \\ 0 \\ ⋮ \end{matrix} \\ \begin{matrix} 0 \\ 0 \end{matrix} \end{matrix} & \begin{matrix} \begin{matrix} c ϕ \\ - s ϕ \end{matrix} \\ \begin{matrix} 0 \\ 0 \\ ⋮ \end{matrix} \\ \begin{matrix} 0 \\ 0 \end{matrix} \end{matrix} & \begin{matrix} \begin{matrix} 0 \\ 0 \end{matrix} \\ \begin{matrix} - s ϕ \\ - c ϕ \\ ⋮ \end{matrix} \\ \begin{matrix} 0 \\ 0 \end{matrix} \end{matrix} & \begin{matrix} \begin{matrix} \begin{matrix} 0 \\ 0 \end{matrix} \\ \begin{matrix} c ϕ \\ - s ϕ \\ ⋮ \end{matrix} \\ \begin{matrix} 0 \\ 0 \end{matrix} \end{matrix} & \begin{matrix} \begin{matrix} 0 \\ 0 \end{matrix} \\ \begin{matrix} 0 \\ 0 \\ ⋮ \end{matrix} \\ \begin{matrix} - s ϕ \\ - c ϕ \end{matrix} \end{matrix} & \begin{matrix} \begin{matrix} 0 \\ 0 \end{matrix} \\ \begin{matrix} 0 \\ 0 \\ ⋮ \end{matrix} \\ \begin{matrix} c ϕ \\ - s ϕ \end{matrix} \end{matrix} \end{matrix} \end{matrix} \end{matrix}]

(23)

where sϕ and cϕ are the simplified modes of $\sin (φ_{R_{k}})$ and $\cos (φ_{R_{k}})$ . Examining the above matrix, we can see that the size of O_k^N is 2M_k × (2M_k + 3). If the Gaussian elimination technique is applied to the observability matrix O_k^N by row operations, we can obtain a reduced matrix in row echelon form where the number of nonzero rows is 2M_k. That is, the number of basic columns in O_k^N is 2M_k. This number is defined as the rank of a matrix [18] which is an extremely important concept in the development of our work. Thus, we can obtain the following conclusion that:

Conclusion 1: The rank of the observability matrix for the general nonlinear SLAM sub-system, with M_k observed landmarks at time step k, is 2M_k.

3.1.2 Local observability analysis for the linearized EKF sub-system

Since the EKF system has been proven to be unobservable [16, 19], the local rather than the global observability matrix is chosen for the observability analysis for the EKF sub-system. The local observability matrix [20, 21] over the time period [k, k + n − 1] is defined as

O_{k}^{L} = [\begin{matrix} H_{k} \\ H_{k + 1} F_{k} \\ ⋮ \\ H_{k + n - 1} F_{k + n - 2} \dots F_{k} \end{matrix}]

(24)

where n is the order of the linear time-varying (LTV) dynamical system, described by the error-state space Equations (7) and (15). According to reference [22], the system order n is defined as the number of state variables in the system, represented by state-space equations. Parameter n here is equal to the size of the state vector x_k in Equation (1). For example, the state vector is $x_{0} = {[\begin{matrix} x_{R_{0}} & y_{R_{0}} & φ_{R_{0}} \end{matrix}]}^{T}$ at the initial time (i.e., k = 0) with no landmarks observed, so the EKF system is locally observable in the time interval [k, k + 2].

Suppose M_k+i landmarks are observed at time step k + i, where i = 0,1, …, n-1, then the measurement matrix can be written as

H_{k + i} = [\begin{matrix} \begin{matrix} H_{R_{k + i}}^{1} \\ ⋮ \\ H_{R_{k + i}}^{M_{k + i}} \end{matrix} & \begin{matrix} H_{L_{k + i}}^{1} & \dots & 0_{2 \times 2} \\ ⋮ & ⋱ & ⋮ \\ 0_{2 \times 2} & \dots & H_{L_{k + i}}^{M_{k + i}} \end{matrix} \end{matrix}]

(25)

Substituting the above equation into Equation (24), the observability matrix O_k^L now becomes

O_{k}^{L} = [\begin{matrix} \begin{matrix} \begin{matrix} H_{R_{k}}^{1} \\ ⋮ \\ H_{R_{k}}^{M_{k}} \end{matrix} \\ \begin{matrix} H_{R_{k + 1}}^{1} F_{R_{k}} \\ ⋮ \\ H_{R_{k + 1}}^{M_{k + 1}} F_{R_{k}} \end{matrix} \\ ⋮ \\ \begin{matrix} H_{R_{k + n - 1}}^{1} F_{R_{k + n - 2}} \dots F_{R_{k}} \\ ⋮ \\ H_{R_{k + n - 1}}^{M_{k + n - 1}} F_{R_{k + n - 2}} \dots F_{R_{k}} \end{matrix} \end{matrix} & \begin{matrix} \begin{matrix} H_{L_{k}}^{1} & \dots & 0_{2 \times 2} \\ ⋮ & ⋱ & ⋮ \\ 0_{2 \times 2} & \dots & H_{L_{k}}^{M_{k}} \end{matrix} \\ \begin{matrix} H_{L_{k + 1}}^{1} & \dots & 0_{2 \times 2} \\ ⋮ & ⋱ & ⋮ \\ 0_{2 \times 2} & \dots & H_{L_{k + 1}}^{M_{k + 1}} \end{matrix} \\ ⋮ \\ \begin{matrix} H_{L_{k + n - 1}}^{1} & \dots & 0_{2 \times 2} \\ ⋮ & ⋱ & ⋮ \\ 0_{2 \times 2} & \dots & H_{L_{k + n - 1}}^{M_{k + n - 1}} \end{matrix} \end{matrix} \end{matrix}] = \underset{O_{1}}{\underset{︸}{Diag (H_{L_{k}}^{1}, \dots, H_{L_{k}}^{M_{k}}, \dots, H_{L_{k + n - 1}}^{1}, \dots, H_{L_{k + n - 1}}^{M_{k + n - 1}})}} \times \underset{O_{2}}{\underset{︸}{[\begin{matrix} \begin{matrix} \begin{matrix} {(H_{L_{k}}^{1})}^{- 1} H_{R_{k}}^{1} \\ ⋮ \\ {(H_{L_{k}}^{M_{k}})}^{- 1} H_{R_{k}}^{M_{k}} \end{matrix} \\ \begin{matrix} {(H_{L_{k + 1}}^{1})}^{- 1} H_{R_{k + 1}}^{1} F_{R_{k}} \\ ⋮ \\ {(H_{L_{k + 1}}^{M_{k + 1}})}^{- 1} H_{R_{k + 1}}^{M_{k + 1}} F_{R_{k}} \end{matrix} \\ ⋮ \\ \begin{matrix} {(H_{L_{k + n - 1}}^{1})}^{- 1} H_{R_{k + n - 1}}^{1} F_{R_{k + n - 2}} \dots F_{R_{k}} \\ ⋮ \\ {(H_{L_{k + n - 1}}^{M_{k + n - 1}})}^{- 1} H_{R_{k + n - 1}}^{M_{k + n - 1}} F_{R_{k + n - 2}} \dots F_{R_{k}} \end{matrix} \end{matrix} & \begin{matrix} \begin{matrix} I_{2}^{} & \dots & 0_{2 \times 2}^{} \\ ⋮ & ⋱ & ⋮ \\ 0_{2 \times 2}^{} & \dots & I_{2}^{} \end{matrix} \\ \begin{matrix} I_{2}^{} & \dots & 0_{2 \times 2}^{} \\ ⋮ & ⋱ & ⋮ \\ 0_{2 \times 2}^{} & \dots & I_{2}^{} \end{matrix} \\ ⋮ \\ \begin{matrix} I_{2}^{} & \dots & 0_{2 \times 2}^{} \\ ⋮ & ⋱ & ⋮ \\ 0_{2 \times 2}^{} & \dots & I_{2}^{} \end{matrix} \end{matrix} \end{matrix}]}}

(26)

1) Ideal EKF sub-system

The ideal EKF sub-system refers to a system where the true value of the state vector is known and thus

F_{R_{k + i}} = [\begin{matrix} I_{2} & {J(p}_{R_{k + i + 1}} - p_{R_{k + i}}) \\ 0_{1 \times 2} & 1 \end{matrix}]

(27)

H_{R_{k + i}}^{j} = (\nabla h_{k + i}) C^{T} (ϕ_{R_{k + i}}) [\begin{matrix} - I_{2} & - J (p_{L_{k + i}}^{j} - p_{R_{k + i}}) \end{matrix}]

(28)

H_{L_{k + i}}^{j} = (\nabla h_{k + i}) C^{T} (ϕ_{R_{k + i}})

(29)

Then, we can obtain that

{(H_{L_{k + i}}^{j})}^{- 1} H_{R_{k + i}}^{j} F_{R_{k + i - 1}} \dots F_{R_{k}} = [\begin{matrix} - I_{2} & - J (p_{L_{k + i}}^{j} - p_{R_{k}}) \end{matrix}]

(30)

Substituting it into Equation (26), we can obtain²

O_{2}^{I} = [\begin{matrix} \begin{matrix} - I_{2} \\ ⋮ \\ - I_{2} \end{matrix} & \begin{matrix} - J (p_{L_{k}}^{1} - p_{R_{k}}) \\ ⋮ \\ - J (p_{L_{k}}^{M_{0}} - p_{R_{k}}) \end{matrix} & \begin{matrix} I_{2} & \dots & 0_{2 \times 2} \\ ⋮ & ⋱ & ⋮ \\ 0_{2 \times 2} & \dots & I_{2} \end{matrix} \\ \begin{matrix} - I_{2} \\ ⋮ \\ - I_{2} \end{matrix} & \begin{matrix} - J (p_{L_{k + 1}}^{1} - p_{R_{k}}) \\ ⋮ \\ - J (p_{L_{k + 1}}^{M_{1}} - p_{R_{k}}) \end{matrix} & \begin{matrix} I_{2} & \dots & 0_{2 \times 2} \\ ⋮ & ⋱ & ⋮ \\ 0_{2 \times 2} & \dots & I_{2} \end{matrix} \\ ⋮ & ⋮ & ⋮ \\ \begin{matrix} - I_{2} \\ ⋮ \\ - I_{2} \end{matrix} & \begin{matrix} - J (p_{L_{k + n - 1}}^{1} - p_{R_{k}}) \\ ⋮ \\ - J (p_{L_{k + n - 1}}^{M_{n - 1}} - p_{R_{k}}) \end{matrix} & \begin{matrix} I_{2} & \dots & 0_{2 \times 2} \\ ⋮ & ⋱ & ⋮ \\ 0_{2 \times 2} & \dots & I_{2} \end{matrix} \end{matrix}]

(31)

Since the landmarks are static and their true values are known, the positions of the landmarks do not change with time, that is

p_{L_{k}}^{j} = p_{L_{k + 1}}^{j} = \dots = p_{L_{k + i}}^{j} = p_{L}^{j}

(32)

So Equation (31) can be simplified as

O_{2}^{I} = [\begin{matrix} \begin{matrix} - I_{2} \\ ⋮ \\ - I_{2} \end{matrix} & \begin{matrix} - J (p_{L}^{1} - p_{R_{k}}) \\ ⋮ \\ - J (p_{L}^{M_{k}} - p_{R_{k}}) \end{matrix} & \begin{matrix} I_{2} & \dots & 0_{2 \times 2} \\ ⋮ & ⋱ & ⋮ \\ 0_{2 \times 2} & \dots & I_{2} \end{matrix} \\ \begin{matrix} - I_{2} \\ ⋮ \\ - I_{2} \end{matrix} & \begin{matrix} - J (p_{L}^{1} - p_{R_{k}}) \\ ⋮ \\ - J (p_{L}^{M_{k + 1}} - p_{R_{k}}) \end{matrix} & \begin{matrix} I_{2} & \dots & 0_{2 \times 2} \\ ⋮ & ⋱ & ⋮ \\ 0_{2 \times 2} & \dots & I_{2} \end{matrix} \\ ⋮ & ⋮ & ⋮ \\ \begin{matrix} - I_{2} \\ ⋮ \\ - I_{2} \end{matrix} & \begin{matrix} - J (p_{L}^{1} - p_{R_{k}}) \\ ⋮ \\ - J (p_{L}^{M_{k + n - 1}} - p_{R_{k}}) \end{matrix} & \begin{matrix} I_{2} & \dots & 0_{2 \times 2} \\ ⋮ & ⋱ & ⋮ \\ 0_{2 \times 2} & \dots & I_{2} \end{matrix} \end{matrix}]

(33)

Since O₂^I is composed of n repetitive block rows, which can be described as:

o_{k + i} = [\begin{matrix} \begin{matrix} - I_{2} \\ ⋮ \\ - I_{2} \end{matrix} & \begin{matrix} - J (p_{L}^{1} - p_{R_{k}}) \\ ⋮ \\ - J (p_{L}^{M_{k + i}} - p_{R_{k}}) \end{matrix} & \begin{matrix} I_{2} & \dots & 0_{2 \times 2} \\ ⋮ & ⋱ & ⋮ \\ 0_{2 \times 2} & \dots & I_{2} \end{matrix} \end{matrix}]

(34)

where i = 0,1, …, n − 1, and the size of each block row O_k+i is 2M_k+i X (2M_k+i + 3). That is,

O_{2}^{I} = [\begin{matrix} o_{k} \\ o_{k + 1} \\ ⋮ \\ o_{k + i} \\ ⋮ \\ o_{k + n - 1} \end{matrix}]

(35)

The elements of o_k+1, ···, o_k+n-1 can be converted to zero by the Gaussian elimination, as o_k+1, ···, o_k+n-1 ⊆ o_k. There are 2M_k nonzero rows in o_k, thus the rank of O₂^I is 2M_k. Therefore, we can reach the following conclusion that:

Conclusion 2: The rank of the observability matrix over the local time period [k, k + n − 1]for the ideal EKF sub-system, with M_k observed landmarks at time step k, is 2M_k, if M_k+1, ···, M_k+n-1 ≤ M_k.

We can see the rank of the observability matrix for the ideal EKF sub-system is equal to the nonlinear SLAM sub-system over the local time period, namely, they have the same observability property.

2) Standard EKF sub-system

For the standard EKF sub-system, Jacobian matrices are calculated using the latest estimated state, and thus

F_{R_{k + i}} = [\begin{matrix} I_{2} & J ({\hat{p}}_{R_{k + i + 1 | k + i}} - {\hat{p}}_{R_{k + i | k + i}}) \\ 0_{1 \times 2} & 1 \end{matrix}]

(36)

H_{R_{k + i}}^{j} = (\nabla h_{k + i}) C^{T} ({\hat{ϕ}}_{R_{k + i | k + i - 1}}) [\begin{matrix} - I_{2} & - J ({\hat{p}}_{L_{k + i | k + i - 1}}^{j} - {\hat{p}}_{R_{k + i | k + i - 1}}) \end{matrix}]

(37)

H_{L_{k + i}}^{j} = (\nabla h_{k + i}) C^{T} ({\hat{ϕ}}_{R_{k + i | k + i - 1}})

(38)

Furthermore, we can also derive a formula analogous to Equation (30) as

\begin{array}{l} {(H_{L_{k + i}}^{j})}^{- 1} H_{R_{k + i}}^{j} F_{R_{k + i - 1}} \dots F_{R_{k}} \\ = [- I_{2} - {J (\hat{p}}_{L_{k + i | k + i - 1}}^{j} - {\hat{p}}_{R_{k | k}} - \sum_{l = k}^{k + i - 2} ({\hat{p}}_{R_{l + 1 | l + 1}} - {\hat{p}}_{R_{l + 1 | l}}))] \end{array}

(39)

Substituting it into Equation (26), we can obtain that

O_{2}^{S} = [\begin{matrix} - I_{2} & - J ({\hat{p}}_{L_{k | k - 1}}^{1} - {\hat{p}}_{R_{k | k - 1}}) & I_{2} & \dots & 0_{2 \times 2} \\ ⋮ & ⋮ & ⋮ & ⋱ & ⋮ \\ - I_{2} & - J ({\hat{p}}_{L_{k | k - 1}}^{M_{k}} - {\hat{p}}_{R_{k | k - 1}}) & 0_{2 \times 2} & \dots & I_{2} \\ - I_{2} & - J ({\hat{p}}_{L_{k + 1 | k}}^{1} - {\hat{p}}_{R_{k | k}}) & I_{2} & \dots & 0_{2 \times 2} \\ ⋮ & ⋮ & ⋮ & ⋱ & ⋮ \\ - I_{2} & - J ({\hat{p}}_{L_{k + 1 | k}}^{M_{k + 1}} - {\hat{p}}_{R_{k | k}}) & 0_{2 \times 2} & \dots & I_{2} \\ - I_{2} & - J ({\hat{p}}_{L_{k + 2 | k + 1}}^{1} - {\hat{p}}_{R_{k | k}} - {(\hat{p}}_{R_{k + 1 | k + 1}} - {\hat{p}}_{R_{k + 1 | k}})) & I_{2} & \dots & 0_{2 \times 2} \\ ⋮ & ⋮ & ⋮ & ⋱ & ⋮ \\ - I_{2} & - J ({\hat{p}}_{L_{k + 2 | k + 1}}^{M_{k + 2}} - {\hat{p}}_{R_{k | k}} - {(\hat{p}}_{R_{k + 1 | k + 1}} - {\hat{p}}_{R_{k + 1 | k}})) & 0_{2 \times 2} & \dots & I_{2} \\ ⋮ & ⋮ & ⋮ & ⋮ & ⋮ \\ - I_{2} & - J ({\hat{p}}_{L_{k + n - 1 | k + n - 2}}^{1} - {\hat{p}}_{R_{k | k}} - \sum_{l = k}^{k + i - 3} ({\hat{p}}_{R_{l + 1 | l + 1}} - {\hat{p}}_{R_{l + 1 | l}})) & I_{2} & \dots & 0_{2 \times 2} \\ ⋮ & ⋮ & ⋮ & ⋱ & ⋮ \\ - I_{2} & - J ({\hat{p}}_{L_{k + n - 1 | k + n - 2}}^{M_{k + n - 1}} - {\hat{p}}_{R_{k | k}} - \sum_{l = k}^{k + i - 3} ({\hat{p}}_{R_{l + 1 | l + 1}} - {\hat{p}}_{R_{l + 1 | l}})) & 0_{2 \times 2} & \dots & I_{2} \end{matrix}]

(40)

As the state estimates generally differ at different times, the following formulas are normally tenable:

{\hat{p}}_{L_{k | k - 1}}^{j} \neq {\hat{p}}_{L_{k + 1 | k}}^{j} \neq \dots \neq {\hat{p}}_{L_{k + i | k + i - 1}}^{j}

(41)

{\hat{p}}_{R_{l + 1 | l + 1}} - {\hat{p}}_{R_{l + 1 | l}} \neq 0_{2 \times 1}

(42)

Therefore, variables in the third column of O₂^S are not equal for different rows. Thus, we have to perform the column operations for the Gaussian elimination. As only the first two columns of O₂^S, which comprise 2M_k + 3 columns, can be eliminated, the rank of O₂^S is 2M_k + 1. Then we can conclude that:

Conclusion 3: The rank of the observability matrix over the local time period [k, k + n − 1] for the standard EKF sub-system, with M_k observed landmarks at time step k, is 2M_k + 1, if M_k+1, ···, M_k+n-1 ≤ M_k.

We can see that the standard EKF sub-system is not identical to the rank of the observability matrix of the nonlinear SLAM sub-system, in other words, the observability properties of the two sub-systems are not the same.

3.2 Local Consistency and LC-EKF estimator

In the preceding section, we performed an observability analysis of the nonlinear SLAM sub-system and the linearized EKF sub-system with any number of landmarks. Specially, when the number of landmarks is just one, we can come to the same conclusions as those in work [13] and find the corresponding relationship between observability and consistency as follows:

1)
The ideal EKF sub-system has the same observability properties as the nonlinear SLAM sub-system within the local time period [k, k + n −1], consequently, the ideal EKF-SLAM system is consistent.
2)
The standard EKF sub-system's observability matrix has a higher rank than that of the nonlinear SLAM subsystem over the local time period [k, k + n − 1], which makes the standard EKF-SLAM system inconsistent.

The two corollaries above, as well as the three conclusions drawn in section 3.1, look similar to those presented in references [13, 14], but they are actually restricted to the local time period rather than the global one, used by Huang et al. We think our conclusions are precise, because the theoretical basis of all the derivations, namely the local observability matrix, is defined over the local time period [k, k + n − 1]. So it is necessary to introduce the concept of local consistency for the EKF-SLAM system.

Definition: The EKF-SLAM system is “locally consistent” if the linearized EKF sub-system has the same observability properties as the nonlinear SLAM sub-system over the time period [k, k + n − 1], where n is the order of the system.

The local consistency, thus defined, differs from the well-known complete consistency, for the latter, the statement in the definition must be true for all values of k. Therefore, a system can be locally consistent for certain time intervals but not completely consistent.

As the extreme limit for FEJ-EKF is the usage of initial state estimates at the global time period for computing the filter Jacobians, we try to solve this problem in the local period by modifying the algorithm as follows:
1)
If the Jacobian matrix in the EKF prediction stage is evaluated using the predicted value ${\hat{p}}_{R_{k + i | k + i - 1}}$ rather than the updated value ${\hat{p}}_{R_{k + i | k + i}}$ as in the standard EKF, we can obtain

$F_{R_{k + i}}^{'} = [\begin{matrix} I_{2} & J ({\hat{p}}_{R_{k + i + 1 | k + i}} - {\hat{p}}_{R_{k + i | k + i - 1}}) \\ 0_{1 \times 2} & 1 \end{matrix}]$
(43)

2)
If the Jacobian matrices $H_{R_{k}}^{j}, \dots, H_{R_{k + n - 1}}^{j}$ of the EKF update stage during every local time period [k, k + n − 1] are all evaluated using the landmark estimate ${\hat{p}}_{L_{k | k - 1}}^{j}$ which is obtained at the first time instant of the local time period, we can obtain

$\begin{array}{l} H_{k + i}^{'} = [\begin{matrix} H_{R_{k + i}}^{'} & H_{L_{k + i}}^{'} \end{matrix}] \\ = (\nabla h_{k + i}) C^{T} ({\hat{ϕ}}_{R_{k + i | k + i - 1}}) [\begin{matrix} - I_{2} & - J ({\hat{p}}_{L_{k | k - 1}}^{j} - {\hat{p}}_{R_{k + i | k + i - 1}}) & - I_{2} \end{matrix}] \end{array}$
(44)

Then it is easy to obtain that

${(H_{L_{k + i}}^{j})}^{- 1} H_{R_{k + i}}^{j} F_{R_{k + i - 1}}^{'} \dots F_{R_{k}}^{'} = [\begin{matrix} - I_{2} & - J ({\hat{p}}_{L_{k | k - 1}}^{j} - {\hat{p}}_{R_{k | k - 1}}) \end{matrix}] F_{R_{k + i}}^{'}$
(45)

So, the observability matrix is

$O_{2}^{'} = [\begin{matrix} \begin{matrix} - I_{2} \\ ⋮ \\ - I_{2} \end{matrix} & \begin{matrix} - J ({\hat{p}}_{L_{k | k - 1}}^{1} - {\hat{p}}_{R_{k | k - 1}}) \\ ⋮ \\ - J ({\hat{p}}_{L_{k | k - 1}}^{M_{k}} - {\hat{p}}_{R_{k | k - 1}}) \end{matrix} & \begin{matrix} I_{2} & \dots & 0_{2 \times 2} \\ ⋮ & ⋱ & ⋮ \\ 0_{2 \times 2} & \dots & I_{2} \end{matrix} \\ \begin{matrix} - I_{2} \\ ⋮ \\ - I_{2} \end{matrix} & \begin{matrix} - J (p_{L_{k | k - 1}}^{1} - {\hat{p}}_{R_{k | k - 1}}) \\ ⋮ \\ - J ({\hat{p}}_{L_{k | k - 1}}^{M_{k + 1}} - {\hat{p}}_{R_{k | k - 1}}) \end{matrix} & \begin{matrix} I_{2} & \dots & 0_{2 \times 2} \\ ⋮ & ⋱ & ⋮ \\ 0_{2 \times 2} & \dots & I_{2} \end{matrix} \\ ⋮ & ⋮ & ⋮ \\ \begin{matrix} - I_{2} \\ ⋮ \\ - I_{2} \end{matrix} & \begin{matrix} - J ({\hat{p}}_{L_{k | k - 1}}^{1} - {\hat{p}}_{R_{k | k - 1}}) \\ ⋮ \\ - J ({\hat{p}}_{L_{k | k - 1}}^{M_{k + n - 1}} - {\hat{p}}_{R_{k | k - 1}}) \end{matrix} & \begin{matrix} I_{2} & \dots & 0_{2 \times 2} \\ ⋮ & ⋱ & ⋮ \\ 0_{2 \times 2} & \dots & I_{2} \end{matrix} \end{matrix}]$
(46)

If _k+1, ···, M_{k+n_-1} ≤ M_k, the rank of the observability matrix is 2M_k. Consequently, the above-modified system, which we call local consistency (LC) EKF SLAM, is locally consistent. The main process of the LC-EKF SLAM can be outlined by the following algorithm description.

Algorithm 1 LC-EKF SLAM
Inputs: initial estimates x_k and P_k Steps: 1. Initialize the filter. 2. Compute the system order n=size (x_k). for t = k: k + n − 1 3. // EKF prediction stage while data from odometry sensor do Predict the system state x_k+1|k by Equation (4)–(6); Compute the prediction Jacobian F′_k by substituting Equation (43) into (8); Predict the covariance matrix P_k+1|k by Equation (12) ; end while 4. // EKF update stage while data from measurement sensor do Data association; if the measurement is an existing landmark Compute the measurement Jacobian H'_k by Equation (44) ; Update the system state x_k+1|k and its covariance matrix P_k+1|k by Equation (19)–(22); end if if the measurement is a new landmark Augment the system state vector; end if end while 5. if = k Save ${\hat{p}}_{L_{k | k - 1}}^{j}$ ; else if M_{k + 1} > M_k end if end for Outputs: estimates ${\hat{x}}_{t | t}$ and P_t|t 4. Overview of the C-Ranger AUV

The C-Ranger vehicle was initially developed as a research platform for the testing and verification of various algorithms in underwater navigation. As detailed in our previous works [23 –25], C-Ranger (Figure 1) is an open-frame-structure vehicle with the architecture of a control system, presented in Figure 2.

Figure 1.

C-Ranger AUV

Figure 2.

Control system of C-Ranger

The C-Ranger is 1.6m long, 1.3m wide and 1.1m high. The total weight of the C-Ranger is about 206kg when working at full load. The maximum speed of the C-Ranger is close to 3 knots (1.5 m/s). Its continuous running time can reach eight hours when the C-Ranger travels at a speed of 1 knot and is fully charged.

Numerous internal sensors, such as gyroscopes, digital compasses, pressure sensors and AHRS (Attitude and Heading Reference Systems), are installed on the C-Ranger. In addition, there are some external sensors–sonar, GPS and DVL (Doppler Velocity Log), for example.

The basic sensors installed on the C-Ranger AUV are detailed in Table 1, as well as the newly added sensors, which enable the C-Ranger to perform the visual SLAM and multi-AUV SLAM.

Table 1.

Sensors installed on C-Ranger AUV

Fundamental sensors	Variable	Update rate	Accuracy	Range
Super SeaKing DST imaging sonar	Polar coordinate	0.05–0.2 Hz	15 mm (distance) 0.225 deg (angle)	0.4–300 m
XW-GPS1000 GPS	XYZ position	1–20 Hz	1.1 m (CEP)	-
NavQuest 600 DVL	Velocity	1–5 Hz	1% ± 1 mm/s	0.3–140 m
Honeywell HMR3000 digital compass	Attitude	1–20 Hz	0.5 deg (RMS)	360 deg (heading) ± 40 deg (tilt)
VG951D Gyroscope	Angular velocity	20 Hz	1 deg/h	60 deg/s
Desert-star SSP-1 300PSIG Digital Pressure Sensor	Depth	0.25–16 Hz	0.1% of full scale	300 m
UWM3000 Acoustic Modems	Communication	5 Kbps	<10^-9	5000 m
New sensors
Gemini 720i Multibeam Imaging Sonar	Polar coordinate	5–30 Hz	8 mm (distance) 0.5 deg (angle)	0.2–120 m
Innalabs® AHRS M2	Attitude	1–100 Hz	0.7 deg (heading) 0.4 deg (roll/pitch)	0–360 deg (heading) ±90 deg (pitch) ±180 deg (roll)
Kongsberg Maritime OE14-376 (PAL) Colour Camera	Colour image	50 Hz	625 Line	3000m

5. Experiments and analysis

In this section, the following five algorithms are compared by using a large number of Monte Carlo simulations and our own sea trial dataset:

(1)
the ideal EKF SLAM
(2)
the standard EKF SLAM
(3)
the FEJ-EKF SLAM
(4)
the OC-EKF SLAM [26]
(5)
the proposed LC-EKF SLAM

It should be noted that for the standard EKF SLAM, we relied on an existing Matlab implementation, developed by Tim Bailey, and the other three algorithms are all modified based on this. All the algorithms are executed in Matlab R2009a with 2.60GHz Pentium® Dual-Core CPU E5300 and 2GB RAM.
5.1 Simulation Experiment

The first test was carried out in a simulated environment, which allows comparison between the true states and the estimates to be made, as the true locations of the vehicle and landmarks are both available. In particular, all the simulations were carried out with known data association, so as to eliminate the influence of mismatching on the estimation of performance. As shown in Figure 3, there are 34 randomly generated point features and a vehicle trajectory with loop closure in a 200mx200m environment. The simulation parameters are detailed in Table 2.

Figure 3.

The simulation environment

Table 2.

Simulation parameters

Parameter	Value
Vehicle speed	5m/s
Max scan range	30m
Deadreckoning sampling time	0.025s
Observation sampling time	0.125s
Vehicle speed std dev	0.3m/s
Steering angle std dev	3°
Range observation std dev	0.1m
Bearing observation std dev	1°

To evaluate the performance of the estimators, 50 Monte Carlo tests are performed in the simulation scenario, where the vehicle fulfils five loops. The average Root Mean Square (RMS) errors of the vehicle pose are shown in Figure 4 to evaluate the accuracy of the five filters: the ideal EKF, the standard EKF, FEJ-EKF, OC-EKF and the LC-EKF. In addition, the average Consistency Index (CI) [5] of the vehicle pose presented in Figure 5 is used to evaluate the filters' consistency. It can be seen that our proposed LC-EKF algorithm performs better than the standard EKF and the FEJ-EKF, and has a performance comparable with the OC-EKF, pertaining to the accuracy and consistency.

Figure 4.

Mean RMS errors of the vehicle pose

Figure 5.

Mean consistency index (CI) of the vehicle pose

5.2 Sea Trial

5.2.1 SLAM for C-Ranger

Consider that in our current system, the C-Ranger always remains stable in the DOFs (degrees of freedom) of roll and pitch, the state vector of the C-Ranger can be presented by:

x_{R}^{} = {[x_{R} y_{R} z_{R} ϕ_{R} v_{x} v_{y} v_{z}]}^{T}

(47)

where [x_R y_R z_R ϕ_R]^T denotes the 3-D coordinates and the heading of the C-Ranger in the global reference frame and [v_x v_y v_z]^T are the linear velocities in the vehicle reference frame.

The motion model of the C-Ranger moving at a constant velocity from time k-1 to k can be written as

x_{R_{k}} = f (x_{R_{k - 1}}, u_{k}) + w_{k}

(48)

{[\begin{array}{l} x_{R} \\ y_{R} \\ z_{R} \\ ϕ_{R} \\ v_{x} \\ v_{y} \\ v_{z} \end{array}]}_{k} = {[\begin{matrix} x_{R} + Δ x \cos (ϕ_{R}) - Δ y \sin (ϕ_{R}) \\ y_{R} + Δ x \sin (ϕ_{R}) + Δ y \cos (ϕ_{R}) \\ z_{R} + Δ z \\ ϕ_{R} + w Δ T \\ v_{x} + a_{x} Δ T \\ v_{y} + a_{y} Δ T \\ v_{z} + a_{z} Δ T \end{matrix}]}_{k - 1} + w_{k}

(49)

with the displacements:

\begin{matrix} Δ x = v_{x} Δ T + \frac{1}{2} a_{x} {(Δ T)}^{2} \\ Δ y = v_{y} Δ T + \frac{1}{2} a_{y} {(Δ T)}^{2} \\ Δ z = v_{z} Δ T + \frac{1}{2} a_{z} {(Δ T)}^{2} \end{matrix}

(50)

where the control input vector u_k = [w a_x a_y a_z]^T is composed of the angular velocity and the accelerations.³ Furthermore, w_k denotes the white Gaussian control noise with a zero mean and covariance matrix Q_k:

Q_{k} = [\begin{matrix} σ_{w}^{2} & 0 & 0 & 0 \\ 0 & σ_{a_{x}}^{2} & 0 & 0 \\ 0 & 0 & σ_{a_{y}}^{2} & 0 \\ 0 & 0 & 0 & σ_{a_{z}}^{2} \end{matrix}]

(51)

For the C-Ranger, using sonar as the observing sensor, the range-bearing measurement model can be written in the form⁴

z_{k}^{S} = [\begin{matrix} r \\ ψ \end{matrix}] = [\begin{matrix} \sqrt{{(x_{L} - x_{R})}^{2} + {(y_{L} - y_{R})}^{2}} \\ \tan^{- 1} (\frac{y_{L} - y_{R}}{x_{L} - x_{R}}) - ϕ_{R} \end{matrix}] + v_{k}^{S}

(52)

where v_k^S denotes the white Gaussian measurement noise for sonar with zero mean and covariance matrix R_k^S:

R_{k}^{S} = [\begin{matrix} σ_{r}^{2} & 0 \\ 0 & σ_{ψ}^{2} \end{matrix}]

(53)

As described in Section 4, the C-Ranger is also equipped with many other sensors, which can supply direct observations of components in the state vector. For the pressure sensor, the digital compass and the DVL, the measurement models are linear, and their measurement matrix and measurement noise can be given as:

\begin{array}{l} H_{k}^{P} = [0 0 1 0 0 0 0] \\ H_{k}^{C} = [0 0 0 1 0 0 0] \\ H_{k}^{D} = [\begin{array}{l} 0 0 0 0 1 0 0 \\ 0 0 0 0 0 1 0 \\ 0 0 0 0 0 0 1 \end{array}] \end{array}

(54)

\begin{array}{l} R_{k}^{P} = σ_{P}^{2} \\ R_{k}^{C} = σ_{C}^{2} \\ R_{k}^{D} = [\begin{matrix} σ_{v_{x}}^{2} & 0 & 0 \\ 0 & σ_{v_{y}}^{2} & 0 \\ 0 & 0 & σ_{v_{z}}^{2} \end{matrix}] \end{array}

(55)

Substituting Equations (54) and (55) into Equations (19)–(22), the sensor data in the state vector can be updated. This process is called sensor data updating, which can make the sensor data more accurate before they are fed into the motion model of the C-Ranger.

5.2.2 Experimental results

We carried out the sea trial at Tuandao Bay, in Qingdao, China. Figure 6 demonstrates the experimental results integrated with the environmental satellite image. The magenta points denote the point features, extracted from the sonar data, which match well with landmarks in the real world.

Figure 6.

Results of the sea trial combined with the satellite image

A clear path comparison is presented in Figure 7, where the GPS trajectory (the cyan line) works as the ground truth for the comparison of the algorithms. It can be seen that the path deviation of our proposed LC-EKF relative to the GPS is smaller than that of the standard EKF and the FEJ-EKF. The LC-EKF's outperformance can also be confirmed by comparisons with RMS (Figure 8) and CI (Figure 9).

Figure 7.

Path comparison

Figure 8.

RMS errors of the vehicle pose

Figure 9.

CI of the vehicle pose

Moreover, the computational cost is used to evaluate the performance of the estimator. The standard EKF, FEJ-EKF and LC-EKF estimators are all based on the EKF-SLAM systems, and the essential differences between them are just their usage of state estimates to compute the Jacobian matrix at different time points. Therefore, the computational costs for the standard EKF, FEJ-EKF and LC-EKF estimators are almost the same. We only need to compare the proposed LC-EKF with the OC-EKF. The total cost of the CPU time for the LC-EKF is 131.5 seconds and that for the OC-EKF is about 1108.8 seconds. The detailed CPU time required for each timestep is shown in Figure 10. As the time cost of the LC-EKF is far less than that of the OC-EKF, the LC-EKF is more appropriate for real time application.

Figure 10.

CPU time for each timestep

6. Conclusion

In this paper a consistency constrained EKF-SLAM algorithm based on the idea of local consistency is proposed and applied for the autonomous navigation of our test platform, the C-Ranger AUV. The concept of local consistency is introduced after an explicit theoretical derivation of the EKF-SLAM system. Furthermore, we present a locally consistency-constrained EKF-SLAM design, LC-EKF, in which the landmark estimates used for linearization are fixed at the beginning of each local time period rather than evaluated at the latest landmark estimates. Finally, the experimental results of our simulation and real-world tests show that our proposed LC-EKF algorithm performs well with respect to consistency, accuracy and computational efficiency. With this improved algorithm, autonomous navigation systems can be extended to numerous engineering applications, such as autonomous underwater vehicles (AUV), unmanned land vehicles and unmanned aerospace vehicles (UAV).

Footnotes

7.

This work is partially supported by the High Technology Research and Development Program of China (Grant No. 2006AA09Z231), Natural Science Foundation of China (Grant No. 41176076, 31202036, 51075377).

1

The superscripts ‘N’ and ‘L’ in equation (23) and stand for the nonlinear and linearized system respectively.

2

We use the superscripts, ‘I’ and ‘S’, to stand for the ideal and standard EKF sub-systems in the following derivation.

3

The accelerations can be obtained from the on-board sensor of the AHRS.

1

superscripts ‘S’, ‘P’, ‘C’, ‘D’ in Equations (52)– denote the sonar, pressure sensor, compass and DVL, respectively.

References

[1] Kalman

(1960) A new approach to linear filtering and prediction problems. J. basic eng. 82(1): 35–45.

[2] Thrun

(2004) Simultaneous localization and mapping with sparse extended information filters. Int. j. rob. res. 23(7–8): 693–716.

[3] Julier

(2000) A new method for the nonlinear transformation of means and covariances in filters and estimators. IEEE trans. autom. control. 45(3): 477–482.

[4] Montemerlo

(2003) FastSLAM: A factored solution to the simultaneous localization and mapping problem with unknown data association [PhD thesis]. Pittsburgh: Carnegie Mellon University.

[5] Paz

(2008) Divide and Conquer: EKF SLAM in O(n). IEEE trans. rob. 24(5): 1107–1120.

[6] He

(2011) A Novel Combined SLAM Based on RBPF-SLAM and EIF-SLAM for Mobile System Sensing in a Large Scale Environment. Sensors 11(11): 10197–10219.

[7] Tungadi

Kleeman

(2012) Autonomous loop exploration and SLAM with fusion of advanced sonar and laser polar scan matching. Robotica 30(1): 91–105.

[8] Cunningham

(2012) Fully distributed scalable smoothing and mapping with robust multirobot data association. In: IEEE International Conference on Robotics and Automation (ICRA). 2012 May 14–18; Saint Paul, MN, USA. pp. 1093–1100.

[9] Julier

Uhlmann

(2001) A counter example to the theory of simultaneous localization and map building. In: IEEE International Conference on Robotics and Automation (ICRA). 2001 May 21–26; Seoul, Korea. pp. 4238–4243.

10.

[10] Castellanos

(2004) Limits to the consistency of EKF-based SLAM. In: 5th IFAC Symposium on Intelligent Autonomous Vehicles. 2004 Jul 5–7; Lisbon, Portugal. pp: 1244–1249.

11.

[11] Bailey

(2006) Consistency of the EKF-SLAM algorithm. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). 2006 Oct 9–15; Beijing, China. pp. 3562–3568.

12.

[12] Huang

Dissanayake

(2006) Convergence and Consistency Analysis for Extended Kalman Filter based SLAM. IEEE trans. rob. 23(5): 1036–1049.

13.

[13] Huang

(2009) A first-estimates Jacobian EKF for improving SLAM consistency. In: 11th International Symposium on Experimental Robotics (ISER). 2008 Jul 14–17; Athens, Greece. pp. 373–382.

14.

[14] Huang

(2008) Analysis and improvement of the consistency of extended Kalman filter based SLAM. In: IEEE International Conference on Robotics and Automation (ICRA). 2008 May 19–23; Pasadena, CA, USA. pp. 473–479.

15.

[15] Welch

Bishop

(1995) An introduction to the Kalman filter [Technical report]. Chapel Hill, NC, USA: Department of Computer Science, University of North Carolina.

16.

[16] Lee

(2006) On the observability and observability analysis of SLAM. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). 2006 Oct 9–15; Beijing, China. pp. 3569–3574.

17.

[17] Hermann

Krener

(1977) Nonlinear controllability and observability. IEEE trans. autom. control. 22(5): 728–740.

18.

[18] Meyer

(2001) Matrix analysis and applied linear algebra. SIAM: In Tech. pp. 44–45.

19.

[19] Andrade-Cetto

Sanfeliu

(2004) The effects of partial observability in SLAM. In: IEEE International Conference on Robotics and Automation (ICRA). 2004 April 26–May 1; New Orleans, LA, USA. pp. 397–402.

20.

[20] Chen

(1990) Local observability matrix and its application to observability analyses. In: 16th Annual Conference of IEEE Industrial Electronics Society (IECON ′90). 1990 Nov 27–30; Pacific Grove, CA, USA. pp. 100–103.

21.

[21] Chen

(1991) Local observability and its application to multiple measurement estimation. IEEE trans. ind. electron. 38(6): 491–496.

22.

[22] System Metrics. 2013. Available from: http://en.wikibooks.org/wiki/Control_Systems/System_Metrics. Accessed on 04 Dec 2013.

23.

[23] He

(2012) AUV SLAM and Experiments Using a Mechanical Scanning Forward-Looking Sonar. Sensors 12(7): 9386–9410.

24.

[24] He

(2011) Autonomous Navigation for Autonomous Underwater Vehicles Based on Information Filters and Active Sensing. Sensors 11(11): 10958–10980.

25.

[25] He

(2013) A Distributed Parallel Motion Control for the Multi-Thruster Autonomous Underwater Vehicle. Mech. based des. struct. mach. 41(2): 236–257.

26.

[26] Huang

(2010) Observability-based rules for designing consistent EKF SLAM estimators. Int. j. rob. res. 29:502–528.