Anne Cuzol

Etienne Mémin

Monte Carlo fixed-lag smoothing in state-space models

This paper presents an algorithm for Monte Carlo fixed-lag smoothing in state-space models defined by a diffusion process observed through noisy discrete-time measurements. Based on a particles approximation of the filtering and smoothing distributions, the method relies on a simulation technique of conditioned diffusions. The proposed sequential smoother can be applied to general non linear and multidimensional models, like the ones used in environmental applications. The smoothing of a turbulent flow in a high-dimensional context is given as a practical example.

Introduction

The framework of this paper concerns state-space models described by general diffusions of the form:

dx(t) = f (x(t))dt + σ(x(t))dB(t), (1)
which are partially observed through noisy measurements at discrete times. Such models can describe many dynamical phenomena in environmental sciences, physics, but also in finance or engineering applications. The main motivation of this work concerns environmental applications, where non linearity and high-dimensionality arise. Indeed, environmental models and data describe non linear phenomena over large domains, with high spatial resolution. The continuous dynamical model

(1) is defined from a priori physical laws, while observations are supplied by sensors (satellite data for instance) and can appear with very low time frequency. As an example, in the application presented in the last part of this paper, the dimension of the state and observations is of the order of many thousands, and the model is described by the non linear Navier-Stokes equation. Filtering

and smoothing in such state-space models aim at coupling model and observations, which is called data assimilation. The goal of the filtering is to estimate the system state distribution knowing past and present observations. This allows for instance to give proper initial conditions to forecast the future state of a system characterizing atmospheric or oceanographic flows. On the other hand, the smoothing aims at estimating the state distribution using past and future observations, and this retrospective state estimation allows to analyze a spatio-temporal phenomenon over a given time period, for climatology studies for instance. Applications of data assimilation are numerous and the interest is growing in environmental sciences with the increase of available data. However, it is still a challenge to develop filtering and smoothing methods that can be used within a general non linear and high-dimensional context.

Monte Carlo sequential methods, contrary to standard Kalman filters, are able to deal with the filtering problem in non linear state-space models. The particle filtering [START_REF] Del Moral | The Monte Carlo Method for filtering with discrete-time observations[END_REF][START_REF] Doucet | On sequential Monte Carlo sampling methods for Bayesian filtering[END_REF] solves the whole filtering equations through Monte Carlo approximations of the state distribution. On the other hand, ensemble Kalman methods [START_REF] Evensen | The ensemble Kalman filter: theoretical formulation and practical implementation[END_REF] take into account in some way the non linearities in the system, but are based on a Gaussian assumption. For highdimensional systems, ensemble Kalman methods are preferred in practice to particle filters [START_REF] Stroud | An ensemble Kalman filter and smoother for satellite data assimilation[END_REF][START_REF] Van Leeuwen | Particle filtering in Geophysical systems[END_REF] since they reach better performance for limited number of particles.

In order to keep this advantage while alleviating the Gaussian assumption, both methods are combined in [START_REF] Papadakis | Data assimilation with the weighted ensemble Kalman filter[END_REF], leading to a particle filter that can be applied to high-dimensional systems. We will use this technique for the filtering step in the high-dimensional application presented in Section 5.

The aim of this paper is to propose a new smoothing method. Within the particle filter framework, the smoothing can be computed backward, reweighting past particles using present observations [START_REF] Briers | Smoothing algorithms for state-space models[END_REF][START_REF] Godsill | Monte Carlo smoothing for nonlinear time series[END_REF]. There is however one main difficulty for continuous models of type (1). As a matter of fact, it is necessary to know the transition density of the process between observation times, which is not available for general diffusions. This transition density can be approximated through Monte Carlo simulations, as proposed by [START_REF] Durham | Numerical techniques for maximum likelihood estimation of continuous-time diffusion processes[END_REF] to solve inference problems for diffusion processes. However, these approximations are based on Brownian bridge (or modified versions of it) simulations, that do not take into account the drift part of the model. For non linear and high-dimensional models with a drift term that dominates, such approximations will be inefficient. It is also possible to obtain an unbiased estimate of the transition density (see [START_REF] Beskos | Exact and computationally efficient likelihood-based estimation for discretely observed diffusion processes (with discussion)[END_REF]), but this approach is not adapted to a multi-dimensional context. As a matter of fact, the use of this technique in a multivariate setting imposes constraints on the diffusion drift (in particular the drift function has to be of gradient type). In parallel, within the framework of ensemble Kalman methods, [START_REF] Evensen | An ensemble Kalman Smoother for nonlinear dynamics[END_REF] have proposed to estimate backward the smoothing distribution in a recursive way, based on existing filtering trajectories; [START_REF] Stroud | An ensemble Kalman filter and smoother for satellite data assimilation[END_REF] presented and applied an ensemble Kalman smoothing method, relying on a linearization of the system dynamics.

All previously mentioned smoothing methods require to perform specific assumptions or simplifications in order to deal with general non linear models of type (1) in a high-dimensional context.

To the best of our knowledge, it remains a challenging problem to develop smoothing methods that can be used in this general setting. In this paper, we deal with this issue sequentially each time a new observation is available, by smoothing the hidden state from this new observation time up to the previous one. This approach, called fixed-lag smoothing, constitutes then a partial answer to the global smoothing problem that would take into account all available observations. Nevertheless, it is reasonable to assume that the distribution of the hidden state depends on future observations through the next observation only, as soon as the time step between measurements is long (which is typically the case in the environmental applications that motivate this work). Under this assumption, a new observation will impact the distribution of the hidden process up to the previous observation only. This point of view justifies the use of a fixed-lag smoothing in our setting as a reasonable approximation of the global smoothing problem.

Such a fixed-lag smoothing may be directly obtained from the particle filtering result, reweighting past trajectories. However, this smoothing will fail in two cases: when the number of particles is too small compared to the size of the system, or when existing trajectories do not correspond to plausible trajectories of the dynamical model. Unfortunately, these two situations have to be faced when smoothing in a high-dimensional state-space model. Firstly, the number of particles has to be reduced for computational reasons. Secondly, particle filters that have been proposed in this highdimensional context require to correct trajectories towards the observations [START_REF] Papadakis | Data assimilation with the weighted ensemble Kalman filter[END_REF][START_REF] Van Leeuwen | Efficient fully nonlinear data assimilation for geophysical fluid dynamics[END_REF]. This implies that filtering states are consistent at observation times, but that filtering trajectories may not be plausible realizations of the underlying physical model.

In that case, a smoothing based on existing trajectories will fail. Note that these remarks are not only valid for the fixed-lag smoothing, but also for previously mentioned global techniques relying on existing trajectories. In particular, a genealogical smoothing based on ancestral particle lines [START_REF] Del Moral | Genealogical and Interacting Particle Systems with Applications[END_REF] will be deficient in a high-dimensional setting since many trajectories will share only a few ancestral lines.

In contrast, our method does not rely on existing particles only. It is built on a conditional simulation technique of diffusions proposed by [START_REF] Delyon | Simulation of conditioned diffusions and applications to parameter estimation[END_REF] that provides new state trajectories at hidden times between observations. This simulation technique is adapted to a multivariate context where the drift dominates, contrary to techniques based on Brownian bridge sampling [START_REF] Durham | Numerical techniques for maximum likelihood estimation of continuous-time diffusion processes[END_REF]. Moreover, it does not require constraining assumptions for multivariate models, contrary to other techniques based on exact simulation of diffusions [START_REF] Beskos | Exact simulation of diffusions[END_REF][START_REF] Beskos | Exact and computationally efficient likelihood-based estimation for discretely observed diffusion processes (with discussion)[END_REF]. The proposed smoothing method can then be applied to high-dimensional systems.

Finally, it does not require model linearization nor Gaussian hypotheses, and so is able to deal with general non linear models.

The remaining of the paper is organized as follows. Section 2 briefly describes sequential Monte Carlo filtering methods in state-space models, and presents the fixed-lag smoothing problem. Section 3 presents the conditional simulation technique of diffusions of [START_REF] Delyon | Simulation of conditioned diffusions and applications to parameter estimation[END_REF], and details the construction of the proposed Monte Carlo estimate of smoothing distributions. The method is then experimented on a one-dimensional example in Section 4. Finally, the method is applied in section 5 to a practical non linear and high-dimensional case, similar to the problems that have to be faced in environmental applications. A discussion is given in Section 6.

Monte Carlo filtering and smoothing in state-space models

In this section we recall briefly the particle filtering and smoothing methods for models of type (1), where the hidden state vector x ∈ R n is observed through the observation vector y ∈ R m at discrete times {t 1 , t 2 , . . .}:

y(t k ) = g(x(t k )) + γ t k . (2)
The drift function f and observation operator g can be non linear. The dynamical model uncertainty is described by a n-dimensional Brownian motion with covariance Σ = σ(x(t))σ(x(t)) T . The functions f , g and σ are assumed to be known, as well as the law of the observation noise γ t k .

In particular, we present the standard particle filter and the Weighted Ensemble Kalman filter, that can be used to face the filtering problem in high-dimensional systems.

Particle-based filtering methods

Filtering aims at estimating recursively the distribution p(x t1:t k |y t1:t k ) (and in particular its marginal distribution p(x t k |y t1:t k )) at each observation time t k . This filtering problem can be solved with a Monte Carlo sequential approach, called particle filtering [START_REF] Del Moral | The Monte Carlo Method for filtering with discrete-time observations[END_REF][START_REF] Doucet | On sequential Monte Carlo sampling methods for Bayesian filtering[END_REF]. The method relies on a Monte Carlo approximation of the filtering distribution over a set of weighted trajectories {x (i)

t1:t k } i=1:N (called particles): p(x t1:t k |y t1:t k ) = N i=1 w (i) t k δ x (i) t 1 :t k (x t1:t k ), (3)
whose marginal distribution at time t k writes:

p(x t k |y t1:t k ) = N i=1 w (i) t k δ x (i) t k (x t k ). (4)
Particle filters rely on a sequential importance sampling scheme that recursively samples particles, and updates their weights at observation times. The weights corresponds to the ratio between the target distribution and the importance sampling distribution π(x t k |x t0:t k-1 , y t1:t k ). They are recur-sively computed as follows:

w (i) t k ∝ w (i) t k-1 p(y t k |x (i) t k )p(x (i) t k |x (i) t k-1 ) π(x (i) t k |x (i) t0:t k-1 , y t1:t k )
.

(5)

In practice, a resampling procedure is added in order to avoid degeneracy. This procedure duplicates trajectories with large weights and remove small weighted trajectories.

Standard particle filter

When the proposal distribution π is set to the prior (i.e. π(

x t k |x t0:t k-1 , y t1:t k ) = p(x t k |x t k-1
), the weights updating rule (5) simplifies to the computation of the data likelihood p(y t k |x (i) t k ). This particular instance of the particle filter is called the Bootstrap filter or sequential importance resampling (SIR) filter [START_REF] Gordon | Novel approach to non-linear/non-Gaussian Bayesian state estimation[END_REF]. Due to its simplicity it is the most commonly used particle filter. It is however a very inefficient distribution for high dimensional space as it does not take into account the current observation and depends only weakly on the past data through the filtering distribution estimated at the previous instant. This distribution requires a great number of particles to explore meaningful areas of the state space.

Weighted Ensemble Kalman filter (WEnKF)

One way to efficiently incorporate observation within the proposal distribution consists to rely on the ensemble Kalman filtering mechanism to define this distribution. This is the idea proposed in the WEnKF technique [START_REF] Papadakis | Data assimilation with the weighted ensemble Kalman filter[END_REF]. In the WEnKF approach the importance sampling is taken as a Gaussian approximation of p(x t k |x t k-1 , y t k ). This approach is close to the technique proposed in [START_REF] Van Leeuwen | Nonlinear data assimilation in geosciences: an extremely efficient particle filter[END_REF]. A variation of a similar technique based on a deterministic squareroot formulation is also described in [START_REF] Beyou | Weighted Ensemble Transform Kalman Filter for Image Assimilation[END_REF]. In order to make the estimation of the filtering distribution exact (up to the sampling), each member of the ensemble must be weighted at each observation instant t k with appropriate weights w (i) t k , defined from (5). Therefore, the Weighted ensemble Kalman filter (WEnKF) procedure can be simply summarized by Algorithm 1.

Algorithm 1 The WEnKF algorithm

For each t k = t 1 , t 2 , . . .:

• Start from particles set {x (i) t k-1 , i = 1, . . . , N } and observation y t k • Obtain particles set {x (i) t k , i = 1, . . . , N } from: -EnKF step: Get x (i)
t k , i = 1, . . . , N , from the assimilation of y t k with an EnKF procedure;

-Computation of weights:

w (i) t k ∝ w (i) t k-1 p(yt k |x (i) t k )p(x (i) t k |x (i) t k-1 ) p(x (i) t k |x (i) t k-1 ,y (i) t k ) ; -Resampling: For j = 1, . . . , N , sample with replacement index I(j) from discrete prob- ability {w (i) t k , i = 1, . . . , N } over {1, . . . , N } and set x (j) t k = x I(j) t k . Set w (i) t k = 1 N ∀i = 1, . . . , N .
Note that particle-based filtering techniques update the filtering distribution at observation times only. However, after the estimate p(x t k |y t1:t k ) has been updated at observation time t k , the filtering distribution can be predicted in order to have a continuous estimation of p(x t |y t1:t k ) for all t ∈ ]t k , t k+1 [ until the next observation time:

p(x t |y t1:t k ) = N i=1 w (i) t k δ x (i) t (x t ), (6)
where, for all i = 1, . . . , N , the state x

(i)
t is sampled from (1), starting from x

(i) t k .

Fixed-lag smoothing problem

Contrary to the filtering approach that uses past and present observations, the smoothing in statespace models aims at estimating p(x t |y t1:tend ) for all t ∈ [t 1 , t end ], using all past and future observations over a given time period. As raised in the introduction, existing smoothing methods do not apply directly to a general non linear model of type (1) in a high-dimensional context, since assumptions have to be made that may not be realistic. Instead of solving the global smoothing, we will concentrate in the rest of the paper on a fixed-lag smoothing, which constitutes a partial answer to the global smoothing problem.

The objective of the fixed-lag smoothing will be to replace the predictive distribution ( 6) by its smoothed version p(x t |y t1:t k+1 ) ∀t ∈]t k , t k+1 ], sequentially each time a new observation y t k+1 arrives. This will allow to reduce the temporal discontinuities inherent to the filtering technique, that successively predicts the distribution of the state between observations, and updates this distribution at observation times.

To achieve this, by construction of the particle filter that weights entire trajectories (see equation

(3)), it is known (see for instance [START_REF] Doucet | On sequential Monte Carlo sampling methods for Bayesian filtering[END_REF]) that the fixed-lag smoothing distribution p(x t |y t1:t k+1 ) can be directly obtained from the marginal at time t of p(x t1:t k+1 |y t1:t k+1 ). The empirical smoothing distribution is then given by:

p(x t |y t1:t k+1 ) = N i=1 w (i) t k+1 δ x (i) t (x t ) ∀t ∈]t k , t k+1 ]. (7)
However, this approximation is simply a reweighting of past existing particle trajectories, and relies on the support of the filtering distribution at time t k . If the number of particles is too small with respect to the state dimension, the support may be greatly reduced by the correction step (assigning small weights to all particles except a few), leading in practice to a bad estimation of p(x t |y t1:t k+1 ).

Moreover, if particle trajectories have been forced towards observations during the filtering step (like in the WEnKF procedure), a smoothing based on those particles will fail because it will not be able to correct discontinuities. Consequently, since we are interested in smoothing techniques that are efficient in a high-dimensional context, this direct smoothing technique can not be used in its basic form and has to be improved.

In the following, we propose to use a conditional simulation technique of diffusions that will enable the sampling of new smoothed trajectories between times t k and t k+1 . The approximation of the smoothing distribution (7) at each hidden time will then be improved. The conditional simulation technique is presented in the next section, before the resulting smoothing procedure we propose.

Fixed-lag smoothing with conditional simulation

The smoothing method we propose is based on a conditional simulation technique that is presented in section 3.1. We develop then in section 3.2 how this technique can be used to improve the estimation of the smoothing distribution (7).

Conditional simulation

Conditional simulation of a diffusion aims at sampling trajectories from a given process:

dx(t) = f (x(t))dt + σ(x(t))dB(t) (8)
between two times t = 0 and t = T , with the constraints x(0) = u and x(T ) = v. This simulation problem is treated by [START_REF] Delyon | Simulation of conditioned diffusions and applications to parameter estimation[END_REF], where the authors show how to obtain the law of the constrained process from a Girsanov theorem. In practice, the proposed algorithms consist in simulating trajectories according to another diffusion process, which is built to respect the constraints and is easy to simulate from. The conditional distribution of the constrained process ( 8) is absolutely continuous with respect to the distribution of the auxiliary process, with explicitly given density. For instance, in the case where the drift is bounded (a similar algorithm is proposed in [START_REF] Delyon | Simulation of conditioned diffusions and applications to parameter estimation[END_REF] for the unbounded case) and for σ invertible, the algorithm is based on the simulation of trajectories from the following process:

dx(t) = f (x(t)) - x(t) -v T -t dt + σ(x(t))dB(t), (9)
with initial condition x(0) = u. Note that Lemma 4 in [START_REF] Delyon | Simulation of conditioned diffusions and applications to parameter estimation[END_REF] deals with the existence of a unique solution for this equation. This process is a simple modification of ( 8), where a deterministic part is added to the drift. It is then easy to simulate unconditional trajectories from this process, and all simulated trajectories will satisfy x(T ) = v by construction. For simplicity we will assume in the following that σ is independent of x(t) (note however that this is not an assumption in [START_REF] Delyon | Simulation of conditioned diffusions and applications to parameter estimation[END_REF]). The law of the conditioned process is given by:

E[h(x)|x(0) = u, x(T ) = v] = E [h(x)α(x)] , (10)
for all measurable function h, where:

α(x) = exp   - T 0 (x(t) -v) T Σ -1 f (x(t)) T -t dt   (11)
is the density coming from Girsanov theorem (see [START_REF] Delyon | Simulation of conditioned diffusions and applications to parameter estimation[END_REF]), with Σ = σ(x(t))σ(x(t)) T .

Let us note that the presence of the drift part of model ( 8) in the auxiliary process ( 9) is crucial to make the simulation efficient. The same process had initially been proposed by [START_REF] Clark | The simulation of pinned diffusions[END_REF] to solve the conditional simulation problem. On the other hand, standard Brownian bridges that could be used as auxiliary processes [START_REF] Durham | Numerical techniques for maximum likelihood estimation of continuous-time diffusion processes[END_REF] lead in practice to poor approximations of the original constrained diffusion in our high-dimensional setting, since Brownian bridge trajectories are too far away from trajectories of (8).

In the following, the conditional marginal of interest p(x t |x(0) = u, x(T ) = v) will then be approximated as follows:

p(x t |x(0) = u, x(T ) = v) = M j=1 α(x (j) )δ x(j) t (x t ) ∀t ∈ [0, T ], (12)
where the M trajectories {x (j) t } j=1:M are simulated from (9) with x(j) 0 = u for all j = 1, . . . , M .

Proposed fixed-lag smoothing method

We show in the following how the conditional simulation technique can be used to improve the estimation of the local smoothing distribution p(x t |y t1:t k+1 ) for all t ∈]t k , t k+1 ].

We first note that this distribution can be decomposed as:

t k+1 δ (x (i) t k+1 ,x (i) t k ) (x t k+1 , x t k ), (15)
where the w (i) t k+1 are the particle filter importance weights. Plugging ( 15) into ( 14) leads then to the following approximation for the fixed-lag smoothing distribution:

p(x t |y t1:t k+1 ) = N i=1 w (i) t k+1 p(x t |x (i) t k , x (i) t k+1 ). ( 16
)
The conditional distribution p(x t |x (i)

t k , x (i)
t k+1 ) can be estimated using ( 12) for each pair of initial and end points x (i)

t k and x (i) t k+1 : p(x t |x (i) t k , x (i) t k+1 ) = M j=1 α(x (i)(j) )δ x(i)(j) t (x t ), (17)
where each x(i)(j)

t is sampled from (9) with initial constraint x(i)(j)

t k = x (i)
t k and final constraint x (i) t k+1 . The estimation of the smoothing distribution of interest writes finally:

p(x t |y t1:t k+1 ) = N i=1 w (i) t k+1 M j=1 α(x (i)(j) )δ x(i)(j) t (x t ), ∀t ∈]t k , t k+1 ]. (18)
The algorithm we propose to compute the fixed-lag smoothing distribution on a given time interval ]t k , t k+1 ] is therefore the following:

Algorithm 2 Fixed-lag conditional smoothing For each t k = t 1 , t 2 , . . .:

-Store {x (i) t k } i=1:N and compute {x (i)
t k+1 } i=1:N and associated weights {w

(i)
t k+1 } i=1:N from a particle filter algorithm; -For each pair {x 9) with an Euler scheme, with the constraints x(i)(j)

(i) t k , x (i) t k+1 }, i = 1, . . . , N : -Simulate M conditional trajectories {x (i)(j) t } j=1:M for t ∈ [t k , t k+1 ] from (
t k = x (i) t k and x(i)(j) t k+1 = x (i) t k+1 ,
-Compute weights α(x (i)(j) ) from ( 11) for all j = 1, . . . , M , with final constraint x (i)

t k+1 ; -Compute p(x t |y t1:t k+1 ) = N i=1 w (i) t k+1 M j=1 α(x (i)(j) )δ x(i)(j) (x t ) for all t ∈]t k , t k+1 ].

One-dimensional simulation study

In this section, the smoothing method is first experimented on a one-dimensional state space model.

Since the proposed approach relies on a preliminary particle filtering step, filtering results are first presented in Section 4.2 (either considering a standard particle filter or the WEnKF). The results obtained with the standard fixed-lag smoothing method are then shown in Section 4.3. Finally, Section 4.4 presents the smoothing results obtained with the proposed technique.

State space model

The one-dimensional state space model of interest is a sine diffusion, partially observed with noise (used as an illustration by [START_REF] Fearnhead | Particle filters for partially observed diffusions[END_REF] for a particle filtering method) :

dx(t) = sin(x(t))dt + σ x dB(t), (19)
y t k = x t k + γ t k , (20)
where σ 2 x = 0.5 and γ t k ∼ N (0, σ y ) with σ 2 y = 0.01. One trajectory of the process is first simulated from ( 19) with an Euler-type discretization scheme of time step ∆t = 0.005. This trajectory will constitute the hidden process, observed through y t k generated according to (20) at every time step

t k , with t k -t k-1 = 20∆t.

Particle filtering results

The filtering results are presented for the standard particle filter (denoted PF in the following) and the Weighted Ensemble Kalman filter (WEnKF). Two situations are shown, with reduced (N = 20)

and high number (N = 10000) of particles. The case with a high number of particles is shown as the reference for comparison purpose, note however that this ideal situation is not reachable in a high-dimensional context, since the number of particles has to be reduced for evident computational cost reasons.

The results for the two configurations are presented on Figure 2, where the dotted lines represents the filtering mean estimates. The filtering distribution p(x t k |y t1:t k ) is estimated at each observation time t k using (4), and predicted between observation times from (6). The mean is then estimated from weighted particles as

N i=1 w (i) t k x (i) t , for all t ∈ [t k , t k+1 [. Figure 2 (a)-(b)
show that the standard particle filter results diverge from the reference solution between observation times, for low or high number of particles. As a matter of fact, when no observation is available, the state distribution is predicted from the dynamics only, so that particles trajectories are not guided towards the next observation. At observation times t k , high weights are given to particles that are close to the observation, so that the estimated mean suddenly gets closer to the solution. These discontinuities between measurement times can also be observed on the WEnKF results (Figure 2 (c)-(d)), because particle trajectories are brutally corrected with the EnKF step at observation times. A smoothing will aim at reducing these temporal discontinuities while providing dynamically consistent solutions.

Standard fixed-lag smoothing results

From the particle filtering results, we present now the results obtained with the direct particles smoothing procedure described in section 2.2. This procedure relies on existing trajectories. The smoothing distribution p(x t |y t1:t k+1 ) is computed backward for all t ∈]t k , t k+1 ] using expression (7), each time a new observation y t k+1 becomes available. The smoothing mean is computed as

N i=1 w (i) t k+1 x (i)
t for all t ∈]t k , t k+1 ], and the standard deviation is computed in the same way from the weighted particles.

It can be observed on Figure 3(a) that the smoothing based on the standard particle filter is not efficient when the number of particles N is small: Only a few particles are close to the observation at time t k and have nonzero weights, implying that the smoothing distribution is poorly estimated (see for instance between observation times t = 100 and t = 120 where the smoothing distribution is artificially peaked but far from the hidden trajectory). The smoothing result obtained from the reference configuration N = 10000 is plotted on Figure 3(b). In that situation, since many trajectories have high weights at observation times, the estimation of backward smoothing distributions is improved and includes the hidden trajectory.

Moreover, Figure 3(c)-(d) shows that the standard smoothing based on the WEnKF result fails for low or high number of particles. As a matter of fact, particle trajectories are artificially corrected by the EnKF step at each observation time. Resulting trajectories are highly non-plausible. Even for a huge number of particles, a smoothing based on those existing trajectories is not able to reduce the induced time discontinuities.

Proposed smoothing results

In this section, we show how the proposed method can improve the estimation of backward smoothing distributions when it is not adequate to rely on existing trajectories only. This is the case if the number of particles is too small, as demonstrated from the experiment presented on Figure 3 Our smoothing is first applied using the filtering output of the standard particle filter with N = 20 particles. Figure 4(a) shows the result obtained with a sampling of M = 50 conditional trajectories between each pair {x (i)

t k , x (i)
t k+1 }, i = 1, . . . , N . The smoothing distribution p(x t |y t1:t k+1 ) is computed from ( 18), so the smoothing mean is computed as

N i=1 w (i) t k M j=1 α(x (i)(j) )x (i)(j) t for all t ∈]t k , t k+1 ],
and similarly for the standard deviation. This result highlights the fact that since the proposed method creates new trajectories, it is able to correct the deficiencies of the standard smoothing approach presented on Figure 3(a) when the initial number of filtering particles is too small. On Figure 4(b), the same experiment is presented using M = 500 conditional trajectories. In that case, the result is very similar to the reference particles smoothing result presented on Figure 3(b), obtained from a particle filter with N = 10000.

In parallel, the proposed smoothing has been tested using the output of the WEnKF filtering technique with N = 20 particles. Again, the smoothing is computed with M = 50 and M = 500 conditional trajectories, and the corresponding results are presented on Figure 3(c)-(d). Instead on relying on existing WEnKF trajectories that may not be plausible trajectories of the model (because of the EnKF correction step), the proposed method samples new trajectories between observation times. This leads to a good estimation of the smoothing distributions, contrary to the standard smoothing presented of Figure 3(c). Note that the smoothing results are very similar to the result obtained from the standard particle filter (Figure 3

Application to a high-dimensional assimilation problem

This section aims at illustrating the applicability of our method to a high-dimensional and non linear scenario, without extensive study at this stage. The method is applied to a turbulence assimilation problem, where the model of interest is of type (1). The goal is to recover temporal estimates of velocity/vorticity over a given spatial domain of size n = 64 * 64, from a sequence of noisy observations and a continuous a priori dynamical model based on a stochastic version of Navier-Stokes equation. Within an environmental framework, a direct application would be the estimation of wind fields or sea surface currents from satellite data.

State space model

Let ξ(x) denote the scalar vorticity at point x = (x, y) T , associated to the 2D velocity w(x) = (w x (x), w y (x)) T through ξ(x) = ∂wy ∂x -∂wx ∂y . Let ξ ∈ R n be the state vector describing the vorticity over a n = 64 * 64 square domain, and w ∈ R 2n the associated velocity field over the domain. We will focus on incompressible flows such that the divergence of the velocity field is null. A stochastic version of Navier-Stokes equation in its velocity-vorticity form can then be written as:

dξ t = -∇ξ t • w t dt + 1 Re ∆ξ t dt + σdB t , (21)
where ℜ denotes the flow Reynolds number (Re = 3000). The uncertainty is modeled by a Brownian motion of size n, with covariance Σ = σσ T , where σ ∈ R n . A velocity field example, generated from the model ( 21), is shown on Figure 5(a), together with the corresponding vorticity map (b).

We assume the hidden vorticity vector ξ is observed through noisy measurements y t k at discrete times t k , where t kt k-1 = 100∆t, and ∆t = 0.1 is the time step used to discretize (21). In our experimental setup, measurements correspond to PIV (Particle Image Velocimetry) image sequences used in fluid mechanics applications. Note however that other kind of data can be used similarly within this state space model, like meteorological or oceanographic data for instance. The state and observation are related in our case through y t k = g(ξ t k ) + γ t k , where g is a non linear function linking the vorticity to the image data, and γ t k is a Gaussian noise, uncorrelated in time.

Implementation details

We recall that the smoothing relies first on a particle filter step. Due to the high dimensionality of the state vector, the use of a standard particle filter is not adapted to solve the filtering problem, as discussed by [START_REF] Snyder | Obstacles to high-dimensional particle filtering[END_REF] or [START_REF] Van Leeuwen | Particle filtering in Geophysical systems[END_REF]. We make then use of the method presented by [START_REF] Papadakis | Data assimilation with the weighted ensemble Kalman filter[END_REF] which combines the benefits of the ensemble Kalman filter, known to perform well in practice for high dimensional systems [START_REF] Stroud | An ensemble Kalman filter and smoother for satellite data assimilation[END_REF], and the particle filter (which solves theoretically the true filtering problem, without approximating the filtering distributions with Gaussian distributions). Since the method of Papadakis et al. ( 2010) is intrinsically a particle filter, it leads then at each observation time t k to a set of particles and weights {ξ (i) t1:t k , w t k } i=1:N , as required by the algorithm proposed in section 3.

The particle filter step requires simulations from the dynamical model ( 21), and the conditional simulation step requires to sample trajectories from its constrained version, which consists in a similar problem with modified drift (see process ( 9)). The model is discretized in time with time step ∆t = 0.1; more information about the discretization scheme may be obtained in [START_REF] Papadakis | Data assimilation with the weighted ensemble Kalman filter[END_REF]. The random perturbations are assumed to be realizations of Gaussian random fields that are correlated in space with Gaussian covariance function Σ(x i , x j ) = η exp(-

||xi-xj || 2 λ
), where η = 0.01 and λ = 13. In practice, the simulation of these perturbations is performed in Fourier space, with the method described in [START_REF] Evensen | The ensemble Kalman filter: theoretical formulation and practical implementation[END_REF].

Finally, the estimation of the smoothing distributions require the computation of conditional trajectories weights, corresponding to Girsanov weights given by ( 11). After a Riemann sum approximation of the integral, the computation of weights requires the inversion of the matrix Σ of size (n, n), where n = 64 * 64 is the number of grid points. We choose to compute Σ -1 empirically using a singular value decomposition computed from the M realizations of the perturbation fields used for the constrained trajectories simulations. Let Z be the matrix of size (n, M ) containing the M centered fields of size n = 64 * 64, the SVD leads to Z = UDV T , so that ZZ T = UDD T U T .

The inverse of the covariance matrix Σ -1 is finally computed as:

M (ZZ T ) -1 = M U(DD T ) -1 U T , (22)
which only requires the inversion of a diagonal. Note that more efficient procedures could be implemented in our case (homogeneous Gaussian covariance) since the covariance function is separable in

x and y directions. This means that the covariance matrix Σ can be written as the Kronecker product of smaller matrices and more easily inverted [START_REF] Sun | Geostatistics for large datasets[END_REF]. However, the SVD inversion can be applied to any covariance structure, in particular it could deal with a non homogeneous covariance matrix.

Results

In this section, we illustrate the capability of the proposed method to reduce the temporal discontinuities inherently introduced by the filtering in continuous-discrete state-space models.

The smoothing result relies on the output of the WEnKF filtering step, computed with N = 500 particles. Compared to the size of the system, the number of particles is theoretically too small for the filter to be truly efficient. In practice, many filtering trajectories have close to zero weights at observation times. Histograms of filtering weights are given as illustration on Figure 6(a)-(b) at two times t = 400 and t = 500. Note however that the filter is not degenerate and is able to provide results that get close to he hidden vorticity at measurement times. This can be observed on Figure 7, where the mean square error is plotted with full line, averaged at each time over the image domain of size n = 64 * 64. Since the ground truth vorticity sequence is known in our experimental setup, the mean square error is computed between the hidden vorticity and the estimated filtering mean, given by

N i=1 w (i) t k ξ (i)
t for all t ∈ [t k , t k+1 [. The correction steps lead to successive error decreases at observation times.

The proposed smoothing method has been applied with M = 200. Note that we take benefit from the fact that many filtering have close to zero weights. Indeed, the smoothing method relies in practice on a reduced number Ñ M of sampled conditional trajectories (with Ñ << N ), which makes the problem computationally tractable. On this experiment, we have retained around 5% of initial filtering trajectories. The smoothing distribution p(ξ t |y t1:t k+1 ) is computed for all t ∈ ]t k , t k+1 ] from (18), and its mean is computed as

N i=1 w (i) t k+1 M j=1 α( ξ(i)(j) ) ξ(i)(j) t .

Histograms

of conditional simulation weights α( ξ(i)(j) ) are given as illustration on Figure 6(c)-(d) for a given particle (i) at two times t = 400 and t = 500.

The mean square error is computed between the true vorticity and the estimated smoothing mean, and plotted on Figure 7 with dotted line. As expected, the smoothing method reduces the error at hidden times between observations. In addition, we present below a qualitative evaluation of the smoothing result for the same exper-425 iment, over a specific time interval.

The WEnKF result is first presented on Figure 8 for the time interval [400,500] t for t = 500. The temporal discontinuity between estimations can be observed when reaching observation time t = 500: the vorticity map is suddenly modified in order to fit to the observations, introducing inconsistencies in the vorticity temporal trajectories. Note that the application of the standard particles smoothing (described in section 2.2) will fail here, and not only because the number of particles is too small. As a matter of fact, we recall that the filtering trajectories have been computed from the method presented in [START_REF] Papadakis | Data assimilation with the weighted ensemble Kalman filter[END_REF], which uses the ensemble Kalman filter step as importance distribution in the particle filter algorithm. The ensemble Kalman filter consists of a prediction step from the dynamical model ( 21), and a correction step which shifts particles towards the observation. Because of this correction step, the sampled filtering trajectories between two observation times do not correspond to trajectories of the dynamical model. This implies that from such a particle filter, the standard smoothing based on existing trajectories will not be able to reduce the temporal discontinuities observed on Figure 8. This can be observed on Figure 9, where smoothed vorticity maps are computed as

Conclusion and discussion

In this paper we introduced a smoothing algorithm based on a conditional simulation technique of diffusions. The proposed smoothing is formulated as fixed-lag, in the sense that it is performed sequentially each time a new observation appears, in order to correct the state at hidden times up to the previous observation. Note that a decomposition similar to equations ( 13) to (18) can be written from an integration up to a previous time t k-h , with h > 1. This implies that the smoother can be formulated with a larger fixed-lag, in order to correct the state backward not only up to the previous observation, but up to further measurement times. Yet, due to the successive resampling steps that have been performed in the filtering steps before time t k , there are in practice only a few distinct filtering trajectories at times t k-h if h is large. Consequently, the estimation of the joint law in (15)

will not be reliable anymore for a too large value of h.

We have shown the practical applicability of the method to a high-dimensional problem. Nevertheless, the algorithm remains costly since a second Monte Carlo step is added to the Monte Carlo nature of particle filter algorithms. Yet, from an algorithmic point of view, the sequential nature of the proposed technique allows the smoothing to be implemented with a similar structure as filtering methods (sequential sampling and weighting of model trajectories). It is then easy to couple this smoothing to an operational filtering system and benefit from parallelization strategies for instance.

Since the proposed smoothing uses the filtering result as input, it relies on the success of the

Fig. 1 :

 1 Fig. 1: Simulated sine diffusion trajectory x(t) and partial observations y(t k ) (dots) with t kt k-1 = 20∆t.

Fig. 2 :

 2 Fig. 2: Standard PF and WEnKF results. Thick line: hidden diffusion; Dots: partial observations; Dotted line: estimated filtering mean.

Fig. 3 :

 3 Fig. 3: Standard smoothing from PF and WEnKF results. Thick line: hidden diffusion; Dots: partial observations; Dotted line: estimated filtering mean.

  (a), or if the existing trajectories do not correspond to plausible trajectories of the model (as shown for the WEnKF result on Figure 3(c)-(d)).

  (a)-(b)) because both filters have similar behaviour at observation times.

Fig. 4 :

 4 Fig. 4: Proposed conditional smoothing result. Thick line: hidden diffusion; Dots: partial observations; Dotted line: estimated backward smoothing mean; Thin line: estimated standard deviation.

Fig. 5 :

 5 Fig. 5: State example. (a) Velocity field wt; (b) Associated vorticity map ξ t .

FigFig. 7 :

 7 Fig. 6: Filtering and conditional simulation weights

Fig. 8 :Fig. 9 :

 89 Fig. 8: Filtering result with the method of Papadakis et al. (2010). Estimated mean vorticity maps for different times t between observation times t = 400 and t = 500.

Fig. 10 :

 10 Fig. 10: Smoothing result with the proposed method. Estimated mean vorticity maps for different times t between observation times t = 400 and t = 500.

  p(x t |y t1:t k+1 ) = p(x t , x t k , x t k+1 |y t1:t k+1 )dx t k dx t k+1 = p(x t k , x t k+1 |y t1:t k+1 )p(x t |x t k , x t k+1 , y t1:t k+1 )dx t k dx t k+1 . (13) Then, from the state-space model properties, we obtain: p(x t |y t1:t k+1 ) = p(x t k , x t k+1 |y t1:t k+1 )p(x t |x t k , x t k+1 )dx t k dx t k+1 . (14) Moreover, from the particle filter Monte Carlo approximation described by (3), the joint law p(x t k , x t k+1 |y t1:t k+1 ) can be replaced by: p(x t k , x t k+1 |y t1:t k+1 ) =

	N
	w	(i)
	i=1

underlying particle filter. For high-dimensional systems, a standard particle filter is not adapted and it is necessary to use filtering techniques that guide particles towards observations. In this paper, we use the WEnKF algorithm. In practice, any efficient particle filtering technique with such a guiding can be used within our framework. Note however that the construction of such techniques remains an open area of research.

480

We plan to work on the application of the smoothing method to a real high-dimensional case (for the estimation of sea surface currents from satellite image sequences). However, such a work will imply numerous difficulties which are not related to the smoothing technique but to the definition of the state-space model: definition of a suitable physical model, good characterization of state noise structure and model parameters. Therefore, this will be part of a future work.


Minh-Duc Hua
email: hua@isir.upmc.fr

Guillaume Allibert

Szymon Krupínski
email: szymon.krupinski@cybernetix.fr

Tarek Hamel
email: thamel@i3s.unice.fr

Homography-based Visual Servoing for Autonomous Underwater Vehicles

A nonlinear visual servoing approach is proposed for the stabilisation of fullyactuated autonomous underwater vehicles (AUVs) by exploiting the homography matrix between the two images of a planar scene. In a cascade manner, an outer-loop control defines a reference setpoint based on the homography matrix, and an inner-loop control ensures the stabilisation of the setpoint by assigning thrust and torque controls. In contrast with conventional kinematic solution, the proposed controller deals with the high nonlinearity and coupling of the system dynamics and ensures almost-global asymptotical stability. In addition, the interactions of the AUV with the surrounding fluid (e.g., added mass and drag effects) are often difficult to model precisely whereas they may significantly perturb its motion. The proposed controller -augmented with an effective integral action-allows for the compensation of model uncertainties and for robust performance against such perturbations. Simulation results illustrating these properties on a realistic AUV model subject to sea current are reported.

INTRODUCTION

The object of the present paper is a class of fullyactuated underwater vehicles whose thrust and torque controls can be assigned independently. While this configuration is typical for remotely operated vehicles (ROVs), it also exists in some autonomous underwater vehicles (AUVs).Underwater environment gives rise to several control difficulties. For instance, the dynamics of an underwater vehicle is highly nonlinear and characterized by a strong coupling between translational and rotational dynamics, mainly due to added mass effects [START_REF] Fossen | Marine Control Systems[END_REF]. In addition, the key issue related to the control of AUVs is the lack of lightweight and reliable position sensors. A vision system, being lightweight, passive and adaptable, can be used, at least partially, to respond to this challenge. By using a camera as the primary sensor for relative position, the control problem can be cast into Image-Based Visual Servo (IBVS) control problem [START_REF] Chaumette | Visual servo control, Part I: Basic approaches[END_REF], which opens the possibility to perform autonomous tasks in low-structured environments with no external assistance. Classical visual servo control was developed for serial-link robotic manipulators [START_REF] Chaumette | Visual servo control, Part I: Basic approaches[END_REF]] and more recently for aerial vehicles [START_REF] Guenard | A practical visual servo control for an unmanned aerial vehicle[END_REF], [START_REF] Gonçalves | Visionbased Autonomous Approach and Landing for an Aircraft using a Direct Visual Tracking Method[END_REF]. In the underwater robotics field few attempts have been made to use vision sensors to perform tasks related to man-made structures such as pipeline following [Rives andBorrelly, 1997, Krupinski et al., 2012] using linear features, or station keeping [START_REF] Lots | A 2-D visual servoing for underwater vehicle station keeping[END_REF] using point ⋆ This work was supported by the French Centre National de la Recherche Scientifique (CNRS) within the PEPS CONGRE project.

features. When the visual target is planar, an IBVS control scheme is proposed in [START_REF] Benhimane | Homography-based 2D Visual Tracking and Servoing[END_REF] using the homography matrix that encodes transformation information between two images of the same planar target and that can be directly retrieved from the corresponding images. This homography-based visual servoing (HBVS) scheme is a purely kinematic control, initially designed for fully-actuated manipulators. However, its stability and convergence properties are only local and are not provable when the system's dynamics is taken into account. The HBVS problem has also been investigated for underactuated aerial vehicles [START_REF] Metni | Visual Tracking Control of Aerial Robotic Systems with Adaptive Depth Estimation[END_REF], [START_REF] De Plinval | Visual servoing for underactuated VTOL UAVs: A linear, Homography-Based framework[END_REF]. In [START_REF] Metni | Visual Tracking Control of Aerial Robotic Systems with Adaptive Depth Estimation[END_REF], additional information such as the orientation measurement of the camera with respect to the target is assumed to be available. In contrast, the HBVS solution proposed in [START_REF] De Plinval | Visual servoing for underactuated VTOL UAVs: A linear, Homography-Based framework[END_REF] only makes use of the homography matrix along with gyroscope's measurements. However, only local (exponential) stability is proved based on Lyapunov-like analysis. In fact, the consideration of the vehicle's dynamics within the control design is crucial to obtain provable (strong) stability. These works also show that significant efforts are required in order to eliminate the assumptions concerning the precise knowledge of environment geometry. The present study is an alignment with these efforts. Although the AUV system under consideration is fully-actuated, the strongly coupled translational and rotational dynamics represents another challenge. Moreover, in contrast with existing HBVS solutions, an important original outcome of the proposed control approach is related to the obtention of almost-global asymptotical stability by means of continuous feedback control.

The paper is organized as follows. Notation, system modeling and HBVS problem are described in Section 2. Control design is presented in Section 3. In Subsection 3.1, an inner-loop control is proposed for the stabilisation of reference velocity setpoint, based on some modifications of the prior work [START_REF] Krupinski | Pipeline tracking for fully-actuated autonomous underwater vehicle using visual servo control[END_REF]. In Subsection 3.2, we present the main result of the paper concerning the design of an outer-loop control directly based on the homography matrix, with the objective of assigning a reference velocities setpoint for the inner-loop control. Section 4 reports simulation results illustrating the performance of the control approach on a realistic AUV model. Finally, conclusions are given in Section 5.

PRELIMINARY MATERIAL

Notation

The following notation is used (see Fig. • {e 1 , e 2 , e 3 } denotes the canonical basis of R 3 . I 3 is the identity matrix of R 3×3 . For all u ∈ R 3 , the notation u × denotes the skew-symmetric matrix associated with the cross product by u, i.e., u

× v = u × v, ∀v ∈ R 3 . The Euclidean norm in R n is denoted as | • | and (•) ⊤ denotes the transpose operator. • sat δ (•) ∈ R n , with δ > 0, is the classical saturation function defined as sat δ (x) x min (1, δ/|x|) , ∀x ∈ R n . • satc ∆ (•) ∈ R 3 , with some positive diagonal matrix ∆ = diag([δ 1 , δ 2 , δ 3 ]), is a saturation function defined as satc ∆ (x) [ sat δ1 (x 1 ) sat δ2 (x 2 ) sat δ3 (x 3 ) ] ⊤ , ∀x ∈ R 3 .

System Modeling

Without loss of generality, let us assume that G lies on the -→ e b 3 -axis and under the center of buoyancy B such that r G = le 3 , i.e., bottom-heavy vehicle.

Define W [V ⊤ Ω ⊤ ] ⊤ ∈ R 6 . When characterized at the center of buoyancy B, the kinetic energy of the vehicle is given by (see [START_REF] Leonard | Stability of a bottom-heavy underwater vehicle[END_REF]):

E B = 1 2 W ⊤ M B W, with M B [ mI 3 -mr G× mr G× J 0 ] .
According to Kirchhoff and Lamb theory [START_REF] Lamb | Hydrodynamics[END_REF], the kinetic energy of the liquid surrounding the vehicle is given by:

E F = 1 2 W ⊤ M A W, with M A [ M 11 A M 12 A M 21 A M 22 A ] ,
where M A ∈ R 6×6 is known as the added mass matrix, which is constant and symmetric. The total kinetic energy of the body-fluid system is

E T = E B +E F = W ⊤ M T W,
where the positive-definite matrix M T is given by:

M T = M B + M A = [ M D ⊤ D J
] ,

with

M mI 3 + M 11 A , J J 0 + M 22 A , D mle 3× + M 21 A .

The matrices M 11

A and M 22 A are often referred to as added mass and added inertia matrices, respectively. One derives the translational and rotational momentums as follows:

P = ∂E T ∂V = MV -DΩ, Π = ∂E T ∂Ω = JΩ + DV.
Then, the equations of motion satisfy [START_REF] Leonard | Stability of a bottom-heavy underwater vehicle[END_REF]:

           ṗ = RV Ṙ = RΩ × Ṗ = P × Ω + (mg -F B )R ⊤ e 3 + F D + F C Π = Π×Ω+P × V+mgle 3 × R ⊤ e 3 +Γ D +Γ C (1a) (1b) (1c) (1d)
The term (mg -F B )R ⊤ e 3 is the contribution of both gravitational and buoyancy forces. The cross term mgle 3 × R ⊤ e 3 represents the gravitational moment w.r.t. the center of buoyancy. The expansion of P × V in Eq. (1d) shows that the term (MV) × V should not be neglected in the control design due the added mass effects [START_REF] Leonard | Stability of a bottom-heavy underwater vehicle[END_REF].

The terms F D and Γ D represent the damping force and torque due to fluid pressure and viscous drag. Finally, F C ∈ R 3 and Γ C ∈ R 3 are the force and torque control vector inputs.

Homography-based Visual Servo Control Problem

The vehicle is assumed to be equipped with a camera, an Inertial Measurement Unit (IMU) and a Doppler velocity log (DVL). The IMU provides the measurements of the angular velocity Ω and the gravitational direction R ⊤ e 3 (i.e., roll and pitch angles), whereas the DVL measures the translational velocity V.

A reference image of a planar target is taken at some desired pose (i.e., position and orientation). Based on this reference image and the current image, the control objective consists in stabilising the pose of the camera to the desired one. Assume that the camera provides the measurement of the homography matrix H, which contains geometric information about the rotation and translation between two reference frames (see Fig. 1). The homography matrix H is given by [START_REF] Benhimane | Homography-based 2D Visual Tracking and Servoing[END_REF]:

H = R ⊤ -(1/d * )R ⊤ p C n * ⊤ ,
(2) where d * is the distance between the target plane and the camera optical center, and

n * = [n * 1 n * 2 n *
3 ] ⊤ is the unit vector normal to the target plane expressed in the reference camera frame. Hypothesis 1. Assume that the reference image is taken when the AUV stays in a horizontal plane. In addition, the inertial frame A are chosen attached to the reference pose of the camera (see Fig. 1). Hypothesis 2. Assume that a rough knowledge of n * is available such that one can choose a unit vector m * ∈ S 2 such that n * ⊤ m * > 0. The control objective can be stated as the stabilisation of H about the identity matrix I 3 , or equivalently the stabilisation of (R, p C ) about (I 3 , 0).

CONTROL DESIGN

The proposed control approach is split into two cascade parts. The first part -termed inner-loop control-ensures the stabilisation of the translational and angular velocities to a desired setpoint. The second one -termed outerloop control-is specifically designed from the homography matrix to define the desired velocities setpoint.

Inner-loop Control

The control objective taken consists in stabilising the AUV's velocities (V, Ω) about the reference velocities (V r , Ω r ), specified by the outer-loop control. The additional objective is the stabilisation of -→ e b 3 about -→ e a 3 or equivalently of Re 3 about e 3 . This objective can be translated as the stabilisation of the AUV in a horizontal plane. To this purpose, let us define the reference angular velocity Ω r as:

Ω r ω 3r e 3 + k ω e 3 × R ⊤ e 3 , (3)
where k ω is a positive gain and the third component ω 3r of the reference angular velocity is dynamically assigned by the outer-loop control. The first two components of Ω r defined by the term k ω e 3 × R ⊤ e 3 are dedicated to the stabilisation of Re 3 about e 3 . The remaining degree of freedom ω 3r can be independently used for other control objectives related to the yaw motion.

Define the velocity error variables

V V -V r , Ω Ω -Ω r .
(4) Then, the control objective is equivalent to the stabilisation of ( V, Ω) about zero.

The inner-loop controller here proposed is reminiscent of the one in [START_REF] Krupinski | Pipeline tracking for fully-actuated autonomous underwater vehicle using visual servo control[END_REF], but it is robustified by means of integral corrections. Let z V and z Ω be the bounded conditional integrators of the linear and angular velocity errors, whose dynamics are given by:

{ żV = -k zV z V +k zV sat δV (z V + V/k zV ), z V (0) = 0 żΩ = -k zΩ z Ω +k zΩ sat δΩ (z Ω + Ω/k zΩ ), z Ω (0) = 0 (5a) (5b) with positive numbers k zV , k zΩ , δ V , δ Ω . Define the follow- ing augmented error variables: V V + k iV z V , Ω Ω + k iΩ z Ω , (6)
with positive integral gains k iV and k iΩ . Then, using Eqs. (1c), (1d), ( 4), ( 6), one obtains the following coupled dynamics of V and Ω:

                 M V -D Ω = (MV-DΩ) × Ω+(M V-D Ω) × Ω r + (mg-F B )R ⊤ e 3 + F D + F + F C J Ω + D V = (JΩ+DV) × Ω + (MV-DΩ) × V + (J Ω + D V) × Ω r +(M V-D Ω) × V r + mgle 3 × R ⊤ e 3 + Γ D + Γ + Γ C (7a)
(7b) with F and Γ computable by the controller and defined by

F -M Vr +D Ωr +k iV M żV -k iΩ D żΩ +(MV r -DΩ r ) × Ω r -k iΩ (MV-DΩ) × z Ω -(k iV Mz V -k iΩ Dz Ω ) × Ω r Γ -J Ωr -D Vr + k iΩ J żΩ + k iV D żV + (JΩ r + DV r ) × Ω r + (MV r -DΩ r ) × V r -k iΩ (JΩ+DV) × z Ω -k iV (MV-DΩ) × z V -(k iΩ Jz Ω + k iV Dz V ) × Ω r -(k iV Mz V -k iΩ Dz Ω ) × V r
From here, the paper's first result is proposed. Proposition 3. Consider the error system (7a)-( 7b) and apply the following controller:

     F C =-satc ∆V (K V V) -(M V) × Ω r + M ⊤ ( Ω× V r ) -Ω × (DΩ r ) -(mg -F B )R ⊤ e 3 -F D -F Γ C =-satc ∆Ω (K Ω Ω)-(I Ω) × Ω r +(D Ω) × V r -Γ D -Γ (8)
where K V , K Ω are some positive diagonal gain matrices and ∆ V , ∆ Ω are some positive diagonal matrices involved in the saturation functions. Assume that V r , Ω r and their derivative are bounded. Then, (1)

(V, Ω, Re 3 , z V , z Ω ) converges to (V r , Ω r , ±e 3 , 0, 0) for all initial conditions. (2) The equilibrium (V, Ω, Re 3 , z V , z Ω ) = (V r , Ω r , e 3 , 0, 0)
is almost globally asymptotically stable and locally exponentially stable. Conversely, the undesired equilibrium (V, Ω, Re 3 , z V , z Ω )=(V r , Ω r ,-e 3 ,0, 0) is unstable. The proof is given in Appendix A. The knowledge about the derivative of the reference velocities V r and Ω r are necessary to compute the feedback control F C and Γ C . In the following, we show how this fact influences the HBVS outer-loop control design.

Outer-loop Control: HBVS Control Design

The outer-loop control design is based directly on the homography matrix H. Control design difficulties lie in the fact that the depth d * and the normal vector n * involved in the expression (2) of H are unknown and that this matrix only contains a coupled information of rotation and translation. For instance, let us recall a well-known result proposed in [START_REF] Benhimane | Homography-based 2D Visual Tracking and Servoing[END_REF]. Let e p , e Θ ∈ R 3 denote the error vectors defined as:

e p (I 3 -H)m * , e Θ vex(H ⊤ -H), (9)
with some arbitrary unit vector m * ∈ S 2 satisfying Hypothesis 2. Then, the kinematic control law

V C = -λ p e p , Ω = -λ Θ e Θ , (10)
with λ p , λ Θ some positive gains, makes the equilibrium (R, p C ) = (I 3 , 0) locally asymptotically stable [START_REF] Benhimane | Homography-based 2D Visual Tracking and Servoing[END_REF]. A weakness of this kinematic controller is the local basin of attraction of the equilibrium. On the other hand, due to the strongly coupled translational and rotational dynamics, this purely kinematic control approach may fail to guarantee the stability of the controlled system. In the present paper, we aim at working down to the dynamical level with the objective of yielding a controller with a significantly enlarged domain of stability and enhanced robustness.

Reference Translational Velocity Design:

Differentiating H given by Eq. ( 2), one obtains (12) For control design insights, let us, for instance, consider the kinematic control design using the camera velocity V C as control input, with the objective of stabilising e p about zero globally. In view of Eq. ( 12), the control difficulty lies in the unknown multiplicative constant a * . However, we know that it is positive in view of Hypothesis 2. Lemma 4. -Kinematic Control -Assume that Ω is bounded for all time and consider the following auxiliary dynamics: żp = -Ω × e p , z p (0) = 0 .

Ḣ = -Ω × H -(1/d * )V C n * ⊤ . ( 11
(13) Then, the kinematic control law

V C = V Cr -k 1 e p -Ω × z p (14
) globally asymptotically stabilise e p about zero.

Proof: Using Eqs. ( 12), ( 13) and ( 14 13) and ( 15), one verifies that the derivative of L 0 satisfies L0 = -k 1 |e p | 2 . From here, one ensures that L 0 and, thus, e p and z p are bounded w.r.t. initial conditions. The resulting boundedness of ėp given in (15) and, thus, of L0 implies the uniform continuity of L0 . Finally, the application of Barbalat's lemma (see [START_REF] Khalil | Nonlinear systems[END_REF]) ensures the convergence of L0 to zero. This concludes the proof.

In view of the relation V = V C -Ω × r C and the kinematic control expression ( 14), one may define the reference velocity

V r as V r V Cr -Ω r × r C , with V Cr -k 1 e p -Ω r × z p .
As mentioned previously the derivative of V r should be computable by the inner-loop control (8). However, since ėp is not measurable, VCr and, thus, Vr are not available to the computation of the innerloop control. The following modification to Lemma 4 is proposed. Proposition 5. Let k 1 , k 2 , k z , ∆, ∇ denote some positive constants. Consider the following augmented system:

{ żp = -Ω r ×sat ∇ (e p )-k z (z p -sat ∆ (z p )), z p (0) = 0 ėp = -Ω×ê p -k 2 (ê p -e p ), êp (0) = e p (0) (16)
with ∆ large enough such that ∆ ≥ 1/a * . Consider the following reference translational velocity:

{ V r V Cr -Ω r × r C V Cr -k 1 êp -Ω r × z p (17)
Choose the gains k 1 and k 2 such that k 2 > 4a * k 1 .

(18) Assume that Hypothesis 2 is satisfied. Assume that the reference angular velocity Ω r and its derivative are bounded. Apply the inner-loop control (8) proposed in Proposition 3. Then, there exists a positive constant ∇ such that for all ∇ > ∇, e p is globally stabilised about zero. The proof is given in Appendix B. In view of Eq. ( 17), the definition of the reference translational velocity V r depends on the reference angular velocity Ω r . In the following, Ω r will be defined such that it and its derivative are bounded by some constants, as a necessary condition of Propositions 3 and 5.

Reference Angular Velocity Design:

As mentioned in inner-loop control design, only the third component ω 3r of Ω r given in ( 3) is required to be defined by the outer-loop control. The derivative ω3r is also needed for the computation of the inner-loop control. Before proceeding the design for ω 3r , let us analyse the asymptotic behaviour of the homography matrix H. Since Re 3 (almost) globally converges to e 3 as a result of Proposition 3, one ensures the convergence of R to R ψ defined by: R ψ

[ cos ψ -sin ψ 0 sin ψ cos ψ 0 0 0 1 ] .
For the sake of simplicity, assume that Hypothesis 2 is satisfied while choosing m * = -e 3 . This implies that He 3e 3 converges to zero as a result of Proposition 5. From here, one deduces

He 3 -e 3 → (R ⊤ ψ -I 3 )e 3 + a * R ⊤ ψ p C → 0, with a * = -n * 3 /d * .
Therefore, p C converges to 1 a * (R ψ -I 3 )e 3 , which is null, and H converges to R ⊤ ψ . Additionally, From Eqs. ( 17), (B.1), and the convergence of (e p , êp , ėp , V, Ω) to zero, one deduces that

V C → V Cr → -(1/a * )ω 3r e 3 × m * = 0. ( 19
) Denote h i,j as the component on the i-th line and j-th column of H. Now, the main result of the paper is stated, with proof given in Appendix C. Theorem 6. Assume that Hypotheses 1 and 2 are satisfied with m * = -e 3 . Define the reference angular velocity Ω r ω 3r e 3 + k ω e 3 × R ⊤ e 3 , where ω 3r is the solution to the following system: ω3r = -k 4 ω 3r -k 3 sat ∆ω (h 1,2 ), ω 3r (0) = 0, (20) with k 3 , k 4 some positive gains and ∆ ω > 1. Define the reference translational velocity V r as in Proposition 5 and apply the inner-loop control (8) given in Proposition 3. Then, the following properties hold:

(1) There exist only two isolated equilibria H = H ⋆ i , (i = 1, 2), with one stable and one unstable.

(2) The desired equilibrium H = I 3 is almost globally asymptotically stable and locally exponentially stable.

The existence of multiple equilibria of the closed-loop system is expected since there exists a topological obstruction to the existence of a globally asymptotically stable equilibrium to a continuous dynamical systems having rotational degrees of freedom (see [START_REF] Bhat | A topological obstruction to continuous global stabilization of rotational motion and the unwinding phenomenon[END_REF]).

Consequently, the almost-global asymptotical stability is the best one can obtain with continuous feedback control, which is the result of the present paper.

SIMULATION RESULTS

Fig. 2. Components of the AUV simulator

The proposed controller has been tested via a realistic simulation of a fully-actuated AUV model. A custom simulator has been developed (see Fig. 2). Equations of motion ( 1a)-( 1d) and basic sensors are implemented in a Matlab/Simulink R ⃝ model, while Morse simulator is used to generate the visual environment and the camera data (see Fig. 3). The images are processed to provide the homography matrix H at about 25Hz by OpenCV functions integrated into a ROS communication graph. Fig. 3. Vehicle visualised in the simulated sea bottom environment above the visual target The following model of hydrodynamic damping force and torque is used in the system dynamics [START_REF] Fossen | Marine Control Systems[END_REF]:

{ F D = -D Vl V a -D Vn [|V a1 |V a1 |V a2 |V a2 |V a3 |V a3 ] ⊤ Γ D = -D Ωl Ω -D Ωn [|ω 1 |ω 1 |ω 2 |ω 2 |ω 3 |ω 3 ] ⊤ ( 21
)
with damping matrices D Vl , D Vn , D Ωl and D Ωn , the apparent velocity

V a =[V a1 , V a2 , V a3 ] ⊤ V-R ⊤ v cur ,
] M 11 A [kg] 10 3 diag(0.07, 1.5, 1.5) M 22 A [kg.m 2 ] 10 3 diag(1.5, 15, 15) M 12 A [kg.m 2 ] 0 3×3 D V l [kg.s -1 ] diag(10, 30, 30) D V n [kg.m -1 ] diag(100, 300, 300) D Ωl [kg.m 2 .s -1 ] diag(5, 15, 15) D Ωn [N.m]
diag (50,150,150) Table 1. Specifications of the simulated AUV.

• Simulation 1: This simulation is dedicated to the performance comparison between the proposed HBVS control approach and the "standard" kinematic HBVS (i.e., Eqs. ( 10)) in perfect situation where all measurements are perfect and the homography matrix H is directly calculated according to Eq. ( 2), with d * = 3 [m] and n * = [-0.0858, 0.1736, -0.9811] ⊤ = -R 10 • ,5 • ,0 • e 3 . In fact, for the two control approaches we apply the same inner-loop control ( 8), but the reference setpoints defined by the outer-loop control are differently specified by the two approaches. More precisely, for the inner-loop control we assume that the system's dynamics and parameters are perfectly known in the sense that "real" physical parameters of the vehicle and the expressions (21) of F D and Γ D are used in the computation of the inner-loop control. Thus, it is unnecessary to activate the integrator parts (i.e., setting k iV = k iΩ = 0). The gains and parameters (other than those involved in the integrators (5a)-( 5b)) for the inner-loop control (8) are chosen as follows:

       K V =
As for the outer-loop control corresponding to the proposed approach, the gains and parameters are given as:

• k 1 = 1.5, k 2 = 4, k z = 8, ∆ = 10, ∇ = 5; • k 3 = 0.81, k 4 = 1.8, ∆ ω = 10, k ω = 0.1.
As for the outer-loop control corresponding to the "standard" kinematic HBVS, the reference velocities are set as:

{ Ω r -λ Θ e Θ , V Cr -λ p e p V r
V Cr -Ω r × r C where e p and e Θ are given by Eq. ( 9) with m * = -e 3 . The gains are chosen as λ p = 1.5 and λ Θ = 0.9, allowing the two control approaches to have closely similar convergence rates. The derivative terms of the reference velocities are set equal to zero, since the derivative terms ėp and ėΘ are not available for the computation of the controller. Initial conditions are as follows:

p(0) = [-3, -2, -4] ⊤ [m], R ϕ,θ,ψ (0) = R -π 12 , π 8 ,π , V(0) = Ω(0) = 0 [m/s]. This implies that p C (0) ≈ [-4.1, -2.1, -3.9] ⊤ [m].
The initial yaw error is very large, i.e., ψ(0) = π, allowing one to verify the large stability domain of the proposed controller. Variations w.r.t. time of the vehicle's position and orientation errors -corresponding to the standard kinematic HBVS and the proposed HBVS-are reported in Figs. 4 and5, respectively. One observes that for both control approaches the position and orientation errors converge to zero despite large initial yaw error. However, the performance of the standard kinematic HBVS approach is rather poor. Too much oscillation occurs in position error and the vehicle even makes an unnecessary 2π-rotation about the roll axis. On the contrary, the performance of the proposed HBVS approach is quite satisfying (see Fig. 5).

• Simulation 2: This simulation is devised to test the robustness of the proposed controller. First, the homography matrix H is now estimated from the observed images using OpenCV functions. Second, to test the robustness w.r.t. imperfect knowledge on the external forces and torques, in the computation of the inner-loop control (8) we simply set to zero the terms F D and Γ D , while their expression ( 21) is still used in the simulated dynamics. A sea current velocity v cur = [0, 1, 0] ⊤ [m/s] is also introduced. Third, realistic actuation limitations are also taken into account by setting physical saturations for the control thrust force and torque vectors at [±2000, ±2000, ±2000] [N ] and [±500, ±1000, ±1000] [N.m], respectively. Finally, the fol- • k iV = 0.2, k iΩ = 0.05, k zV = k zΩ = 1, δ V = 10, δ Ω = 10;

• K V , K Ω , ∆ V and ∆ Ω are given in Eq. ( 22);

• k 1 = 0.4, k 2 = 1, k z = 8, ∆ = 10, ∇ = 5;

• k 3 = 0.64, k 4 = 1.6, ∆ ω = 10, k ω = 0.1. The gains of the outer-loop control (i.e., k 1,2,3,4 ) are chosen smaller than in Sim. 1 so as to reduce the chattering effect due to measurement noises. Initial conditions of the vehicle are given by: p

(0) = [0, -3, -4] ⊤ , R ϕ,θ,ψ (0) = R π 12 , π 8 , 3π 4 , V(0) = Ω(0) = 0. Thus, p C (0) ≈ [-0.7, -2.1, -4.4] ⊤ [m].
Simulation results are reported in Figs. 6789. Fig. 7 shows that the visual errors e p and h 1,2 (computed from the estimated homography matrix) and the augmented variable êp converge to zero. One can observe the noise effects induced by imperfect homography matrix estimated from image processing. Fig. 6 shows the convergence of position and orientation errors to zero. Overshoots occur mainly due to important unknown disturbance force F D . However, thanks to the integral action in the inner-loop control, the controller manages to compensate for this force as well as other model errors, and thus still ensures asymptotical stability. The effect of the sea current on the integral terms z V and z Ω is illustrated in Fig. 9, where the second component of z V well converges to a nonnull value. The evolution of the control force and torque vectors w.r.t. time is shown in Fig. 8 where torque control saturation occurred in a short period of time marginally affects the overall control performance. The control forces in the directions -→ e b 2 and -→ e b 3 asymptotically converge to the values dictated by the compensation of the current and buoyancy, respectively. Finally, the reader is encouraged to watch a video clip showing this simulation result at http://youtu.be/07MhNBJJ-PA. Fig. 9. z V and z Ω vs. time (Sim. 2). 5. CONCLUSIONS The stabilisation of fully-actuated AUVs using imagebased homography matrix opens a way to several new applications of autonomous vehicles, for example the inspection of underwater objects, too risky using traditional acoustic means. Underwater docking on flat targets can also benefit from this development. Using relatively available and cheap technology of underwater vision, navigation in a structured environment can be carried out using mathematically efficient formulation, rather than complicated Appendix B. PROOF OF PROPOSITION 5 Using Eqs. ( 12) and ( 17), one deduces ėp

= -Ω × e p -a * Ω r× zp -a * k 1 êp + γ( V, Ω), (B.1) with γ( V, Ω) a * ( V + Ω × r c ) + Ω × m * , zp z p -z * p .
From here, the proof proceeds by three steps:

Step 1: We will show that z p is bounded by some constant. Consider the positive function S 1 0.5|z p | 2 . Its derivative satisfies (using ( 16)

) Ṡ1 ≤ -k z |z p | 2 + |z p |( Ωr ∇ + k z ∆), with Ωr sup(|Ω r |).
From here, it is straightforward to deduce that ∀t ≥ 0 |z p (t)| ≤ ( Ωr ∇)/k z +∆ = ∆(1+α Ωr ), with α ∇/(k z ∆).

Step 2: We will show next that there exists a time instant T such that ∀τ ≥ T one has |e p (τ )| ≤ ∇ and, thus, sat ∇ (e p (τ )) = e p (τ ). Denote X [x y] ⊤ ∈ R 6 , with x Rê p , y Re p . One verifies from ( 16 The condition (18) ensures that A has two triple distinct real negative eigenvalues λ 1,2 (λ 1 < λ 2 < 0) given by λ 1,2 = 0.5(-k 2 ∓ √ k 2 2 -4a * k 1 k 2 ). This implies that A is diagonalisable and can be decomposed in the Jordan normal form A = PΛP -1 , with Λ = diag(λ 1 I 3 , λ 2 I 3 ) and , which allows one to deduce

P =    λ 1 I 3 η 1 λ 2 I 3 η 2 -a * 1 k 1 I 3 η 1 -a * 1 k 1 I 3 η 2   , P -1 = 1 λ 2 -λ 1    -η 1 I 3 -λ 2 η 1 I 3 a * 1 k 1 η 2 I 3 λ 1 η 2 I 3 a * 1 k 1    η 1 √ λ 2 1 + (a * 1 k 1 ) 2 ,
|P -1 X| ≥ η 1 η 2 a * k 1 √ η 2 1 +η 2 2 |y| = η 1 η 2 a * k 1 √ η 2 1 +η 2 2 |e p |. (B.3)
On the other hand, as a result of Proposition 3, Ω and V are uniformly continuous and bounded, and converge asymptotically to zero. Consequently, γ( V, Ω) also converges to zero. Thus, for some positive number ε (to be specified hereafter) there exists a time instant T 1 such that ∀t ≥ T 1 one has |γ( V, Ω)| ≤ ε. Then, ∀t ≥ T 1 one verifies

|P -1 B| = √ λ 2 1 η 2 2 + λ 2 2 η 2 1 a * k 1 (λ 2 -λ 1 ) -a * Ω r× zp +γ( V, Ω) ≤ √ λ 2 1 η 2 2 + λ 2 2 η 2 1 a * k 1 (λ 2 -λ 1 )
(( a * ∆(1 + α Ωr )+1

) Ωr +ε ) .

(B.4) Using Eq. (B.2), the derivative of S 2 0.5|P -1 X| 2 satisfies Ṡ2 = (P -1 X) ⊤ Λ(P -1 X) + (P -1 X) ⊤ (P -1 B) ≤ λ 2 |P -1 X| 2 + |P -1 X| |P -1 B|. (B.5) Since z p , Ω r , Ω and V are bounded, B and P -1 B are also bounded. From Eq. (B.5) and the definition of S 2 , one ensures that P -1 X and X are bounded w.r.t. initial conditions. Consequently, e p and êp remain bounded w.r.t. initial conditions. Then, it is straightforward to verify that ėp , ėp and żp are bounded w.r.t. initial conditions, which implies the uniform continuity of e p , êp and z p . Since X remains bounded w.r.t. initial conditions on the time-interval [0, T 1 ] (as proved previously), from Eq. (B.5) and the definition of S 2 there exists another time-instant T > T 1 such that ∀τ ≥ T one has |P -1 X(τ )| ≤ (-λ 2 ) -1 sup

  Fig. 1. Notation. • G and B are the vehicle's center of mass and center of buoyancy, respectively, m its mass and J 0 its inertia matrix. Let l denote the distance between G and B. • A = {O; -→ e a 1 , -→ e a 2 , -→ e a 3 } is an inertial frame chosen such that its -→ e a 3 -axis points downwards and coincides with the gravity direction. B = {B; -→ e b 1 , -→ e b 2 , -→ e b 3 } is a frame attached to the body whose origin coincides with the vehicle's center of buoyancy. C = {C; -→ e c 1 , -→ e c 2 , -→ e c 3 } is a frame attached to the camera, which is displaced from the origin of B by a vector --→ BC and keeps its base vectors parallel to those of B. Let r C ∈ R 3 and r G ∈ R 3 denote the vectors of coordinates expressed in the frame B of --→ BC and --→ BG, respectively. • The orientation of the body-fixed frame B with respect to (w.r.t.) the inertial frame A is represented by the rotation matrix R ∈ SO(3). • The position vectors of the origins of the body-fixed frame B and the camera frame C, expressed in the inertial frame A, are denoted as p and p C , respectively. Their relation is p = p C -Rr C . • The angular velocity vector of the body-fixed frame B relative to the inertial frame A, expressed in the frame B, is denoted as Ω = [ω 1 , ω 2 , ω 3 ] ⊤ ∈ R 3 . The translational velocity vectors of the origins and the frame B and the frame C, expressed in the frame B, are denoted as V ∈ R 3 and V C ∈ R 3 , respectively.• {e 1 , e 2 , e 3 } denotes the canonical basis of R 3 . I 3 is the identity matrix of R 3×3 . For all u ∈ R 3 , the notation u × denotes the skew-symmetric matrix associated with the cross product by u, i.e., u × v = u × v, ∀v ∈ R 3 . The

  ) It follows that the derivative of e p defined in (9) satisfies ėp = -Ω × (e pm * ) + a * V C , a * (n * ⊤ m * )/d * .

  ), one verifies that ėp = -Ω × e p -a * k 1 e p -a * Ω × (z pz * p ), (15) with z * p m * /a * . Consider the following Lyapunov function candidate L 0 1/(2a * )|e p | 2 + 1/2|z pz * p | 2 . Using Eqs. (

  and the current velocity v cur expressed in the inertial frame. The physical parameters of the simulated vehicle, given in Tab. 1, closely follow that of a real AUV.

  10 3 diag(0.93, 0.464, 2.5) K Ω = 10 4 diag(0.759, 5.623, 1.8) ∆ V = diag(1500, 1500, 1500) ∆ Ω = diag(375, 750, 750)

Fig. 4 .Fig. 5 .

 45 Fig. 4. Standard kinematic HBVS approach: Position and orientation errors vs. time (Sim. 1).

Fig. 6 .Fig. 7 .

 67 Fig. 6. Position and orientation errors vs. time (Sim. 2).

Fig. 8 .

 8 Fig. 8. F C , Γ C calculated by the inner-loop control and the "saturated" values F C , Γ C vs. time (Sim. 2).

  η 2 By simple calculations, it can be verified that(λ 2 -λ 1 ) 2 |P -1 X| 2 = η 2 1 x + (λ 2 -λ 1 ) 2 |y| 2 (a * k 1 ) 2 (η 2 1 +η 2 2 )

	=	√ η 2 1 +η 2 2	( x+	λ 1 y a * k 1	) +	√ 2 + (a λ 2 λ 2 (a * k 1 ) y 2 + η 2 2 λ 1 y 2 x + a * k 1 1 (λ 2 -λ 1 )y η 2 a * k 1 √ η 2 2 1 +η 2 2 + η 2 1 η 2 2

* 1 k 1 ) 2 .

pose reconstruction from multiple sensors. A testing campaign with a real AUV is now envisioned so as to validate the proposed approach in the challenging sea environment.

Appendix A. PROOF OF PROPOSITION 3

The proof is based on the analysis of the Lyapunov function candidate L inner 1 2 W⊤ M T W + mgl(1-e ⊤ 3 R ⊤ e 3 ), with W [ V⊤ Ω⊤ ] ⊤ . After some tedious computation, it can be verified that the derivative of L v along any solution to the closed-loop system satisfies Linner = -V⊤ satc

By application of Barbalat's lemma, one deduces the convergence of Linner and, thus, of V, Ω and 1e ⊤ 3 R ⊤ e 3 to zero. The latter implies that Re 3 converges to ±e 3 .

Let us now prove the local exponential stability (L.E.S.) of the equilibrium ( V, Ω, Re 3 ) = (0, 0, e 3 ). By denoting Θ the angle between Re 3 and e 3 , i.e., cos(Θ) = e ⊤ 3 Re 3 , around a small neighborhood of the equilibrium ( V, Ω, Re 3 ) = (0, 0, e 3 ) the Lyapunov function L inner can be locally approximated by L inner ≈ 0.5 W⊤ M T W + 0.5mglΘ 2 and its derivative (A.1) is approximatively given by Linner ≈ -V⊤ K V V-Ω⊤ K Ω Ω-mglk ω Θ 2 . As a consequence, there exists a positive number α such that locally Linner ≤ -αL inner . This ensures the L.E.S. of L inner about zero and, thus, the L.E.S. of the equilibrium ( V, Ω, Re 3 ) = (0, 0, e 3 ). Finally, the exponential convergence of V and Ω to zero naturally leads to same property of V and Ω as a consequence of the technical lemma 7 (Append. D). The proof of instability of the equilibrium ( V, Ω, Re 3 ) = (0, 0, -e 3 ) is based on the Chetaev's theorem. Define y = [y 1 y 2 y 3 ] ⊤ e 3 + Re 3 and consider the following continuously differentiable function: S(y)

3 Re 3 ≥ 0, which is null at the origin, i.e., S(0) = 0. For some positive number r > 0, define a set U r {y | S(y) > 0, |y| < r}, and note that U r is non-null for all r > 0. By neglecting all high-order terms, the derivative of S can be approximatively given by Ṡ

). The positivity of y 3 is equivalent to the positivity of y 2 1 +y 2 2 . Thus, for all y ∈ U r one ensures that Ṡ > 0. Since all the conditions of Chetaev's theorem are satisfied [START_REF] Khalil | Nonlinear systems[END_REF], the origin y = 0 of the linearized system is unstable.

, then one deduces from inequality (B.7) that |e p (τ )| < ∇, ∀τ ≥ T , and thus sat ∇ (e p (τ )) = e p (τ ).

Step 3: Consider the Lyapunov candidate function

Using the following property [START_REF] Hua | A control approach for thrust-propelled underactuated vehicles and its application to VTOL drones[END_REF]:

p Ω r× (e p -sat ∇ (e p )) + 1/a * e ⊤ p γ( V, Ω). Since sat ∇ (e p (τ )) = e p (τ ) and |e p (τ )| ≤ ∇, ∀τ ≥ T (as proved in Step 2), one obtains

(B.8) As proved previously, e p , êp and z p cannot escape in finitetime. Thus, L(t) remains bounded on the time-interval [0, T ]. In addition, e p (τ ), êp (τ ), z p (τ ) and, thus, L(τ ), ∀τ ≥ T , remain bounded by some positive constants independent from all initial conditions (as proved previously). Since the equilibrium ( V, Ω, R ⊤ e 3 ) = (0, 0, e 3 ) is locally exponentially stable as a result of Proposition 3, there exist some time-instant T 2 > T and some positive constants α 1 and α 2 such that γ( V(τ ), Ω(τ )) ≤ α 1 e -α2τ , ∀τ ≥ T 2 . (B.9) From Eqs. (B.8) and (B.9), one deduces

From here, the resulting boundedness of integral term ∫ ∞ T2 |ê p (τ )| 2 dτ and the uniform continuity of êp implies the convergence of êp to zero (Barbalat's lemma). From Eq. ( 16), one verifies that ėp can be rewritten as ėp (t) = a(t) + b(t), with a(t)

k 2 e p the uniformly continuous term and b(t) -Ω × êp -k 2 êp the vanishing term. Then, the application of the extended Barbalat's lemma [START_REF] Hua | A control approach for thrust-propelled underactuated vehicles and its application to VTOL drones[END_REF] ensures the convergence of ėp to zero, which in turn implies the convergence of e p to zero.

Appendix C. PROOF OF THEOREM 6 From (20), it is straightforward that ω 3r and ω3r remain bounded by k 3 ∆ ω /k 4 and 2k 3 ∆ ω , respectively. The boundedness of ω 3r and ω3r is a necessary condition of Propositions 3 and 5. One ensures that at the zero dynamics ψ = ω 3 = ω 3r and h 1,2 = sin ψ ≤ 1 < ∆ ω . Thus, at the zero dynamics, the dynamics of ω 3r given by ( 20 -1, -1, 1). To prove the local stability results, it suffices to study the linearized system about the equilibrium. For the equilibrium ψ = 0, the linearized system is ψ = ω 3 , ω3 = -k 4 ω 3 -k 3 ψ with ψ = ψ. Since its characteristic polynomial p 2 + k 4 p + k 3 is Hurwitz, the equilibrium ψ = ψ ⋆ 1 = 0 is exponentially stable. On the other hand, the linearized systems for the equilibrium ψ = π is given by: ψ = ω 3 , ω3 = -k 4 ω 3 + k 3 ψ with ψ = ψ -π. Using Hurwitz criteria, one easily verifies that the origin of this linearized system is unstable.

Appendix D. TECHNICAL LEMMA Lemma 7. (Omitted proof) If x(t) + k ∫ t t0 x(τ )dτ , with x ∈ R n and k > 0, converges exponentially to some constant vector, then x(t) converges exponentially to zero.