Achille Murangira

Christian Musso

Karim Dahia

A Mixture Regularized Rao-Blackwellized Particle Filter for Terrain Positioning

This study is concerned with the development of a robust particle filtering algorithm tailored to the problem of terrain aided positioning (TAP) via radaraltimeter measurements. The Rao-Blackwellized Particle Filter (RBPF) is a popular particle filtering algorithm for TAP that takes advantage of the nature of the state-space model by sampling particles in a subspace of the state space, yielding more efficient estimators than the standard particle filter. Like most Monte Carlo filters, the standard RBPF uses the transition kernel as the proposal distribution during the particle update step. However, in contexts where the likelihood function is peaky, this may be highly inefficient since samples may fall in regions of low posterior probability. To address this issue, it is often advocated to use an importance sampling density which takes into account the latest observation. In a sequential importance sampling context, an optimal importance density is available but can only be easily sampled for specific state-space models, which raises the question of how to design a proposal density that is efficient, yet easy to sample from. In this article, we propose a particle filtering importance sampling method adapted to multimodal distributions. It hinges on the use of a robust proposal density as well as a cluster-based representation of the multimodal posterior. This leads to a novel marginalized particle filter, the regularized Rao-Blackwellized Particle Filter, that is evaluated on a challenging terrain positioning application.

I. INTRODUCTION

I NERTIAL NAVIGATION SYSTEMS (INS) are often the main source of navigation data in aircraft since they are autonomous and reliable. The aircraft position, velocity, attitude and heading components are computed by dead-reckoning. However, it is well known that these components drift due to alignment error and the accumulation of sensor errors over time. For certain classes of INS, this drift can be inconsistent with the navigation performance requirements. To ensure accurate navigation, one or more aiding source can be used in conjunction with the INS. A widespread aiding source is the GPS (Global Positioning System) and in this case, the GPS/INS sensor fusion can be performed with an Extended Kalman Filter (EKF) or an Unscented Kalman Filter (UKF). One drawback of this approach is that the GPS can experience intentional jamming. This is especially true of military grade aircraft. These vehicles would use alternate aiding sources such as radaraltimeters to ensure autonomous navigation [START_REF] Runnalls | Terrain-referenced navigation using the igmap data fusion algorithm[END_REF]. A Radaraltimeter provides ground clearance measurements along the flight path enabling navigation by comparing accumulated relative heights with a terrain elevation database. This form of navigation is commonly termed Terrain Aided Positioning (TAP). In order to perform TAP/INS integration to estimate the drift of all kinematic components, it's necessary to resort to non-linear filtering algorithms. The goal is to approximate the probability density function (pdf) of a state vector (INS error) given the sequence of groundclearance measurements accumulated up until the current time step. Due to the multimodality of this posterior (at least in the early stages of the INS update), methods such as the EKF or the UKF are inefficient since the Gaussian approximations are no longer relevant. Grid based methods such as the Point Mass Filter (PMF, [START_REF] Bergman | Recursive Bayesian Estimation: Navigation and Tracking Applications[END_REF]) perform better than the EKF but are mostly suited to estimating the position components only. Particle filters (PF) [START_REF] Gordon | Novel approach to nonlinear/non-Gaussian Bayesian state estimation[END_REF] are an efficient solution to the non-linear filtering problem since they can, in theory, approximate any posterior density without any hypothesis regarding the linearity of the process or observation model or the Gaussian nature of process and observation noise. Moreover particle filters remain somewhat tractable when the state dimension increases. In TAP, the state vector may include attitude sensor bias components additionally to position and velocity yielding a dimension greater than 9. The problem of TAP/INS integration using particle filters can be addressed by a specific algorithm, the Rao-Blackwellized Particle Filter (RBPF) otherwise known as the Marginalized Particle Filter [START_REF] Schon | Marginalized particle filters for mixed linear/nonlinear state-space models[END_REF], [START_REF] Nordlund | Marginalized particle filter for accurate and reliable terrain-aided navigation[END_REF]. This filter takes advantage of the state-space model structure where, conditionally to part of the state vector (the horizontal position error component), the model is linear Gaussian. As reported in [START_REF] Nordlund | Marginalized particle filter for accurate and reliable terrain-aided navigation[END_REF], [START_REF] Schon | Marginalized particle filters for mixed linear/nonlinear state-space models[END_REF], compared to a standard particle filter, less particles are needed to achieve similar performance.

In this paper, issues encountered when using particle filtering algorithms such as the Regularized Particle Filter (RPF) [START_REF] Musso | Regularization schemes for branching particle systems as a numerical solving method of the nonlinear filtering problem[END_REF] or the Rao-Blackwellized Particle Filter in the context of terrain aided positioning are addressed. It is well known that standard particle filtering methods struggle when the standard deviation of the measurement noise is small w.r.t. the typical range of the observations. The influence of the intensity of the observation noise statistics on PF performance is not specific to TAP/INS integration and has been reported in various applications ranging from robot positioning [START_REF] Fox | Particle filters for mobile robot localization[END_REF] to target tracking [START_REF] Musso | Improving regularized particle filters[END_REF]- [START_REF] Ristic | Beyond the Kalman filter : particle filters for tracking applications[END_REF]. This is caused by a mismatch between the proposal distribution used at the prediction step, and the likelihood function. A small observation noise can thus cause the likelihood function to be peaky and have little overlap with the proposal density. From a filtering standpoint, very few particles are significantly weighted, which can lead to filter divergence despite the resampling step. While the RBPF improves upon the standard particle filter by sampling particles in a lower dimensional state space, it can still suffer from the previous limitations if the transition density is used as the proposal in the prediction step. Different approaches have been proposed in the particle filtering literature in order to design filters that are more robust in the context of low observation noise and/or peaky observation function. Most enhanced particle filtering algorithms proceed by replacing the usual transition kernel density with an approximation of the optimal importance density (OID). For instance, the particle filter with EKF proposal [START_REF] Doucet | On sequential Monte Carlo sampling methods for Bayesian filtering[END_REF] proceeds by local linearisation of the state-space model yielding. The EKF proposal tends to perform well when the optimal importance density is unimodal and the non-linearities are moderate, conditions which are rarely met in terrain positioning and many other applications.The contribution of this paper is a novel particle filtering methodology dealing with highly multimodal posterior. To this end, we reconsider mixture particle filtering and its efficient implementation through particle clustering. This framework allows us to derive an importance sampling method aimed at generating particles around the modes of the posterior distribution in the event of a mismatch between the transition density and the likelihood function typically caused by low sensor noise. This proposal is then used during the particle update step of a Rao-Blackwellized Particle Filter. The paper's outline is as follows: Section II recalls the basics of particle filtering and Rao-Blackwellization. We also describe mixture modelling for PF via clustering for the sake of a practical implementation of a particle filtering algorithm with a mixture proposal; section III introduces the importance sampling methodology and offers an analysis of the influence of the choice of the covariance matrix on the stability of the algorithm in case of a Gaussian proposal. The resulting algorithm, the mixture Rao-Blackwellized Particle Filter, is detailed at the end of this section. Finally, in section IV, the resulting filter is compared to standard PF algorithms in terms of mean squared error and robustness to divergence, in typically challenging settings.

II. THE RAO-BLACKWELLIZED PARTICLE FILTER AND MIXTURE MODELS IN NON-LINEAR FILTERING

A. Bayesian estimation and particle filtering

Consider the following general state-space model:

x k = f k (x k-1 ) + w k y k = h k (x k ) + v k ( 1
)
where k is the time index and {x k , k ≥ 0} is the unobserved R d valued discrete time process, d ≥ 1 and {y k , k ≥ 0} is the sequence of observations. {w k , k ≥ 0} and {v k , k ≥ 0} are respectively the process and measurement noise sequences. The sequences {w k , k ≥ 0} and {v k , k ≥ 0} are assumed i.i.d., mutually independent and independent of x 0 . The aim of sequential filtering is the recursive computation of the posterior density p(x k |y 0:k ) where y 0:k = {y 0 , . . . , y k }. This can be done in two steps:

• prediction:

p(x k |y 0:k-1 ) = p(x k |x k-1 )p(x k-1 |y 0:k-1 )dx k-1 (2) • correction: p(x k |y 0:k ) = p(y k |x k )p(x k |y 0:k-1 ) p(y k |x k )p(x k |y 0:k-1 ) dx k (3)
It is generally assumed that w and v have known probability density functions, which yields closed form the transition p(x k |x k-1 ) and the likelihood p(y k |x k ).

In the remainder, we will sometimes use the short hand notation

g k (x k ) = p(y k |x k ).
The idea of particle filtering is to approximate the posterior distribution with a weighted sum of Dirac measures:

p(x k |y 0:k ) ≈ N i=1 ω i k δ x i k ( 4
)
where δ x denotes the the Dirac measure centered at x and the x i k , i = 1, . . . , N are the particles. The importance weights {ω i k } N i=1 sum up to one. Particle filtering algorithms stem from the Sequential Importance Resampling (SIR) algorithm that recursively updates the particle/weights systems [START_REF] Gordon | Novel approach to nonlinear/non-Gaussian Bayesian state estimation[END_REF]. Assuming a weighted particle set {x i k-1 , ω i k-1 } N i=1 is available at time k -1, the SIR computes the particle approximation at time k through the following steps :

1) Particle update: sample

x i k ∼ iid q(x i k |x k-1 , y k ), i = 1, . . . , N where q is suitably chosen proposal den- sity 2) Weight update: compute ωi k = ω i k-1 p(x i k |x i k-1 )p(yk|x i k ) q(x i k |x i k-1 ,yk)
and

ω i k = ωi k N j=1 ωj k 3) Resampling: sample with replacement N particles xi k so that ∀j = 1, . . . , N, P(x i k = x j k ) = ω i k . Set x i k = xi k and ω i k = 1 N
The resampling step avoids in principle the weight degeneracy where after a few iterations, all but one particle have negligible weight. It essentially discards particles with low weight and duplicates those with high weight.

In practice, resampling is triggered by monitoring a criterion such as the effective sample size:

N ef f ≈ 1 N i=1 (ω i k ) 2 (5)
Resampling is triggered whenever N ef f ≥ N th where N th is a threshold between 0 and N .

B. The Rao-Blackwellized Particle Filter

The Rao-Blackwellized Particle Filter, or marginalized particle filter, is an efficient implementation for conditionally linear Gaussian models. It has been widely used in several engineering applications such as aircraft navigation [START_REF] Nordlund | Marginalized particle filter for accurate and reliable terrain-aided navigation[END_REF], target tracking [START_REF] Gustafsson | Particle filters for positioning, navigation, and tracking[END_REF] and robot positioning [START_REF] Grisetti | Fast and accurate slam with rao-blackwellized particle filters[END_REF]. For a general discussion regarding the algorithm we refer the reader to [START_REF] Andrieu | Particle filtering for partially observed gaussian state space models[END_REF], [START_REF] Casella | Rao-Blackwellisation of Sampling Schemes[END_REF], [START_REF] Chen | Mixture Kalman filters[END_REF] and [START_REF] Schon | Marginalized particle filters for mixed linear/nonlinear state-space models[END_REF]. Assume the state-space model takes the following form :

⎧ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎩ x k,1 = f k-1,1 (x k-1,1 ) + F k-1,1 (x k-1,1 )x k-1,2 + G k-1,1 (x k-1,1 )w k-1,1 x k,2 = f k-1,2 (x k-1,1 ) + F k-1,2 (x k-1,1 )x k-1,2 + G k-1,2 (x k-1,1 )w k-1,2 y k = h k (x k,1 ) + H k (x k,1 )x k,2 + v k (6) where x T k = x T k,1 , x T k,2 .
x k,1 is the non-linear part of x k and x k,2 the linear part. Assume that w k and v k are white mutually independent Gaussian noises with respective covariance Q k and R k and such that

Q k = Q 11 k Q 12 k (Q 12 k ) T Q 22 k
We also suppose that x 0,2 ∼ N (x 0,2 , P 0 ). The initial density p(x 0,1 ) for the non-linear substate is also known. The RBPF proceeds by interleaving a Kalman filter that updates the linear part and a particle filter that targets the non-linear part. For a detailed derivation of the algorithm, we refer the reader to [START_REF] Schon | Marginalized particle filters for mixed linear/nonlinear state-space models[END_REF] and only describe the RBPF algorithm in appendix A.

C. Mixture particle filters

Mixture filtering [START_REF] Vermaak | Maintaining multimodality through mixture tracking[END_REF], [START_REF] Okuma | A boosted particle filter: Multitarget detection and tracking[END_REF] is based on the use of a mixture model for the posterior density, i.e. p(x k |y 0:k ) is expressed as

p(x k |y 0:k ) = M j=1 α j,k p j (x k |y 0:k ) ( 7
)
where M is the number of mixture components and the mixture weights α j,k are positive and satisfy M j=1 α j,k = 1. The filter prediction step is performed according to

p(x k |y 0:k-1 ) = M j=1 α j,k-1 p j (x k |y 0:k-1 ) (8)
where

p j (x k |y 0:k-1 ) = p(x k |x k-1 )p j (x k-1 |y 0:k-1 )dx k-1
and the filter correction step involves both the update of the mixture weights and the measurement update of the mixture densities:

p(x k |y 0:k ) = M j=1 α j,k p j (x k |y 0:k )
where

p j (x k |y 0:k ) = p(y k |x k )p j (x k |y 0:k-1 ) p(y k |x k )p j (x k |y 0:k-1 ) dx k (9) and α j,k = α j,k-1 p j (y k |y 0:k-1 ) M l=1 α l,k-1 p l (y k |y 0:k-1 ) (10)
Mixture particle filters are obtained by considering a set of N particles {x i k } N i=1 and associated weights {ω i k } N i=1 so that each mixture component is targeted by a subset of weights and particles. The particle approximation of the posterior density is expressed as:

p(x k |y 0:k ) ≈ M j=1 α j,k i∈Ij ω i k δ x i k ( 11
)
where I j ⊆ {1, . . . , N} is the subset of particles targeting the j-th mixture component p j (x k |y 0:k ) and i∈Ij ω i k = 1. The {ω i k , i ∈ I j } are the intra-component importance weights. The particle prediction step at time k is identical to that of the SIR and consists in sampling N particles according to some proposal q(x k |x k-1 , y k ). The measurement update steps involves the update of the importance weights and the update of the mixture weights : [START_REF] Gustafsson | Particle filters for positioning, navigation, and tracking[END_REF] where w + j,k = i∈Ij ωi k

• compute ωi k = ω i k-1 p(yk|x i k )p(x i k |x i k-1 ) q(x i k |x i k-1 ,yk) • for i = 1 ∈ I j , ω i k = ωi k l∈Ij ωl k • for j = 1, . . . , M, α j,k ≈ α j,k-1 w + j,k M p=1 α p,k-1 w + p,k
The main advantage of mixture particle filters is that they are able to maintain multimodality longer than standard particle filtering algorithms. Moreover, we have demonstrated in a previous paper [START_REF] Murangira | Robust regularized particle filter for terrain navigation[END_REF] that they allow a more robust implementation of the regularization procedure. Indeed, the standard regularized particle filter [START_REF] Musso | Regularization schemes for branching particle systems as a numerical solving method of the nonlinear filtering problem[END_REF] assumes a unimodal posterior which is not the case when dealing with ambiguous measurements. Another advantage which will be highlighted in the next section is the possibility to use a different proposal for each mixture component p j (x k |y 0:k ). This makes it easier to adapt the sampling distribution to the particular shape of p j (x k |y 0:k ).

Note that in practice, we wish to have one mixture component per mode of the posterior density. Since the number of modes may change with time as ambiguity is progressively resolved, a clustering procedure may be necessary to group together particles associated to the same modes. In [START_REF] Vermaak | Maintaining multimodality through mixture tracking[END_REF], the authors use the k-means algorithm. In terrain navigation since the number of modes of the posterior is usually unknown, we advocate the use of the mean-shift clustering algorithm [START_REF] Murangira | Robust regularized particle filter for terrain navigation[END_REF]. Denoting M k the number of modes at time k, the posterior is then expressed as

p(x k |y 0:k ) = Mk j=1 α j,k p j (x k |y 0:k ) (13)
≈ Mk j=1 α j,k i∈Ij,k ω i k δ x i k ( 14
)
where

I j,k ⊆ {1, . . . ,
ω i k+1 δ x i k+1 (16) = N i=1 α c1(i),k+1 ω i k+1 δ x i k+1 ( 17
)
where c 1 (i) = j if i ∈ I j,k . In reality, each empirical mixture component i∈Ij,k ω i k+1 δ x i k+1 , j = 1, . . . , M k may contain multiple modes or be merged with another spatially close component (in particular M k+1 > M k is possible). To account for this variation in the number of modes, the posterior is expressed as a mixture of M k+1 components as

pk+1|k+1 = Mk+1 l=1 β l,k+1 i∈Il,k+1 ν i k+1 δ x i k+1 ( 18
)
where Mk+1 l=1 β l,k+1 = 1 , i∈Ij,k+1 ν i k+1 = 1. Equation ( 18) can be rewritten as

pk+1|k+1 = N i=1 β c2(j),k+1 ν j k+1 δ x j k+1 ( 19
)
where c 2 (i) = l if i ∈ I l,k+1 . It follows that (see [START_REF] Vermaak | Maintaining multimodality through mixture tracking[END_REF] for instance),

β l,k+1 = i∈Il,k+1 α c1(i),k+1 ω i k+1 ( 20
)
and

ν i k+1 = α c1(i),k+1 ω i k+1 β c2(i),k+1 (21)

III. AN IMPORTANCE SAMPLING METHOD FOR MULTIMODAL DISTRIBUTIONS BASED ON POSTERIOR

MODES

As discussed in the introduction, choosing an adequate proposal density is crucial for the robustness of the filter, especially in challenging situations of interest, namely when the observation noise is small or when the observation function exhibits peakiness, more generally when the Fisher information is high. Many alternate importance sampling schemes involve some approximation of the optimal importance density (OID) at time k, p(x k |x k-1 , y k ) such as the particle filter with EKF proposal [START_REF] Doucet | On sequential Monte Carlo sampling methods for Bayesian filtering[END_REF], the unscented particle filter [START_REF] Van Der Merwe | The unscented particle filter[END_REF] or the progressive proposal particle filter [START_REF] Bunch | Particle filtering with progressive gaussian approximations to the optimal importance density[END_REF]. In this work, we take another perspective by using mixture modelling and proposing a different importance distribution for each mixture component p j (x k |y 0:k ). Each proposal is centered on the maximum a posteriori of p j (x k |y 0:k ).

It is well known that the proposal density should be as close as possible in shape to the target density. Since we assume each component p j (x k |y 0:k ) to be unimodal, proximity can be achieved by centering the proposal at the mode of p j . If the proposal is a Gaussian density, the covariance can be chosen so as to match the orientation and dispersion of p j (x k |y 0:k ). Choices of the corresponding covariance will also be discussed in this section. In order to implement this importance sampling strategy, it is necessary to determine, without excessive computation overhead, the modes of the posterior density. We have previously introduced [START_REF] Murangira | Proposal distribution for particle filtering applied to terrain navigation[END_REF] a method suitable for partially linear models which will be detailed here for the sake of clarity. We will also discuss some issues related to the stability of the importance sampling procedure, which will lead in particular, to robust choices of the covariance matrix for Gaussian proposals. Finally, a mixture Rao-Blackwellized Particle Filter using this importance sampling strategy will be presented.

A. Standard importance sampling in Bayesian inference

Let X be a hidden state distributed according to a density q and partially observed through a measurement Y = h(X) + V where h is a non-linear observation function and V is a measurement noise independent from X. Importance sampling targets posterior expectations of the form

E p (ϕ(X)) = R d ϕ(x)p(x|y)dx ( 22
)
where p(x|y) = g(x)q(x) g(x)q(x)dx is the conditional density of X given Y , ϕ is a real-valued measurable test function of R d , g is the likelihood function and E p refers to the expectation w.r.t. density p. In general, it is not straightforward to sample from the posterior which is one of the reasons for using importance sampling. Let q be a density whose support included that of ϕq. The importance sampling (IS) density estimator of ( 22) is given by

E p (ϕ(X)) ≈ N i=1 ω i ϕ(x i ) (23)
where x i ∼ q, i = 1, . . . , N are i.i.d. samples from q, a so-called importance density. The importance weights ω i are computed according to

ω i = ωi N j=1
ωj where ωi = g(x i )q(x i ) q(x i ) .

If additionally, 23) is unbiased and strongly consistent [START_REF] Geweke | Bayesian inference in econometric models using Monte Carlo integration[END_REF]. Another indicator of importance sampling performance which is of interest in this paper is the asymptotic variance of the importance weights V q defined by

• E(ϕ 2 (X)|Y ) < ∞ • E(ϕ 2 (X) q(X) q(X) |Y ) < ∞ then the estimator (
V q = 1 N ⎛ ⎜ ⎜ ⎜ ⎝ R d g 2 (x)q 2 (x) q(x) dx R d g(x)q(x)dx 2 -1 ⎞ ⎟ ⎟ ⎟ ⎠ (24)
(24) is a measure of IS performance [START_REF] Kong | Sequential imputations and Bayesian missing data problems[END_REF] and will be used as a criterion to design the importance density q. Indeed, it is known (see [START_REF] Kong | Sequential imputations and Bayesian missing data problems[END_REF] for example) that V q = 0 for q = p(x|y) which would indicate that a good choice of q is one that is similar to p(x|y).

For a general mixture model p(x|y) = M j=1 α j p j (x|y) where p j (x|y) = g(x)qj(x) g(x)qj(x)dx and q j is the prior density corresponding the j-th component, we may use a different proposal for each component. In this case,

E p (ϕ(X)|Y ) = ϕ(x) M j=1 α j p j (x|y)dx = M j=1 α j ϕ(x) g(x)q j (x) g(z)q j (z)dz dx = M j=1 α j E qj g(X)qj(X)ϕ(X) qj(X) E qj g(X)qj(X) qj(X) ≈ M j=1 α j 1 Nj i∈Ij ωi ϕ(x i ) 1 Nj l∈Ij ωl ≈ M j=1 α j i∈Ij ω i ϕ(x i ) where x i , i ∈ I j are i.i.d. samples from qj , ω i = ωi l∈I j ω i
and I j ⊆ {1, . . . , N} is the subset of samples targeting the j-th mixture component as in section II-C and such that |I j | = N j .

B. Designing the importance density

Given that we have indicated how to implement importance sampling when targeting a mixture density, we shall assume that the posterior is unimodal in this subsection: if this is not the case then it suffices to find a decomposition of the posterior into a mixture of unimodal densities as outlined in III-A. As argued previously, a general rule of thumb to design a suitable importance density is to choose one that is close to the posterior. For instance, we can use a proposal q centeredon an approximation of the conditional expectation E(X|Y ) and with covariance close to the posterior covariance Cov(X|Y ). However, for this choice of q we must also ensure V q < ∞. In the sequel we study several proposal densities, designed to match the posterior while guaranteeing a finite estimation variance, as measured by the asymptotic variance of the importance weights V q. These covariances will be candidates for the proposals used in the mixture Rao-Blackwellized particle filter introduced in III-D.

In order to approximate Cov(X|Y ) we use Laplace's method: this approach has been used in the context of importance sampling in [START_REF] Jungbacker | Monte Carlo estimation for nonlinear non-Gaussian state space models[END_REF] and [START_REF] Musso | Introducing the laplace approximation in particle filtering[END_REF]. Laplace's method approaches the posterior covariance by the inverse of the observed Fisher information matrix J evaluated at the mode of p(x|y), while a first order approximation of the posterior expectation is given by the mode of p(x|y).

E(X|Y ) = x * -F 0 (J(x * ), J (x * )) (25)
Cov(X|Y ) = J -1 (x * ) -G 0 (J(x * ), J (x * ), J (x * )) (26)
where x * = arg max x∈R d p(x|y) is the maximum a posteriori (MAP) and

J(x) = - ∂ 2 log g ∂x 2 (x) - ∂ 2 log q ∂x 2 (x) (27)
and F 0 and G 0 are given functions (see [START_REF] Musso | Introducing the laplace approximation in particle filtering[END_REF] for instance).

In the sequel, we only keep the first terms of expansions ( 25) and ( 26), ie.

E(X|Y ) ≈ x * Cov(X|Y ) ≈ J -1 (x * )
Assuming a gaussian prior q with covariance P and zero mean gaussian noise with covariance R, as will be done subsequently, one can compute J as

J(x) = 2 ∂ 2 h ∂x 2 R -1 (y -h(x)) -R -1 ∂h ∂x ∂h ∂x T + P -1 (28)
The assumption of a gaussian prior in a filtering setting is somewhat reasonable since q is supposed unimodal here.

Therefore, assuming we have an estimate for x * , we can use a Gaussian proposal density q with mean x * and covariance J -1 (x * ). However, as pointed out in [START_REF] Murangira | Proposal distribution for particle filtering applied to terrain navigation[END_REF], [START_REF] Geweke | Bayesian inference in econometric models using Monte Carlo integration[END_REF], this can lead to non-consistent importance sampling estimates since the tails of the proposal may be lighter than the tails of the posterior. It is straightforward that whenever sup x∈R d q(x) q(x) < ∞, V q is automatically finite. Hence, assuming that the prior q is Gaussian, one can choose a t-distribution centered on the mode x * , with a suitable scale matrix Σ with ν > 2 degrees of freedom (d.o.f.). For ν > 2, the covariance matrix of a variable distributed according to the t-distribution with scale matrix Σ is equal to ν ν-2 Σ. Therefore, to achieve a covariance equal to Laplace's approximation of Cov(X|Y ) ≈ J -1 (x * ), one must set the scale matrix to

Σ = ν -2 ν J -1 (x * ) (29)
The d.o.f. parameter ν tunes the fatness of the tdistribution's tail : lower values correspond to a fat tail while higher ones lead to a lighter tail.

Let us now examine the choice of a Gaussian proposal density q(x) = N (x; x * , Σ) whose covariance Σ is chosen under the constraint that V q < ∞.

Since sup

x∈R d q(x) q(x) < ∞ ⇒ V q < ∞
, one only needs to find a matrix Σ such that sup q q < ∞. The following proposition provides such a matrix.

Proposition 1. Let P -1 = D T D be the Choleski decomposition of P -1 . If the largest eigenvalue of D -T Σ -1 D -1 is smaller than 1, then q
q is bounded from above and V q is finite.

Proof. See appendix B.

This proposition gives us a practical way of designing Gaussian proposals with covariance Σ that yield consistent importance sampling estimates. We have previously proposed in [START_REF] Murangira | Proposal distribution for particle filtering applied to terrain navigation[END_REF] a covariance Σ. While giving good results in simulations, there was no guarantee that the asymptotic variance would be finite. This choice is obtained by rotating the ellipsoid of the prior covariance P along the principal axes of the ellipsoid of the Laplace approximation of the posterior covariance. More precisely, this covariance is defined by

P rot = E J Λ P E T J (30)
where E J la the matrix of eigenvectors of Ĵ-1 = J -1 (x * ) and Λ P the matrix of eigenvalues of P . In [START_REF] Murangira | Proposal distribution for particle filtering applied to terrain navigation[END_REF], we have found that setting Σ = P rot in the proposal q(x) = N (x; x * , Σ), could, in some cases, yield a lower estimation error compared to the use of the prior covariance Σ = P .

Still, to ensure that the variance of the asymptotic weights V q remains finite, we may set Σ = sP rot where s > 0 and find the smallest s such that V q < ∞ . According to the previous proposition, we only need to determine

inf{s > 0 | λ max ( 1 s D -T P -1 rot D -1 ) < 1}
where λ max (B) is the largest eigenvalue of matrix B when it exists. Hence, for all s > s * , V q < ∞ where s * is defined as

s * = λ max (D -T P -1 rot D -1 ) (31)
Finally, we may also seek a value Σ that is close to the Laplace approximation of the posterior covariance, while also ensuring that V q is finite. Since the latter constraint is met whenever Σ -P > 0 (cf. lemma 1), we can solve the following optimization problem :

minimise Σ -Ĵ-1 2 F such that Σ -P > 0
where • F is the Frobenius norm. The minimizer may be obtained via (see proof in appendix C)

Σ F = 1 2 ( Ĵ-1 + P + H) + κI d ( 32
)
where κ is a small positive constant, U and H are such that Ĵ-1 -P = UH, U T U = I and I d is the d dimensional identity matrix.

Up until now, we have two different choices for the covariance of the Gaussian proposal density. Each one of them tries to be close to the posterior covariance while guaranteeing stable importance sampling estimates, in the sense that the asymptotic variance of the importance weights remains finite :

• Σ = s * P rot as defined by ( 30) and (31). This essentially consists in rotating the prior covariance P along the principal axes of the Laplace approximation of the posterior covariance, denoted Ĵ-1 . • Σ = Σ F as defined by (62). Of course, as outlined in section III-A, it remains possible to use a Student t-distribution density centered on the mode of the posterior and with scale matrix proportional to Ĵ-1 (or P rot ). As a summary, we have plotted in figure 1 the 99% confidence ellipsoids of each covariance/scale matrix associated to the different Gaussian or t-density proposals centeredon the MAP:

1) Laplace approximation of the posterior covariance: Ĵ-1 2) prior covariance with rotation along posterior covariance main direction: P rot 3) scaled rotated prior covariance: s * P rot (achieves finite variance) 4) closest covariance matrix to Ĵ-1 for the Frobenius norm: Σ F (achieves finite variance) These ellipsoids have equation

(x -x * ) T Σ -1 (x -x * ) = χ 2 2,.99
where χ 2 2,.99 is the 0.99 quantile of the Chi-squared distribution with 2 degrees of freedom. This illustration assumes a Gaussian prior with covariance P . The first choice is not new and has been widely used in the P J -1 s * Prot Prot ΣF Fig. 1. 99% confidence ellipsoids associated to covariance matrix P , J -1 , Prot, s * Prot and ΣF particle filtering literature, while the second one has been introduced in a previous paper [START_REF] Murangira | Proposal distribution for particle filtering applied to terrain navigation[END_REF] and the next two have been introduced in this paper. The main take away from this figure is that all ellipsoids are aligned with the ellipsoid corresponding to the Gaussian approximation of the posterior which is given by Ĵ-1 . However, as argued previously, the size of the covariance should be sufficient to guarantee consistent estimation as attested by the greater areas covered by the proposal ellipsoids.

C. Efficient MAP approximation for partially linear models

In accordance to the previous, section we assume a static setting where the posterior density is unimodal. We wish to compute the mode of the posterior density defined as

x * = arg max

x∈R d p(x|y) = arg max x∈R d g(x)q(x) (33)
When dealing with a multimodal density p(x|y), we will assume that a finite mixture representation p(x|y) = M j=1 α j p j (x|y) is available where each p j (x|y) is assumed unimodal with unknown modes. Hence the problem of localizing all the modes of the full posterior consists in finding the mode of each individual mixture component. Most filtering applications (target tracking, data assimilation, etc.) deal with high-dimensional state-vectors therefore deriving an exact or an approximate expression (33) is a non-trivial task. We have previously proposed [START_REF] Murangira | Proposal distribution for particle filtering applied to terrain navigation[END_REF] and we detail further a methodology suited for partially linear and Gaussian observation models, i.e. models where the observation equation takes the following form :

y = Ax 2 -h(x 1 ) + v (34)
where x 1 and x 2 are sub-vectors of x, i.e. x = [x T 1 , x T 2 ] T , h a non-linear function of x 1 and A a matrix independent of x. Radar-altimeter observations can be modelled by such an equation, and more generally, any observation model where only a substate x 1 of the state-vector affects the measurement function. We suggested in [START_REF] Murangira | Proposal distribution for particle filtering applied to terrain navigation[END_REF] a method to reduce the optimization dimension by assuming that the prior density q is Gaussian with mean μ and covariance matrix P . The impact of this hypothesis will be discussed in the following section. We also assume the likelihood g to be a zero mean Gaussian density with covariance R.

Let μ = (μ T 1 , μ T 2 )
T and P = P 11 P 12 P 21 P 22 . We showed in [START_REF] Murangira | Proposal distribution for particle filtering applied to terrain navigation[END_REF] that the maximization can be performed in two steps :

• find x * 1 = arg min x1 (x 1 -μ 1 ) T P -1 11 (x 1 -μ 1 ) + Γ(x 1 ) • set x * 2 = γ(x * 1 ) and (x * ) T = [x * 1 , x * 2 ]
where Γ and γ are non-linear functions of x 1 defined by

Γ(x1) = [y + h(x1) -Aγ(x1)] R -1 [y + h(x1) -Aγ(x1)] T + [γ(x1) -E1(x2)] P -1 2|1 [γ(x1) -E1(x2)] T γ(x1) = Ω A T R -1 (y + h(x1)) + P -1 2|1 E1(x2) Ω = A T R -1 A + P -1 2|1 -1
and

E 1 (x 2 ) = E(x 2 |x 1 ) = E(x 2 ) + P 21 P -1 11 (x 1 -E(x 1 )) = μ 2 + P 21 P -1 11 (x 1 -μ 1 )
Likewise, P 2|1 = P 22 -P 21 P -1 11 P 12 is the covariance of x 2 given x 1 .

D. Adaptive importance sampling in a multimodal filtering setting

In the previous subsections, we have reviewed and suggested possible methods for designing proposal densities based on local modes of the posterior in a static setting. We also recalled a method for approximate MAP computation developed in a previous paper. These methods can be extended to a filtering context where we deal with multimodal posterior.

In accordance to the previous notations, the observation model is assumed partially linear and is expressed as

y k = A k x k,2 -h n k (x k,1 ) + v k ( 35
)
where v k ∼ N (0, R) and A k is matrix independent of x k . The state vector is decomposed into its linear part

x k,2 and its non-linear part x k,1 ,

x k = x T k,1 , x T k,2 T while h n k is a non-linear function of x T k,1 .
To deal with multimodal posterior densities, we resort to a mixture representation of the posterior as in section II-C, i.e.:

p(x k |y

0:k ) = M j=1 α j,k p j (x k |y 0:k )
where M j=1 α j,k = 1 and

p(x k |y 0:k ) = g k (x k )p(x k |y 0:k-1 ) g k (x k )p(x k |y 0:k-1 ) dx k (36)
where we recall that g k is the likelihood function at time k. Now, assume a particle approximation of the predictive density p(x k |y 0:k-1 ) is available.

p(x k |y 0:k-1 ) ≈ M j=1 α j,k-1 i∈Ij ω i k-1 δ x i k|k-1 (37)
where

x i k|k-1 ∼ p(x k |x i k-1 ) are i.i.d. samples. Recall p(x k |y 0:k ) = M j=1 α j,k-1 g k (x k )p j (x k |y 0:k-1 ) M l=1 α l,k-1 g k (x k )p l (x k |y 0:k-1 ) dx k
Obtaining an approximation of p(x k |y 0:k ) through importance sampling requires sampling particles x i k according to some suitable proposal as well as computing the importance weights {ω i k } N i=1 and the mixture weights {α j,k } M j=1 . As suggested previously, a different importance density qj may be used for each mixture component p j (x k |y 0:k ). Note that, in the context of filtering, the prior q is the predictive density p j (x k |y 0:k-1 ). Let us introduce M k importance densities qj , each centered on the mode of p j (x k |y 0:k ). When using Gaussian densities, qj (x k ) = ϕ(x k |x * j,k , Σ j,k ) where ϕ(•|m, Θ) denotes the multivariate normal density with mean m and covariance matrix Θ. The covariance Σ j,k of the proposal can be chosen as described in subsection III-B. This scheme requires the computation of M k modes {x * j,k } Mk j=1 defined by x * j,k = arg max

xk∈R d g k (x k )p j (x k |y 0:k-1 ) (38)
We obtain these modes by using the method detailed in subsection III-C. It is then necessary to work out a Gaussian approximation for the predictive density p j (x k |y 0:k-1 ), which is assumed unimodal, by using for instance the sample mean and sample covariance defined respectively as

xj,k|k-1 = i∈Ij,k-1 ω i k-1 x i k (39)
and

Pj,k|k-1 = i∈Ij,k-1 ω i k-1 (x i k -xj,k|k-1 )(x i k -xj,k|k-1 ) T
(40) This Gaussian approximation is also used to work out the covariance of the proposal Σ j,k .

Once parameters for the current proposal qj are derived, we first obtain particles {x i k } N i=1 by sampling for each mixture component of index j, N j samples x i k ∼ qj , i ∈ I j,k . Hence, the particle weights read :

• ωi k = ω i k-1 gk(x i k )pj(x i k |y0:k-1) q(x i k ) • ω i k = ωi k l∈I j ωl k
Notice that in order to compute the importance weights it is necessary to evaluate p j (x i k |y 0:k-1 ), the predictive density for each sampled particle. A possibility is to use the approximation

p j (x i k |y 0:k-1 ) ≈ l∈Ij ω l k-1 p(x i k |x l k-1
). However, this requires O(N 2 ) operations, which can be costly for online applications. As an alternative, we use a Gaussian approximation for the predictive density p j (x k |y 0:k-1 ) which is readily available since it is also used for the approximate mode computation. In this case the particle weight update becomes

ωi k = ω i k-1 g k (x i k )ϕ(x i k |x * j,k , Σ j,k ) q(x i k ) (41
)

ω i k = ωi k l∈Ij ωl k ∀i ∈ I j,k (42)
This approach has the disadvantage that the particle estimate is no longer consistent. However, simulations on a TAP/INS integration and on a range/bearings based target tracking scenario have suggested good filter behaviour. It remains to compute the mixture weights {α j,k } M j=1 . According to [START_REF] Ristic | Beyond the Kalman filter : particle filters for tracking applications[END_REF],

α j,k = α j,k-1 g k (x k )p j (x k |y 0:k-1 ) dx k M l=1 α l,k-1 g k (x k )p l (x k |y 0:k-1 ) dx k (43)
Importance sampling based on the proposal qj can be used to work out the integrals in the numerator and the denominator which yields the following mixture weight estimate:

α j,k ≈ αj,k-1 Nj i∈Ij,k-1 ωi k M l=1 αl,k-1 Nl i∈Il,k-1 ωi k (44)
The general IS approach taken here is similar to the Gaussian approximation of the optimal importance density [START_REF] Doucet | On sequential Monte Carlo sampling methods for Bayesian filtering[END_REF], which is, in a sequential importance sampling framework, equal to p(x k |x k-1 , y k ). However, the main difference is that the mode of the OID p(x k |x i k-1 , y k ) has to be computed for each particle which makes this method more computationally intensive, unless a straightforward expression for these modes is available. It is also necessary to evaluate the inverse of Hessian of the log-likelihood N times ; this can be time consuming whenever the evaluation of the observation function per particle is more costly than other standard particle filter operations (random variable generation, resampling).

To limit the algorithm's computing cost, we can choose to use the importance sampling only when detecting excessive variance in the particle weights. We monitor the effective sample size (ESS) for each mixture component represented by a cluster of weighted particles and trigger importance sampling only when the ESS falls significantly below the resampling threshold. The rationale is that when the mismatch between the predictive density p j (x k |y 0:k-1 ) and the likelihood g k (x k ) isn't too high, resampling/regularization can suffice to avoid filter divergence. Therefore, we only trigger the MAP computation and importance sampling only in the event of very low ESS. More specifically, let N j ef f be the ESS of the particle approximation of pj = i∈Ij,k ω i k δ x i k of p j (x k |y 0:k ) where the particles x i k and associated weights are computed by using the transition density as the proposal. N j ef f is computed similarly to (5):

N j ef f ≈ 1 Nj i=1 (ω i k ) 2 (45)
The threshold N th,M AP for triggering importance sampling is defined as a fraction ζ ∈ [0, 1] of the resampling threshold N th , i.e.

N th,M AP = ζN th ( 46
)
Low values of ζ imply a moderate use of importance sampling (that is, only in cases of severe weight degeneracy) while higher ones lead to a more frequent use. Let us observe that the formula for the mixture weight update when using MAP based importance sampling for every mixture component measurement update (see equation ( 44)) is different from the case where IS is not systematically triggered. In the latter case, the following formula (47) should be used:

α j,k = α j,k-1 β j,k M l=1 α l,k-1 β l,k (47)
where

β j,k = ⎧ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎩ i∈Ij,k-1 ω i k-1 g k (x i k ) if N j ef f N j ≤ N th,M AP 1 Nj i∈Ij,k-1 g k (x i k )p j (x i k |y 0:k-1 ) qj (x i k )
otherwise (48) Let us now detail the mixture Rao-Blackwellized particle filter algorithm using MAP based importance sampling. In the standard Rao-Blackwellized particle filter (see Algorithm 1), the Kalman filter measurement update is done after the particle measurement update and the resampling step. However, in our importance sampling strategy, the mixture component posterior density of the entire state

x k = [x T k,1 , x T k,2 ]
T , p j (x k |y 0:k ) is obtained. This entails that the Kalman measurement update and the particle measurement update cannot be split into different stages separated by the resampling step if we mix the adaptive importance sampling scheme in a standard RBPF framework. The solution is then to have a variable indexMAP j that equals 1 if importance sampling is triggered for the j-th mixture component and equals 0 otherwise. In the event where indexMAP j = 1, we only need to compute the covariance matrix P k during the Kalman measurement update step while the computation of the updated means xi k,2 , i = 1, . . . , N is not performed since it is already carried out at the importance sampling stage. The resulting algorithm, called MRBPF-MAP, is outlined in appendix D.

IV. PERFORMANCE EVALUATION

In this section, we study the performance of the MRBPF-MAP algorithm (Mixture Rao-Blackwellized Particle Filter with MAP aide proposal), in a context of low process and observation noise. As mentioned in the introduction, this can impact negatively standard particle filters as the prediction step using the transition kernel may sample particles in low probability regions of the state-space. We consider the problem of TAP/INS integration via particle filtering methods and compare our algorithm with standard filters used in terrain navigation applications. Performance indicators such as RMSE (Root Mean Squared Error) and percentage of nondivergent tracks are presented.

A. Motion and observation model

We consider an aircraft equipped with an Inertial Navigation System (INS). The accumulation of sensor errors in the rate gyros and in the accelerometers is responsible for the drift in the position, velocity and attitude component. letting k be the time index, we denote λ φ z ṽn ṽe ṽd T k the vector comprising the inertial latitude, longitude and altitude as well as the north (n), east (e) and down (d) components of the velocity components in the navigation frame. We wish to estimate the drift vector x k defined as the metric error between the true position and velocity coordinates and their inertial equivalent and expressed in the navigation frame. It is shown in [START_REF] Dahia | Nouvelles méthodes en filtrage particulaire. Application au recalage de navigation inertielle par mesures altimétriques[END_REF] that the drift can be accurately modelled by a linear model according to

x k+1 = F x k + G k w k
where

F = I 3 ΔI 3 0 3 I 3 G k = Δ 2 2 I 3 ΔI 3 (49)
Δ is the sampling period and

w k ∼ N (0, Q) is white Gaussian noise.
The aircraft is equipped with a DTED (Digital Terrain Elevation Data) with 3 arc seconds accuracy (≈ 100 m), which enables a matching with the reconstructed elevation profile from altimeter measurements. Let y k be the observation delivered by the radar-altimeter and let h DT ED (λ, φ) be the digital terrain elevation function which maps a latitude λ and longitude φ to the corresponding terrain elevation h DT ED (λ, φ). Then the ground clearance y k is modelled according to

y k = zk -x k,3 -h DT ED ⎛ ⎝λ + x k,1 R λ + zk , φk + x k,2 R φ + zk cos λk ⎞ ⎠ + v k (50)
where v k is an additive white Gaussian noise with standard deviation σ v . This simple model does not take into account possible biases or altitude-dependent errors and is used to generate altimeter measurements at a frequency of 10 Hz. R λ and R φ are respectively the north and east radius and are defined as

R λ = a 1 -e 2 (1 -e 2 sin 2 ( λ)) 3/2 (51) R φ = a (1 -e 2 sin 2 ( λ)) 1/2 (52

B. Simulation settings and numerical results

We compare hereafter, the performance of the RBPF, the MRBPF and MRBPF-MAP in a challenging setting. The initial drift x 0 is assumed to follow a zero-mean Gaussian distribution with covariance

P INS 0 = diag(1000 2 , 1000 2 , 100 2 , 3 2 , 3 2 , 1 2 )
all units being in m or m.s -1 where appropriate. This initial uncertainty is simply obtained by integration of the INS sensor errors from an initially known position, and during a given time interval, after which INS/TAP integration starts. The INS sensor errors are modelled by a Gauss-Markov type bias term plus a white Gaussian noise as in [START_REF] Nordlund | Marginalized particle filter for accurate and reliable terrain-aided navigation[END_REF]:

• accelerometer bias standard deviation/correlation period: σ a = 3 × 10 -5 m.s -2 , τ a = 60 s • accelerometer wide band Gaussian noise noise st.dev. σ wa = 5 × 10 -4 m.s -2 • gyrometer bias st.dev/correlation period : σ a = 10 -6 rad.s -1 , τ g = 60 s

• gyrometer wide band Gaussian noise noise st.dev. σ wg = 1.7 × 10 -4 π 180 rad.s -1 The radar-altimeter noise standard deviation is set to two different values σ v = 5m and σ v = 15m. Given the initial uncertainty and the nature of the elevation data, σ v = 5m corresponds to very informative measurements and proves in practical simulations to be a challenging noise setting. The baseline value σ v = 15m corresponds to medium level noise similar to the work [START_REF] Nordlund | Marginalized particle filter for accurate and reliable terrain-aided navigation[END_REF]. Additionally, for the state dynamics, we considered Q = diag 1 2 1 2 0.01 2 which gives rather low process noise, which is another source of difficulty for particle filters as particles tend to be stranded in the same regions limiting their ability to explore the state-space. The filter parameters were set as follows: The different number of particles between the MRBPF-MAP and the other algorithms ensures a similar computational load between all algorithms. To obtain a mixture representation for the particle cloud, a mean-shift algorithm was used as in [START_REF] Murangira | Proposal distribution for particle filtering applied to terrain navigation[END_REF]. Moreover, the modes were located as described in III-C by using a non-linear conjugate gradient optimization algorithm. To reduce the algorithmic complexity associated to this mode search, MAP based estimation is only carried out when 20 or less modes are detected by the clustering routine. For this choice of parameters, the computing cost of the MRBPF-MAP is approximately twice that of the RBPF. a) Influence of the modified proposal: First, we focus on the impact of a MAP based proposal compared with the sole use of a mixture representation (MRBPF algorithm) and with the standard RBPF algorithm. In this analysis, the chosen proposal is a Gaussian density, with covariance matrix equal to P rot (see (30)). For each algorithm, we have computed the RMSE for the nondivergent tracks by carrying out 200 filter runs as well as the percentage of non-divergent tracks. A filter run is said to be non-divergent if the final estimate xn is inside the 99% confidence ellipsoid associated to the Gaussian distribution centered on the true state with covariance the empirical covariance Pn given by the particle cloud. The posterior Cramer-Rao lower bound (PCRB) [START_REF] Tichavsky | Posterior cramerrao bounds for discrete-time nonlinear filtering[END_REF] is also worked out for this model to assess filtering efficiency. Note that when the posterior is multimodal, this bound is not tight hence the large gap between the filter RMSE and the PCRB usually observed in the first few iterations.

• N RBP F =
As can be seen in figures 3 and 4 which correspond to a very informative measurements setting, the RMSE given by the MRBPF-MAP, which uses a proposal centered around the posterior modes, is significantly lower for a good portion of the trajectory, especially for the position components the state vector. In the baseline setting (increased measurement noise), it appears that the improvement in mean squared error yielded by the MRBPF-MAP is less significant (see fig. 5 and6). The percentage of non-divergent tracks for both sensor noise levels is displayed in table I. The results indicate both a gain in robustness (fewer divergent tracks) and accuracy for the MRBPF-MAP compared to the standard Rao-Blackwellized Particle filter (RBPF) and its adaptation (MRBPF) in a multimodal setting. b) Influence of the covariance matrix in the MAP based proposal: Second, let us examine the choice of the covariance of the proposal density q that is used locally in each mixture distribution. To this end, we have used either a Gaussian density or a Student t-density proposal with scale matrix proportional to the target posterior covariance Σ, as explained in section III. Here, three different implementations of the MRBPF-MAP using different target posterior covariance Σ are compared :

• MRBPF-MAP 1: this algorithm uses for each mixture component, a Gaussian proposal centered on the MAP and with covariance Σ = P rot • MRBPF-MAP 2: this algorithm uses a Gaussian proposal with covariance Σ = Σ F • MRBPF-MAP 3: this version uses a t-density proposal with ν = 8 degrees of freedom. For each component, the proposal covariance is Σ = Ĵ-1 computed via the Laplace approximation given by ( 26) and [START_REF] Horn | Matrix Analysis[END_REF], where R = σ 2 v . As discussed in previous sections, the use of a Gaussian proposal with covariance Σ = Ĵ-1 can potentially lead to infinite variance for the importance sampling estimator, whereas the t-Student density theoretically avoids this pitfall. As in the previous case study the number of particles is set to 3000 for the three versions of the algorithm.

There is no change in other particle filtering parameters.

The percentages of non-divergent tracks for each version of the MRBPF-MAP is shown in table II. The use of the student density in conjunction with covariances Σ F or Σ = Ĵ-1 improves the percentage of non-divergent tracks in both sensor noise situations. However, as can be seen in figures 7 and 8 which correspond to σ v = 5m, there is no significant difference in terms of RMSE. For σ v = 15m (not shown here), the same observation holds. We therefore conclude that the covariance matrix used is of moderate importance in this particular case.

V. CONCLUSION

This paper introduces and discusses a practical importance sampling procedure in case of state-space models with informative and ambiguous measurements. The motivation is essentially terrain navigation but the algorithm we have presented can be used in other applications with similar characteristics (multimodality and/or low observation noise) such as target tracking or gravity gradiometer navigation. We have proposed a mixture proposal density whose components are centered on the modes of the posterior distribution. This is different from the use of a proxy to the optimal importance density in the sequential importance sampling/resampling framework, since in the latter, the proposal density depends on the locations of the particles at the previous time step and its parameters have to be computed as many times as there are particles. In contrast, the proposed methodology only requires the approximation of the modes of the posterior which is a priori less time consuming than computing the mode of the OID for each particle. We have recalled an efficient method for maximum a posteriori approximation in cases of interest, i.e. when the likelihood function is a function of a smaller part of the state vector. The ensuing mixture proposal lends itself to the mixture particle filtering framework which is convenient when dealing with multimodality. We have then combined this importance sampling procedure with the Mixture Rao-Blackwellized Particle Filter. Moreover, we have proposed several choices for the covariance matrix as an additional parameter of the importance density. These are based on an approximation of the mixture component posterior covariance which ensures that the importance sampling asymptotic variance remains finite. We have compared the mixture RBPF with mode based importance sampling, with the simple mixture RBPF and the standard RBPF in a terrain aided positioning scenario. We considered both a challenging setting consisting in very accurate measurements as well as more usual situation with a higher variance of the additive noise. In both situations, it was found that the use of the mode based mixture in the mixture RBPF yielded less divergent tracks. In the case of a low sensor noise, the filter accuracy in terms of mean squared error was also greatly improved. The effect of different covariance matrices used in the proposal density was also investigated and it was found to have moderate effect in the positioning application. However, we believe it can have a higher impact in other applications where the measurement is not as ambiguous since the difference in shape and orientation between the posterior and the predictive distribution at a given time step may be important since, in that case, one single measurement carries noisy but near-complete information about the hidden state. Overall, this implementation of a mixture particle filter is an alternative to the auxiliary particle filter in cases where it is difficult to give an appropriate approximation of the predictive likelihood, a step necessary to compute the auxiliary weights : for instance, strategies that fit a Gaussian distribution may fail when dealing with multimodal distributions. In the same way, the MRBPF avoids linearizations such as in the particle filter with EKF proposal that can yield an unstable filter whenever the measurement function is highly non-linear. Nevertheless, care must be taken when the number of modes in the posterior is unknown and high as the clustering step can be time consuming since non-parametric techniques (e.g. the mean-shift algorithm) have to be used.

APPENDIX B PROOF OF PROPOSITION 1

Lemma 1. Assume the prior q is a Gaussian density with mean μ and covariance P . Then if Σ -P is positive definite then sup x∈R d q(x) q(x) < ∞ and V q is finite.

The proof is readily obtained by working out the log ratio log q(x) q(x) and is omitted here for brevity. Since a sufficient condition for finite asymptotic variance of the importance weights is given by Σ -P > 0, let us characterize this inequality . First, this is equivalent to P

-1 -Σ -1 > 0. Σ -1 -P -1 < 0 ⇔ x T (Σ -1 -P -1 )x < 0 ∀x ∈ R d \ {0} ⇔ sup x∈R d \{0} x T Σ -1 x x T P -1 x < 1 (58)
Let P -1 = D T D be the Choleski decomposition of P -1 where D is an upper triangular matrix.

Let C = D -T Σ -1 D -1 where D -T = (D -1 ) T . Then, x T Σ -1 x x T P -1 x = x T D T CDx x T D T Dx Setting y = Dx,
x T Σ -1 x x T P -1 x = y T Cy y T y yields sup

x∈R d \{0}
x T Σ -1 x x T P -1 x < 1 ⇔ sup

y∈R d \{0} y T Cy y T y < 1 (59)
It is a well known result ( [28, p. 176]) that the supremum of the Rayleigh quotient y T Cy y T y equals the largest eigenvalue of C.

APPENDIX C PROOF: NEAREST SYMMETRIC POSITIVE DEFINITE MATRIX SATISFYING FINITE IMPORTANCE WEIGHTS ASYMPTOTIC VARIANCE

Recall the optimization problem minimise Σ -Ĵ-1 2 F such that Σ -P > 0 The minimizer may be obtained thanks to Higham's theorem.

Theorem 1 (N. Higham [START_REF] Higham | Computing a nearest symmetric positive semidefinite matrix[END_REF]). Let A a real symmetric matrix. The following optimization problem: minimise X -A 2 F s.t. X ≥ 0 has a unique solution X * defined by

X * = 1 2 (A + H) ( 60
)
where H is a symmetric positive semidefinite matrix such that A = UH where U T U = I. A = UH is the polar decomposition of A.

Noticing that Σ -Ĵ-1 2 F = (Σ -P ) -( Ĵ-1 -P ) 2 F , we have min

Σ-P ≥0 Σ -Ĵ-1 2 F = min X≥0 X -( Ĵ-1 -P ) 2

F

Let X F = arg min X≥0 X -( Ĵ-1 -P ) F . Then according to Higham's theorem,

X F = 1 2 ( Ĵ-1 -P + H)
where U and H are such that Ĵ-1 -P = UH and U T U = I. It follows that

Σ F = 1 2 ( Ĵ-1 + P + H) (61)
Note that the initial goal was to minimise Σ -Ĵ-1 2 F subject to Σ -P > 0 while the above minimizer satisfies the constraint Σ -P ≥ 0. Therefore, to satisfy the positive definite constraint, one may consider the matrix

Σ F = 1 2 ( Ĵ-1 + P + H) + κI d
where κ is a small positive constant and I d is the d dimensional identity matrix. Formula (61) requires the derivation of H which can be obtained as outlined in [START_REF] Higham | Computing a nearest symmetric positive semidefinite matrix[END_REF] as follows: Let Ĵ-1 -P = Z ΛZ T be the orthogonal diagonalization of the symmetric real matrix Ĵ-1 -P , where Λ = diag(λ 1 , . . . , λ d ) is the diagonal eigenvalue matrix and ZZ T = I. Then setting Λ+ = diag(max(0, λ1 ), . . . , max(0, λd )) • for j = 1, . . . , M k , compute α j,k as a function of {α j,k-1 } 1≤j≤M k-1 and {β j,k-1 } 1≤j≤M k-1 using equation (47)

• Compute the weighted average of the non-linear substate xk,1 =

M k j=1 i∈I j,k ω i k x i k,1 . • [Resampling] let j = 1.
while j ≤ M k if α j,k < α min remove the cluster C j,k and sample N j new particles according to the empirical mixture distribution with weights α l,k r =j α r,k , l = j else * compute N j ef f according to (45) * if

N j ef f
Nj < N th resample the particle cloud of the current cluster. Additional local regularization may be performed as described in [START_REF] Murangira | Robust regularized particle filter for terrain navigation[END_REF] * j = j + 1

  ) a = 6378137 m, b = 6356752.3 m are the earth's ellipsoid semi major axis and semi minor axis as defined by the World Geodetic System (WGS 84), while e = b(a-b) ais the eccentricity. The true aircraft trajectory is a straight line with near constant velocity as shown in figure2and an altitude of 2923 m.

Fig. 2 .

 2 Fig. 2. Aircraft trajectory (a) and terrain elevation profile (b)

  4000 particles for the RBPF • N MRBP F = 4000 particles for the MRBPF • N MRBP F -MAP = 3000 particles for the MRBPF-MAP • resampling threshold : N th = N 3 where N is the number of particles • threshold for cluster elimination α min = 10 -20

Fig. 3 .Fig. 4 .Fig. 5 .Fig. 6 .Fig. 7 .Fig. 8 .

 345678 Fig. 3. σv = 5m: RMSE of the position drift estimates ; red line : Posterior Cramer-Rao bound (PCRB)

. 1 ξ i 2 .

 12 * let q j (x) = ϕ(x|x j,k|k-1 , Pj,k|k-1 ) be the Gaussian approximation of the prior of the j-th component.* compute x * j,k = arg max x k ∈R d g k (x k )q j (x k ) as outlined in section III-C. * set qj (x) = ϕ(x|x * j , Σ * j ) where Σ * j = Pj,k|k-1 , Σ * j = P rot or Σ * j = Σ F (see (32)) * draw N j i.i.d. samples ξ i ∼ qj , i ∈ I j,k and N j = Card(I j,k ) * set x i k,1 = ξ i 1 and x i k,2 = ξ i 2 where ξ i = ξ i * compute ωi k and ω i k according to (42) * set β j,k = 1 Nj i∈I j,k ωi k * indexMAP j = 1

  N} is the set of particle index targeting the component p j (x k |y 0:k ) at time k.

	identical,
	p(x k+1 |y 0:k+1 ) ≈ pk+1|k+1	(15)
	Mk
	=	α j,k+1
	j=1	i∈Ij,k
	At time
	k + 1, assuming that the number of components remains

  sample x i 0,1 ∼ p(x 0,1 ), i = 1, . . . , N • set x i 0,2 = x0,2 andP i 0 = P0 end if if k = 1, . . . , n, Cluster the particle set into M k clusters C j,k using meanshift clustering for instance with bandwidth parameter h MS . [C j,k , I j,k ] = meanshiftClustering({x i k } N i=1 , {ω i k } N i=1 , K, h MS ) • [Particle measurement update]Compute the unnormalized weights ωi Importance sampling] for j = 1, . . . , M k -β j = i∈I j,k ωi

	entails
	Σ F = Z Λ+ Z T + P	(62)
	APPENDIX D
	MRBPF-MAP ALGORITHM
	Algorithm 2: MRBPF-MAP
	if k = 0
	k
	-ω i k = -compute N j ωi k βj , i ∈ I j,k ef f according to (45)
	-indexMAP j = 0 -if N j ef f /N j ≤ N th,M AP
	* set xj k|k-1,1 = i∈I j,k ω i k|k-1 x i k|k-1,1
	* set xj k|k-1,2 = i∈I j,k ω i k|k-1 x i k|k-1,2 * set P 11 j,k|k-1 = i∈I j,k ω i k|k-1 (x i k|k-1,1 -xj k|k-1,1 )(x i k|k-1,1 -xj k|k-1,1 ) T
	* set P 22 j,k|k-1 = i∈I j,k P i k|k-1 + ω i k|k-1 (x i k|k-1,2 -xj k|k-1,2 )(x i k|k-1,2 -xj k|k-1,2 ) T
	* set P 12 j,k|k-1 = i∈I j,k ω i k|k-1 (x i k|k-1,1 -xj k|k-1,1 )(x i k|k-1,2 -xj k|k-1,2 ) T * set Pj,k|k-1 = ⎛ ⎝ P 11 j,k|k-1 P 12 j,k|k-1

• • [Particle time update] draw x i k|k-1,1 ∼ p(x n k |x n 0:k-1 , y 0:k-1 ) (cf. (

56

)), i = 1, . . . , N. Set ω i k|k-1 = ω i k-1 , i = 1, . . . , N.

• [Kalman filter time update] for i = 1, . . . , N, compute xi k|k-1,2 and P i k|k-1 according to (57).

• [Clustering] k = ω i k-1 p(y k |x i k,1 , y 0:k-1 ), i = 1, . . . ,

N where the likelihood p(y k |x i k,1 , y 0:k-1 ) is given by (53). • [

  Kalman measurement update] • compute P i k according to (55). for j = 1, . . . , M k * if indexMAP j = 0, compute xl,i k , i ∈ I j,k using (54) • work out the posterior mean of the linear substate xk,2 =

	M k j=1	i∈I j,k ω i k x i k,2
	end for

• [

APPENDIX A RBPF ALGORITHM

Algorithm 1 : Rao-Blackwellized Particle Filter for i = 1, . . . , N

• sample x i 0,1 ∼ p(x 0,1 ) • set x i 0|-1,2 = x0,2 and P i 0|-1 = P 0 end for for k = 1, 2, . . .

where

• [Kalman filter time update] for i = 1, . . . , N, compute xi k+1|k,2 and P i k+1|k

and

Karim Dahia Karim Dahia is a senior navigation engineer with the ONERA French Aerospace Lab in Palaiseau, France. His Ph.D. from the Université Joseph Fourier -Grenoble focused on the application of particle filtering to aircraft motion estimation. Dr. Dahia's research interests include robust and optimal navigation as well as filtering for aerospace systems.