An optimization based Moving Horizon Estimation with application to localization of Autonomous Underwater Vehicles

Robotics and Autonomous Systems (

)

–

Contents lists available at ScienceDirect

Robotics and Autonomous Systems journal homepage: www.elsevier.com/locate/robot

An optimization based Moving Horizon Estimation with application to localization of Autonomous Underwater Vehicles Sen Wang ∗ , Ling Chen, Dongbing Gu, Huosheng Hu School of Computer Science and Electronic Engineering, University of Essex, UK

highlights • • • • •

A novel localization method for Autonomous Underwater Vehicle is proposed. Moving Horizon Estimation is designed for 3D single beacon based localization. Connection between filter- and optimization-based methods is explicitly studied. Observability is analyzed in the context of nonlinear discrete time systems. Simulation verifies the observability and performance of proposed method.

article

info

Article history: Received 21 October 2013 Received in revised form 14 April 2014 Accepted 5 May 2014 Available online xxxx Keywords: Localization Autonomous Underwater Vehicles Observability Optimization Moving Horizon Estimation Extended Kalman filter

abstract Localizing small Autonomous Underwater Vehicles (AUVs) that have limited payload and perception capability is of importance to promote popularization of underwater applications. Two different methodologies, filter and optimization based methods, can both be used to address the localization problem. But they are seldom rigorously compared and their relative advantages are rarely established. This paper presents a rigorous investigation on the relationship between these two methods. Based on this examination, a novel cooperative localization algorithm for the scenario where AUVs are localized by using range measurements from a single surface mobile beacon is proposed. The main contribution of this paper is threefold. First, major difference and close connection between filter based method and optimization based Maximum a Posteriori method are explicitly clarified by analytically solving optimization problems. Second, a novel localization algorithm combining a filter based extended Kalman filter and an optimization based Moving Horizon Estimation is developed for three-dimensional underwater localization in real-time and long-term applications. The algorithm allows data fusion of multiple sensors, imposes physical constraints on states and noises, bounds computational complexity, and achieves a compromise between better accuracy and lower computational requirement. Third, observability analysis of single beacon based localization algorithm is conducted in the context of nonlinear discrete time systems and a sufficient condition is derived. The observability and improved localization accuracy of the proposed localization algorithm are verified in a customized underwater simulator by extensive numerical simulations. © 2014 Elsevier B.V. All rights reserved.

1. Introduction Recently, localization problem of Autonomous Underwater Vehicles (AUVs) has been attracting enormous attention because localization is acknowledged as an essential capability for an AUV. The traditional techniques, such as dead-reckoning and acoustic baseline system with arrays of pre-deployed static beacons, suffer from unbounded localization errors, costly setting up, restricted

∗

Corresponding author. Tel.: +44 1206874092. E-mail addresses: [email protected], [email protected] (S. Wang), [email protected] (L. Chen), [email protected] (D. Gu), [email protected] (H. Hu). http://dx.doi.org/10.1016/j.robot.2014.05.004 0921-8890/© 2014 Elsevier B.V. All rights reserved.

operating area, etc. In addition, in order to decrease the costs and simplify the design complexity of AUVs, the small and simple AUVs are increasingly adopted. This makes the conventional underwater localization systems unsuitable due to the limited size, power and payload of AUVs and the high cost of localization systems in terms of hardware complexity and energy consumption. Therefore, a new underwater localization scheme based on the cooperation between AUVs and a single mobile surface beacon has been studied in recent years [1–4]. In a cooperative localization system shown in Fig. 1, a mobile surface vehicle and a number of underwater AUVs cooperate with each other to realize accurate localization. The surface vehicle can obtain its absolute position in real-time, and then broadcast it

2

S. Wang et al. / Robotics and Autonomous Systems (

Fig. 1. Architecture of underwater cooperative localization.

to AUVs. The AUVs measure the distances to this mobile surface vehicle in order to use them to bound the localization errors accumulated by dead-reckoning. Because the surface vehicle and AUVs are moving as a team, their operating area is dramatically enlarged compared with the static beacon scheme. It is well known in the robotics community that multi-sensor data fusion is an effective solution to robot localization problem [5], and a generic framework for multi-sensor fusion based underwater localization is proposed in [6]. Two different types of methodologies, filter based and optimization based methods, are frequently used. The filter based methods, such as Kalman filter (KF) and extended Kalman filter (EKF), recursively estimate the position of a robot. The series of KF based filtering methods are the most classic and widely used techniques for robot localization since they are easy-to-use and low-computational. In [4,7], the centralized EKF and decentralized extended information filter are proposed for off-line and on-line cooperative localizations of AUVs with highly accurate sensors, such as Doppler Velocity Log (DVL). In order to perform the cooperative localization of AUVs in the absence of DVL, algorithms based on EKF and particle filter (PF) are proposed in [1,3]. On the other hand, optimization based method, as an alternative to its filter based counterpart, formulates the localization problem into an optimization problem. In [3], the nonlinear Least Squares (NLS) optimization, a Maximum a Posteriori (MAP) method, is designed for underwater localization. As a consequence, two questions of significance have arisen. What are the essential differences between filter based method and optimization based MAP method?1 How can we choose the one that is more suitable for a specific application? To find solutions to these questions, it is necessary to clarify the characteristics of these two methods and the connection between them. Although these two methods both have been well known and widely used for many years, they are seldom rigorously compared and their relationship is rarely established except that a pioneering study on this is proposed in [8]. But this work specializes on visual simultaneous localization and mapping (SLAM) without considering optimization constraints and is mainly conducted by evaluating the experimental performance. In fact, KF produces a recursive solution to the unconstrained MAP in the context of linear systems subject to Gaussian noise (see Section 4.1). In other words, KF is identical to MAP when the system is linear, the noise is Gaussian and no constraint is imposed. In such cases, it is not necessary to use optimization based method because additional computation is needed to solve the optimization problem. However, there are two cases in which the optimization based method is appealing.

1 Unless otherwise stated, the optimization based method refers to the optimization based MAP method throughout the remaining paper.

)

–

First, the optimization based method can handle nonlinear dynamics more elegantly than filter based method. This is because the optimization framework allows the explicit use of nonlinear system models, and it iteratively calculates the optimal value by multiple linearizations, achieving higher accuracy than EKF. Second case is that constraints on states and noises can be easily introduced into the optimization based method, while it is difficult to incorporate them into the classic filter based method. Therefore, this paper is dedicated to explicitly clarifying the filter and optimization based methods and rigorously building a bridge between them. From the computational complexity perspective, the optimization based approach is usually unsuitable for real-time or longterm application, which seriously hiders its widespread use in robot localization. The computational complexities of optimization based methods grow over time since all the previously calculated states and observed measurements are incorporated into every computation. Meanwhile, the required time to iteratively solve a general optimization problem greatly depends on an initial guess, the number of iterations, etc., which means the total time for solving the problem is almost unpredictable. Some techniques can be adopted to reduce or bound the computational complexity. Sliding window filter (SWF) is proposed in [9] for vision based landing by using Schur Complement to marginalize out old data. However, none of the previous algorithms considers the importance of constraints on state variables and process uncertainties. In fact, the constraints, which are usually derived from prior knowledge, are of importance. For example, it is useful to employ the constraints on noises to precisely and realistically model the system noises as truncated normal distribution since physical quantities are almost always bounded. In [10,11], Moving Horizon Estimation (MHE), the dual of Model Predictive Control [12], is used to formulate the robot localization problem into a fixed window optimization problem incorporating the constraints. The MHE is designed for two-dimensional localization problem with the aid of several static beacons or landmarks. Our localization method is also based on MHE, but it combines with EKF for real-time implementation and only uses a single mobile beacon for three-dimensional cooperative localization. Since only range measurements from a single mobile beacon are used to bound the localization error for a group of AUVs, the observability, which can determine the robot localizability, should be investigated. By considering a nonlinear system as a linear timevarying system, observability Gramian is adopted in [13,14] to analyze the observability of a single range based localization system. In [15–17], the rank of observability matrix is proposed for observability analysis of linear systems or linearized nonlinear systems. However, the original localization systems are usually nonlinear and the linearization might an over confident estimation result [3]. Therefore, the observability rank condition derived from Lie derivative for nonlinear continuous time system is used in [3,18,19] to study the observability of robot localization. However, all the studies mentioned above transform discrete time systems into continuous time systems for observability analysis with the assumption that the sample time is small enough, and no research has considered the observability of robot localization using a nonlinear discrete system directly. Since most of the localization systems are discrete time system, it is worth analyzing the observability of localization in the context of its discrete representation. There is another challenge in our proposed system. The propagation speed of underwater acoustic communication is much slower than that of wireless signal in air, which results in the low frequency of underwater ranging. This high latency degrades the localization accuracy or even makes the estimator divergent if no special care is taken. In this paper, the relationship between two classic methodologies, filter and optimization based methods, of robot localization is investigated. According to this examination, a cooperative

S. Wang et al. / Robotics and Autonomous Systems (

localization algorithm which is effective for real-time and longterm scenario is designed. The main contribution of this paper is threefold: (1) The major difference and close connection between filter based KF and EKF method and optimization based unconstrained and constrained MAP are explicitly clarified by derivations on solving optimization problems. This also provides insight into the optimization constraints with the aid of interior point methods. (2) A novel localization algorithm combining filter based EKF method and optimization based MHE method is developed for three-dimensional underwater localization in real-time and longterm application. It allows data fusion of multiple sensors, imposes physical constraints on states and noises, bounds computational complexity, and achieves a compromise between better accuracy and lower computational requirement. (3) An approach to directly analyzing the observability of single beacon localization in the context of discrete time system is proposed, and sufficient conditions for the observability of both nonlinear system and linearized system are derived and compared. The rest of this paper is organized as follows. Section 2 outlines the research problem and system models. In Section 3, our localization algorithm is proposed after describing the classic filter based and optimization based localization algorithms. The relationship between filter and optimization based methods are derived in Section 4, followed by discussions on real-time implementation and optimization constraints. After analyzing the observability in Section 5, Section 6 presents the simulation results to verify the feasibility and performance of the proposed algorithm. Finally, the conclusion is drawn in Section 7. 2. Localization with a single beacon

)

–

3

2.1.2. Measurement models The system model (1) cannot provide accurate movement prediction due to the random characteristics and unavailability of the true control input uk . Therefore, an on-board IMU and a pressure sensor are employed for dead-reckoning to refine the predicted attitude and depth of the AUV. However, the error of the deadreckoning increases unboundedly over time. Then, a single beacon is introduced to reduce the uncertainty and error of the localization by providing acoustic range measurements. In the presence of an acoustic modem, an IMU and a pressure sensor, the measurement model of AUV with three types of measurements (range zr , attitude za and depth zd ) is zk = h(xk ) + µk = zr ,k

zaT,k



zd,k

T

(2)

where µk ∼ N (0, R ) is the Gaussian noise on measurements. We denote Hk as the Jacobian matrix of h(xk ) with respect to xk . The details of these three measurements are below: Range measurement. The single beacon broadcasts its position periodically via an acoustic modem for AUV to estimate the range by means of Time of Arrival. Denote the reference position of single beacon at time k as xbk = (xbk , ybk , zkb ). Then, the range measurement is zr ,k = hr (xk ) + µr ,k

=



(xbk − xk )2 + (ybk − yk )2 + (zkb − zk )2 + µr ,k .

We denote Hr ,k as the Jacobian matrix of hr (xk ) with respect to xk , and Rr as the variance of range measurement. Attitude measurement. IMU measurement is used to estimate the attitude by the following measurement model:

2.1. Problem description

za,k = ha (xk ) + µa,k = Ha,k xk + µa,k = 03×3



In this cooperative localization system, there are a team of AUVs and a single surface beacon. Each AUV is equipped with a lowcost pressure sensor, an Inertial Measurement Unit (IMU) and an acoustic modem instead of the expensive and energy consuming ones, such as DVL. AUV can measure ranges between the surface beacon and itself by the on-board acoustic modem. 2.1.1. Kinematic model Because the underwater AUV cannot access its velocities directly like the mobile robots using odometry or the AUVs with DVL, the kinematic model of AUVs used in [4,20] is adopted and slightly changed in this paper. Consider the position sk and attitude ϕk (roll, pitch and yaw) of AUV in global coordinate frame as the state xk = [sTk , ϕTk ]T to be estimated at time k, where



sk = xk

yk

zk

T

 ϕk = φk

,

θk

ψk

T

.

Control input is uk = [vkT , ωTk ]T where



v k = uk

νk

wk

T

,

 ω k = pk

qk

rk

T

are body-frame linear and angular velocities of AUV respectively. Then, under the assumption that sampling time interval is ∆T , the process model for AUV can be represented as the following nonlinear discrete time system: xk+1 = f (xk , uk ) + wk = xk + ∆T J (xk )uk + wk

(1)

where J (x) (given in Box I) is the transformation matrix and 0 denotes the zero matrix of compatible dimensions. Since uk cannot be perceived in the absence of DVL, it is set to be constant herein. However, it is still assumed to be affected by an additive Gaussian noise wk ∼ N (0, Qk ). We denote Fk as the Jacobian matrix of f (xk , uk ) with respect to xk , i.e., the system matrix of the linearized system.

I3×3 xk + µa,k



where I denotes the identity matrix of compatible dimensions. The covariance of IMU measurement is Ra . Depth measurement. Depth measured by pressure sensor is related to the vertical position of AUV. Then, the depth is zd,k = hd (xk ) + µd,k = Hd,k xk + µd,k

 = 0

0

1

01×3 xk + µd,k



with the variance Rd . According to the above description, the specific formulations of

 T µk and R are µk = µr ,k , µTa,k , µd,k and R = diag([Rr , Ra , Rd ]T ), respectively, where diag(·) is a function to map a block vector to a block diagonal matrix. 2.1.3. Surface beacon vehicle and cooperative localization mechanism Some powerful and precise sensors are equipped on the surface beacon vehicle to make sure that the reference position can be provided for the submerged AUVs. The reference can be directly obtained by GPS when surfacing or estimated by SLAM when submerged [21,22]. This heterogeneous scheme where a single beacon has many powerful sensors while no expensive equipments are deployed on the AUVs can substantially reduce the hardware and energy cost of the whole system, especially for a team of AUVs. In terms of the cooperative localization mechanism, several waypoints are pre-defined for the surface vehicle to pass through during the operation. Meanwhile, the underwater AUVs are properly controlled to cooperate with surface beacon so that all of them are always kept in the ranging area of the surface beacon. Because this paper mainly focuses on the localization problem, the cooperative control is simplified by controlling the vehicles passing through the waypoints. Readers interested in control and path planning of AUVs can refer to [23,24] for more details.

4

S. Wang et al. / Robotics and Autonomous Systems (

 J (x) = 

a11×3

T

a21×3

T

a31×3

T

T

cos ψ cos θ

b11×3

T

b21×3

T

cos ψ sin θ sin φ − sin ψ cos φ sin φ sin ψ sin θ + cos ψ cos φ cos θ sin φ 0 0 0

 sin ψ cos θ  − sin θ =  0  0 0

–



03×3



03×3

)

b31×3

T

T 

cos ψ sin θ cos φ + sin ψ sin φ sin ψ sin θ c φ + sin φ cos ψ cos θ cos φ 0 0 0

0 0 0 1 0 0

0 0 0 sin φ tan θ cos φ sin φ/ cos θ

0 0   0  cos φ tan θ   − sin φ cos φ/ cos θ



Box I.

The single beacon based multi-AUV cooperative localization problem is how to simultaneously and accurately localize several AUVs modeled as (1) with the measurement model (2). However, because the system is nonlinear and discrete time, the inherent nonlinear and discrete features have to be considered in the subsequent localization algorithm design and observability analysis.

3.1. Two different types of localization algorithms

2.2. State constraints

3.1.1. Optimization based method In an optimization based method, the current robot pose xk , along with its historical ones x0:k−1 , can be optimally estimated by solving the optimization problem (5). Because the models f (·) and h(·) in (5) are nonlinear, the MAP becomes a NLS problem, which indicates that analytical formulation of NLS cannot be derived. Therefore, numerical methods, such as Gauss–Newton method, are applied to solve this problem. A procedure to solve this is given in the Appendix. More details on numerical methods can be referred to [25]. Based on the derivation in the Appendix, the optimization problem (5) can be iteratively solved by solving a sequence of Least Squares (LS) problems. Then, for each iteration, it can be written in a LS form by omitting the iteration index (j) and using the property of a symmetric positive definite matrix 5 that 5−1 =

Some available prior knowledge of the system and the environment can be applied to constrain the state variables and noises, and improve the estimation accuracy. The constraints usually exist in the form of equality or inequality, e.g., the operating area of the robot can be represented by a geometric model with inequalities. Suppose the state variables, control inputs and disturbances satisfy the following constraints: gi (xi , ui , wi ) ≤ di ,

i = 0, . . . , k.

(3)

When performing the state estimation, these constraints should be taken into account to make sure the results are reasonable and accurate. 3. Localization algorithms

1

p(x0 , x1 , . . . , xk |z0 , z1 , . . . , zk ).

argmin Φk (x0:k ) = argmin ∥x0 − xˆ 0 ∥250 + x0 ,...,xk

+

∆x∗0:k = argmin ∥A∆x0:k − b∥22

(6)

∆x0:k

where ∆x0:k = ∆xT0 , . . . , ∆xTk ∆x∗0:k is the optimal value,



T

is the step-size in each iteration,

(4)

Under the assumption that the process is Markov and Gaussian noises are mutually independent, maximizing (4) with respect to {x0 , . . . , xk } corresponds to the optimization problem below:

k−1 

1

(5− 2 )(5− 2 )T :

With the system model (1) and the measurement model (2), localization problem can be solved by sensor fusion techniques. Two methodologies have been prevailing: filter based method and optimization based method. Specifically, in a filter based method, the obtained measurements are recursively summarized into probability distributions over current robot pose to be estimated. In contrast, for an optimization based method, the localization problem is formulated as an optimization problem over a batch of current and previous robot poses to minimize the sum of the estimation errors. At time k, suppose a sequence of measurements {zi }k0 = {z0 , z1 , . . . , zk } has been observed up to now. Then, the conditional probability density function (PDF) of state {x0 , x1 , . . . , xk } given measurements {zi }k0 at time k is:

x0 ,...,xk

In this subsection, both filter and optimization based methods are presented. We start reviewing these two different methods from the optimization based method.

k 

(7) and

∥zi − h(xi )∥2R

i =0

∥xi+1 − f (xi , ui )∥2Qi

(5)

i=0

T

where ∥z ∥2A = z T A−1 z, x0:k = xT0 , . . . , xTk and p(x0 ) ∼ N (ˆx0 , 50 ) is prior knowledge on the initial state. This formulation is the so-called MAP estimation, an optimization based method.



.

(8)

Once the optimum results of MAP problem (5) are derived, the robot poses x0:k are estimated.

S. Wang et al. / Robotics and Autonomous Systems (

)

–

5

3.2.2. MHE based update using range measurement When a range measurement zr is received by the AUV, the current state propagated by the dead-reckoning step is updated by MHE method. The MHE method has the same optimized problem as the optimization based method. However, it treats the entire time interval into two parts [26]. In the first part, a filter based method is used and its result is defined as an arrival cost for the second part to use. The second part operates as a sliding window optimization based method where an optimization problem is solved with consideration of the arrival cost. More specifically, the objective function Φk (x0:k ) of MAP problem (5) can be represented by separating the time interval into two sections {t : 0 ≤ t ≤ k − N } and {t : k − N + 1 ≤ t ≤ k}: Fig. 2. MHE with fixed-size window and the visualization of its cost function.

3.1.2. Filter based method It is acknowledged that KF based filtering approaches are the most classic filter based methods. By fusing different measurements, KF recursively estimates a Gaussian distribution over current robot pose. Since our localization system is nonlinear, EKF should be adopted. There are two steps in EKF. Prediction. In order to localize the current robot pose xk , the estimated pose xˆ k|k−1 at time k − 1 is predicted by using xˆ k|k−1 = f (ˆxk−1|k−1 , uk−1 ). This prediction is conducted using a constant velocity uk−1 due to the absence of DVL. Meanwhile, prediction covariance Pk|k−1 is computed by Pk|k−1 = Fk−1 Pk−1|k−1 FkT−1 + Qk−1 where Pk−1|k−1 is the updated covariance at time k − 1. Update. Once the measurement zk is obtained, the predicted robot pose xˆ k|k−1 and covariance Pk|k−1 are updated by using the following equations: xˆ k|k = xˆ k|k−1 +

Pk|k−1 HkT

(R +

)

Hk Pk|k−1 HkT −1

 zk − h(ˆxk|k−1 ) (9)



Pk|k = Pk|k−1 − Pk|k−1 HkT (R + Hk Pk|k−1 HkT )−1 Hk Pk|k−1 where xˆ k|k and Pk|k are the estimated current robot pose and covariance. 3.2. MHE based localization algorithm In the previous subsection, both the optimization based method and the filter based method are examined. In the next, our proposed localization algorithm which combines these two methods is given with implementation details. Its first step is a dead-reckoning step, which predicts the AUV position and updates its attitude and depth by using the EKF method in Section 3.1.2. The second step is to fuse range measurements by using a MHE based optimization method. 3.2.1. Dead-reckoning An EKF using attitude and depth measurements comprise a dead-reckoning step for the AUV. It estimates the changes of displacement and direction with respect to previous pose. This EKF includes two steps: prediction and updating. Due to the absence of DVL, a constant velocity is employed for prediction, which suffers from serious error over time. Then, an IMU and a pressure sensor are introduced in updating to reduce the error accumulated by the prediction. Because both measurements are acquired much faster than range measurement, the prediction is updated by attitude and depth observations at a high rate. Therefore, once an attitude or depth measurement is received, a standard EKF filter is used to update the corresponding mean state and covariance using the EKF method described in Section 3.1.2.

Φk (x0:k ) = ∥x0 − xˆ 0 ∥250 +

k−N 

∥xi+1 − f (xi , ui )∥2Qi

i =0

+

k−N 

∥zr ,i − hr (xi )∥2Rr

i=0

where N is the size of estimation window. Note that the measurement function h(·) in (5) is specified as range measurement model hr (·) because only the range measurement is incorporated into MHE method. Since the second part (shadowed) only relies on the states xk−N +1:k and the range measurements {zr ,i }kk−N +1 , the optimization can be transformed into a problem with fixed-window size, as shown in Fig. 2, by approximating the first part of the objec1 tive function to an arrival cost. By denoting {wi }kk− −N +1 as the set of input disturbances from time k − N + 1 to time k − 1, the states of in1 terest can be calculated by xk−N +1 and {wi }kk− −N +1 thanks to the process model (1). Therefore, MHE with the constraints (3) at time k is argmin

k−1 

k−1 xTs ,{wi }T i=Ts s

∥wi ∥2Qi +

k 

∥zr ,i − hr (xi )∥2Rr + ZTs (xTs )

i=Ts

s.t. gi (xi , ui , wi ) ≤ di ,

i = Ts , . . . , k

where Ts = k − N + 1 is the starting time of MHE window, and ZTs is the approximation of arrival cost at time Ts . The graphical interpretation of MHE is shown in Fig. 2. In this figure, the shadowed area represents the errors wi and µr ,i to be minimized. This MHE only relies on the latest N historical measurements, while the past measurements obtained in the period (0, k − N ) are considered in the arrival cost. The arrival cost ZTs is essential in the MHE estimation because it bounds the original optimization problem into a fixed size problem. Then, the key is how to properly approximate the past data acquired in the period (0, k − N ). By employing the EKF approximation in [26], the arrival cost is given by

ZTs (xTs ) = ∥xTs − xˆ Ts ∥2PT

(10)

s

where the covariance P is propagated by Pi+1 = Qi + Fi (Pi − Pi HrT,i (Rr + Hr ,i Pi HrT,i )−1 Hr ,i Pi )FiT . The current estimate xˆ k can be yielded by substituting the obtained xTs , {wi }Tks−1 and control input {ui }kTS−1 into (1) recursively. The

intuitive idea to incorporate the set of inputs {u}kTS−1 is to augment j

the optimization variables by including the positions x¯ i , i = Ts , . . . , k − 1 maintained by the calculation in the dead-reckoning step, see Fig. 3. However, this causes extremely high computational complexity (for this work, it is an optimization problem with several hundreds valuables). Therefore, we accumulate the control

6

S. Wang et al. / Robotics and Autonomous Systems (

)

–

Then, the optimization problem (5) changes to the LS problem: argmin Φk (x0:k ) = argmin ∥x0 − xˆ 0 ∥250 +

k 

x0 ,...,xk

x0 ,...,xk

+

k−1 

∥zi − Hxi ∥2R

i =0

∥xi+1 − Fxi ∥2Qi .

(12)

i =0

Block factorization is employed here to derive KF from LS. According to (12), the corresponding LS for x0 at k = 0 is formulated as

Fig. 3. System description between two range measurements.

argmin Φ0 (x0 ) = argmin ∥x0 − xˆ 0 ∥250 + ∥z0 − Hx0 ∥2R x0

x0

which can be rewritten as an optimization problem with equality constraints: argmin ∥x0 − xˆ 0 ∥250 + ∥µ0 ∥2R x0

s.t. z0 − Hx0 − µ0 = 0. By formulating Lagrangian and using Karush–Kuhn–Tucker (KKT) conditions [25], it gives Fig. 4. MHE (both solid and dashed circles) and EKF (solid circle) comparison for range based localization.

input of each dead-reckoning during the period of two consecutive range measurements t and t + 1 as the approximate control input ut for this stage. Note that the MHE method is identical to the MAP method when current time is less than window size N. The intuitive comparison between MHE and EKF based localization is described in Fig. 4. Because no bearing information is available, the possible location of the vehicle is distributed around a sphere in three-dimension, as the possible positions shown. Because the EKF method only adopts one range measurement in each update, the potential pose is not constrained by the historical ranges. By contrast, the MHE method can employ previous range measurements to further confine the reasonable area.

 −1 50  H 0

0

where λ0 is Lagrange multiplier associated with equality constraints. Using Schur Complement, we can obtain 1 1 T −1 T −1 ˆ z0 . (5− H )x0 = 5− 0 +H R 0 x0 + H R

Hence, it can be derived that xˆ 0 

x0 =

state prediction

+ 50 H T (R + H 50 H T )−1   

× (z0 −

Kalman gain

H xˆ 0 

)

(14)

predicted measurement

1 T −1 (5− H )−1 = 50 − 50 H T (R + H 50 H T )−1 H 50 0 +H R

The two different methodologies, filter based and optimization based methods, are presented in the preceding section for robot localization. Although these two methods are well known and widely used to address various sensor fusion problems, their connection is rarely established explicitly. It is helpful to study them in the same framework to gain some insights into their own characteristics and clarify the connection between them. To this end, we examine both of them under a unified optimization framework to analyze their performances. The optimization problem (5) is taken as the foundation for the following analysis. The computational complexity is also analyzed in this section, followed by the discussion on optimization constraints. 4.1. From optimization based method to filter based method In this subsection, the relationship between filter based and optimization based methods is discussed.

1 T −1 (5− H )−1 H T R −1 = 50 H T (R + H 50 H T )−1 . 0 +H R

Comparing (14) with the EKF state update (9), it is clear that (14) is the well known formulation of KF based method. Consider the next step k = 1. The corresponding Lagrangian is

L(x, λ) = ∥x0 − xˆ 0 ∥250 + ∥µ0 ∥2R + ∥w0 ∥2Q0 + ∥µ1 ∥2R

+ λ0 (z0 − Hx0 − µ0 ) + λ1 (z1 − H (Fx0 + w0 ) − µ1 ). The following equation can be obtained by applying KKT conditions:

0 I  0  T H 0

I R −1 0 0 0 0

0 4.1.1. Unconstrained optimization to KF In order to clearly examine the relation between KF and optimization based method, the models f (·) and h(·) are assumed to be linear: zk = Hxk + µk .

(13)

according to the results of Matrix Inversion Lemma:

4. Performance analysis of localization algorithms

xk+1 = Fxk + wk

 x    1 0 0 ˆ 5− 0 x0 1   I   − λ 0  =  z0  2 0 R −1 µ

HT 0 I

(11)



z0

0 0 Q0−1 0 H 0



 0     0  =  −1  . 50 xˆ 0   z  1

0

H 0 0 1 5− 0

HF 0

0 0 HT FT HT 0 I

   −1λ 0 0  2  0   µ0    0   w0   x  0  0   1  I − λ1  2 R −1 µ1

S. Wang et al. / Robotics and Autonomous Systems (

By using Schur Complement, it yields

 −1 50 + H T R −1 H  HF

FT HT −HQ0 H T I

0

 x  0 0  1   I − λ1  2 R −1 µ1

  1 T −1 ˆ 5− z0 0 x0 + H R . = z1

(15)

0 1 Define x1 and 5− 1 as

x1 = Fx0 − Q0 H T



1

− λ1



2

51 = F (50 + H R −1 H )−1 F T + Q0 . −1

T





(16)



KF covariance prediction in this step

0

HT 0 I





 x1 0  1   I − λ1  2 R −1 µ 1

  1 −1 1 T −1 T −1 ˆ z0 ) 5− H )−1 (5− 1 F (50 + H R 0 x0 + H R  = z1 0

  1 5− 1 Fx0 =  z1  .

For a nonlinear system, the unconstrained optimization based method (5) can be solved by numerical algorithms, as discussed in Section 3.1.1. According to Appendix, it is clear that (A.1) has the identical structure with (12), which means this unconstrained optimization problem is also related to KF. In fact, solving the original nonlinear problem (5) is similar to EKF by first linearizing the nonlinear system and then solving LS. However, the optimization based method is superior to the filter based EKF method since it iterates multiple times until convergence to a local minimum of (5). This indicates that the optimization based method is more accurate than EKF, especially when no good linearization point is available. In addition, similar to KF, it is also difficult for EKF to employ constraints on states and noises. Therefore, the optimization based method is more appealing than the filter based EKF method, as a viable alternative for nonlinear system estimation.

(17)

∆x∗0:k = argmin ∥∆x0 − a0 ∥250 +

0

∆x0:k

Note that (17) has the similar structure as (13), which indicates that the remaining time steps can be inductively generalized to this representation. By applying Schur Complement to (17), it gives x1 =

Fx

0 

state prediction

7

4.1.3. Constrained optimization to EKF In practice, many systems are not only nonlinear but also subject to physical constraints. These constraints normally exist as prior information on system states and noises, such as maximum velocity of the robot and reasonable operating area. The capability to take these constraints into consideration is critical. The prior knowledge cannot be easily incorporated into recursive fashion based filtering methods, e.g., KF or EKF. In contrast, optimization based methods can introduce this information as equality and inequality constraints on system states or noises. Consider the optimization problem (5) subject to the nonlinear constraints (3). When solving the system in each iteration, similar to Section 3.1.1, it is linearized and we have



Then, (15) can be rewritten as

 −1 51  H

–



KF covariance update in last step



)

+ 51 H T (R + H 51 H T )−1 (z1 −    Kalman gain

HFx

0

)

predicted measurement

+

k 

∥ci − Hi ∆xi ∥2R

i =0

k−1 

∥∆xi+1 − Fi ∆xi − ai+1 ∥2Qi

i =0

s.t. gi (xi ) + ∇ gi (xi )T ∆xi ≤ di ,

i = 0, . . . , k.

(18)

The constraints can be rewritten more compactly as

which is the well known KF compared with (9). 51 given in (16) is identical to the covariance prediction and update in KF. Extending the results to the subsequent steps by adopting the analogous technique can derive the KF recursively. This derivation of KF from LS demonstrates that KF is equivalent to MAP for linear systems. Indeed, for linear systems KF is recognized as a recursive solution algorithm to MAP. The significance of applying KF instead of MAP is that it provides an optimal solution by solving MAP recursively, which means it can dramatically reduce the computational demand since the problem size of MAP grows over time. Although KF based method offer a computationally efficient way to derive the optimal solutions of optimization problem compared with MAP, it cannot handle nonlinear systems. In addition, it cannot easily introduce constraints on states and noises. Therefore, the optimization based method tends to be more desirable than KF when a system is nonlinear or there are some system constraints that are expected to be considered.

G∆x0:k ≤ d

4.1.2. Unconstrained optimization to EKF In the preceding subsection, the KF filter method is discussed by mainly showing it is equivalent to the optimization based MAP method (5) when the system is linear. However, most of the systems encountered in practical are nonlinear, e.g., the localization system (1) and (2). Then, the filter based EKF method can be adopted for nonlinear systems, as shown in Section 3.1.2.

where λ is the vector of Lagrange multipliers. According to the block matrices defined in (7) and (8), the KKT necessary and sufficient conditions of optimization problem (18) are given by

(19)

where

  ∇ g0 (x0 )T T ∇ g1 (x1 )    G = diag   ..   .

d0 − g0 (x0 ) d1 − g1 (x1 )





d= 

∇ gk (xk )T

.. .

. 

dk − gk (xk )

In order to solve this constrained optimization problem, the primal–dual interior point method [25] can be applied. The Lagrangian of (18) is

L(∆x0:k , λ) =

1 2

∥∆x0 − a0 ∥250 +

k 1

2 i=0

∥ci − Hi ∆xi ∥2R

k−1 1

∥∆xi+1 − Fi ∆xi − ai+1 ∥2Qi 2 i=0 + λ(G∆x0:k − d) +

∇∆x0:k L = (FT Q−1 F + HT R−1 H)∆x0:k − (HT R−1 c + FT Q−1 a) + GT λ = 0 diag(λ)(G∆x0:k − d) = 0.

(20)

8

S. Wang et al. / Robotics and Autonomous Systems (

In primal–dual interior point methods, the key idea is to directly solve the KKT conditions by using Newton’s method. Assume the current point and the Newton step are (∆x0:k , λ) and (δ x0:k , δλ), respectively. Therefore, Newton step for solving (20) is given by  T −1   δ x0:k (F Q F + HT R−1 H) GT δλ diag(λ)G −diag(d − G∆x0:k )   T −1 T −1 T −1 −(F Q F + H R H)∆x0:k + (H R c + FT Q−1 a) − GT λ = . (21) −diag(λ)(G∆x0:k − d) By eliminating δλ, δ x0:k can be first obtained. The remaining δλ can then be derived by back-solving. Applying Schur Complement to (21), the coefficient of δ x0:k is Aδ x0:k = (FT Q−1 F + HT R−1 H)

+ GT (diag(d − G∆x0:k ))−1 diag(λ)G    extra part

 T H

= F T Q −1 F + G  −1   R 0 H × −1 0 (diag(d − G∆x0:k )) diag(λ) G = F T Q −1 F +  HT  R−1 H.

(22)

Since d − G∆x0:k ≥ 0 (according to (19)), λ ≥ 0 (according to KKT conditions [25]) and R is positive definite,  R is also positive definite. The constrained optimization (18) is related to EKF in the same form as discussed in Section 4.1.2. In fact, if (22) is re-examined in the perspective of Fisher Information Matrix, the extra part, which is contributed by the constraints, can be regarded as additional information obtained from a new ‘‘sensor’’. Therefore, the optimization constraints can be intuitively considered as a supplementary ‘‘sensor’’. Meanwhile, thanks to this extra information, the uncertainty of constrained optimization problem should be further reduced compared with that of EKF. 4.2. Computational complexity analysis In this subsection, techniques adopted in our proposed localization algorithm to reduce computational complexity are discussed in Section 4.2.1, before presenting a general analysis on real-time implementation in Section 4.2.2. 4.2.1. Our proposed localization algorithm It is known that the filter based method can update the estimation result recursively and it is more suitable for real-time applications. On the other hand, it is popularly believed that the optimization based method requires heavy computation and is not applicable for on-line use. This is one of the main reasons that the filtering techniques are preferred and the optimization based methods have not been widely used. However, with the recent advance in computing power, e.g., Graphics Processing Unit accelerated computing, the optimization based method is increasingly popular. Meanwhile, special techniques can be employed to decrease the computational complexity. To achieve a compromise between the accuracy performance of the optimization based method and the computational requirements, two special measures are taken in our localization algorithm. The first is a hybrid structure. The recursive-style EKF is used to update the prediction using attitude and depth measurements, which are normally obtained at high frequencies and then have to be fused at high rates. This serves as the dead-reckoning step and compensates for the low rate range measurement. In contrast, due to the low propagation speed of acoustic signal and

)

–

usually long distance between the AUVs, the range measurement is obtained at a low rate and can be updated by using an optimization based method. This optimization based method cannot only highly restrain the noise and smooth the estimate but also consider the constraints on the operating environment and physical system. Secondly, to reduce the computing power, MHE is employed to marginalize out the historical information. By limiting the amount of information involved in each update, the computational complexity of optimization portion is bounded, which insures the real-time and long-term use. 4.2.2. Real-time implementation In order to accelerate the computation of optimization based method, several techniques can be applied. Since the essential to solve an optimization problem is to compute a sequence of linear equations, it is critical to decrease the computing cost by solving linear system efficiently. Thus, marginalizing out historical data to bound the problem size of optimization can make it applicable for real-time applications. This is the strategy employed in our localization algorithm. Moreover, the special structure of coefficient matrix can be explicitly exploited to speed up the computation. Methods to bound computational complexity. An efficient method to bound computational complexity is to reduce the amount of information processed at each step and select the valuable information rather than full information. Then, the number of the linear equations can be limited and the computational complexity is bounded. Techniques to select information of interest can be time based, entropy based, keyframe based, etc. Time based approaches, which try to discard old information by using a size-fixed window that slides over the measurements, are the most intuitive one. This window size compromises the accuracy performance of the estimation and the computational requirements to solve the optimization problem. This is the strategy used in our proposed MHE based localization algorithm. The MHE method is examined here again in terms of the computational complexity. In order to simplify the explanation, we assume the system is linear one as (11) and there is no constraint. If the window size is set to be N = 1 and the algebraic expression of arrival cost (10) is employed, then the MHE problem becomes argmin ∥zk − Hxk ∥2R + ∥xk − xˆ k ∥2Pk . xk

Then, by using KKT condition and Matrix Inversion Lemma, we can derive that xk = xˆ k + Pk H T (R + HPk H T )−1 (zk − H xˆ k ). It can be seen that this is the state update equation of KF. Therefore, the well-known KF is a special case of the MHE method when its window size is 1. In order to keep the effect of the old marginalized information, KF method uses Gaussian distribution and covariance to summarize the historical information. This is similar to approximating the arrival cost in MHE by KF. This equivalence between KF and MHE for linear system verifies that the MHE method is not only optimal but also can balance estimation accuracy and computational complexity. Special pattern exploration. For the optimization based method, the key is to solve the LS problem (6). According to (7), it can be noticed that the coefficient matrix A has a special block pattern and it is sparse. Therefore, by explicitly exploiting these structure properties, the computational complexity of solving (6) can be dramatically reduced. By examining the unconstrained optimization problem (6), it yields that the optimal solution is (AT A)∆x∗0:k = AT b. By studying the structure of A in (7), we can find that AT A has band-diagonal

S. Wang et al. / Robotics and Autonomous Systems (

)

–

9

pattern. Similarly, ATδ x Aδ x in the constrained optimization problem (22) is band-diagonal as well. Equation systems with this special structure can be efficiently solved by performing LU decomposition and forward- and back-substitution with O(N ) operations [27]. 4.3. Optimization constraints One of the merits of our proposed localization algorithm is that constraints can be incorporated. The ability to take this information into consideration is useful to exactly model the system in practice and increase the localization accuracy. Specifically, for real systems, it is not likely for physical quantities to be unbounded, which indicates that the normal distribution assumption usually made could be improved. In fact, truncated normal distribution is more appealing and realistic for the distribution of quantities of real systems in the perspective of probabilistic interpretation. The certain region or the physical limitations of unknown variables can be represented as the constraints in the optimization problem. By using this prior knowledge, the constrained optimization based methods can outperform the standard filter based methods, e.g., KF and EKF. To intuitively exposit the importance of constraints, a simple linear system is taken as an example for demonstration. The system is



x1k+1 x2k+1



 =

yk = 1.5



0.3 0.5



1 −0.1



 x1 k

−6

x2k



x1 k x2 k



Fig. 5. Comparison between constrained optimization and KF for a linear system with prior knowledge.

where hr1 = (xb − x)2 + (yb − y)2 + (z b − z )2 . Then, the gradient of the measurement equation with respect to the system state x is



  +



0 wk 2

 h  r1  ∇ 21 =  0  0  0

+ vk

where the noises are wk ∼ N (0, 1) and vk ∼ N (0, 0.01). Suppose the noise wk is non-negative only. Then, the constraint wk ≥ 0 can be introduced to the MHE method, while KF can only assume it is normal distribution. The window size of our example is N = 10. The simulation result of this example is shown in Fig. 5. We can see that by employing this constraint, the MHE method can estimate accurately while KF cannot track the ground truth. Although this prior knowledge is very useful, the equality or inequality constraints are difficult to be introduced into the recursive form filter based methods. Therefore, in some cases KF cannot compute the probability distribution of unknown variables accurately. 5. Observability analysis In a localization system, the localizability of robots is measured by using observability of the localization system. If a state is observable, then it can be updated by using the measurements. Thus, it is localizable. The criteria based on codistribution for observability of a nonlinear discrete time system [28] is adopted to analyze the observability and derive the sufficient condition of this single beacon based localization directly. Consider the observation space 2 =  k≥1 2k constructed by the set of functions

21 = {h} 2k = {h ◦ fu1 ◦ · · · ◦ fuj , 1 ≤ j ≤ k − 1},

xb − x

k≥2

(23)

where fu (x) = f (x, u) and ◦ denotes the function composition, i.e., g ◦ f (x) = g (f (x)). According to the criteria in [28], a nonlinear discrete time system is locally weakly observable if dim d2 = n where d2 is the differential of space 2 and n is the system order. For the system described above, the number of system state variables to be tested is 6. According to (2) and (23), it yields

0

yb − y

zb − z

hr1 0 0 0 0

hr1 0 0 0 1

 0

0

0

1 0 0 0

0 1 0 0

0 . 0 1 0



(24)

It can be seen that the minimum rank of ∇ 21 is 4. In order to guarantee the local observability of the system by keeping the full rank of d2, the first two columns of ∇ 2 should be independent of each other. Therefore, for clarity and simplicity, only the first two columns of the remaining ∇ 2i is considered. The composition 22 = h ◦ f is

22 = hr2



φ + ∆B1

θ + ∆B2

ψ + ∆B3

s3

T

where



(xb − s1 )2 + (yb − s2 )2 + (z b − s3 )2 s1 = x + ∆ T a 1 v , s2 = y + ∆ T a 2 v , s3 = z + ∆ T a 3 v ∆Bi = ∆T bi ω, i = 1, 2, 3. Therefore, by calculating the gradient of 22 , we have  b  x − s1 y b − s2 ∗ ∗ ∗ ∗  h hr2  r2   0 ∗ ∗ ∗ ∗ ∇ 22 =  0 (25)  0 ∗ ∗ ∗ ∗  0  0  0 ∗ ∗ ∗ ∗

hr2 =

0

∗

0

∗

∗

∗

where ∗ is the corresponding equations whose specifics are ignored for clarity. Consequently, from (24) and (25), the observability submatrix formed by extracting the first two columns is obtained as follows:



hr1 O=  xb − x − ∆T a1 v



hr2



yb − y

xb − x

hr1

 . y − y − ∆T a v  b

2

hr2

In order to satisfy dim d2 = n, the determinant of O must be nonzero. Under the assumption that the measured range cannot be zero, i.e., hr ̸= 0, the sufficient condition for observability is

(yb − y)∆T a1 v − (xb − x)∆T a2 v ̸= 0.

(26)

10

S. Wang et al. / Robotics and Autonomous Systems (

)

–

Fig. 6. Local observability using single static beacon.

Because a1 and a2 in (1) are related to ϕ, the observability of the system is determined by ϕ, v and relative position between the beacon and the AUV. The sufficient condition (26) can only ensure the local observability rather than the global one. There exist some unobservable regions where the pose of the robot cannot be uniquely determined as shown in Fig. 6. A solution is to improve the observability by designing the appropriate path planning with the aid of mobile beacon [29]. It is interesting to employ the local observability matrix condition [30] to study the observability of the linearized system again. The measurement model (2) is linearized as

Fig. 7. A scenario where linearized system is locally unobservable but original nonlinear system is observable. The relative directions αk+i between robot and beacon remain constant during the period.

zk = Hk xk + µk where xbk − xk



ybk − yk

zkb − zk

hr1,k 0 0 0 0

hr1,k 0 0 0 1

 hr1,k   Hk =  0  0  0 0

 0

0

0

1 0 0 0

0 1 0 0

0 .  0  1 0



Fig. 8. Simulated underwater environment in ROS Gazebo simulator.

Then, the local observability matrix over the time interval [k, k + n − 1] is Hk

 



Hk+1 Fk

 . 

.. .

OL =  

Hk+n−1 Fk+n−2 · · · Fk If rank(OL ) equals n, then the system is locally observable during the period. By performing the elementary row and column operations to OL , we have

This indicates that the linearized system is locally unobservable if the relative direction between robot and beacon is time-invariant, see Fig. 7. However, the observability of the original nonlinear system can still be held in this scenario according to the sufficient condition (26). The observability analysis verifies that the measurement equation (2) is sufficient for pose estimate of the robot if the corresponding sufficient condition for observability can be satisfied. However, the comparison of observability analysis between the nonlinear and linearized systems implies that some states which cannot be observed by a linearized observer, such as EKF, can be successfully estimated by using a nonlinear filter. Therefore, a localization algorithm based on nonlinear estimator is more applicable for a robot localization system with nonlinear dynamics. 6. Simulation results

. According to the principle of partitioned matrix, the rank of OL can be obtained by

In this section, the performance of the proposed localization algorithm is evaluated by simulation. The simulation is conducted by using a customized underwater simulator based on Robot Operating System (ROS). The system setup and simulator are first introduced. Then, the results of observability analysis and localization are presented.

rank(OL ) = rank(I4 ) + rank(M ) = 4 + rank(M ).

6.1. System setup and underwater simulator

In order to keep the observability, rank(OL ) has to be full rank, i.e., rank(M ) = 2. Therefore, the sufficient condition for observability derived by local observability matrix is that

In order to provide a simulation environment that is realistic, an underwater simulator based on ROS is developed. The underwater localization algorithm can be tested on it. Fig. 8 illustrates the simulated underwater environment in ROS Gazebo simulator. The AUV can shuttle in the underwater environment and perceive the surrounding.

ybk+i − yk+i xbk+i − xk+i

̸=

ybk+j − yk+j xbk+j − xk+j

,

i, j ∈ [0, n − 1] and i ̸= j.

(27)

S. Wang et al. / Robotics and Autonomous Systems (

)

–

11

The performance of the EKF is presented in Fig. 10(a) to verify that the designed EKF for this example is well tuned, ensuring it is ideal for comparison. Specifically, examining the innovation of EKF can effectively evaluate the filter consistency. As shown in Fig. 10(a), the innovation sequence has zero mean and most of the innovations are within the ±2σ confidence bounds. Meanwhile, the average normalized innovation converges to 1, the degree of freedom with measurement. Note that the measurement here is range only since IMU and altimeter measurements are fused in the prediction step. These indicate that the innovation is unbiased and white, and the EKF is well designed. However, even this EKF is carefully designed to strive for the best performance, its localization results still diverge from the ground truth, as shown in Fig. 10(b). In contrast, the result of our proposed MHE method can converge to the ground truth with a bounded error. The reason is that our nonlinear estimator has better observability compared with the EKF method, making the unobservable region for EKF be recovered by MHE. Fig. 9. Simulated operating field with the route of robot. (For interpretation of the references to colour in this figure legend, the reader is referred to the web version of this article.)

The operating field of this simulation is presented in Fig. 9, in which the route of the robot and its starting and ending positions are marked. The size of this operating area is 55 m × 50 m. There are many islands, which are obstacles for AUV, located in the shipping channel. An AUV starts from the starting position, travels along the river channel without crashing on the islands, and finally arrives at the ending point. It takes about 20 min for our simulated AUV to travel the whole trip in the simulator. According to the specification of Tritech micron acoustic modem which can be employed for ranging, the frequency of range update is set to be 1 Hz and Rr = (0.2 m)2 . The frequencies of IMU and pressure sensor are 100 Hz with covariance Ra = (0.36 rad/s)2 I and Rd = (0.2 m)2 . The window size N of the MHE method is set to be 5. Different localization algorithms, i.e., deadreckoning (DR) in Section 3.2.1, EKF in Section 3.1.2 and MHE in Section 3.2, are tested to estimate the position of the robot. Note that these different localization algorithms are all provided with the same control input and observation in our simulation. 6.2. Observability analysis In order to interpret the observability analysis proposed in Section 5, an example is taken to test our localization method and EKF approach. In this example, the robot is stationary relative to the beacon during the motion, see the trajectories of ground truth and beacon in Fig. 10(b). For this stationary scenario, the local observability matrix condition (27) is violated while our nonlinear observability analysis method (26) is satisfied.

(a) Innovation of EKF. CBs stands for confidence bounds.

6.3. Localization accuracy In order to evaluate the performance of different localization algorithms, the AUV is operated to travel in the scenario shown in Fig. 9 through our underwater simulator. The localization results of DR, EKF and MHE in the scenario with a single static beacon are shown in Fig. 11. Note that in this situation, the system constraints of MHE are not introduced. Fig. 11(a) presents the localization trajectories of DR, EKF and MHE. The yellow areas are the islands in the operating environment as shown in Fig. 9. It can be seen that the DR (magenta dashed line with circles) gradually diverges from the ground truth (red solid line), which is because the DR only employs the inertial measurements and velocities. In contrast, the estimated trajectory of the EKF has bounded localization errors thanks to the use of range measurements from the beacon. However, the EKF method cannot estimate the position accurately during the entire path, especially the big offset around the second turning. Our MHE approach is more accurate than the EKF, which can be seen clearly from the localization errors of EKF and MHE in Fig. 11(b). One of the reasons for this is that the EKF method which only utilizes current range observation in the update cannot acquire sufficient range information, resulting in the ineffective estimate of the position, while the proposed MHE can make use of several previous range measurements in one estimation. Furthermore, the linearization of nonlinear system in the EKF degrades the accuracy of estimation, while the MHE can handle the nonlinearity more precisely than EKF by multiple linearization and iteratively solving the optimization problem. However, as shown in the enlargement in Fig. 11(a), the trajectory of MHE without state constraints crosses the islands occasionally, which is not reasonable in reality. Fig. 12 presents the localization trajectories and errors of EKF and MHE in the context of a single surface mobile beacon. In

(b) Trajectories of ground truth (GT), dead-reckoning (DR), EKF, MHE and beacon on X –Y coordinate system.

Fig. 10. Localization results under the scenario where the observabilities of EKF and MHE are different.

12

S. Wang et al. / Robotics and Autonomous Systems (

)

–

(a) Localization trajectories of dead-reckoning (DR), EKF and MHE.

(b) Localization errors of EKF and MHE. Fig. 11. Localization trajectories and errors of scenario with static surface beacon.

Fig. 12(a), the black line with plus sign represents the route of the mobile beacon. The constraints on system states x and y positions are imposed onto the MHE method. It can be seen from Fig. 12(a) that the estimated trajectory of AUV by using EKF is still inferior to MHE. The corresponding localization errors in Fig. 12(b) also verifies that the performance of EKF in terms of localization error is worse than that of MHE. The reasons why MHE is superior to EKF are similar to the ones described in the static beacon scenario. However, the estimate of the entire trajectory is ensured to be within the channel by introducing the state constraints into MHE, see the magnified parts in Fig. 12(a). Compared with MHE with constraints, the positions estimated by EKF cannot always be confined in the river channel, and the constraints on operating area cannot be easily imposed in the classic EKF method. By these numerical evaluations, it is verified that the proposed MHE method can achieve more accurate localization estimation than EKF in the

context of single beacon and it is more suitable for the single beacon based localization. The overall uncertainties of localization using DR, EKF and MHE in one trail are shown in Fig. 13. As expected, the uncertainty of dead-reckoning grows monotonically without bound while these of EKF and MHE are not increased over time in both x, y, z and x, y with the aid of range measurements. Due to the lack of absolute position information or position update, uncertainties of DR on spatial x, y, z and x, y, which are solely based on system model and inertial sensors, are unbounded. Note that the uncertainties on z and orientation are bounded thanks to the observation from depth sensor and IMU. In contrast, with the aid of range measurements from the beacon, the uncertainties of EKF and MHE methods are restrained. To further examine the uncertainty, the corresponding localization errors on x, y and z of MHE method with constraints are shown in Fig. 14 against their ±3σ confidence intervals. It can be seen that

S. Wang et al. / Robotics and Autonomous Systems (

)

–

13

(a) Localization trajectories of EKF and MHE with constraints.

0

(b) Localization errors of EKF and MHE. Fig. 12. Localization trajectories and errors of scenario with mobile surface beacon and MHE constraints.

the errors on x, y and z are all within their lower and upper confidence bounds with extra allowance, which indicates that the estimation of this MHE based localization is reliable and the proposed algorithm is effective. 6.4. Computational complexity To evaluate the time computational complexity of our proposed algorithm, different window sizes of MHE are tested. Note that all the tests are executed on Matlab 2013b installed on a computer with Intel(R) [email protected] GHz CPU and 4 GB memory. For each trial, there are 1103 range measurements in total. Similar to the previous simulations, the frequency of range observation remains to be 1 Hz. The computational complexity of MHE method is analyzed when its window size is 5, 10, 20 and 50, respectively. In

Table 1, the total required running time and the reachable update frequency of MHE method are presented. It can be seen that the real-time requirement (the update frequency of algorithm should be higher than that of range measurement) can be achieved if the window size is less than 20. For our previous MHE localization simulations whose window size is set to be 5, the frequency can be as high as 7.5287 Hz. Because it is well known that the frequency of range measurement in underwater is low due to the slow speed of sonar propagation and long distance between robots, the MHE method whose update frequency can be several Hz is capable for real-time application. The running time of each update is shown in Fig. 15. It can be seen that, as expected, the computational complexities of MHE with different window sizes are bounded rather than increasing over time. The reason is that MHE introduces the fixed window size technique to bound the computational complexity. Meanwhile, in the scenarios where the window sizes are 5 and 10, the computing

14

S. Wang et al. / Robotics and Autonomous Systems (

Fig. 13. Overall uncertainty on position x, y, z and x, y only.

)

–

Fig. 15. Computational complexities of MHE method when its window size is 5, 10, 20 and 50.

advanced techniques are employed to speed up the solving of optimization problem, it can be run much faster. 7. Conclusions

Fig. 14. Errors on x, y, z and 3σ confidence intervals. Table 1 Computational complexity of MHE method with respect to the different window sizes. Total time is for 1103 updates. Window size (N)

Total time (s)

Frequency (Hz)

5 10 20 50

146.507 236.2890 581.4850 2509.026

7.5287 4.6680 1.8969 0.4396

time of each update remains at about 0.1 and 0.2 s, respectively. This means that the update frequency of MHE method is stable, which ensures the successful update of every range measurement in real-time. However, when the window size is 50, the required running time for each update varies considerably. In fact, this is because that as the size of MHE window becomes bigger, the optimization problem is more complex and it is harder to guarantee an approximately constant time interval to solve it. The computational complexity analysis discussed here verifies that our proposed MHE algorithm, which can perform the update with several Hz and has bounded computational complexity, is capable for real-time and long-term underwater localization. Note that all these tests are implemented in Matlab, which is slow since it is an interpreted language, and no special care is taken to accelerate the solver of optimization. If the algorithm is implemented in a compiled language, such as C++, or some

In this paper, three-dimensional underwater localization for the resource constrained AUVs is addressed with the aid of a single mobile beacon. A novel three-dimensional localization algorithm, which integrates optimization based MHE method with filter based EKF method, is designed. It can incorporate constraints on states and noises, bound the computational complexity by marginalizing out the historical information, and balance the accuracy and required computing time by regularizing the window size. The relative advantages of filter and optimization based methods are rigorously examined, which serves as the comprehensives performance analysis on our proposed localization algorithm. The derivation on the connection between filter and optimization based methods reveals that if the systems have nonlinear dynamics or there are constraints on states or noises, the optimization based method is preferred. In contrast, filter based KF or EKF method is more applicable when the system is linear or the update should be over certain frequencies (hundreds Hz). The observability of single beacon based localization with the nonlinear discrete time system is also analyzed, deriving a sufficient condition. Compared with EKF, the high localization accuracy and effectiveness of the proposed approach are validated by simulation in our customized underwater simulator. Our future work will focus on practical testing on our real AUVs to map the environment in real-time. Acknowledgments This research is financially supported by China Scholarship Council, British Council: Sino-UK Higher Education Research Partnership for Ph.D. Studies, and EU FP7-PEOPLE-2012CIRSES, ECROBOT: European and Chinese Platform for Robotics and Applications. Appendix. Numerical method to solve unconstrained optimization The unconstrained optimization problem formulated as (5) is a NLS problem. It can be solved by iteratively linearizing the nonlinear function f (·) and h(·), gradually converging to the

S. Wang et al. / Robotics and Autonomous Systems (

optimal value. Specifically, in each jth iteration, the objective function Φk should satisfy (j)

(j−1)

Φk (x0:k ) < Φk (x0:k ) (j)

by updating x0:k using (j)

(j−1)

+ ∆x(0j:−k 1) .

x0:k = x0:k

To this end, in each iteration, f (·) and h(·) are linearized as follows: (j)

(j)

xi+1 − f (xi , ui ) ≈



(j−1)

(j−1)

xi+1 + ∆xi+1



  − f (x(i j−1) , ui ) + Fi(j−1) ∆x(i j−1) , i = 0, . . . , k − 1 where (j−1)

Fi

 ∂ f (xi , ui )  ∂ xi xi =x(j−1)

=

i

and (j)

(j−1)



zi − h(xi ) ≈ zi − h(xi

 ) + Hi(j−1) ∆x(i j−1) ,

i = 0, . . . , k

where (j−1)

Hi

=

 ∂ h(xi )  . ∂ xi xi =x(j−1) i

Then, solving (5) in one iteration can be changed to minimize a succession of linear approximations (j−1)

∆x0∗:k

= argmin ∥∆x(0j−1) − a(0j−1) ∥250 (j−1)

∆x0:k

+

k 

∥ci(j−1) − Hi(j−1) ∆x(i j−1) ∥2R

i=0

+

k−1 

∥∆x(i+j−11) − Fi(j−1) ∆x(i j−1) − a(i+j−11) ∥2Qi

(A.1)

i=0

= xˆ 0 − x(0j−1) , ci(j−1) = zi − h(x(i j−1) ), and a(i+j−11) = , ui ) − x(i+j−11) . It is apparent that in each iteration to solve (j−1)

where a0 (j−1)

f (xi (5), the original NLS problem becomes a linear Least Squares one (A.1) after the linearization. References [1] G. Papadopoulos, M.F. Fallon, J.J. Leonard, N.M. Patrikalakis, Cooperative localization of marine vehicles using nonlinear state estimation, in: Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., IEEE, 2010, pp. 4874–4879. [2] S.E. Webster, R. Eustice, H. Singh, L. Whitcomb, Preliminary deep water results in single-beacon one-way-travel-time acoustic navigation for underwater vehicles, in: Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., IEEE, 2009, pp. 2053–2060. [3] M.F. Fallon, G. Papadopoulos, J.J. Leonard, N.M. Patrikalakis, Cooperative AUV navigation using a single maneuvering surface craft, Int. J. Robot. Res. 29 (2010) 1461–1474. [4] S.E. Webster, R.M. Eustice, H. Singh, L.L. Whitcomb, Advances in single-beacon one-way-travel-time acoustic navigation for underwater vehicles, Int. J. Robot. Res. 31 (2012) 935–950. [5] S.B. Lazarus, I. Ashokaraj, A. Tsourdos, R. Zbikowski, P.M. Silson, N. Aouf, B.A. White, Vehicle localization using sensors data fusion via integration of covariance intersection and interval analysis, IEEE Sens. J. 7 (2007) 1302–1314. [6] S. Majumder, S. Scheding, H.F. Durrant-Whyte, Multisensor data fusion for underwater navigation, Robot. Auton. Syst. 35 (2001) 97–108. [7] S.E. Webster, J.M. Walls, L.L. Whitcomb, R.M. Eustice, Decentralized extended information filter for single-beacon cooperative acoustic navigation: theory and experiments, IEEE Trans. Robot. 29 (2013) 957–974. [8] H. Strasdat, J.M. Montiel, A.J. Davison, Visual slam: why filter? Image Vision Comput. 30 (2012) 65–77. [9] G. Sibley, L. Matthies, G. Sukhatme, Sliding window filter with application to planetary landing, J. Field Robot. 27 (2010) 587–608.

)

–

15

[10] A. Simonetto, D. Balzaretti, T. Keviczky, A distributed moving horizon estimator for mobile robot localization problems, in: World Congress, 2011, pp. 8902–8907. [11] G. Pillonetto, A. Aravkin, S. Carpin, The unconstrained and inequality constrained moving horizon approach to robot localization, in: Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., IEEE, 2010, pp. 3830–3835. [12] F. Allgöwer, T.A. Badgwell, J.S. Qin, J.B. Rawlings, S.J. Wright, Nonlinear predictive control and moving horizon estimation—an introductory overview, in: Advances in Control, Springer, 1999, pp. 391–449. [13] D. Viegas, P. Batista, P. Oliveira, C. Silvestre, Linear motion observers for ASC/AUV tandems based on single range readings, in: American Contr. Conf., IEEE, 2011, pp. 3966–3971. [14] D. Viegas, P. Batista, P. Oliveira, C. Silvestre, Position and velocity filters for ASC/I-AUV tandems based on single range measurements, J. Intell. Robot. Syst. (2014) in press. [15] A. Gadre, D. Stilwell, A complete solution to underwater navigation in the presence of unknown currents based on range measurements from a single location, in: Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., IEEE, 2005, pp. 1420–1425. [16] A. Gadre, Observability analysis in navigation systems with an underwater vehicle application (Ph.D. thesis), Virginia Polytechnic Institute and State University, 2007. [17] G.P. Huang, N. Trawny, A.I. Mourikis, S.I. Roumeliotis, Observability-based consistent EKF estimators for multi-robot cooperative localization, Auton. Robots. 30 (2010) 99–122. [18] A. Martinelli, R. Siegwart, Observability analysis for mobile robot localization, in: Proc. IEEE/RSJ Int. Conf. Intell. Robot. Syst., IEEE, 2005, pp. 1471–1476. [19] G. Antonelli, F. Arrichiello, S. Chiaverini, G.S. Sukhatme, Observability analysis of relative localization for AUVs based on ranging and depth measurements, in: Proc. IEEE Int. Conf. Robot. Autom., IEEE, 2010, pp. 4276–4281. [20] E. Hernández, P. Ridao, D. Ribas, J. Batlle, MSISpIC: a probabilistic scan matching algorithm using a mechanical scanned imaging sonar, J. Phys. Agents 3 (2009) 3–12. [21] L. Chen, S. Wang, K. McDonald-Maier, H. Hu, Towards autonomous localization and mapping of AUVs: a survey, Int. J. Intel. Unmanned Syst. 1 (2013) 97–120. [22] D. Ribas, P. Ridao, J. Neira, J.D. Tardos, SLAM using an imaging sonar for partially structured underwater environments, in: Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., IEEE, 2006, pp. 5040–5045. [23] W.M. Bessa, M.S. Dutra, E. Kreuzer, An adaptive fuzzy sliding mode controller for remotely operated underwater vehicles, Robot. Auton. Syst. 58 (2010) 16–26. [24] M. Eichhorn, Optimal routing strategies for autonomous underwater vehicles in time-varying environment, Robot. Auton. Syst. (2014) in press. [25] D.G. Luenberger, Y. Ye, Linear and Nonlinear Programming, Vol. 116, Springer, 2008. [26] C. Rao, J. Rawlings, D. Mayne, Constrained state estimation for nonlinear discrete-time systems: stability and moving horizon approximations, IEEE Trans. Automat. Control 48 (2003) 246–258. [27] B.P. Flannery, S.A. Teukolsky, W.T. Vetterling, Numerical Recipes in C: The Art of Scientific Computing, Cambridge University Press, 1995. [28] F. Albertini, D. D’Alessandro, Observability and forward–backward observability of discrete-time nonlinear systems, Math. Control Signal 15 (2002) 275–290. [29] G. Antonelli, A. Caiti, V. Calabro, S. Chiaverini, Designing behaviors to improve observability for relative localization of AUVs, in: Proc. IEEE Int. Conf. Robot. Autom., IEEE, 2010, pp. 4270–4275. [30] Z. Chen, Local observability and its application to multiple measurement estimation, IEEE Trans. Ind. Electron. 38 (1991) 491–496.

Sen Wang received the B.Eng. degree in Automation from the Guangdong University of Technology, Guangzhou, China in 2009 and the M.Eng. degree in Control Science and Engineering from Harbin Institute of Technology, Harbin, China, in 2011. He is currently pursuing the Ph.D. degree in robotics at the University of Essex, Colchester, UK. His research interests include robot localization, SLAM, multiple sensor fusion.

Ling Chen received the B.Eng. degree in Automation and the M.Eng. degree in Control Science and Engineering from Central South University, Changsha, China, in 2008 and 2010 respectively. He is currently pursuing the Ph.D. degree in robotics at the University of Essex, Colchester, UK. His research interests include Simultaneous Localization and Mapping (SLAM) and underwater SLAM for AUVs.

16

S. Wang et al. / Robotics and Autonomous Systems (

Dongbing Gu received the B.Sc. and M.Sc. degrees in control engineering from the Beijing Institute of Technology, Beijing, China, and the Ph.D. degree in robotics from the University of Essex, Essex, UK. He was an Academic Visiting Scholar with the Department of Engineering Science, University of Oxford, Oxford, UK, from October 1996 to October 1997. In 2000, he joined the University of Essex as a Lecturer. Currently, he is a Professor with the School of Computer Science and Electronic Engineering, University of Essex. His current research interests include multiagent systems, wireless sensor networks, distributed control algorithms, distributed information fusion, cooperative control, reinforcement learning, fuzzy logic and neural network-based motion control, and model predictive control.

)

–

Huosheng Hu received the M.Sc. degree in industrial automation from Central South University, Changsha, China, in 1982 and the Ph.D. degree in robotics from the University of Oxford, Oxford, UK, in 1993. He is currently a Professor with the School of Computer Science and Electronic Engineering, University of Essex, Colchester, UK, leading the Human Centred Robotics Group. His research interests include autonomous robots, human–robot interaction, multi-robot collaboration, embedded systems, pervasive computing, sensor integration, intelligent control, cognitive robotics, and networked robots. He has published more than 370 research articles in journals, books and conference proceedings. He is a Fellow of Institute of Engineering & Technology and Institution of Measurement & Control in the UK, a senior member of IEEE and ACM, and a Chartered Engineer. He is currently an Editor-in-Chief for the International Journal of Automation and Computing, Founding Editor-in-Chief for Robotics Journal and an Executive Editor for International Journal of Mechatronics and Automation.