Quentin Clairon
email: quentin.clairon@u-bordeaux.fr

Adeline Samson

Optimal control for parameter estimation in partially observed hypoelliptic stochastic differential equations

Keywords: Stochastic differential equations, Parameter estimation, Hypoellipticity, Optimal control theory

We deal with the problem of parameter estimation in stochastic differential equations (SDEs) in a partially observed framework. We aim to design a method working for both elliptic and hypoelliptic SDEs, the latters being characterized by degenerate diffusion coefficients. This feature often causes the failure of constrast estimator based on Euler Maruyama discretization scheme and dramatically impairs classic stochastic filtering methods used to reconstruct the unobserved states. All of theses issues make the estimation problem in hypoelliptic SDEs difficult to solve. To overcome this, we construct a well-defined cost function no matter the elliptic nature of the SDEs. We also bypass the filtering step by considering a control theory perspective. The unobserved states are estimated by solving deterministic optimal control problems using numerical methods which do not need strong assumptions on the diffusion coefficient conditioning. Numerical simulations made on different partially observed hypoelliptic SDEs reveal our method produces accurate estimate while dramatically reducing the computational price comparing to other estimation procedures.

Introduction

Stochastic differential equations (SDEs) are widely used in several fields to model stochastic temporal dynamics. We focus on hypoelliptic SDE, that is when the diffusion matrix is not of full rank but its solution has a smooth density. More precisely, we consider a process Z t = (V t , U t ) ∈ R d , of dimension d on a time interval [0, T ], defined as the solution of the following hypoelliptic SDE:

dV t = g θ (V t , U t , t)dt dU t = h θ (V t , U t , t)dt + B σ (Z t , t)dW t Z (0) = Z 0 (1)
where V t is the d V -dimensional component corresponding to the smooth part of the process, that is no directly affected by stochastic perturbations, and U t is the d Udimensional rough part, W t is a d U dimensional Brownian motion acting on the system through the d U squared diffusion matrix B σ (Z t , t). In some applications, only the smooth component can be measured.

Let us give some examples of such models. Stochastic damping Hamiltonian systems or Langevin equations have been developed to describe a particle moving in an environment defined by a potential [START_REF] Wu | Large and moderate deviations and exponential convergence for stochastic damping Hamiltonian systems[END_REF]. The system is usually two dimensional, the first equation describes the position of the particle and the other its velocity. The noise is degenerate because it acts only on the velocity and not on the position. Only the position is directly measured. We can cite applications of these models in molecular dynamics [START_REF] Leimkuhler | Molecular Dynamics with deterministic and stochastic numerical methods[END_REF], paleoclimate research [START_REF] Ditlevsen | The fast climate fluctuations during the stadial and interstadial climate states[END_REF], neural field models [START_REF] Coombes | Lecture notes in nonlinear dynamics in computational neuroscience: from physics and biology to ICT, Springer, chap Next generation neural mass models[END_REF][START_REF] Ditlevsen | Multi-class oscillating systems of interacting neurons[END_REF]. Another application is neuronal models of membrane potential dynamics. Examples are the hypoelliptic FitzHugh-Nagumo (FHN) model [START_REF] Gerstner | Spiking neuron models[END_REF][START_REF] Deville | Two distinct mechanisms of coherence in randomly perturbed dynamical systems[END_REF][START_REF] Leon | Hypoelliptic stochastic FitzHugh-Nagumo neuronal model: mixing, up-crossing and estimation of the spike rate[END_REF], the hypoelliptic Hodgkin-Huxley model [START_REF] Goldwyn | The what and where of adding channel noise to the Hodgkin-Huxley equations[END_REF][START_REF] Tuckwell | The space-clamped Hodgkin-Huxley system with random synaptic input: inhibition of spiking by weak noise and analysis with moment equations[END_REF], or synapticconductance based models with stochastic channel dynamics [START_REF] Paninski | Inferring synaptic inputs given a noisy voltage trace via sequential Monte Carlo methods[END_REF][START_REF] Ditlevsen | The Morris-Lecar neuron model embeds a leaky integrate-and-fire model[END_REF]. Only the first equation, corresponding to the membrane potential, can be measured by intra-cellular recordings. It is therefore important to develop estimation methods for this class of models.

In the rest of the paper, we assume the drift functions g θ and h θ depend on a d θ dimensional parameter θ and the matrix B σ on the d σ dimensional parameter σ known as volatility. Partial observations of (Z t ) are defined as:

Y t = C Z t
where C is the d 0 × d observation matrix. We assume (Y t ) is discretely observed on the interval [0, T ] at times 0 = t 0 < • • • < t n = T and without measurement error. Our aim is to estimate the unknown parameter ψ = (θ, σ ) of model (1) using the discrete and partial observations (Y 0 , . . . , Y n ) and possibly without knowing the initial condition Z 0 .

Parametric estimation of hypoelliptic models faces several difficulties. When the first equation reduces to g θ (V t , U t ) = U t , different contrasts estimators have been proposed [START_REF] Genon-Catalot | Stochastic volatility models as hidden Markov models and statistical applications[END_REF][START_REF] Gloter | Discrete sampling of an integrated diffusion process and parameter estimation of the diffusion coefficient[END_REF][START_REF] Ditlevsen | Inference for observations of integrated diffusion processes[END_REF][START_REF] Gloter | Parameter estimation for a discretely observed integrated diffusion process[END_REF][START_REF] Samson | Contrast estimator for completely or partially observed hypoelliptic diffusion[END_REF][START_REF] Lu | Comparison of continuous and discrete-time data-based modeling for hypoelliptic systems[END_REF][START_REF] Leon | Consistency of a likelihood estimator for stochastic damping hamiltonian systems. Totally observed data[END_REF]. This specific case allows to deal with partial observations, the second coordinate U t being replaced by increments of V t . When the drift g θ is more complex, we deal with several entangled issues. First, solutions of the SDE are generally non explicit. Numerical schemes are used to approximate the discretized process and derive an estimation 123 criteria. However, compared to elliptic system, the degeneracy of the noise complicates the statistical estimation. Thus estimation cost function that would be derived directly from the Euler-Maruyama fails because the covariance matrix

Γ T σ Γ σ where Γ σ (Z t , t) = 0 d V ,d U , B σ (Z t , t)
t , is not full rank. [START_REF] Gloter | Adaptive estimation for degenerate diffusion process[END_REF] propose a local Gaussian approximation of the transition density in the case of totally observed coordinates. Several papers propose to use a likelihood based on higher order schemes of approximation, the 1.5 order scheme [START_REF] Ditlevsen | Hypoelliptic diffusions: discretization, filtering and inference from complete and partial observations[END_REF], the local linearization scheme [START_REF] Melnykova | Parametric inference for hypoelliptic ergodic diffusions with full observations[END_REF]). In the case of partial observations, the unobserved coordinates U t has to be filtered or imputed, either in a Bayesian settings with a Markov Chain Monte Carlo algorithm [START_REF] Pokern | Parameter estimation for partially observed hypoelliptic diffusions[END_REF][START_REF] Graham | Manifold Markov chain Monte Carlo methods for Bayesian inference in a wide class of diffusion models[END_REF] or in a frequentist settings with a particle filter or sequential Monte Carlo coupled with a stochastic approximation expectation maximization (SAEM) algorithm (Ditlevsen andSamson 2014, 2019). A recent simulation filter has been proposed by [START_REF] Bierkens | Simulation of elliptic and hypo-elliptic conditional diffusions[END_REF] for hypoelliptic diffusions. Still the theoretical assessment and practical efficiency of such numerical methods critically rely on Γ σ being well conditioned. These methods are also time consuming.

In this work, we propose an estimation method based on optimal control theory which applies to nonlinear hypoelliptic systems and partial observations. The idea of using optimal control theory is to treat the coordinate filtering problem as a deterministic tracking problem. Then, numerical devices coming from control theory allow us to estimate the optimal control bringing the observed system states the closest possible to the observations. This was proposed for ODE system [START_REF] Clairon | Tracking for parameter and state estimation in possibly misspecified partially observed linear ordinary differential equations[END_REF][START_REF] Clairon | A regularization method for the parameter estimation problem in ordinary differential equations via discrete optimal control theory[END_REF][START_REF] Iolov | Optimal design for estimation in diffusion processes from first hitting times[END_REF]. For SDE, the only reference is [START_REF] Clairon | Optimal control for estimation in partially observed elliptic and hypoelliptic linear stochastic differential equations[END_REF] which uses linear-quadratic theory, a particular case of the Pontryagin maximum principle, to solve the tracking problem. But this limits the application of the method to linear systems. Also, this method separatly estimates σ and θ via a nested procedure because of identifiability issues. Volatility σ was estimated via the minimization of an outer criteria demanding repeated estimation of θ based on the optimization of an inner criteria. This dramatically increases the computation time of the method when σ is unknown. Additionally, the criteria used for σ inference was properly defined only for linear SDEs.

In this work, we construct an estimation procedure which simultaneously estimate (θ, σ ) by relying on a contrast function bypassing identifiability issues for σ estimation encountered in [START_REF] Clairon | Optimal control for estimation in partially observed elliptic and hypoelliptic linear stochastic differential equations[END_REF]. This contrast function is well defined for linear and nonlinear SDEs, both elliptics and hypoelliptics. Also, the control problem solutions required by our approach are easily obtained for non-linear SDEs via an adaptation of the method proposed by Cimen and Banks (2004a).

Our statistical criteria exploits the hypoelliptic nature of (1) which assumes "enough" interactions between V t and U t such that the Wiener process W t influences all the coordinates of the SDE, potentially with some delay. More precisely, we consider a lagged contrast function linking W t and the observed part of the system state C Z t+m B after a delay m B > 0 such that the dependence C Z t+m B with respect to W t is regular enough to ensure a well-conditioned variance for C Z t+m B . This requires to know explicitly the dependence of C Z t+m B to W t . For this we rely on the so-called pseudo-linear representation of the discretized SDE (1) [START_REF] Cimen | State-dependent Riccati equation (SDRE) control: a survey[END_REF]). The scheme is 123 expressed at each time t as a function of the m B previous lagged states. The discretized process is no more Markovian, this is a direct consequence of hypoellipticity. From this lagged formulation of the discretized process, we deduce a lagged contrast which is the pseudo-likelihood of the representation. This statistical contrast depends on the whole state variable, even the unobserved ones. We thus need to filter or predict the unobserved coordinate U t . We propose a predictor which is a balance between fidelity to the observations and to the solution of SDE (1). This predictor is defined via an optimal control problem that can be solved easily without assumption on Γ σ rank. The optimal control problem is solved with the linear quadratic theory for linear SDEs and for nonlinear models with an adaptation of the procedure developed by Cimen and Banks (2004b) based on the pseudo-linear representation. Our criteria is somewhat similar to the generalized profiling introduced in [START_REF] Ramsay | Parameter estimation for differential equations: a generalized smoothing approach[END_REF] for ordinary differential equations but here dedicated to SDEs with potentially degenerate diffusions. We also consider a criteria making a tradeoff between data and model fidelity to regularize the inverse problem of parameter estimation. This method provides estimators for the parameters and for the unobserved coordinate U t . The simulation study illustrates the good performances of our method.

Section 2 studies the hypoellipticity of the continuous process, provides a discrete approximation scheme and the corresponding concept of hypoellipticity. In Sect. 3, we derive the expression of our statistical criteria which defines our estimator. The construction of the state predictor and the optimal control numerical methods are presented in Sect. 4. Then, in Sect. 5 we conduct a simulation study on three hypoelliptic models to evaluate the practical accuracy of our method as well as its computational efficiency.

Model and its discrete approximation

This section describes the properties of hypoelliptic systems and the connexity property that allows to propagate the noise to the smooth coordinates. Then we introduce the approximate Euler-Maruyama scheme and the lagged pseudo-linear formulation, the key of our estimation method.

Models and its assumptions

Let us denote:

f θ (Z t , t) = g θ (V t , U t , t) h θ (V t , U t , t) , Γ σ (Z t , t) = 0 d V ,d U B σ (Z t , t)
the drift and diffusion coefficient of the diffusion process Z t respectively, then

d Z t = f θ (Z t , t)dt + Γ σ (Z t , t)dW t .
We assume B σ is full rank and we consider two classes of models called elliptic and hypoelliptic.

Elliptic models are defined by d V = 0. In that case, the Brownian process directly acts on the whole system, the diffusion coefficient is not degenerated. Girsanov formula applies and the process has a continuous smooth density.

Hypoelliptic models are defined with d V > 0 and verify the Hörmander condition detailed below. Let us first define the Lie brackets. For f a function with values in R d , f l stands for its l-th component. For a matrix Γ , Γ j denotes its j-th column.

Definition 1 The Lie bracket of two functions f , g : R d → R is defined as

[ f , g] l = ∂ g l (z) ∂z T f (z) - ∂ f l (z) ∂z T g(z).
Hypoellipticity is then defined as follows:

Definition 2 (Hörmander condition) Let us consider the set L defined iteratively as:

• Initialization step: L is composed of vectors L 0 = Γ j , for j = 1, . . . d U . • Generalization step at iteration k, if L k-1 ∈ L then vectors L k defined by L k = f (Z t , t), L k-1 and by L k = Γ j (Z t , t), L k-1 for j = 1, . . . d U belong to L.
If at some iteration, L spans R d , the weak Hörmander condition is fulfilled. This implies that the system is hypoelliptic.

Hypoellipticity properties

We derive a necessary condition on the drift g θ respected by hypoelliptic systems (the proof is given in Appendix 1 in Supplementary Materials):

Proposition 1 (Connexity property) If the SDE (1) is hypoelliptic then for each l ∈ 1, d V , it exists j ∈ 1, d U and a sequence q 1 , . . . q m l ∈ 1, d V m l with m l ≤ d V such that ∂ g θ,l (Z t , t) ∂ V q 1 ∂ g θ,q 1 (Z t , t) ∂ V q 2 • • • ∂ g θ,q m l -1 (Z t , t) ∂ V q m l ∂ g θ,q m l (Z t , t) ∂U j = 0. (2)
This can be seen as a connexity property between the set of smooth variables and rough ones. It means that each variable V l,t is influenced by at least one component of U t at least undirectly through a path of auxiliary smooth variables (V q 1 , . . . , V q m l ). Of course, from a given sequence (q 1 , . . . , q m l ), it is possible to construct bigger ones. To remove ambiguity m l denotes the length of the shortest possible path such that the connexity property (2) holds.

Although the diffusion coefficient Γ σ (Z t , t) is rank deficient, the solution of (1) has a smooth density with respect to the Lebesgue measure.

If the drift f (Z t ) satisfies a dissipativity condition

< f (z), z >≤ α -δ z 2 , ∀z ∈ R d 123
where α, δ > 0, the function L(z) = 1 + z 2 is a Lyapounov function. It ensures the geometric ergodicity of the solution of the SDE. It means that (Z (t)) t∈[0,T ] converges exponentially fast to a unique invariant distribution. However, usually, the solution of SDE ( 1) is not explicit and a numerical scheme is needed to approximate the solution.

Euler-Maruyama discretization

Let us introduce the Euler-Maruyama discretized process ( Zi , for i = 1, . . . , n) of SDE (1). To simplify the notations, we assume that the time points are equidistant, that is i = t i+1t i = , ∀i = 1, . . . , n. Then the discretized approximate process is defined by:

Zi+1 = Zi + f θ ( Zi , t i ) + √ Γ σ ( Zi , t i )u i Z0 = Z 0 (3)
where

u i ∼ N (0, I d U )
is the normalized increment of the Brownian motion at t i , i.e.

u i = 1 √ W t i+1 -W t i and u = (u 0 , . . . , u n-1
). The connexity property can be illustrated on this Euler-Maruyama discretization. Proposition 2 proves that m l +1 iterations of (3) are required to have Ṽl,i+m l +1 affected by some entry Ũ j,i of the rough component.

Proposition 2 Let us assume that for each l ∈ 1, d V it exists a unique minimal length sequence q 1 , . . . q m l such that the connexity property holds. Then the Euler-Maruyama approximation ( Zi , for i = 1, . . . , n) is such that for each l ∈ 1, d V it exists j ∈ 1, d U such that:

∂ Ṽl,i+k ∂ Ũ j,i = 0, for k = 1, . . . , m l and ∂ Ṽl,i+m l +1 ∂ Ũ j,i = 0
So, in model ( 3), each coordinate of Ṽi+m B +1 with m B := max l m l is influenced by Ũi . It also means that m B + 1 is the minimal delay to propagate the noise u i from the rough components to all the smooth ones in the discretized model (3).

Lagged pseudo-linear formulation of the discretized process

Proposition 2 proves the existence of a minimal delay m B . For estimation purpose, we need the explicit link between Zi+m B +1 and u i . For this, we rely on what we call later the lagged pseudo-linear formulation of (3).

Let us introduce the matrix A θ ( Zi , t i ) and the vector r

θ (t i ) such that A θ ( Zi , t i ) Zi + r θ (t i ) = f θ ( Zi , t i ).
This allows to reformulate (3) with a pseudo-linear expression:

⎧ ⎨ ⎩ Zi+m B +1 = A θ,i+m B Zi+m B + r θ (t i+m B ) + √ Γ σ ( Zi+m B , t i+m B ) u i+m B Z0 = Z 0 (4) 123 with A θ,i := I d + A θ ( Zi , t i ).
We can replace Zi+m B by its state-space expression (3) and iterate to obtain the lagged pseudo-linear formulation.

Proposition 3 The lagged pseudo-linear formulation of the discretized process ( 3) is defined as follows. For i = 0, . . . , nm B -1:

Zi+m B +1 = m B r =0 A θ,i+r Zi + m B r =0 m B l=r +1 A θ,i+l r θ (t i+r ) + m B r =0 m B l=r +1 A θ,i+l √ Γ σ ( Zi+r , t i+r )u i+r .
(5)

In the last expression the multiplication symbol m B -1

l=0 denotes the sequential left multiplication i.e. b j=a A θ, j = A θ,b × A θ,b-1 × • • • × A θ,a with the convention b j=b+1 A θ, j = I d .
The pseudo-linear formulation gives the explicit link between Zi+m B +1 and u i . The Markovian property is no more fulfilled for the discretized process: the smooth density is obtained only if we express Zi+m B +1 as a function of the m B previous states. This is a direct consequence of hypoellipticity.

Let us introduce a new observability assumption that reinforces hypoellipticity for a discrete process. It requires that the observed variables give enough information about the diffusion process, or equivalently that the Y t are influenced by all the rough components.

Assumption 1 (H1): The d o × d U matrix C m B l=1 A θ,i+l Γ σ (z i , t i ) is full rank for all 1 ≤ i ≤ n -m B and for all possible values {z i } 0≤l≤m B ∈ R d×m B .
Let us detail this assumption for elliptic and some hypoelliptic models.

• Elliptic models verify m B = 0. Assumption (H1) imposes that CΓ σ ( Zi , t i ) is full rank. It ensures a smooth transition density for the discrete process (Y i ). This is similar to the continuous process.

• We focus on hypoelliptic SDE with ∂ g θ ∂ V (V t , U t , t)
full rank and Y t = V t . This corresponds to the case m B = 1, that is all the components of V t have a direct interaction with at least one component of U t and stochasticity only acts on the unobserved variables. This corresponds to the 1-step hypoellipticity defined by [START_REF] Buckwar | Splitting methods for SDEs with locally lipschitz drift[END_REF].

Cost function construction

Our aim is to estimate the unknown parameter ψ = (θ, σ ) of model (1) using the discrete and partial observations (Y 0 , . . . , Y n ) and possibly without knowing the initial condition Z 0 .

We first assume that the process Z is observed and construct an estimation contrast based on the lagged pseudo-linear representation. Then, we propose to filter Z by optimal control.

Statistical criteria assuming Z observed

Let us consider in this section that the variables Z are observed. We can use formula (5) to build a statistical criteria. Let us denote t i i+m B = t i , . . . , t i+m B , Zi:i+m B = Zi . . . , Zi+m B and define

X i:i+m B +1 = Y i+m B +1 -C m B r =0 A θ,i+r Zi - m B r =0 F i+r :i+m B F i+r :i+m B = C m B l=r +1 A θ,i+l r θ (t i+r ) G i+r :i+m B = C m B l=r +1 A θ,i+l Γ σ ( Zi+r , t i+r ).
Then we have

X i:i+m B = m B r =0 G i+r :i+m B u i+r . ( 6
)
Under assumption (H1), the matrix

Σ i:i+m B = m B r =0 G i+r :i+m B G T i+r :i+m B
is nonsingular. Then, conditionally on Zi:i+m B , we have

X i:i+m B +1 | Zi:i+m B ∼ N (0 d o ,1 , Σ i:i+m B )
and we can define the following cost function based on the log likelihood:

H m B (ψ | Y , Z ) = -log P ψ X i:i+m B +1 | Zi:i+m B i∈ 1, n-m B -1 (7)
and the associated estimator:

ψ = arg min ψ H m B (ψ | Y , Z ). ( 8
)
From equation ( 6), we see that X i:i+m B +1 depends at most of u i , . . . , u i+m B . Thus the sequence (X i:i+m B +1 ) for varying i can be composed of dependant terms which makes H m B not easy to calculate in this general setting. We detail in the next subsection some cases where H m B has an explicit form.

Simplification of H m B for some models

Note first that when the sequence of (X i:i+m B +1 ) is composed of independent terms, the expression of H m B simplifies to

H m B (ψ | Y , Z ) = n-m B -1 i=1 X T i:i+m B +1 Σ -1 i:i+m B X i:i+m B +1 + log Det Σ i:i+m B .
This is the case for elliptic and 1-step hypoelliptic systems. Let us give more detailed expressions for Σ i:i+m B for these two cases.

Elliptic system Here, we have m B = 0:

X i:i+1 = Y i+1 -C A θ,i Zi + r θ (t i )
and

X i:i+1 | Zi ∼ N (0 d o ,1 , Σ i:i ) with Σ i:i = CΓ σ ( Zi , t i )Γ σ ( Zi , t i ) T C T .
Thus, we fall back to the classic contrast estimator of elliptic SDE.

1-step hypoelliptic system

In the case where m B = 1 and V t = C Z t , we end up with

CΓ σ ( Zi , t i ) = 0. So condition (H1) is respected if C A θ ( Zi+1 , t i+1 )Γ σ ( Zi , t i ) is of full rank.
Then X i:i+m B +1 is given by:

X i:i+2 = Y i+2 -CA θ,i+1 A θ,i Zi -C A θ,i+1 r θ (t i ) + r θ (t i+1 )
and follows the Gaussian law X i:i+2 | Zi:i+1 ∼ N (0 d o ,1 , Σ i:i+1 ) with:

Σ i:i+1 = C A θ ( Zi+1 , t i+1 )Γ σ ( Zi , t i )Γ σ ( Zi , t i ) T A θ ( Zi+1 , t i+1 )C T .

State predictor Z Â,

Now, we propose a state predictor of the hidden process Z using optimal control theory and then we plug it in the statistical criteria.

Formalization of the related optimal control problem

In this section, we denote ( Z (Z 0 , u)) the solution of (4) for a given sequence u = (u 0 , . . . , u n-1 ) and a given initial condition Z 0 . Similarly to trajectory fitting estimators [START_REF] Kutoyants | Minimum distance parameter estimation for diffusion type observation[END_REF][START_REF] Dietz | Asymptotic behaviour of trajectory fitting estimators for certain non-ergodic SDE[END_REF]), a first predictor could be the solution Z of (4) which is the closest to the observations. This solution corresponds to the sequence u M Z 0 such that:

u M Z 0 = arg min u n i=0 C Zi (Z 0 , u) -Y i 2 2 (9)
when the initial condition Z 0 is fixed. However, this optimization problem is ill-posed. The solution is not necessarily unique and would not always be continuous as a function of the parameters and observations. We thus propose to introduce a penalization term into the optimization problem which will lead to a Tikhonov regularized version of it. This is known to remove sources of ill-posedness (Engl et al. 2009), in particular it re-establishes uniqueness and the continuity of the solution for linear SDEs. The penalized problem is the following:

u Z 0 := arg min u n i=0 C Zi (Z 0 , u) -Y i 2 2 - 2 w log P(u)
where P(u) is the density of the increment u. The term -2 w log P(u) penalizes the sequences which are unlikely to be a realization of the Brownian motion and w > 0 makes the balance between the model and data fidelity. We have

log P(u) = log n-1 i=0 P(u i ) ∝ log n-1 i=0 e -1 2 u T i u i = - n-1 i=0 1 2 u T i u i thus u Z 0 = arg min u ⎧ ⎨ ⎩ n-1 j=1 C Zi (Z 0 , u) -Y i 2 2 + 1 w u T i u i + C Zn (Z 0 , u) -Y n 2 2 ⎫ ⎬ ⎭ .
The predictor corresponds to the solution u Z 0 of the following deterministic discrete optimal control problem:

Minimize

C w (u | Y ; Z 0 ) = n-1 j=1 C Zi (Z 0 , u) -Y i 2 2 + 1 w u T i u i + C Zn (Z 0 , u) -Y n 2 2
Subject to:

⎧ ⎨ ⎩ Zi+1 (Z 0 , u) = A θ,i Zi (Z 0 , u) + Δr θ (t i ) + √ Γ σ ( Zi (Z 0 , u), t i )u i Z0 = Z 0 . ( 10
)
Thus, if we are able to solve problem (10) to derive u Z 0 , we can solve (4) to obtain the related state predictor Z Z 0 := Z (Z 0 , u Z 0 ). However, our optimal control still depends on Z 0 which is potentially unknown. If required, its estimation is bypassed by profiling the cost C w and defining as initial condition estimator:

Z 0 = arg min Z 0 min u C w (u | Y ; Z 0 ) .
Let us now denote u := u Z 0 the corresponding optimal control and Z := Z Z 0 the related state-space predictor.

We propose to solve (10) with numerical control theory methods. For linear models, ( 10) is a Linear-Quadratic problem. The control theory then ensures:

• existence and uniqueness of the solutions u and Z , which are continuous functions of (θ, σ ) and Y , under mild regularity hypothesis on A θ and Γ σ (in particular, Γ σ is not required to be of full rank), • computation of u, Z and, if required, Z 0 by solving a finite difference equation.

For nonlinear models, we use an adaptation of Cimen and Banks (2004b) which substitutes the original problem with a finite sequence of Linear-Quadratic ones to benefit of the points listed above. The linear and nonlinear cases are detailed in the next subsection.

Numerical methods for solving (10)

We have defined our state predictor as solution of a deterministic tracking problem. Several algorithms or tools have been developed to solve this kind of control problem. In this section, θ is fixed at a given value. Then to simplify the notations, we omit it.

Linear models

We consider the case where

A i := A θ,i = I d + A θ ( Zi , t i ) := I d + A θ (t i ) and Γ (t i ) := Γ σ Zi , t i for i = 1, . . . , n.
In this framework u Z 0 is derived by solving a finite difference equation known as Riccati equation defined by

E n = C T C h n = -C T Y n
and for i = n -1, . . . , 1:

E i = A T i E i+1 A i + C T C -A T i E i+1 Γ (t i )G(E i+1 )Γ (t i ) T E i+1 A i h i = Δ i A T i E i+1 r (t i ) + A T i h i+1 -C T Y i -A T i E i+1 Γ (t i )G(E i+1 )Γ (t i ) T (h i+1 + Δ i E i+1 r (t i )) (11) with G(E i+1 ) = 1 w × I d + Γ (t i ) T E i+1 Γ (t i ) -1
. Then u Z 0 ,i is given by:

u Z 0 ,i = - G(R k+1 )Γ (t i ) T E i+1 A i Z i + Δ i r (t i ) + h i+1 (12)
and u Z 0 ,i is used in (1) to obtain Z Z 0 . Moreover, the initial condition estimator Z 0 is given by Z 0 = -(E 0 ) -1 h 0 . Computational details are given in Appendix 2.

Nonlinear models

For non-linear model, we propose to apply the previous algorithm thanks to the pseudo-linear representation. We replace the original problem by a sequence of Linear-Quadratic control problems solved iteratively until a convergence criteria is verified. This method is an adaptation of Cimen and Banks (2004b) to the case of discrete models. We propose the following algorithm to solve (10).

1. Initialisation ∀i ∈ 0, n , Z 0 i = Z r 0 where Z r 0 is an arbitrary starting point if Z 0 is unknown or Z 0 0 = Z 0 otherwise. 2. At iteration l, compute Z l , u l by solving the Linear-Quadratic optimal control problem derived from the pseudo-linear representation of the SDE:

Minimize

C l w (u | Y ; Z 0 ) = n-1 j=1 C Zi (Z 0 , u) -Y i 2 2 + 1 w u T i u i + C Zn (Z 0 , u) -Y n 2 2
Subject to:

Zi+1 (Z 0 , u) = A l θ,i Zi (Z 0 , u) + Δr θ (t i ) + √ Γ l σ (t i )u i Z0 = Z 0 with A l i = I d + A(Z l-1 i , t i ), Γ l (t i ) := Γ Z l-1
i , t i and Z l-1 the state variable corresponding to the optimal control u l-1 , the minimizer of the cost C l-1 w (u | Y ; Z 0 ).

If

n i=0 Z l i -Z l-1 i 2 2
< ε then stop otherwise go back to step 2.

4. Use Z Z l as state variable predictor.

The interest of this algorithm is that at each iteration l, the optimal control problem of step 2 can be solved using Linear-Quadratic theory which ensures:

• the existence and uniqueness of the solution problem u l , • that u l and the corresponding state predictor Z l can be computed by solving the Riccati equation ( 11) with

A l i := I d + A θ (Z l-1 i , t i ) and Γ l (t i ) := Γ Z l-1 i , t i .

Selection of weights w

A data driven selection method for w needs to be specified. Let us denote u w the solution of the control problem (10) obtained for a given weight value w and the corresponding estimator ψ = θ, σ . The sequence u w = (u w,0 , . . . u w,n-1 )

is supposed to mimic increments of a Brownian motion. So ideally u w,i

2 2 ∼ χ 2 (d U ) with χ 2 (d U ) the χ 2 distribution with d U degrees of freedom. Thus the i.i.d sequence ( u w,0 2 2 , . . . , u w,n-1 2 2 ) has ideally a density proportional to i u w,i 2 d U 2 -1 2 e -u w,i 2 2 /2 .
Based on that, we choose the optimal weight w among a set of values W which maximizes the external criteria:

K (w) = i u w,i 2 2 d U 2 -1 e -u w,i 2 2 /2 . ( 13
)
There are some similarities between our method and generalized Profiling [START_REF] Ramsay | Parameter estimation for differential equations: a generalized smoothing approach[END_REF]). In both cases, estimation is based on a nested optimization procedure. 1/ The hyperparameter w balancing the model and data fidelity is chosen via the minimization of an outer criteria K given by ( 13). 2/ For a given w, the structural parameter ψ is estimated by minimizing the middle criteria H m B given by ( 8). 3/For a given set (w, ψ), a state variable predictor is computed by optimizing the inner criteria C w given by (10).

Summary of the estimation procedure

We summarize the method with a pseudo-algorithm formalism:

• Outer criteria: estimation of optimal weight w defined by: w := arg min w∈W K (w).

• Middle criteria: estimate ψ of ψ defined by:

ψ := arg min ψ H m B (ψ | Y , Z ψ ).
• Inner criteria: compute state predictor Z ψ via algorithm presented in Sect. 4.2.1 for linear SDEs or Sect. 4.2.2 for nonlinear ones.

Simulation study

Experimental design for the simulations

A simulation study is conducted to analyze the practical accuracy of our method and its computational cost on three partially observed hypoelliptic systems, one linear and two non-linear ones. Each system is observed on an interval [0, T ] and is sampled at n times uniformly at every T (n-1) time points with Euler-Maruyama scheme. Three different values for the set (T , n) are tested to quantify the effects of the interval length T and the sample size n on estimation accuracy. Estimation results are given in terms of empirical bias and variance computed after Monte-Carlo simulations based on N MC = 1000 trials. The sample size has an important impact on the computational efficiency of the method. Thus, we also give the mean computational time for a given w.

Examples

Monotone cyclic feedback system

We consider a neuronal monotone cyclic feedback system proposed in [START_REF] Ditlevsen | Multi-class oscillating systems of interacting neurons[END_REF]. This model describes the oscillatory behavior of a system of three populations of neurons in interaction:

d X 1 (t) = (-ν X 1 (t) + X 2 (t)) dt d X 2 (t) = (-ν X 2 (t) + X 3 (t)) dt d X 3 (t) = -ν X 3 (t)dt + cdW t (14)
where X 1 (t), X 2 (t) and X 3 (t) are the limit dynamics of each population.

We consider partial observations with C = (1 0 0). The model is defined by

g θ (x 1 , x 2 , x 3 ) = -νx 1 + x 2 -νx 2 + x 3 and h θ (x 1 , x 2 , x 3 ) = -νx 3 , A θ (t) = ⎛ ⎝ -ν 1 0 0 -ν 1 0 0 -ν ⎞ ⎠ , r θ (t) = (0 0 0) T and Γ σ (t) = (0 0 c) T . The system is hypoelliptic.
Applying the generation step of the weak Hörmander condition gives

L 0 = (0 0, c) T , L 1 = [ f θ (Z ), L 0 ] T = (0 -c νc) T and L 2 = [ f θ (Z ), L 1 ] T = c 0 νc 2 T . Matrix (L 0 L 1 L 2 ) spans R 3 .
Interestingly the need for a second iteration to ensure hypoellipticity is mimicked by our connexity condition (2) which needs the auxiliary variable x 2 to verify

∂ g θ,1 ∂ x 2 ∂ g θ,2 ∂ x 3 = 1 = 0.
Let us now detail assumption (H1). For m B = 2, we have G i+2:i+m B = 0, G i+1:i+m B = 0 and G i:i+m B = √ c. Thus m B = 2 implies hypothesis (H1) and X i:i+m B only depends on u i . So the estimator is defined as the minimizer of H m B given by equation (8).

A thousand simulations are performed with initial conditions (X 1 (0), X 2 (0), X 3 (0)) = (0, 0, 0) and true parameter values set to ν = 0.2 and c = 0.15. Figure 1 As said before, the estimation of (ν, c) is made from the observation of X 1 only. The initial condition are considered known. We choose w among W = 10 15 , 10 20 , 10 25 , 10 30 . Results are given in Table 1 for different values of T and n: (T , n) = (10, 10 3 ), (100, 10 3 ), (10, 10 4 ). The estimators have a good precision which increases with T and mesh refinement. Also, we point out the computation time is expressed in terms of second for the case n = 10 3 where it was expressed in terms of hours in [START_REF] Clairon | Optimal control for estimation in partially observed elliptic and hypoelliptic linear stochastic differential equations[END_REF] for an equivalently complex linear model (see Harmonic Oscillator model, section 6.2.1). This is due to the complexity of our method which repeatedly solves the control problem to obtain our state variable predictor. It requires to integrate a sequence of finite difference equations of length n. This explains why the computational time of our method increases with n.

Hypoelliptic FitzHugh-Nagumo model

We consider now the hypoelliptic neuronal model, which is a minimal representation of a spiking neuron model such as the Hodgkin-Huxley model [START_REF] Leon | Hypoelliptic stochastic FitzHugh-Nagumo neuronal model: mixing, up-crossing and estimation of the spike rate[END_REF]. It is defined as:

dV t = 1 ε (V t -V 3 t -U t + s)dt dU t = (γ V t -U t + β)dt + σ dW t ( 15
)
where the variable V t represents the membrane potential of a neuron at time t, and U t is a recovery variable, which could represent channel kinetics. Parameter s is the magnitude of the stimulus current. Often s represents injected current and is thus controlled in a given experiment. It is therefore assumed known and set to s = 0. We consider partial observations with C = (1 0). The model is non-linear. Several choices for A θ (Z t , t) are possible, we take Estimation of parameters from 1000 simulated trajectories (mean and variance) and mean computational time for a given w model is a 1-step hypoelliptic system and we can use the corresponding simplified expression of H m B for the parameter estimation.

A θ (Z t , t) = (1 -V 2 t )/ε -1/ε γ -1 and r θ (t) = 0 β . Since Γ σ (t) = (0 σ ) T , we got CΓ σ = 0, ∂ ∂U ( 1 ε (V t -V 3 t -U t )) = -1 = 0 and C A θ ( Zi+1 , t i+1 )Γ σ ( Zi , t i ) = -σ/ε = 0. As described in Sect. 3.2, the
A thousand simulations are performed with initial conditions set to (V 0 , U 0 ) = (0, 0) and true parameter values (ε, γ , β) = (0.1, 1.5, 0.8) and σ = 0.3. Figure 2 illustrates a simulation on the observation interval [0, T ] = [0, 20] .

The estimation is made from the observation of V only and the initial conditions are considered unknown and need to be estimated. We choose w among W = 10 16 , 10 18 , 10 20 , 10 25 .

Results are given in Table 2 for (T , n) = (1, 10 3 ), (10, 10 3 ), (1, 10 4 ). As in the previous example, we observe the bias and variance decreasing with T and n. Regarding the computation time and comparative accuracy with other methods, we recall in Table 3, the results obtained for the case T = 10, n = 10 3 by [START_REF] Clairon | Optimal control for estimation in partially observed elliptic and hypoelliptic linear stochastic differential equations[END_REF] and [START_REF] Ditlevsen | Hypoelliptic diffusions: discretization, filtering and inference from complete and partial observations[END_REF] (they are originally presented in [START_REF] Clairon | Optimal control for estimation in partially observed elliptic and hypoelliptic linear stochastic differential equations[END_REF] table 4, section 6.2.2). We obtain estimation with equivalent or higher accuracy and always with significantly reduced computational cost.

Synaptic-conductance model

We consider the conductance-based model with diffusion synaptic input defined in [START_REF] Ditlevsen | Hypoelliptic diffusions: discretization, filtering and inference from complete and partial observations[END_REF]. It describes the voltage dynamics accross the membrane Estimation results for the case T = 10, n = 10 3 with our method (first row), [START_REF] Clairon | Optimal control for estimation in partially observed elliptic and hypoelliptic linear stochastic differential equations[END_REF] (second row), [START_REF] Ditlevsen | Hypoelliptic diffusions: discretization, filtering and inference from complete and partial observations[END_REF] third row of a neuron:

C c dV t = (-G L (V t -V L ) -G E,t (V t -V E ) -G I ,t (V t -V I ) + I inj )dt dG E,t = -1 τ E (G E,t -g E ) + σ E G E,t dW E,t dG I ,t = -1 τ I (G I ,t -g I ) + σ I G I ,t dW I ,t (16)
where C c is the total capacitance, G L , G E and G I are the leak, excitation and inhibition conductances, V L , V E and V I are their respective reversal potentials, and I inj is the injected current. The conductances G E,t and G I ,t are assumed to be stochastic functions of time, where W E,t and W I ,t are two independent Brownian motions. The square root in the diffusion coefficient ensures that the conductances stay positive.

Parameters τ E , τ I are time constants, g E , g I are the mean conductances and σ E , σ I the diffusion coefficients. Here U t = (G E,t , G I ,t ). We assume to know the capacitance, the reversal potentials, and the injected current. We consider partial observations with only component V t observed which corresponds to the observation matrix C = (1 0 0). For the pseudo-linear representation,

we choose A θ (Z t , t) = ⎛ ⎝ -G L /C c -(V t -V E )/C c -(V t -V I )/C c 0 -1/τ E 0 0 0 -1/τ I ⎞ ⎠ , r θ (t) = ⎛ ⎝ G L V L + I inj /C c g E /τ E g I /τ I ⎞ ⎠ . Since g θ (V , G E , G I ) = (-G L (V -V L ) -G E (V -V E ) - G I (V -V I )+ I inj )/C c , C = (1 0 0) and Γ σ (Z t , t) = 0 σ E G E,t 0 0 0 σ I G I ,t T , we got CΓ σ = (0 0), ∂ g θ ∂(G E ,G I ) (V , G E , G I ) is of full rank and C A θ ( Zi+1 , t i+1 )Γ σ ( Zi , t i ) = 0.
It is a 1-step hypoelliptic system and enters into the framework described in Sect. (3.2).

A thousand simulations are performed with initial conditions set to V 0 , G E,0 , G I ,0 = (-60, 10, 1) and true parameters values set to G L , V L , V E , V I , I inj , g E = (50, -70, 0, -80, -60, 17.8), (τ E , τ I , g I ) = (0.5, 1, 9.4) and (σ E , σ I ) = (0.1, 0.1). The estimation of (τ E , τ I , g I , σ E , σ I ) is made from the observation of V only. For the sake of identifiability, the initial conditions are assumed known. We choose w among W = 10 8 , 5 × 10 8 , 10 9 , 5 × 10 9 .

123 Results are given in Table 4 for (T , n) = (20, 10 3 ), (200, 10 3 ), (20, 10 4 ). We observe a difference in terms of accuracy between the estimation of θ = (τ E , τ I , g I ) and σ = (σ E , σ I ), the latter being more biased than the former. This is understandable and already noticed in [START_REF] Ditlevsen | Hypoelliptic diffusions: discretization, filtering and inference from complete and partial observations[END_REF]. Indeed we have to estimate the diffusion of two sources of stochastic disturbance from only one resulting signal. This estimation problem is more complex than the previous ones as the number of parameters to estimate is higher and the ratio of observed/unobserved states is also higher. This explains the higher computational time for the case T = 20, n = 10 4 .

Discussion

In this work, we propose a new estimation method which gives an unifying framework for elliptic and hypoelliptic systems, partially or fully observed. For this, we rely on a lagged discretization of the original SDE which lets enough time to the stochastic perturbations to affect all the state variables. By doing so, we have constructed a statistical criteria based on a well-defined density even for partially observed hypoelliptic SDEs. This criteria requires a state variable predictor obtained by solving a control problem balancing data and model fidelity. The numerical procedures used to solve it do not require B σ (Z t , t) to be of full rank and so are well adapted to hypoelliptic systems. Because of this, the deterministic control perspective constitutes a relevant alternative to MCMC approaches and explain the reasonable computational cost of our method. It only provides the pointwise estimator Z we need for ψ estimation and does not aim to reconstruct its whole distribution. Now, we conclude this work by presenting refinements and extensions of the presented method which will be investigated in the future.

Our method requires to select an hyperparameter w. To bypass this, we aim to define in future works our predictor u as a maximum a posteriori estimator (MAP) in a functional space. For this, we will rely on the work of [START_REF] Dashti | Map estimators and their consistency in Bayesian nonparametric inverse problems[END_REF] in which the MAP is defined as the solution of a new deterministic optimal control problem where our regularization term 2 w ln P(u) is replaced by u E . Here, E denotes the so-called Cameron-Martin space which is totally determined by the covariance operator of the Brownian motion u and does not involve a nuisance parameter.

Also, we think our method is well suited for generalization to SDEs driven by processes W t different from the Wiener ones. All it requires would be to know their densities to modify H m B accordingly and the penalization term appearing in the control problem. Interestingly in this general setting, to a given type of SDE will be associated a given type of deterministic optimal control problem.

We end this work by going one step further about the connection between our estimation problem and control theory by pointing out its similarity with the issue of structural assessment of a system controllability as exposed in Daoutidis and Kravaris (1992). Their problem is the following: given an ODE ẋ = f (x) + Bu, can we control the behavior of some system outputs y i = h i (x) knowing that u = u j j∈ 1,d U only affects directly a subset of x? This leads to define in a similar way as our m l 's the integers r i j , named relative orders, quantifying the sluggishness of y i response to u j variation. Then, conditions for controllability are formulated via the non-singularity of a so-called characteristic matrix C(x) constructed from the r i j 's and Lie derivative in a way mirroring the matrix M appearing in the proof of proposition 1. From this, we hope to derive m B in a less exploratory manner directly from a given model structure and make explicit its link with the required number of iteration of generalization step 2 to fulfill Hörmander condition.

  illustrates a simulation on the observation interval [0, T ] = [0, 20] .

Fig. 1

 1 Fig.1Simulated trajectory of the neuronal monotone cyclic feedback system (14) with parameters ν = 0.2 and c = 0.15. X 1 : Solid line, X 2 : dashed line, X 3 : dotted line

Fig. 2

 2 Fig. 2 Simulated trajectory of the neuronal hypoelliptic FitzHugh-Nagumo model (15) with parameters (ε, γ , β) = (0.1, 1.5, 0.8) and σ = 0.3. V : solid line, U : dashed line

Table 1

 1 Cyclic feedback model

		ν	c	Comp time
	True value	0.2	0.15
	T = 10, n = 10 3	0.23 (5e-3)	0.14 (1e-5)	9 s
	T = 100, n = 10 3	0.21 (6e-4)	0.15 (1e-6)	19 s
	T = 10, n = 10 4	0.22 (2e-3)	0.15 (1e-6)	2 min 50 s
	Estimation of parameters from 1000 simulated trajectories (mean and variance) and mean computational
	time for a given w

Table 2

 2 Hypoelliptic FitzHugh-Nagumo model

		ε	γ	β	σ	Comp time
	True value	0.1	1.5	0.8	0.3
	T = 1, n = 10 3	0.11 (2e-3)	2.13 (1e-1)	1.18 (3e-1)	0.32 (2e-2)	1 min 10 s
	T = 10, n = 10 3	0.09 (2e-5)	1.58 (6e-2)	0.87 (5e-2)	0.29 (2e-4)	3 min 15 s
	T = 1, n = 10 4	0.10 (4e-4)	1.90 (8e-2)	1.10 (2e-1)	0.31 (3e-3)	8 min 30 s

Table 3

 3 Hypoelliptic FitzHugh-Nagumo model

		ε	γ	β	σ	Comp time
	True value	0.1	1.5	0.8	0.3
	ψ	0.09 (2e-5)	1.58 (6e-2)	0.87 (5e-2)	0.29 (2e-4)	3 min 15 s
	Clairon and Samson	0.09 (1e-4)	1.27 (2e-2)	0.65 (1e-2)	0.31 (4e-3)	45 min
	Ditlevsen and Samson	0.10 (1e-4)	1.59 (2e-2)	0.87 (2e-2)	0.31 (4e-4)	6 h 00 min

Table 4

 4 Synaptic-conductance model

		τ E	τ I	g I	σ E	σ I	Comp time
	True value	0.5	1	9.4	0.1	0.1
	T = 20, n = 10 3	0.51 (2e-3) 1.11 (0.04) 9.45 (0.02)	0.05 (3e-4) 0.15 (1e-3) 1 min 40 s
	T = 200, n = 10 3 0.47 (4e-4) 1.11 (0.01) 9.41 (2e-3) 0.09 (7e-6) 0.14 (8e-5) 2 min 00 s
	T = 20, n = 10 4	0.52 (2e-3) 1.08 (0.04) 9.40 (0.02)	0.11 (4e-5) 0.07 (1e-3) 1 h 40 min
	Estimation of parameters from 1000 simulated trajectories (mean and variance) and mean computational
	time for a given w

CNRS, Inria, Grenoble Institute of Engineering, LJK, Univ. Grenoble Alpes,

Grenoble, France

Acknowledgements Quentin Clairon gratefully acknowledges the Innovative Medicines Initiative 2 Joint Undertaking under projects EBOVAC1 and EBOVAC3 (respectively Grant Agreement Nos. 115854 and 800176). The IMI2 Joint Undertaking receives support from the European Union's Horizon 2020 research and innovation programme and the European Federation of Pharmaceutical Industries and Association. Adeline Samson gratefully acknowledges the LabEx PERSYVAL-Lab (ANR-11-LABX-0025-01), MIAI@Grenoble Alpes, (ANR-19-P3IA-0003) and the Grant ANR ChaMaNe, ANR-19-CE40-0024.

Code availability Our estimation method is implemented in R and a code reproducing the examples of Sect. 5 is available on a GitHub repository located https://github.com/QuentinClairon/SDE_estimation_via_optimal _control.git here.

Supplementary information A supplementary file containing the proofs of propositions 1 and 2 and the derivation of the algorithm presented in Sect. 4.2.1 is available alongside this article.

Declarations

Conflict of interest The authors have no conflicts of interest to declare that are relevant to the content of this article.

Publisher's Note Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.


domain: spi info math phys

text:
Clément Chahbazian

Karim Dahia
email: karim.dahia@onera.fr

Nicolas Merlinge

Bénedicte Winter-Bonnet

Kévin Honore
email: kevin.honore@mbda-systems.com

Christian Musso
email: christian.musso@onera.fr

Improved Kalman-Particle Kernel Filter on Lie Groups Applied to Angles-Only UAV Navigation

Kalman-Particle Kernel Filter (KPKF) is a sub-class of Particle Filter (PF) that uses Gaussian kernels as particles, which enables a local Kalman update for each measurement in addition to the usual weight update. Besides, recent research about filtering on Lie groups brought powerful theoretical results, and showed the superiority of this approach. Hence, this paper extends the Euclidean KPKF to a new formulation on Lie groups and introduces substantial improvements based on Lie groups Kalman filters theory and Laplace Particle Filters on Lie groups (LG-LPF) for improved resampling. The proposed algorithm is tested on an angles-only UAV navigation scenario with challenging initial errors. It shows superior robustness and accuracy compared to Lie group Extended Kalman Filter (LG-EKF), with near-to optimal performance, even with a limited amount of particles.

I. INTRODUCTION

Nonlinear state estimation is an active field of research in robotics. It consists of fusing data from multiple sensors to obtain the evolution of variables of interest, aiming to minimize the impact of sensor flaws. A general formulation of this problem is the optimal filter, described by the Chapman-Kolmogorov equation and the Bayes rule. Sampling-based approaches such as Particle Filters (PF) are suitable to solve the optimal filter in non-Gaussian and highly nonlinear cases [START_REF] Gordon | Novel approach to nonlinear/non-Gaussian Bayesian state estimation[END_REF], whereas algorithms based on the Extended Kalman Filter (EKF) or Unscented Kalman Filters (UKF) are popular choices for Gaussian cases. Kalman-Particle Kernel Filter (KPKF) principle is to use weighted Gaussian kernels as particles [START_REF] Dahia | Application of the kalman-particle kernel filter to the updated inertial navigation system[END_REF]. Thus, the particles can be propagated and updated using EKF equations, and another step adjusts their weights with respect to the measurement likelihood. To cope with the EKF framework, each particle covariance is chosen small enough to assume that the system is locally Gaussian [START_REF] Dahia | Application of the kalman-particle kernel filter to the updated inertial navigation system[END_REF]. However, for particle filters in general, particles weights degenerate after a few updates. Thus, a resampling step is often triggered to select a new set of particles. In its original implementation, KPKF showed improved accuracy and robustness compared to classic PF, as it provides better coverage of the estimated posterior density and requires fewer resampling steps [START_REF] Pham | A Kalman-particle kernel filter and its application to terrain navigation[END_REF] [START_REF] Dahia | Application of the kalman-particle kernel filter to the updated inertial navigation system[END_REF].

outperformed their Euclidean counterparts on many aspects including robustness, accuracy and computational load [START_REF] Bourmaud | Discrete Extended Kalman Filter on Lie groups[END_REF] [5] [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF]. Besides, the Invariant Extended Kalman Filter (IEKF) demonstrated original stability and observability properties that are beyond the reach of Euclidean EKF [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF]. In addition, the novel Laplace Particle Filter (LPF) [START_REF] Musso | Introducing the Laplace approximation in particle filtering[END_REF] significantly enhanced particle filters resampling step, as it accounts for the measurement likelihood to select new particles. A Lie group version of the LPF (LG-LPF) was lately established in [START_REF] Chahbazian | The Laplace particle filter on Lie groups applied to angles-only navigation[END_REF] showing the interest of Lie groups in stochastic filtering. This paper introduces the Lie groups Laplace KPKF (LG-KPKF) which uses powerful results from IEKF theory, and accurate resampling from LG-LPF. In particular, the propagation step uses invariance properties introduced in the IEKF theory [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF] for fast and accurate particles propagation. The KPKF Kalman update [START_REF] Pham | A Kalman-particle kernel filter and its application to terrain navigation[END_REF] is performed with an extended kalman filter on the Lie group (LG-EKF), leading to improved precision and robustness. Finally, the KPKF particle update uses the Laplace method on Lie group for a close-to-optimal resampling. In the sequel, section II states the estimation problem and provides basic definitions about Lie groups, alongside specific properties of IEKF. Then, section III details the improved LG-KPKF algorithm, which is the main contribution of this paper. Finally, this filter is tested in section IV on the simulated optimal navigation of a fixed-wing drone with angles-only measurements and section V concludes the paper.

II. PROBLEM STATEMENT

A. Introduction to Lie groups 1) Preliminary definitions: A Lie group (G, •) is a differential manifold endowed with a group structure [START_REF] Hilgert | Structure and Geometry of Lie Groups[END_REF]. It admits a tangent space at its identity point, which is called the Lie algebra and denoted g. Besides, in the case of matrix Lie groups, there exist two bijective maps at the vicinity of I d , namely the group exponential exp G : g → G and the group logarithm log G : G → g, whose expressions reduce to the following matrix power series [START_REF] Hilgert | Structure and Geometry of Lie Groups[END_REF]:

exp G (X) = ∞ k=0 X k k! ; log G (X) = ∞ k=1 (-1) k+1 k (X -Id) k .
(1) Since g is a linear space, there exist linear mappings:

[•]

∧ : R d → g, and

[•] ∨ : g → R d , (2)
and their compositions with exp G and log G are denoted: A summary of these mappings is provided in Figure 1.

exp G ([•] ∧ ) = exp ∧ G (•) ; log G ([•] ∨ ) = log ∨ G (•). (3)
2) Group errors: Let (X, X) ∈ (G, •) two state matrices. The group error between X and X can be defined on the right η R = X • X -1 and the left η L = X -1 • X as the group law does not commute. Besides, the representation of the (left or right) group error η in the Euclidean space denoted ε is defined as:

ε = log ∨ G (η). (4)
3) Uncertainties on Lie groups: Let ∼ ϕ(0, P ) be a centred random vector following the probability law ϕ of covariance P , and µ ∈ G a matrix. A random matrix X ∈ G follows a left concentrated distribution of mean µ and covariance P on G if:

X ∼ ϕ L G (X; µ, P ) ; X = µ • exp ∧ G ( ). (5)
This definition holds when the density is concentrated around its mean, that is to say, all eigenvalues of P are small enough [START_REF] Chirikjian | Gaussian approximation of non-linear measurement models on Lie groups[END_REF] to stay in the bijective area of exp G . For the sake of brevity, the sequel only describes the left distribution case. Adaptation to the right distribution case is possible with minor adjustments. Let X ∼ N L G (µ, P ) a left random variable on a Lie group, and || • || P the Mahalanobis norm with respect to P . The probability density function of a concentrated Gaussian distribution on a Lie group writes [START_REF] Bourmaud | Estimation de pramètres évoluant sur des groupes de Lie : application la cartographie et la loalisation d'une caméra monoculaire[END_REF] [17]:

p G (X) ≈ 1 (2π) d det [P ] e -1 2 || log ∨ G (µ -1 X)|| 2 P . (6)
B. State Estimation 1) Stochastic Filtering Scheme: Let the discrete-time hidden state describing a Markov process {X k } k∈N ∈ G, where G is a Lie group, according to a set of observations {y k } k∈N ∈ G , all mutually independent given X k :

X k+1 = f (X k , n q,k ), y k+1 = h(X k+1 , n r,k+1 ), (7)
where (n q,k , n r,k ) are noise vectors, G is the Lie group of the measurement, and (f, h) two possibly nonlinear smooth mappings. The filtering problem lies in the estimation of the posterior density p(X k |y 1:k ), where p(X 0 ) is known, and

y 1:k = [y 1 , ... , y k ].
First, the state density is propagated using the Chapman-Kolmogorov equation:

p(X k+1 |y 1:k ) = p(X k+1 |X k )p(X k |y 1:k )dX k . (8)
Then, an update step computes the posterior density based on the Bayes rule:

p(X k+1 |y 1:k+1 ) = p(y k+1 |X k+1 )p(X k+1 |y 1:k ) p(y k+1 |X k+1 )p(X k+1 |y 1:k )dX k+1
.

(9) Equations ( 8) and ( 9) are referred to as the optimal filter.

2) Particle kernel filters: An interesting approach to solve the optimal filter is to represent the probability densities with a set of kernels. Different methods address this problem [START_REF] Pham | A Kalman-particle kernel filter and its application to terrain navigation[END_REF] [10] [START_REF] Merlinge | A Box Regularized Particle Filter for state estimation with severely ambiguous and non-linear measurements[END_REF]. This paper focuses on Gaussian kernels, as shown in Figure [START_REF] Dahia | Application of the kalman-particle kernel filter to the updated inertial navigation system[END_REF]. The mixture of N p Gaussian kernels denoted ϕ writes:

p(X k ) ≈ Np i=1 w i k ϕ(X k ; X i k , P i k ), (10)
When a new measurement is available, the weights are updated using the likelihood density:

w i k+1 ∝ w i k p(y k+1 |X i k+1|k ) , i ∈ [1, N p ]. (11)
The weights degeneracy phenomenon, which occurs after a few updates, is monitored using the criterion [START_REF] Kong | Sequential Imputations and Bayesian Missing Data Problems[END_REF]:

N eff = 1 Np i=1 (w i ) 2 < N th . (12)
When N eff goes below a given threshold N th = θ.N p where θ ∈ (0, 1), the particles are considered degenerated [START_REF] Arulampalam | A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking[END_REF] and a resampling step is triggered. 3) Laplace Resampling: The resampling step is critical for the accuracy and robustness of the filter. The weak point of most resampling strategies lies in the selection criteria for the new particles, which mostly account for the prior density. The idea developed in Laplace Particle Filter (LPF) [START_REF] Quang | Particle filtering and the Laplace method for target tracking[END_REF] is to harness measurement likelihood to select an importance function q, which improves accuracy and robustness. Indeed, the optimal importance sampling approximation is obtained for qopt = p(x k+1 |y 1:k+1 ), which is the density to be estimated [START_REF] Musso | Introducing the Laplace approximation in particle filtering[END_REF]. To that extent, the Laplace resampling method uses the conditional expectancy x L ≈ E [x k+1 |y 1:k+1 ] and variance (J L ) -1 ≈ V [x k+1 |y 1:k+1 ] of p(x k+1 |y 1:k+1 ) as the mean and covariance of q. It ensures the latter to be close to p. Then, a new sample is drawn according to the importance density q(x; x L , (J L ) -1 ) [START_REF] Quang | Particle filtering and the Laplace method for target tracking[END_REF].

C. Log-linear error

The log-linear error property applies to a specific class of nonlinear systems from which are derived the Invariant Extended Kalman Filter (IEKF). Let X ∈ G a Gaussian state matrix following [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF], with the further assumption that f verifies:

∀X 1 , X 2 ∈ G : f (X 1 • X 2 ) = f (X 1 ) • f (I d ) -1 f (X 2 ). (13)
Let X be the variable representing the true state trajectory. The left error on the Lie group G is η L k = X -1 k X k , thus the corresponding error on the Euclidean space is

ε k = log ∨ G (η L k ).
Then, [START_REF] Barrau | Linear observed systems on groups[END_REF] shows that η L follows an autonomous evolution with respect to the estimated state, i.e. there exists a function a k : G → G such that:

η L k+1|k = a k (η L k ). (14)
In addition, ε follows a linear and exact evolution during the propagation:

∃A k : ε k+1|k = A k ε k , (15)
in which A k is a square matrix independent of X k . Thus, the propagated covariance is independent of the estimated state. This unique property in non-linear filtering is at the gist of the IEKF theory and one of the keys to its stability properties.

Note that [START_REF] Barrau | Linear observed systems on groups[END_REF] proves that it is only applicable to the class of Gaussian systems on Lie groups with a dynamical model verifying [START_REF] Arulampalam | A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking[END_REF].

III. THE KALMAN-PARTICLE KERNEL FILTER ON LIE GROUPS

This section aims to derive a generic formulation of a Kalman-Particle Kernel Filter on Lie groups (LG-KPKF) with invariant propagation. Also, it introduces the Laplace method on Lie groups for KPKF. These are the main contributions of this paper. The core idea of a KPKF [START_REF] Pham | A Kalman-particle kernel filter and its application to terrain navigation[END_REF] is to solve (8) and ( 9) with a mixture of Gaussian kernels as shown in Figure [START_REF] Dahia | Application of the kalman-particle kernel filter to the updated inertial navigation system[END_REF]:

p(X k ) ≈ Np i=1 w i k ϕ G (X k ; X i k , P i k ), (16)
where

X i k ∈ G, ∀i ∈ [1, N p ] and ϕ G (X k ; X i k , P i k
) is a concentrated distribution on G as defined in [START_REF] Snoussi | Particle Filtering on Riemannian Manifolds[END_REF] which follows the state model of [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF]. This approach keeps the genericity of the particle filter, and benefits from the accuracy of the Kalman filter. Besides, the particles cover a broader area of the state space and their means are updated at each measurement, whereas only the weights are updated in classic particle filters. The sequel details the steps of the Lie groups KPKF (LG-KPKF) which is described Algorithm 1.

1) Sampling: The particles means are sampled around the posterior mean µ ∈ G according to [START_REF] Snoussi | Particle Filtering on Riemannian Manifolds[END_REF]. Then, their covariances are scaled with respect to the posterior covariance such that P i = h 2 1+h 2 P where h is the bandwidth [START_REF] Green | Density Estimation for Statistics and Data Analysis[END_REF].

2) Propagation: Since the particles are Gaussian kernels, their propagation is similar to the one of a local EKF on G:

X i k+1|k = f (X i k ), P i k+1|k = F i k P i k F i k T + Q k . (17)
In the general case, the matrix F i k must be computed for each particle according to [START_REF] Bourmaud | Discrete Extended Kalman Filter on Lie groups[END_REF]. However, when (13) holds, the error log-linearity described in paragraph II-C implies that F i k does not depend on X i k , and since the particles follow the same determinisitc dynamical model:

∀i ∈ [1, N p ] : F i k = F k . ( 18
)
When applicable, this property spares the computation of N p -1 Jacobians at each propagation and with increased precision as the computation of F k is exact.

3) Kalman Update: The Kalman update consists in performing an extended Kalman filter update on each particle when a new measurement y ∈ G is available. In the general case, the framework described in [START_REF] Bourmaud | Discrete Extended Kalman Filter on Lie groups[END_REF] is applied:

           K i k+1 = P i k+1|k H i,T k+1 H i k+1 P i k+1|k H i,T k+1 + R k+1 -1 , m i k+1 = K i k+1 log ∨ G (h(X i k+1|k ) -1 y), X i k+1 = X i k+1|k • exp ∧ G (m i k+1 ), P i k+1 = Φ G (m i k+1 )(I d -K i k+1 H i k+1 )P i k+1|k Φ T G (m i k+1 ). (19)
Where Φ G is the group exponential Jacobian defined as:

Φ G (x) = ∂ exp ∧ G ( ) ∂ =x , (20)
and H i k+1 is the functional Jacobian of h, computed for each particle:

H i k+1 = ∂h(X i k+1|k exp ∧ G (ε))y -1 ∂ε ε=0 . ( 21
)
4) Particle Update: The PF update enables the mixture to be consistent with respect to the posterior density. It consists in a weight update for each particle using the likelihood:

w i k+1 ∝ w i k p(y k+1 |X i k+1 ). (22)
5) Gaussian Laplace Resampling: This resampling step occurs when the criteria ( 12) is triggered. The Laplace resampling algorithm on Lie groups fits a concentrated Gaussian on G to the posterior density using a Gauss-Newton algorithm (GN) and uses it as an importance function for resampling [START_REF] Chahbazian | The Laplace particle filter on Lie groups applied to angles-only navigation[END_REF]. The Gaussian assumption suits most practical cases and enables a simplified process to compute the information matrix J * and the Maximum A Posteriori (MAP) on the Lie group, denoted X * . However, if the model is strongly non-Gaussian, the framework detailed in [START_REF] Musso | Introducing the Laplace approximation in particle filtering[END_REF] provides a general method for Laplace resampling. The introduction of a Laplace resampling step for a KPKF is a novelty of this paper.

6) Covariance and mean:

As Lie groups are not a linear space, the computation of the mean is done through a non-linear process described in [START_REF] Chahbazian | The Laplace particle filter on Lie groups applied to angles-only navigation[END_REF]. Also, the covariance of the prior density is required for the Laplace resampling. It is defined with respect to the state error on the Lie algebra and computed as [START_REF] Pham | A Kalman-particle kernel filter and its application to terrain navigation[END_REF]:

P k = Np i=1 w i (P i k + ε i k ε i k T ), (23)
where

ε i k = log ∨ G ( X -1 k X i k )
, and X k is the mean of the posterior density.

Algorithm 1:

The LG-KPKF Result: X 0:T and P 0:T Init particles means: X i 0 ∼ p 0 (X; X 0 , 1 1+h 2 P 0 ) ; Init particles covariances:

P i 0 = h 2 1+h 2 P 0 ; Init weights: sample w i 0 = 1/N p ; Propagation: (X i k+1|k , P i k+1|k ) from (17) Kalman Update: (X i k+1 , P i k+1 ) from (19) PF Update: w i k+1 ∝ w i k • p(y k+1 |X i k+1|k ) if N eff < N th then
Compute prior mean: X k+1|k Compute prior covariance: P k+1|k from (23) Compute: X * k+1 , J * k+1 from the Gauss-Newton Algorithm [START_REF] Chahbazian | The Laplace particle filter on Lie groups applied to angles-only navigation[END_REF] Drawn:

X i,L k+1 ∼ q(X k+1 ; X * k+1 , (J * k+1 ) -1 ), i ∈ [1, N p ] Weights Update: wi k+1 = p(y k+1 |X i,L k+1 )p(X i,L k+1 |y k ) q(X i,L k+1 ) Normalize: w i k+1 = wi k+1 / Np i=1 wi k+1
Covariances Update:

P i k+1 = h 2 w i k+1 ε i k+1 ε iT k+1
where :

ε i k+1 = log ∨ G (X * k+1 ) -1 X i k+1 end
Compute mean: X k+1 Compute covariance: P k+1 from (23)

A first-order approximation on the conditional expectancy and variance often provides sufficient accuracy. Thus this algorithm takes X L ≈ X * and J L ≈ J * . If higher precision is required, [START_REF] Quang | Particle filtering and the Laplace method for target tracking[END_REF] provides the complete framework in the Euclidean case.

IV. APPLICATION TO ANGLES-ONLY NAVIGATION

A. Context

The LG-KPKF is applied to the optimal navigation of an Unmanned Aerial Vehicle (UAV) whose trajectory is shown in Figure 3

       C e b,k+1 = C e b,k exp SO(3) (δtΩ b eb,k ), v e eb,k+1
= v e eb,k+1 + δt(C e b,k f b eb,k + g e ), x e eb,k+1 = x e eb,k+1 + δtv e eb , p e,i n,k+1

= p e,i,n n,k , ∀n ∈ [1, N l ], (24)
where Ω b eb is the skew-symmetric matrix of the rotation rate, and f b eb is the acceleration, both measured by a class-A IMU. An antenna on the UAV measures the angles of arrival with respect to each landmark. The situation is shown in Figure 3, and the angular measurements equations are:

α b n = arctan 2 ∆ b n,y , ∆ b n,x , β b n = arctan 2 ∆ b n,z , (∆ b n,x ) 2 + (∆ b n,y ) 2 , (25)
where

∆ b n = ∆ b n,x ∆ b n,y ∆ b n,z T = C b e (p e n -x e eb )
is the relative distance between a landmark and the UAV resolved in the body frame [b], and arctan 2(y, x) is such that ∀(x, y) = (0, 0):

arctan 2(y, x) =    sign(y) arctan y x x ≤ 0, sign(y) π 2 x = 0, sign(y)(π -arctan y x ) x ≤ 0. (26)

B. Filter's Implementation

To design the filter, the considered Lie group is SE 2+N l (3) in which the state matrix writes:

X = C e b v e eb x e eb [p e n ] n∈[1,N l ] 0 2+N l ,3 I 2+N k (27)
With this group structure, (24) verifies the property [START_REF] Arulampalam | A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking[END_REF]. Thus, the covariance of the particles can be propagated with a single and exact Jacobian F k = exp(δtF k ), where F k is such that:

F k =     -Ω b eb,k 0 0 0 -[f b eb,k ] × -Ω b eb,k 0 0 0 I 3 -Ω b eb,k 0 0 0 0 -I N l ⊗ Ω b eb,k     , (28
) where ⊗ is the Kronecker product, and [u] × denotes the skew-symmetric matrix of the vector u The last element required to implement the filter is the measurement Jacobian of (25), which has to be computed for each particle according to (21):

H n k =   H i k (1) ... H i k (N l )   , (29)
where, ∀n ∈ [1, N l ] : H i k (n) = J n M n such that:

J n =     - ∆ b n,y ρ 2 n ∆ b n,x ρ 2 n 0 ∆ b n,x ∆ b n,z ρ n ||∆ b n || 2 ∆ b n,y ∆ b n,z ρ n ||∆ b n || 2 -ρ n ||∆ b n || 2     , (30)
M n = [∆ b n ] × 0 3 -I 3 0 0:3n-3 I 3 0 3n+4:3N l , ( 31
) with ρ n = (∆ b n,x ) 2 + (∆ b n,y ) 2 .

C. Simulations

The simulations were completed with the parameters detailed in Table I the LG-KPKF introduced in this paper and the LG-EKF of [START_REF] Bourmaud | Discrete Extended Kalman Filter on Lie groups[END_REF] on SE 2+N l (3) with an IEKF propagation. The latter can be directly reproduced using [START_REF] Chirikjian | Gaussian approximation of non-linear measurement models on Lie groups[END_REF] for propagation and [START_REF] Green | Density Estimation for Statistics and Data Analysis[END_REF] for the update step where the jacobians are given in (28) and (21). The filters robustness is evaluated with the percentage of convergent Monte Carlo runs, while their precision can be assessed with the Average RMSE (ARMSE) after convergence. Note that RMSE are only computed on the convergent runs in the sense of (32).

The initial states are taken according to a uniform law centred on the true state, in a ±3σ 0 interval, where σ 0 is the initial standard deviation of the state. A run is considered convergent if the mean position of the state is contained inside the confidence ellipsoid Γ k computed by the Posterior Cramer-Rao Bound (PCRB) [START_REF] Tichavský | Posterior Cramer-Rao bounds for discrete-time nonlinear filtering[END_REF], for the last five measurement iterations as follows:

Γ k = x e be,k |(x e be,k -x e be,k ) T PCRB -1 k (x e be,k -x e be,k ) ≤ κ , (32) where the threshold κ is chosen from the test p(χ 2 (d) ≤ κ 2 ) = 0.99 with d = 9 + 3N l the dimension of the state vector. The RMSE at time k is computed from convergent runs only based on the criterion (32) as:

RMSE e (k) = 1 N conv Nconv m=1 ||e(k)|| 2 2 , (33)
where N conv represents the number of convergent runs, and the error vector e(k) is defined as:

e(k) =      log ∨ SO(3) (C e b,k ) T C e b,k,m v e be,k -v e be,k,m
x e be,k -x e be,k,m

The RMSE of each filter is computed according to (33) and displayed in Figure 4. The ARMSE is computed from (33) over the time period T of N T steps ranging from k ini to k end :

ARMSE T = 1 N T kend k=kini RMSE e (k). (35)
In the later, k ini starts after 60 seconds of simulation, which is sufficiently large to compare the filters after the simulations have converged, and k end is the last point of the trajectory.

D. Results

The simulation results are gathered table II, and the plots of LG-KPKF are compared to LG-EKF for different numbers of particles.

LG

1) Position and Attitude:

The LG-KPKF converges at the vicinity of the PCRB just after the first measurement, whereas LG-EKF takes more time. This is due to the KPKF structure which guides the particles towards the best area (Kalman update) and selects those making sense with respect to the posterior density (PF update and Laplace resampling). Thus, LG-KPKF shows close-to-optimal performance at the very beginning of the estimation process. After the Fig. 4: RMSE and PCRB for the LG-KPKF with 100, 500 and 1000 particles, and the LG-EKF. RMSE are only computed on the convergent runs in the sense of (32). convergence of the filters, LG-KPKF still demonstrates an improved precision. Note that the LG-KPKF is very competitive even with 100 particles, while conventional particle filters require several thousand particles to converge.

2) Velocity: Despite a challenging start (see Figure ( 4)), LG-EKF manages to converge thanks to its intrinsic robustness coming from the invariant propagation and the linearization on the Lie group for the update. On the other hand, LG-KPKF shortly converges toward the PCRB and shows improved accuracy even after convergence.

V. CONCLUSION

This paper describes LG-KPKF, a novel algorithm on Lie groups with several improvements compared to the original method. When applicable, the invariant filtering theory provides a lighter propagation step, and the Laplace method provides an accurate and robust resampling step. Besides, LG-KPKF was applied to an angles-only UAV navigation scenario with challenging initial errors, and shows improved robustness, fast convergence, and close-to-optimal accuracy compared to LG-EKF, even with a limited number of particles. Future work will focus on strategies for multimodal scenarios and different measurement models.

Fig. 1 :

 1 Fig. 1: Illustration of the Lie group structure. The group exponential exp G and logarithm log G define a bijection of G into R d , and the algebra g is the tangent space at I d .

Fig. 2 :

 2 Fig. 2: Illustration of a Gaussian (left) and a Dirac (right) mixture as approximations of p.

  . The ground frame [e] is fixed with respect to the Earth and its axis are pointing East, North, Up at its local position. The frame attached to the vehicle is denoted [b] and its axis are pointing forward, rightward and downward. The aim of the sequel is to estimate the position of the UAV x e eb , its velocity v e eb , and its attitude represented with the rotation matrix C e b . We consider a set of N l landmarks [p e n ] n∈[1,N l ] well initialized in the filter and continuously observed through the flight. The discretized kinematics equations give, with a time-step δt:

Fig. 3 :

 3 Fig. 3: Horizontal velocity and Euler angles evolution over time (top-left), horizontal position trajectory with the three landmarks (top-right), and parametric illustration of the angle of arrival measurements (bottom).

  . Two filters are compared in the sequel:

		Sensor Parameters
	Sensor rates (Hz)	IMU:	100 Hz	Angles:	1 Hz
	IMU noise (1σ)	Gyro:	2 • /h	Accel:	10 -4 m/s 2
	Angles noise (1σ)	Azimuth: 0.6 •	Elevation: 0.6 •
		Filter parameters
	Initial errors	Attitude	Velocity	Position	Landmarks
	(1σ)	11.50 •	10 m/s	1 km	10 m
	Process noise (1σ)	Attitude	Velocity	Position	Landmarks
		0.6 •	10 -4 m/s ∅	∅
	Update noise (1σ)	Azimuth	1.8 •	Elevation 1.8 •
	Resampling threshold	N th = 0.6Np

TABLE II :

 II Comparison of the ARMSE and convergence rate of the LG-KPKF and LG-EKF for different amounts of particles. ARMSE are computed starting 60s of simulation and only on the convergent runs in the sense of (32).

Agence Innovation Défense: https://www.defense.gouv.fr/aid

ACKNOWLEDGMENT

The authors are thankful to the Innovation and Defense Agency 1 from the French Ministry of the Armies for their support on this research work.