C Parellier

A Barrau

S Bonnabel

A computationally efficient global indicator to detect spurious measurement drifts in Kalman filtering

We consider the problem of detecting additive structured correlated perturbations affecting the measurement outputs of a system whose state is estimated by a Kalman filter. We advocate the time series of the gradients of the loglikelihood with respect to the output measurements as an indicator, notably through its fast Fourier transform (FFT). This provides a novel unifying method to detect structured perturbations, namely small sinusoidal perturbations with unknown frequency, and slowly growing errors, such as a ramp, or more generally any known incipient profile with unknown starting time. The method allows for identification of their parameters too, i.e., frequency, and starting time of the ramp. Thanks to recent results on backpropagation in Kalman filters, and the use of the FFT, the method remains numerically tractable even for large datasets, as demonstrated by simulations.

Introduction

In this paper, we consider a (physical) linear system whose state is estimated at all times by a Kalman filter (KF) based on linear noisy measurements. In this context one may face the problem of a faulty sensor, which would deviate from the values it is supposed to deliver, due to a malfunction.

The field of fault detection and isolation provides too many tools to be listed exhaustively. In a nutshell, within the context of filtering, most approaches revolve around the residuals, also called innovation process, whose values and statistics are inherently computed by the KF. For linear systems with Gaussian noise, the normalized innovation squared (NIS) follows a chi-square distribution which allows for testing incoming measurements under the nominal model [START_REF] Bar-Shalom | Estimation with applications to tracking and navigation: theory algorithms and software[END_REF]. In this way, biases in the instrument may be identified when testing for the mean of the innovation. Hypothesis testing on the residuals has opened up for powerful techniques based on likelihood ratios [START_REF] Willsky | A survey of design methods for failure detection in dynamic systems[END_REF], that provide efficient tools to detect abrupt changes [START_REF] Willsky | A generalized likelihood ratio approach to the detection and estimation of jumps in linear systems[END_REF][START_REF] Basseville | Detection of abrupt changes: theory and application[END_REF].

In the present paper, we propose a novel approach which is as follows. We consider the offline change detection setting, where given a data sample of N measurements, one tries to detect if an output fault has occurred, of which type it is, and the time instant it started to occur. In spite of the terminology, we aim at onboard implementations, where the data consist of the last N measurements. We assume the system is linear with Gaussian noises, and a Kalman filter estimates its state. We suppose faults come as additive perturbations of the output measurements of two different types:

• Oscillating fault: a sinusoidal signal of unknown moderate amplitude and unknown frequency corrupts the output measurements;

• Incipient faults: a function (typically a ramp with unknown slope) starts corrupting measurements at unknown time instant and slowly grows.

These types of faults are more difficult to detect than outliers because they do not correspond to an abrupt change in the signal. Additive ramps on the outputs or "slowlygrowing errors" are a typical failure of the Gobal Positioning System (GPS), see [START_REF] Diesel | Integration of navigation systems for fault detection, exclusion, and integrity determination-without waas[END_REF][START_REF] Feng | A difference test method for early detection of slowly growing errors in GNSS positioning[END_REF][START_REF] Bhatti | Integrity of an integrated GPS/INS system in the presence of slowly growing errors. part i: A critical review[END_REF][START_REF] Bhatti | Performance of rate detector algorithms for an integrated gps/ins system in the presence of slowly growing error[END_REF], with recent further developments in [START_REF] Zhao | A double-threshold test method for soft faults assisted by bp neural network[END_REF][START_REF] Sun | A novel slow-growing gross error detection method for gnss/accelerometer integrated deformation monitoring based on state domain consistency theory[END_REF], and arise also in process control [START_REF] Kiasi | Detection and diagnosis of incipient faults in sensors of an LTI system using a modified GLR-based approach[END_REF].

Our idea is then to consider the likelihood over a time window of N measurementswhich is readily computed from the Kalman filter's estimates-and to evaluate how a given perturbation profile makes it vary. This constitutes a global indicator in that the log-likelihood is a sum over all measurements. Any profile which yields a large increase in likelihood when subtracted to the measurement signal is then likely to corrupt the measurements.

The problem with this idea is its computation complexity, which has made it prohibitively costly so far. Indeed, it may prove intractable to recompute the entire likelihood for each candidate profile (for instance each frequency of an injected sinusoid) and see how it varies, especially if we target onboard applications. We advocate herein this can be done much more efficiently, leveraging a number of tools. First, we consider the perturbation to be small, and compute a first-order expansion of the negative log-likelihood w.r.t. a candidate perturbation function δy 1 , . . . , δy N :

L(y 1 -δy 1 , . . . , y N -δy N ) ≈ L(y) - N k=1 ∂L ∂y k δy k .
This implies computing the gradients w.r.t. all the measurements denoted y 1 , y 2 , . . . , that is, ( ∂L ∂y k ) 1≤k≤N , which is generally very costly. Our very recent work on backpropagation in Kalman filters [START_REF] Parellier | Speeding up backpropagation of gradients through the Kalman filter via closed-form expressions[END_REF], though, has rendered it possible in very limited running time. Then, we use the fast Fourier transform (FFT) of the obtained signal (the gradients), and its properties to efficiently evaluate the variation in the likelihood corresponding to a given profile (that is, the righmost term above), which yields a fast way of finding the parameters we seek, whether a frequency or a time when an incipient fault starts corrupting the signal.

The paper is organized as follows. In Section II we recall the Kalman filter equations and provide formulas for the sensitivity of the likelihood with respect to measured outputs. We then specify the types of faults our method is able to detect. In Section III we detail the method, and discuss its situation with respect to other-more classicalmethods and its advantages in terms of numerical complexity. Section IV illustrates the interest through numerical simulations.

Sensitivity equations and considered types of faults

In this section we recall a few facts on the Kalman filter and the associated sensitivity equations, and then describe the types of faults we intend to address.

Kalman filter equations in discrete time

Consider a linear system defined by the state-transition matrix F n and the measurement model matrix H n ,

x n = F n x n-1 + w n (1)
y n = H n x n + v n , (2)
corrupted by centered white Gaussian noises denoted by w n and v n , with respectively covariance matrices Q and R. We suppose the initial information on the state comes in the following form x 0 ∼ N(µ, P 0 ) with µ a known vector. At a given time n, the goal is to estimate with maximum accuracy the state x n , given current and past observations y n , y n-1 , . . . , y 1 . This can be achieved by computing the statistics p(x n | y n , y n-1 , . . . , y 1 ) which enables computation of state estimates, and the uncertainty they convey. It turns out the latter is a Gaussian density, whose mean xn|n and covariance P n|n can be obtained recursively through the KF's equations. They consist of a prediction step in which variables are evolved through the noise-free dynamical model

xn|n-1 = F n xn-1|n-1 + B n u n , P n|n-1 = F n P n-1|n-1 F T n + Q , (3)
where B n u n is a known input. It is followed by an update step in which state is corrected in the light of the latest measurement

S n = H n P n|n-1 H T n + R, K n = P n|n-1 H T n S -1 n , (4)
P n|n = (I -K n H n )P n|n-1 , z n = y n -H n xn|n-1 , (5)
xn|n = xn|n-1 + K n z n , (6)
where K n is the (optimal) Kalman gain. The innovation z n = y n -H n xn|n-1 is the prediction error between the measurement y n and its predicted value H n xn|n-1 . It is a centered Gaussian random variable with covariance S n .

Likelihood function

A well-known calculation [START_REF] Bar-Shalom | Estimation with applications to tracking and navigation: theory algorithms and software[END_REF] shows that up to an additive constant, the negative loglikelihood of the measurements over a time window, that is, L :=log p(y 1 , y 2 , . . . , y N ), writes

L = N n=1 log det S n + z T n S -1 n z n . (7)
This is also known as the energy function [START_REF] Särkkä | Bayesian filtering and smoothing[END_REF]. In the sequel we will refer to it as the "inconsistency", as we found it easier to think about it as a measure of inconsistency in the present context. The weighted prediction error terms are easily interpreted: the commensurateness of a prediction error y -Hx is to be evaluated against the associated covariance S n (a large discrepancy is to be expected if the associated covariance is large). This results in a normalized innovation squared (NIS) 1 N N n=1 z T n S -1 n z n which follows a chi-square distribution with N × dim z degrees of freedom under the nominal model [START_REF] Bar-Shalom | Estimation with applications to tracking and navigation: theory algorithms and software[END_REF].

Sensitivity with respect to measurements

It is customary to differentiate the log-likelihood L with respect to the parameter matrices of the Kalman filter Q and R, see [START_REF] Gupta | Computational aspects of maximum likelihood estimation and reduction in sensitivity function calculations[END_REF]. It is less frequent to study its sensibility with respect to measurements y n 's. In this paper, we propose to use the log-likelihood L for fault detection through its sensibility with respect to measurements. The corresponding equations are easily derived through the chain rule of calculus.

Proposition 1. For each 1 ≤ k ≤ N the gradient writes ∂L ∂y k = 2S -1 k z k -2 N n=k+1 K T k Π n-1 i=k+1 F T i (I -K i H i ) T F T n H T n S -1 n z n , (8)
where by convention N N+1 = 0 and Π j j+1 = I (identity matrix).

Proof. To use the chain rule of calculus it is easier to denote by ∂a ∂b the Jacobian matrix of vector a with respect to vector b, which makes gradients row vectors. In this formalism what we seek to prove is the transposed expression

∂L ∂y k = 2z T k S -1 k -2 N n=k+1 z T n S -1 n H n F n Π n-1 i=k+1 (I -K i H i )F i K k ,
where the product is now in reverse order, i.e., Π n-1 i=k+1 (I -

K i H i )F i := (I -K n-1 H n-1 )F n-1 . . . (I - K k+1 H k+1 )F k+1 . Writing l n|n-1 := z T n S -1 n z n = (y n -H n xn|n-1 ) T S -1 n (y n -H n xn|n-1 )
and using standard calculus the Jacobians write

∂l n|n-1 ∂y n = 2z T n S -1 n , ∂l n|n-1 ∂ xn|n-1 = -2z T n S -1 n H n . (9)
We have ∂z n ∂y n = I, and a simple recursion proves that ∂ xn|n-1

∂y k = F n (I -K n-1 H n-1 )F n-1 . . . (I -K k+1 H k+1 )F k+1 K k ,
which describes how y k affects subsequent state estimates n ≥ k +1. The desired formula follows from the chain rule. □ Expression (8) may look cumbersome, but we will see it may be computed quite efficiently in the sequel.

Considered Types of Failures

Throughout the paper, we suppose the measurements to be corrupted by an additive perturbation, that is, what we actually measure is (y + δy) 1≤n≤N with (δy) 1≤n≤N a perturbation function. The considered errors are not independent, in that (δy) 1≤n≤N is a (deterministic) structured known function of time n, parameterized by quantities to be identified.

Oscillating fault (sinusoidal error)

This type of failure adds a periodic disturbance that oscillates at a certain frequency, that is, is of the sinusoidal type. It is more difficult to detect it than a punctual defect since it describes a specific signature, but locally the perturbation may remain a small deviation. The challenge is then to be able to detect that such a perturbation is occurring, and to identify its frequency.

Incipient fault (ramp error)

This failure is characterized by a deviation from healthy measurements that increases linearly over time. As ramps act slowly, they do not correspond to abrupt changes making them more difficult to detect than instantaneous degradations, see [START_REF] Kiasi | Detection and diagnosis of incipient faults in sensors of an LTI system using a modified GLR-based approach[END_REF]. These types of errors occur frequently when using a GPS. They are known in the related literature as slowly growing errors (SGE), see e.g., [START_REF] Bhatti | Integrity of an integrated GPS/INS system in the presence of slowly growing errors. part i: A critical review[END_REF][START_REF] Diesel | Integration of navigation systems for fault detection, exclusion, and integrity determination-without waas[END_REF][START_REF] Feng | A difference test method for early detection of slowly growing errors in GNSS positioning[END_REF][START_REF] Bhatti | Performance of rate detector algorithms for an integrated gps/ins system in the presence of slowly growing error[END_REF][START_REF] Zhao | A double-threshold test method for soft faults assisted by bp neural network[END_REF][START_REF] Sun | A novel slow-growing gross error detection method for gnss/accelerometer integrated deformation monitoring based on state domain consistency theory[END_REF]. This is illustrated on Figure 2. They should be opposed to standard outliers, as illustrated on 1. They also occur in a variety of other domains [START_REF] Kiasi | Detection and diagnosis of incipient faults in sensors of an LTI system using a modified GLR-based approach[END_REF]. The challenge then, in terms of fault identification, is to be able to localize precisely the time when the ramp is injected.

Proposed method

The method proposed in the present paper for failure detection is based on the sensitivity of the likelihood with respect to the measurements. The negative log-likelihood L (or inconsistency) is a function of all measurements. Suppose the system was fed with spurious measurements y k = ỹk +δy k for 1 ≤ k ≤ N, where ỹk is the output corresponding to the system without defect. The likelihood satisfies

L(y 1 -δy 1 , . . . , y N -δy N ) = L(y) - N k=1 ∂L ∂y k δy k + h.o.t. ( 10
)
where h.o.t. stands for "higher order terms" in the perturbation function δy, and where we recall we are dealing with scalar products between the gradients and the δy k . The inconsistency associated with the healthy measurements ỹ1 , . . . , ỹN is expected to be significantly smaller.

Our method builds upon [START_REF] Kiasi | Detection and diagnosis of incipient faults in sensors of an LTI system using a modified GLR-based approach[END_REF], which indicates how inconsistency varies when subtracting the candidate error signal δy from the measured output. This may be schematically written

L measured -L healthy = L(y) -L(ỹ) = ∆L ≈ ∂L ∂y • δy (11)
where "•" denotes a scalar product 1 . If ∂L ∂y • δy is high, it is likely to affect the measurements, as subtracting δy 1 , . . . , δy N from the measurements makes the inconsistency [START_REF] Diesel | Integration of navigation systems for fault detection, exclusion, and integrity determination-without waas[END_REF] drop.

The rationale is as follows. Contrary to standard innovation tests, we calculate the global impact of a failure profile on the likelihood. Ultimately, it may prove a better indicator capable of identifying a broader spectrum of failures. Morevoer, structured failures may be better detected in that the perturbation may be small (and thus the signal to noise ratio low) and yet its effect may build up in the sum in the right-hand-side of (10). 1 There are two different scalar products, not to be confused. Each ∂L ∂y k δy k is a scalar product between vectors of dimension dim y. We omit the dot as it can be written as a row times column matrix multiplication. The • is to be understood as a scalar product between N-dimensional elements, i.e., the sequence of gradients ( ∂L ∂y k ) 1≤k≤N and that of errors (y k ) 1≤k≤N .

Fourier transform

When we turn to errors being related to each other over time, in the sense that (δy k ) 1≤k≤N is a structured function of time, it may prove useful to resort to the discrete Fourier transform (DFT) of the functions. To simplify, let us assume that y k is a scalar output, and let us denote by ∂L ∂y the sequence ( ∂L ∂y k ) 1≤k≤N . From [START_REF] Kiasi | Detection and diagnosis of incipient faults in sensors of an LTI system using a modified GLR-based approach[END_REF], we see we are interested in the quantity N k=1 ∂L ∂y k δy k . Using Pareseval's theorem we have

N k=1 ∂L ∂y k • δy k = 1 N N k=1 F { ∂L ∂y } k • F {δy} * k , (12)
where F (•) denotes the DFT. We now leverage this formula.

Application to sinusoidal error detection

Assume the signal is corrupted by a sine wave, that is,

δy k = ϵ sin(kl 2π N ) (13)
for 1 ≤ j ≤ N, where ϵ is a small scalar. The number of cycles l represents the frequency, which is the quantity we are seeking to identify. It corresponds to an oscillation period of T = N/l. The DFT of this sine wave writes:

F {δy} k =              -ϵ N 2 ı if k = l ϵ N 2 ı if k = N -l 0 otherwise (14)
Recalling ( 12) and ( 11), This leads to

∆L = ϵ 2 ıF { ∂L ∂y } l -ıF { ∂L ∂y } N-l . ( 15
)
As a result, if a sinusoidal wave with number of cycles l is corrupting the measurements, we shall see two peaks in the DFT of the computed function. More precisely, owing to the properties of the DFT, it is in fact sufficient to evaluate the Fourier transform on half the interval, leading to the following mathematical result.

Proposition 2. Assume the signal to be corrupted by an additive sine wave (13) with period T = N/l. Then the maximum of the drop in inconsistency in the sense of (11) corresponds to

max 1≤l≤N/2 -ϵI F { ∂L ∂y } l , (16)
where I denotes the imaginary part.

Proof. As ∂L ∂y is a real function, we have

F { ∂L ∂y } k = F { ∂L ∂y } * N-k , so ∆L = ϵR ıF { ∂L ∂y } l = -ϵI F { ∂L ∂y } l . □
It is thus sufficient to compute the DFT on half the interval for the present purpose, and to seek a peak.

Application to ramp error detection

Suppose the signal is corrupted by a ramp that starts at some time instant m and of the following form

δy (m) k =        0 if k < m ϵ(k -m) if k ≥ m ( 17
)
and that one seeks to identify the starting point m.

It turns out this can be efficiently detected too, using once again the DFT. Indeed, in the present case the inconsistency drop [START_REF] Magill | Optimal adaptive estimation of sampled stochastic processes[END_REF] due to the ramp writes

∆L = N k=1 ∂L ∂y k δy (m) k = N k=1 ∂L ∂y k x m-k
where x is a fixed sequence defined by

x l =        -ϵl if l ≤ 0 0 if l > 0.
Prolonging the sequence ( ∂L ∂y k ) 1≤k≤N with null values outside the interval {1, 2, . . . , N}, the sum of interest may be written in turn as a convolution product

∆L = [( ∂L ∂y ) * x] m . (18)
Applying the convolution theorem for discrete sequences we get our final result for ramp detection.

Proposition 3. The drop in inconsistency related to a ramp starting at time m is given by the value

∆L = F -1 {F ({ ∂L ∂y }) F ({x})} m (19)
This immediately stems from the convolution theorem, as

∆L = N k=1 ∂L ∂y k δy (m) k = [( ∂L ∂y ) * x] m = F -1 {F ({ ∂L ∂y })F ({x})} m .
As a result, a large value of function ( 19) at m indicates a likely time at which the ramp has started corrupting the measurements. It thus remains to evaluate and compare the function at different m.

We see that the method may more generally be applied to detect the time when any (fixed) known profile x is added to the signal, beyond a ramp, e.g., a truncated ramp [START_REF] Kiasi | Detection and diagnosis of incipient faults in sensors of an LTI system using a modified GLR-based approach[END_REF], or step functions. Note that, the slope of the ramp does not play a role for detection, since it acts linearly on the scalar product.

Numerical complexity

We now show that the whole pipeline necessitates a computation cost of O(N log N) operations, and can thus be implemented even if the number of measurements N is large.

Efficient computation of all gradients

The first step of our method consists in computing all the gradients of the inconsistency ( ∂L ∂y k ) 1≤k≤N . From ( 8), we see the overall computation time for obtaining all gradients is at least quadratic in N.

However, leveraging our very recent results on backpropagation through Kalman filters [START_REF] Parellier | Speeding up backpropagation of gradients through the Kalman filter via closed-form expressions[END_REF], it turns out one may compute all the gradients linearly in the number of measurements N. Indeed, (8) satisfies a simple recursion, so that intermediary computations steps are reused in the previous gradient, along the lines of dynamic programming. It is proved in [START_REF] Parellier | Speeding up backpropagation of gradients through the Kalman filter via closed-form expressions[END_REF] indeed that the gradients satisfy the following backward recursion

u N = 0 1×d (20) ∂L ∂y n = K T n u n + 2S -1 n z n n ≤ N (21) u n-1 = F T n [(I -K n H n ) T u n -2H T n S -1 n z n ] (22)
It can be checked that this coincides with (8) indeed, but it makes the computation cost of ( ∂L ∂y k ) 1≤k≤N of order O(N) as it amounts to implementing a recursive backward calculation over the time window. Those equations thus appear as an enabler for efficient implementation.

Efficient computation of the DFT Once the sequence of gradients has been obtained, we may use the FFT to compute its DFT, leading to a O(N log N) computation cost.

Interest of the method

The method bears a superficial resemblance to the local approach [START_REF] Basseville | On-board component fault detection and isolation using the statistical local approach[END_REF], based on a Taylor expansion of the log-likelihood, but the latter performs statistical tests related to the asymptotic behavior. More recently, [START_REF] Kiasi | Detection and diagnosis of incipient faults in sensors of an LTI system using a modified GLR-based approach[END_REF] proposed methods to detect ramps, based on a likelihood ratio between nominal model and ramp starting at time m, for all possible m. In the latter and more generally in the fault detection literature, one tests one or multiple hypotheses for a change in a parameter, which yields a large computation cost when the number of hypotheses increases. Let us compare our approach to the existing for both considered types of faults.

Oscillating fault A standard approach in this case would be to compute the Fourier transform (DFT) of the output signal. However, this shall not prove efficient for frequency recovery, as the signal to noise ratio may be weak, especially if the amplitude of the perturbation is small. By contrast, our approach allows for a much clearer identification since it assesses the direct effect of additive sinusoids on the likelihood with all possible frequencies: by subtracting the correct sinusoid the likelihood dramatically changes, and this shows directly on the FFT of the gradients.

Incipient fault Detecting ramps is a challenge as they do not come as abrupt changes. The innovation does not allow for detection as the fault is present in all measurements, and starts slowly. A classical route would consist in using the multiple model adaptive estimation (MMAE) [START_REF] Magill | Optimal adaptive estimation of sampled stochastic processes[END_REF], where in essence we test multiple candidate models, each corresponding to a different starting time in the interval. This leads to a computationally very demanding solution as many Kalman filters must be run in parallel. To lower computation demands, one could test the interacting multiple models (IMM) instead [START_REF] Bar-Shalom | Estimation with applications to tracking and navigation: theory algorithms and software[END_REF][START_REF] Zhang | Detection and diagnosis of sensor and actuator failures using IMM estimator[END_REF] but it is suboptimal and testing is efficiency and tuning it on the given problem is beyond the scope of this paper. In any case, our point is that we do not have to test many different models with combinatorial explosion: indicators are obtained with linear complexity in the number of measurements data. Another approach [START_REF] Brumback | A chi-square test for fault-detection in kalman filters[END_REF] to such soft failures in navigation consists in comparing the Kalman filter estimates with the prior (dead-reckoning) model and perform a chi-square test, but this does not readily yield the starting point of occurrence.

Numerical Experiments

We now illustrate on a simple example the relevance of the proposed indicators.

Setting

We prove the efficiency of our approach on synthetic data from a particle moving on the real line, and whose acceleration is controlled. The dynamics are

p n = p n-1 + ∆t v n-1 + w p n (23) v n = v n-1 + ∆t a n-1 + w v n (24)
where ∆t = 1 is the integration step, a n ∈ R the acceleration (control input) and w n is the process noise which is assumed to be zero mean Gaussian white noise with diagonal covariance matrix Q. The measurements y n 's consist of the noisy position:

y n = p n + ν n
where ν n is the measurement noise which is assumed to be a centered Gaussian white noise with variance R.

We let2 Q = diag(1e -8 , 1e -4 ) and R = 1.5. The state is estimated by a Kalman filter (see Section 2.1) with input u n = a n and position measurement y n .

The true system's trajectory and the KF's estimates are displayed on Figure 3. The trajectory is obtained by injecting a large acceleration a n roughly every 20 seconds.

Results: Ramp error detection

The measured position is then corrupted by an additive incipient ramp (SGE) starting at m = 39 and defined by: Figure 5 shows the result of the detection of the ramp, plotting the inconsistency drop given by [START_REF] Zhao | A double-threshold test method for soft faults assisted by bp neural network[END_REF] or alternatively (19). The theory is confirmed, as the maximum of the inconsistency drop is clearly seen at the starting time of the ramp. This is remarkable as we can see from Figure 4 that the signal-to-noise ratio is very low. When compared to the normalized innovation squared (NIS), we see the proposed indicator is much more relevant. The plot of the NIS confirms that no dramatic change is visible in the innovation process.

δy k =        0 if k < 39 0.1(k -39) + 0.1 if k ≥ 39 (

Results: Sinusoidal error

Figure 6 shows the measured position in the presence of measurement noise plus a sine wave t → 2.5sin( t 2 ). This corresponds to a frequency f = 1 4π and as we have N = 100 measurements, it corresponds to a number of cycles l = N/T = 100/(4π) ≈ 8.

Figure 7 shows the result of the detection of the sinusoid, plotting the (normalized) quantity I F { ∂L ∂y } l that is opposite to the inconsistency drop, see [START_REF] Willsky | A survey of design methods for failure detection in dynamic systems[END_REF]. The theory is confirmed, as the maximum (of the opposite signal) is clearly seen at the actual frequency of the sinusoid. This is remarkable, here again, as we can see from Figure 6 the signal-to-noise ratio seems very degraded. It is instructive to see the Fourier transform of the innovation process does not reveal any relevant information regarding the frequency that corrupts the measured signal.

Conclusion

In this paper we have proposed a method to detect structured faults in the measured outputs of a dynamical system. It relies on the use of a Kalman filter, and a Taylor expansion of the likelihood of the measurements, and its efficient implementation is enabled by the recent results of [START_REF] Parellier | Speeding up backpropagation of gradients through the Kalman filter via closed-form expressions[END_REF]. This provides a global indicator that allows for detection of structured profiles along with their parameters in spite of an unfavorable signal-to-noise ratio, and without the need to know their magnitude. Contrary to multiple hypothesis testing, the method is very efficient computationally and requires (almost) linear runtime in the data size, enabling onboard implementations.

Although the method may be run onboard, it operates on the fixed sequence of the N past measurements. In the future, it would be worth investigating whether an online counterpart may be derived. Another advantage of the method is that it is not reserved for scalar outputs, and suits multidimensional outputs. In this regard, it would be interesting to test it on more complex datasets obtained through real experiments, notably when navigating with a GPS. Finally, it would be nice to adapt it to the context where the additive perturbation depends on more parameters, for instance a ramp that starts at unknown time m and saturates at unknown time m ′ > m.

Figure 1 :

 1 Figure 1: Illustration of an outlier GPS measurement in case of a navigating vehicle with an inertial measurement unit (IMU) and a GPS receiver.

Figure 2 :

 2 Figure 2: Illustration of a GPS failure of the ramp type, i.e., a slowly growing error (SGE), in case of a navigating vehicle with an inertial measurement unit (IMU) and a GPS receiver.

Figure 3 :Figure 4 :

 34 Figure 3: True (simulated) position and velocity over time, and their estimates obtained by the Kalman filter, in the absence of fault.

Figure 5 :Figure 6 :

 56 Figure 5: Comparison of our proposed indicator and the classic NIS to detect a ramp fault starting at m = 39. 0 10 20 30 40 50 60 70 80 90 100 0 20 40 60 80 time

IFigure 7 :

 7 Figure 7: Comparison of our indicator with the Fourier transform of the innovation process when the signal is corrupted by white noise plus a sinusoidal defect of frequency 1 4π ≈ 0.08.

The magnitudes of the noises were chosen having in mind inertial navigation with an IMU and a GPS as a motivation. In commercial airliners, the IMUs are highly accurate and come with very low process noise.

* This work is supported by CIFRE Grant 2019/1974 from French Agence Nationale de la Recherche et de la Technologie (ANRT).


Sven Goffin
email: sven.goffin@uliege.be

Silvère Bonnabel
email: silvere.bonnabel@mines-paristech.fr

Olivier Brüls
email: o.bruls@uliege.be

Pierre Sacré
email: p.sacre@uliege.be

† S Goffin

Invariant Kalman Filtering with Noise-Free Pseudo-Measurements

In this paper, we focus on developing an Invariant Extended Kalman Filter (IEKF) for extended pose estimation for a noisy system with state equality constraints. We treat those constraints as noise-free pseudomeasurements. To this aim, we provide a formula for the Kalman gain in the limit of noise-free measurements and rank-deficient covariance matrix. We relate the constraints to group-theoretic properties and study the behavior of the IEKF in the presence of such noise-free measurements. We illustrate this perspective on the estimation of the motion of the load of an overhead crane, when a wireless inertial measurement unit is mounted on the hook.

Introduction

Since its introduction in the 1960s, the Extended Kalman Filter (EKF) is probably the most used filter in the industry [START_REF] Kalman | A new approach to linear filtering and prediction problems[END_REF]. Despite its use in many real world applications, it relies on strong assumptions that are rarely met in practice. Moreover, its development is solely based on a probabilistic description of a dynamical and a measurement model, disregarding the geometrical structure of the considered problem. The latter concern gave birth to a proper field that focuses on geometric filtering methods. This research direction provided improved solutions for many useful tasks, like Simultaneous Localization And Mapping (SLAM) [START_REF] Mahony | A geometric nonlinear observer for simultaneous localisation and mapping[END_REF][START_REF] Barrau | Invariant kalman filtering[END_REF][START_REF] Van Goor | A geometric observer design for visual localisation and mapping[END_REF][START_REF] Mahony | An homogeneous space geometry for simultaneous localisation and mapping[END_REF], inertial navigation and localization [START_REF] Barrau | The invariant extended kalman filter as a stable observer[END_REF][START_REF] Hartley | Contact-aided invariant extended kalman filtering for robot state estimation[END_REF][START_REF] Barrau | The geometry of navigation problems[END_REF], or attitude and pose estimation [START_REF] Bonnabel | Control and observer design for nonlinear finite and infinite dimensional systems[END_REF][START_REF] Mahony | Nonlinear complementary filters on the special orthogonal group[END_REF][START_REF] Cohen | Navigation and control of unconventional vtol uavs in forward-flight with explicit wind velocity estimation[END_REF]. The invariance and equivariance properties of frameworks developed in [START_REF] Barrau | The invariant extended kalman filter as a stable observer[END_REF][START_REF] Barrau | The geometry of navigation problems[END_REF][START_REF] Barrau | Non-linear state error based extended kalman filters with applications to navigation[END_REF][START_REF] Mahony | Equivariant filter design for kinematic systems on lie groups[END_REF] are reminiscent of those of linear systems in many regards.

In various contexts, some additional (deterministic) information further constrain the state to belong to a subspace of the state space. Incorporating equality constraints in probabilistic filtering constitutes a significant challenge that has led to the realm of state constrained extended Kalman filtering, see [START_REF] Simon | Kalman filtering with state equality constraints[END_REF]. In this paper, we consider nonlinear constraints, dictated by kinematic of mechanical relations, which arise for instance when estimating the configuration of a multi-body mechanical system whose parts are rigidly linked and equipped with one or several Inertial Measurement Units (IMUs). Especially, we consider the equations associated to an IMU that may move freely in space and assume it is mounted at the end of a rigid "link" whose other end's location is known, or fixed. This may be the case when mounting an IMU on a robotic arm, or on the hook of a crane (a pendulum), as in [START_REF] Rauscher | Motion control of an overhead crane using a wireless hook mounted IMU[END_REF]. In those cases, the known location of the other end of the link provides an (nonlinear) equality constraint that may help estimating the state of the IMU.

One may then resort to state constrained Kalman filtering, see [START_REF] Simon | Kalman filtering with state equality constraints[END_REF]. In the nonlinear case, such methods based on (re)projections are a means of enforcing the constraint. However, they come with no guarantee of consistency whatsoever with the estimation problem at hand, and a brutal (re)projection onto the constraint contains a degree of arbitrariness. Herein, we advocate that such nonlinear constraints may rather be incorporated in the extended Kalman filter and its variants through pseudo-measurements without noise, as they constitute information known with certainty. This technique is perfectly justified in the linear case, and allows for optimal fusion of noisy sensors and deterministic side information. This poses two challenges, though. First, the matrix that must be inverted in the Kalman filter may become rank-deficient, see e.g., [START_REF] Gurumoorthy | Rank deficiency of kalman error covariance matrices in linear time-varying system with deterministic evolution[END_REF]. Second, in the nonlinear case, there is no guarantee the constraint is respected after the pseudo-measurement has been incorporated, contrary to the linear case.

We derive a formulation of the Kalman gain that accommodates the rank deficiency issues that may stem from the noise-free setting. Furthermore, we cast the problem into the framework of the Invariant Extended Kalman Filter (IEKF) by embedding the state space into a Lie group. Although [START_REF] Barrau | Extended kalman filtering with nonlinear equality constraints: a geometric approach[END_REF] provides us with both an inspiration and a mathematical framework, we consider different problems: [START_REF] Barrau | Extended kalman filtering with nonlinear equality constraints: a geometric approach[END_REF] considers the limit case of a noise-free (deterministic) dynamics with noisy measurements, along with an initial condition lying in a constrained set, so that deterministic information is propagated. Herein, we consider arbitrary noisy dynamics (and possibly noisy measurements) starting from an unrestricted initial configuration, and investigate how to incorporate noise-free measurements corresponding to deterministic side information. This comes with different challenges and allows for addressing different problems. Recently, [START_REF] Chauchat | Robust linearly constrained invariant filtering for a class of mismatched nonlinear systems[END_REF] proposed to linearly constrain the Riccati equation for robust invariant filtering, which is a wholly different problem.

The paper is structured as follows. Section 2 specifies the considered class of estimation problems, provides two motivating examples of engineering interest, and highlights the properties one shall pursue and the challenges that are raised in both the linear and nonlinear cases. Section 3 derives a formulation for the Kalman gain in the limit of noise-free measurements in the linear case and shows its corresponding properties. Section 4 extends the use of this gain formulation in the framework of invariant Kalman filtering, and proposes an alternative update for the IEKF in the presence of noise-free measurements. Finally, Section 5 illustrates the performance of the proposed approach for the estimation of the extended pose of the hook of a crane.

Motivating problem

Since the advent of smartphones, the use of cheap IMUs has increased significantly. In this paper, we consider the problem of fusing the IMU measurements with deterministic constraints, treated as noise-free pseudo-measurements.

Mathematical formulation

Free from any prior knowledge about the motion, the general dynamics of an IMU write (e.g., [START_REF] Barrau | The invariant extended kalman filter as a stable observer[END_REF])

R k+1 = R k exp((ω k + w ω k ) × dt), (1a)
v k+1 = v k + (R k (a k + w a k ) + g) dt, (1b)
p k+1 = p k + v k dt, (1c)
where we neglected sensors' biases. The matrix R k ∈ SO(3) is the rotation matrix between the IMU and world frames, and the vectors v k , p k ∈ R 3 are respectively the IMU velocity and position vectors expressed in the world frame at time instant k. The angular velocity ω k and linear acceleration a k output by the IMU are used as inputs to the dynamical system. The vector g denotes the gravity vector expressed in the world frame. The map exp(•) denotes the matrix exponential map and the notation (b) × denotes the skew-symmetric matrix associated with cross product with vector b ∈ R 3 . Even if no assumption is made on the dynamics, the structural and mechanical properties of the body to which the IMU is attached may constrain the IMU pose. When dealing with a rigid body, this comes as noise-free equality constraints of the form

R k r k + α k v k + β k p k = y k , (2)
where

r k , y k ∈ R 3 and α k , β k ∈ R.
Considered problem We aim at devising a meaningful (Kalman) filter to estimate the state of the noisy system (1) from noise-free measurements (2) in real time.

Application examples

Estimating the pose of an IMU under constraints like (2) applies to a wide range of applications. We provide two concrete applications for this problem, which is nonlinear, owing to the state variable R k being a rotation matrix.

Crane state estimation

As a first example, consider a crane on a construction site (see Figure 1). It is technically very feasible nowadays to mount an IMU on the hook that transmits its sensor readings [START_REF] Rauscher | Motion control of an overhead crane using a wireless hook mounted IMU[END_REF]. Estimating the position and velocity of the load from the IMU measurements may open the door to new automation capabilities since it allows for feedback. However, the underlying dynamical model is unknown, as the hook does not necessarily follow a simple pendulum motion, because of external forces like friction and wind. The IMU dynamics (1), though, consist of kinematic relations which are satisfied at all times, whatever the motion. Moreover, when the cable is hanging, the distance between the hook and the cable hang-up point is the cable length l k , which is very accurately measured in modern cranes for safety reasons, see, e.g., [START_REF] Bonnabel | The industrial control of tower cranes: An operator-in-the-loop approach [applications in control[END_REF]. This provides a constraint of the form of (2) with r k = (0, 0, -l k ), α k = 0, β k = 1, and y k = 0 3×1 .

𝑂

Figure 1: Crane with a wireless IMU mounted on its hook.

Robotic arm state estimation

The field of robotics also uses IMUs extensively. As a second example, robotic arms are usually equipped with IMUs in order to precisely estimate the pose of their end effector. Although the dynamics of the arm cannot be predicted in advance, kinematic constraints enforce that the segments of the arm are attached to each other at the corresponding joints, providing constraints of the form [START_REF] Mahony | A geometric nonlinear observer for simultaneous localisation and mapping[END_REF]. This is what is exploited in [START_REF] Hartley | Contact-aided invariant extended kalman filtering for robot state estimation[END_REF] in the form of noisy measurements.

Kalman filtering with noise-free pseudo-measurements

Our problem of estimating noisy systems while treating noise-free equality constraints like (2) fits into the framework of probabilistic filtering. In this framework, the belief at time index k, taking into account measurements up to time j, consists of a multivariate Normal distribution parameterized by a mean xk|j ∈ R n (the state estimate) and a covariance matrix P k|j ∈ R n×n (the associated uncertainty). We advocate indeed that the constraints can be incorporated in a probabilistic filter by using y k as a noise-free pseudo-measurement

y k = h(x k ), letting h(x k ) = h(R k , v k , p k ) = R k r k + α k v k + β k p k . (3)
From the recent theory of two-frames [START_REF] Barrau | The geometry of navigation problems[END_REF], it can readily be checked (3) defines an output that fits into the framework of invariant filtering, and so do the IMU equations ( 1). Noise-free pseudo-measurements convey information that is known for certain. Hence there are two desirable properties one would expect from a (Normal) probabilistic filter.

• Property 1 : After a noise-free pseudo-measurement y k , the state is known to belong to the subset

{x ∈ R n | h(x) = y k }
owing to noise-free measurement. This should translate into h(x k|k ) = y k .

• Property 2 : After a noise-free pseudo-measurement y k , the error covariance matrix P k|k should encode some deterministic information: null variance is to be expected in the perfectly observed directions. Mathematically, it should translate into H k P k|k H T k = 0 m×m with H k ∈ R m×n the Jacobian matrix of h at the estimate xk|k .

The linear case

In the linear case, the Kalman filter-whose equations are recalled in Appendix A-is optimal. Thus, we can expect both properties above to be satisfied. We have

H k xk|k = H k K k y k + (H k -H k K k H k )x k|k-1 (4)
and

H k P k|k H T k = (H k -H k K k H k )P k|k-1 H T k . (5)
Both desirable properties above are satisfied indeed, as soon as

H k K k = I m , since it ensures that H k -H k K k H k = 0 m×n . Recalling that K k = P k|k-1 H T k (H k P k|k-1 H T k ) -1
whenever measurements are noise-free, the desired relation holds if

H k P k|k-1 H T k is invertible. If it
is not, the Kalman gain is not even defined. This may occur when a noise-free measurement is made for the second time: the rank of H k P k|k-1 H T k has dropped and the matrix cannot be inverted anymore. This may also occur when performing a subsequent noise-free measurement along directions that overlap with the span of H k so that the matrix to be inverted is not full-rank.

The nonlinear case

In the nonlinear case, that is, when using an Extended Kalman Filter (EKF), problems accumulate. Indeed, even if

H k P k|k-1 H T k is invertible leading to H k K k = I m ,
there is no reason one should have h(x k|k ) = y k , as we see that (4) relies on h being linear, which is not the case in our problem, see (3), as R k is a rotation matrix that cannot be treated linearly. Moreover, the Jacobian matrix H k depends on the linearization point in EKF design, so that the desired relation H k P k|k H T k = 0 m×m is not properly defined: the Jacobian matrices at different linearization points span different directions, making the condition unclear, in the sense that the dispersion encoded by P k|k has no clear relation to the subset defined by h(x) = y k .

In the sequel, we address the issues that arise both in the linear and nonlinear case.

Handling rank deficiency in the linear case

Let us consider the discrete-time linear system

x k+1 = F k x k + B k u k + w k , (6a)
y k = H k x k . ( 6b
)
where u k ∈ R b is the system input, and where

F k ∈ R n×n , B k ∈ R n×b , and H k ∈ R m×n .
When the measurement y k is noise-free, as in (6b), the Kalman gain (35c) is not defined if H k P k|k-1 H T k is singular, since the measurement noise covariance N k is null. This is logical as then the same measurements does not bring additional information. However, in the noisy setting we may need to update the state regularly to combat the dispersion due to noise. Then H k P k|k-1 H T k may be nearly singular, and we may run into numerical issues. We now address this problem.

Solving for the Kalman gain in the noise-free limit

The following result proves the limit of the Kalman gain when N k → 0 is well defined and provides its expression.

Theorem 1 Assume P k|k-1 to be of rank l ≤ n and write it as

P k|k-1 = L k L T k
where L k ∈ R n×l has linearly independent columns. The following Kalman gain

K k := L k (H k L k ) † , (7)
is the limit of

P k|k-1 H T k (H k P k|k-1 H T k +N k ) -1
as the measurement noise covariance matrix N k shrinks to 0 m×m , where A † denotes the Moore-Penrose pseudoinverse of A.

Note that the given expression is valid whatever the invertibility of the innovation covariance H k P k|k-1 H T k as (H k L k ) † always exists. Proof: Given any matrix A, its Moore-Penrose pseudo-inverse can be expressed as the limit

A † = lim δ→0 A T (AA T + δ I) -1 . ( 8
)
The limit is finite and exists even if AA T is singular [START_REF] Golub | Matrix computations[END_REF]. We pose

A k = H k L k and assume for now that N k = δ I m . lim δ→0 K k = lim δ→0 P k|k-1 H T k (H k P k|k-1 H T k + δ I m ) -1 , (9)
= lim δ→0

L k A T k (A k A T k + δ I m ) -1 , (10)
= L k lim δ→0 A T k A k A T k + δ I m -1 , (11) (8)
= L k A † k , (12)
= L k (H k L k ) † . (13)
When N k is not of the form δ I m , we may upper and lower bound its eigenvalues by matrices of the desired form, and use the squeeze theorem (in the sense of positive semidefinite matrix inequalities), using the fact matrices A k A T k and N k are symmetric.

Properties of the Riccati update (Property 2)

When a noise-free measurement (6b) has been made, it is logical that the corresponding variance drops to zero, that is, H k P k|k H T k = 0 m×m , along the lines of Property 2. We now show this is the case with the obtained limit gain.

Theorem 2 Assume P k|k-1 to be of rank l ≤ n and write it as

P k|k-1 = L k L T k
where L k ∈ R n×l has linearly independent columns. Consider the limit gain [START_REF] Hartley | Contact-aided invariant extended kalman filtering for robot state estimation[END_REF] and the corresponding Riccati update (35e). Then we have

H k P k|k H T k = 0 or equivalently H k P k|k = 0. Proof: We have H k (I -K k H k )P k|k-1 (14) = H k (I -L k (H k L k ) † H k )P k|k-1 (15) = (H k L k -H k L k (H k L k ) † H k L k )L T k (16) = 0 m×n , (17)
where we used the property of the pseudo-inverse that that AA † A = A. This proves the result.

Invariant Kalman filtering with noise-free pseudomeasurements

Consider the discrete-time nonlinear system (1) along with nonlinear output map [START_REF] Barrau | Invariant kalman filtering[END_REF]. The EKF first linearizes the system before applying the machinery of linear Kalman filtering. Gain formulation (7) may thus be applied in the nonlinear case too. Nevertheless, the dependence of Jacobian matrix H k = ∂h(x) ∂x on the linearization point makes the interpretation obscure: the update ensures Property 2 at the linearization point, which is not the true state. The invariant filtering framework brings clarification in this regard.

The Invariant EKF for the considered problem

Let us modify the representation to cast the problem within the invariant framework of [START_REF] Barrau | Invariant kalman filtering[END_REF]. We embed the state into the matrix Lie group SE 2 (3), namely the group of extended poses [START_REF] Barrau | The invariant extended kalman filter as a stable observer[END_REF][START_REF] Brossard | Associating uncertainty to extended poses for on lie group imu preintegration with rotating earth[END_REF]. The state turns into

χ k =   R k v k p k 0 1×3 1 0 0 1×3 0 1   . (18)
Its dynamics satisfies Equation ( 11) of [START_REF] Barrau | Invariant kalman filtering[END_REF] and is said to be group affine, insuring it fits the framework of the Invariant Extended Kalman Filter (IEKF). In this embedding, the measurement function may write as

h(χ k ) = χ k d k =   R k r k + α k v k + β k p k α k β k   , (19)
where d k = (r k , α k , β k ), which fits into the theory of [START_REF] Barrau | The invariant extended kalman filter as a stable observer[END_REF] and corresponds to leftinvariant outputs (observations in the fixed frame). Along the lines of [START_REF] Barrau | The invariant extended kalman filter as a stable observer[END_REF] we define the innovation as z k = χ-1 k|k-1 y k -d k . The invariant framework introduces the nonlinear and linearized errors η k|j ∈ SE 2 (3) and ξ k|j ∈ R 9 defined by

η k|j = χ-1 k|j χ k = exp(ξ k|j ), (20)
where exp(•) now denotes the Lie exponential map of group SE 2 (3). To derive the Jacobian matrices, the fastest way is to readily retrieve them from the formulas of the recent two-frames theory [START_REF] Barrau | The geometry of navigation problems[END_REF]. In this context the left-

invariant error is R-1 k|j R k , R-1 k|j (v k -vk|j ), R-1 k|j (p k -p) , the innovation is z k = ( R-1 k|k-1 R k -I)r k +α k R-1 k|k-1 (v k -v k|k-1 )+β k R-1 k|k-1 (p k -pk|k-1
) and the theory yields, see Proposition 13 of [START_REF] Barrau | The geometry of navigation problems[END_REF],

H k = -(r k ) × α k I 3 β k I 3 , (21)
and denoting Ω k := exp(dt(ω k ) × ), we have as in [START_REF] Barrau | The geometry of navigation problems[END_REF]]

F k =   Ω -1 k 0 0 -dt Ω -1 k (a k ) × Ω -1 k 0 0 dt Ω -1 k Ω -1 k   . (22)
We see we recover the important property of IEKF that the Jacobian matrices are state-independent. The IEKF updates its estimate according to

χk|k = χk|k-1 exp(K k z k ). ( 23
)
The following sections presents how the invariant framework naturally ensures Property 2 and what can be done to satisfy at best Property 1. Since the problems tackled here arise from the noise-free nature of the considered pseudo-measurements, our work constitutes already a contribution in its own right compared to [START_REF] Barrau | Invariant kalman filtering[END_REF], as they only consider noisy measurements.

What about desirable Property 2?

In the formalism of invariant filtering, the constraint (3) writes χ k d k = y k . This noise-free pseudo-measurement informs us that the state belongs to the subset

H = {χ ∈ SE 2 (3) | χd k = y k }. (24)
We first recall how the IEKF encodes uncertainty, see [START_REF] Barrau | Invariant kalman filtering[END_REF].

Definition 1

The IEKF belief at time instant k, accounting for measurements up to time j, for the true state χ k is a concentrated Gaussian distribution on the Lie group [START_REF] Bourmaud | Continuousdiscrete extended kalman filter on matrix lie groups using concentrated gaussian distributions[END_REF] χ k = χk|j exp(ξ k|j ), ξ k|j ∼ N (0 n×1 , P k|j ), [START_REF] Bourmaud | From intrinsic optimization to iterated extended kalman filtering on lie groups[END_REF] with χ the "noise-free" mean estimate, and P the covariance matrix encoding the statistical dispersion around the mean.

As a result, for the belief to be consistent with the set H, we would ideally like the entire updated distribution to lie within H. It turns out to be the case indeed, as long as the updated mean is in the right subspace of course. This proves the structure of the invariant filtering framework is consistent with the physical problem indeed.

Theorem 3 Consider the IEKF's covariance matrix P k|k after updating its state via observation (3). Then if the updated estimate χk|k satisfies χk|k ∈ H, all the probabilistic dispersion encoded by the updated covariance matrix according to our belief model [START_REF] Bourmaud | From intrinsic optimization to iterated extended kalman filtering on lie groups[END_REF] lies within H, that is, for all random ξ ∈ N (0, P k|k ), we have χk|k exp(ξ) ∈ H.

Proof:

The result is a direct application of the following property from [START_REF] Barrau | Extended kalman filtering with nonlinear equality constraints: a geometric approach[END_REF][START_REF] Chauchat | Kalman filtering with a class of geometric state equality constraints[END_REF], which can be proved through a series expansion of the exponential, see the proof of (iv) of Theorem 1 in [START_REF] Chauchat | Kalman filtering with a class of geometric state equality constraints[END_REF].

Proposition 1 Consider an element ζ of the Lie algebra of SE 2 (3) identified with R 9 . Let H k denote the Jacobian from invariant filtering associated to map h(χ) := χd k , i.e. given by [START_REF] Brossard | Associating uncertainty to extended poses for on lie group imu preintegration with rotating earth[END_REF] in our problem. Then we have (χ ∈ H and

H k ζ = 0) ⇒ χ exp ζ ∈ H.
Now, the Riccati update with the limit gain [START_REF] Hartley | Contact-aided invariant extended kalman filtering for robot state estimation[END_REF] ensures that H k P k|k = 0 m×n , see Theorem 2. Thus any element ξ in the span of P k|k satisfies H k ξ = 0

We thus see that Property 2 is satisfied in a way that is meaningful and leads to a consistent belief: the covariance matrix P k|k being aligned with the actual dispersion, regardless of the linearization point. This is a clear improvement over the conventional EKF, that possesses none of the desired properties. In practice, this explains why the IEKF outperforms the EKF for the considered problem, as can be seen below. But before, let us turn to Property 1.

What about Property 1?

In the previous theorem we had to assume Property 1 to be satisfied, that is, χk|k ∈ H for Property 2 to make complete sense. Unfortunately, due to the nonlinear nature of the problem, the IEKF does not satisfy Property 1. All we know is that the residual innovation (in other terms the prediction error) is null up to second order terms after the update, that is,

z k|k = χ-1 k|k y k -d k = O ∥ξ k|k ∥ 2 , (26)
and as a result χk|k may not belong to the expected set H.

To address this issue, we now propose an alternative IEKF update procedure to reduce the norm of the residual innovation. The rationale is the following. Pseudo-measurements come for free, in the sense that they are not actual sensor measurements, but side information that one has for certain. In principle, once a noise-free measurement is made, the drop in uncertainty is fully incorporated in the belief, so that making the same measurement immediately does not change the belief. In practice, we have seen this is not the case, owing to undesirable effects of nonlinearity. However, there is no reason one could not reuse this same information several times in a row.

Our idea is thus to update the estimate with the same noise-free pseudomeasurement y k and the same Kalman gain K k as long as it makes the prediction error decrease, in other words cycle on the noise-free measurement until ∥z k|k ∥ stabilizes. This procedure makes sense if we keep the gain K k constant when performing the same noise-free measurement several times, since as soon as K k is updated it does not correct the state along the measurement direction, owing to Property 2 (as K k z k lies in the span of P k|k ). The corresponding procedure is detailed in Algorithm 1.

Remark 1 Algorithm 1 reduces the residual innovation but is not always able to completely eliminate it. Indeed, the innovation can be decomposed as

z k = z k,∥ + z k,⊥ , (27)
Algorithm 1 Update procedure for the IEKF with noise-free pseudomeasurements.

1: Compute K k using (7) 2: z 0 k ← χ-1 k|k-1 y k -d k 3: χ ← χk|k-1 exp(K k z 0 k ) 4: z 1 k ← χ -1 y k -d k 5: i ← 1 6: while ∥z i k -z i-1 k ∥ > tol do 7: χ ← χ exp(K k z i k ) 8: z i+1 k ← χ -1 y k -d k 9: i ← i + 1 10: end while 11: χk|k ← χ 12: P k|k ← (I -K k H k )P k|k-1 (I -K k H k ) T
where z k,∥ ∈ R(S k ) and z k,⊥ ∈ R(S k ) ⊥ , with z k,⊥ = O ∥ξ k|k-1 ∥ 2 being entirely due to linearization errors and S k = H k P k|k-1 H T k . The component z k,⊥ lies in the kernel of K k , i.e., K k z k,⊥ = 0 n×1 . This means this part of the innovation accounts for a component ξ k,⊥ of ξ k|k-1 that lives within R(P k|k-1 ) ⊥ and cannot be corrected using Kalman filtering techniques. As a consequence, the updated state χk|k becomes slightly inconsistent with P k|k . In most applications, this inconsistency issue is immediately fixed at the propagation stage as process noise reintroduces uncertainty in the directions for which the last noise-free update negated the variance. In the absence of process noise, this is problematic as it makes the filter become quickly overconfident. This problem is left unsolved here and will be the subject of future work.

Estimation of the pendulum angle for a crane

We consider the problem of estimating the extended pose (orientation, position, velocity) of an IMU fixed on the hook of a crane as presented in section 2 and illustrated in Figure 1, and compare the three following filters:

• EKF : a conventional extended Kalman filter.

• IEKF : a conventional invariant extended Kalman filter.

• Noise-free IEKF : the invariant extended Kalman filter that implements gain [START_REF] Hartley | Contact-aided invariant extended kalman filtering for robot state estimation[END_REF] and uses Algorithm 1 to further refine its estimate at each pseudomeasurement.

For the conventional EKF and IEKF, the gain was computed by setting the noise covariance N k to the small value of 10 -4 I 2 and letting

K k = P k|k-1 H T k (H k P k|k-1 H T k + N k ) -1 .
Estimating the IMU pose in the 3D space with cable length as the unique constraint raises observability issues that might obscure the big picture of the present paper. For the sake of simplicity, the IMU is assumed to stay in the plane ϕ k = 0 so that we consider the associated 2D problem, letting χ k ∈ SE 2 (2) and ξ k|j ∈ R 5 .

The ground-truth simulation is the trajectory followed by the hook when it starts with an initial angle θ 0 = 20 • and no angular velocity. No other external force than gravity is simulated. The crane cable is assumed to stay straight during the entire simulation and its length varies according to the profile displayed in Figure 2. The simulation is stopped after 2 s. The IMU and the three filters operate at the same frequency of 100 Hz. The initial error covariance matrix is set to

P 0|0 = 0.05 2 0 1×4 0 4×1 0.5 2 I 4 . (28)
The gyroscope and accelerometer of the IMU are affected by Normal noise of zero mean and covariance

E (w ω k ) 2 = (0.005) 2 , ( 29
) E w a k (w a k ) T = (0.005) 2 I 2 , (30)
where E(•) denotes the expectation operator. The error

ξ k = log χ-1 k|k χ k (31)
is used to compare the performances of the three filters, where log(•) : SE 2 (2) → R 5 is the logarithmic map of SE 2 (2). The comparison is carried over 30 simulations in which the initial error ξ 0 is drawn randomly from the distribution N (0 5×1 , P 0|0 ). The average and standard deviations of the norm of the error ξ k are computed for the three filters. Results are plotted in Figure 3. Two general comments can be made:

1. The conventional extended Kalman filter is outperformed by its invariant filtering counterparts: its convergence is way slower, with a large variability across simulations.

2. The noise-free IEKF exhibits the best convergence rate, with only 6 time steps needed on average for ∥ξ k ∥ to go bellow 1% of the initial error ∥ξ 0 ∥, against 59 and 191 for the classical IEKF and EKF. It is also the method with the lowest variability from one simulation to another. Note that with a tolerance tol = 10 -7 , Algorithm 1 performs 2.03 cycles on average before ∥z k|k ∥ stabilizes.

This simulation shows that treating equality constraints like (2) as proper noise-free pseudo-measurements clearly benefits the estimation and results in extremely fast convergence.

Remark 2

We indicate the failure to ensure Property 1 can sometimes make the noise-free IEKF diverge, when the residual innovation is too large. This leaves room for improvement, which is the subject of ongoing work. This work presented a practical and efficient solution for incorporating equality constraints of the form (2) in the (extended) Kalman filtering framework. We advocated such constraints should be expressed as noise-free pseudo-measurements. We also derived a formulation of the Kalman gain that solves singular matrix inversion issues that may be encountered when dealing with noise-free pseudomeasurements. Using a proper matrix Lie group embedding and the theory of invariant filtering [START_REF] Barrau | Invariant kalman filtering[END_REF] leads to Normal filters that correctly encode the actual physical uncertainty in the problem: their belief is consistent with the constraint in that the covariance matrix after update corresponds to an uncertainty that is wholly contained within the constrained subset. Finally, an alternative update for the IEKF was proposed in order to mitigate the impact of linearization errors in the noise-free update process. The performance of this method was evaluated on the task of estimating the extended pose of the hook of a crane, and was proved to outperform both the EKF and IEKF in this context.

As a perspective, we would like to thoroughly treat the case of the 3D crane, which poses some potential observability issues. We also intend to derive a general theory for noise-free measurements in the context of invariant filtering, namely for two-frame systems [START_REF] Barrau | The geometry of navigation problems[END_REF]. In this context, we would like to explore how to fully enforce the constraint y k = h( χk|k ) at update in a natural way, that is, while retaining all the information that has been acquired before update. This might be achieved via the iterated Kalman filter [START_REF] Bell | The iterated kalman filter update as a gauss-newton method[END_REF][START_REF] Bourmaud | From intrinsic optimization to iterated extended kalman filtering on lie groups[END_REF]. To do iterated EKF in the present context, we anticipate the results developed at Theorem 2 may prove useful.

Appendices A Kalman filter

Let x k ∈ R n be the state to estimate. The discrete-time Kalman Filter (KF) is an optimal probabilistic filter that fuses information coming from two noisy sources: the state dynamics model

x k+1 = F k x k + B k u k + w k , (32)
with F k ∈ R n×n , B k ∈ R n×b , input u k ∈ R b , and process noise w k ∈ R n with covariance Q k , and measurements

y k = H k x k + n k (33)
with H k ∈ R m×n and measurement noise n k ∈ R m with covariance N k . The KF makes the assumption that ξ k|j = x k -xk|j ∼ N (0 n×1 , P k|j ), where x, ξ and P respectively denote the filter estimate, the estimation error and the associated covariance matrix. One iteration of the KF consists of two stages: the propagation and the update.

Propagation The KF propagates its belief through the state dynamics model as follows:

xk+1|k = F k xk|k + B k u k , (34a)
P k+1|k = F k P k|k F T k + Q k . ( 34b
)
Update Receiving measurement y k , the KF updates its belief as follows:

z k = y k -H k xk|k-1 , (35a)
S k = H k P k|k-1 H T k + N k , (35b)
K k = P k|k-1 H T k S -1 k , (35c) xk|k
= xk|k-1 + K k z k , (35d)
P k|k = (I n -K k H k )P k|k-1 (35e)
where z k is called the innovation, S k is the innovation covariance, and K k is the Kalman gain.

Figure 2 :

 2 Figure 2: Evolution of the length of the crane cable l k as a function of the time index.

Figure 3 :

 3 Figure 3: Mean norm of the error function ξ k as a function of the time index, computed over 30 simulations. The standard deviation of the norm is displayed in light colors.


Valerie Ciarletti

Dirk Plettemeier

Yann Hervé

Alice Le Gall

Wolf-Stefan Benedix

Yun Lu

The radar WISDOM for the ExoMars rover mission Interpretation of the polarimetric data and contribution to the operations

WISDOM (Water Ice Subsurface Deposits

Observation on Mars) is a ground penetrating radar designed to characterize the shallow subsurface of Mars, it will be accommodated on the rover of the ExoMars mission and will provide global information about the landing site geological history as well as accurate descriptions of the buried structures that will be essential to collect the samples at depth. In this paper, we focus on the tools that have been developed to process the signal and the polarimetric data in order to achieve the technical and scientific objectives of WISDOM and contribute to the success of the rover mission. Experimental data acquired in natural environments will be shown and analyzed.

The WISDOM radar for ExoMars

WISDOM is a polarimetric radar, it can be operated in co/cross-polar mode, which is essential to characterize the shape of buried reflectors, estimate the roughness of the surface.

WISDOM will be accommodated on the rover of the ExoMars 2020 mission [START_REF] Ciarletti | WISDOM GPR Designed for Shallow and High-Resolution Sounding of the Martian Subsurface[END_REF] [START_REF] Ciarletti | The WISDOM radar: Unveiling the subsurface beneath the ExoMars Rover and identifying for the best location for drilling[END_REF].

The main objectives of WISDOM are to:

• Give clues into the geological context of the investigated site by providing information about the structure of the subsurface (layers, blocks,...), constraining the composition of the detected units (porosity, composition) and mapping the distribution and state of the subsurface water • Provide input for the identification of the most promising locations for sampling • Provide guidance for the drilling operations

The experimental data set

We are leading a series of field tests in natural environments order to acquire experimental data in a variety of environments. These data are currently used to design the data processing pipeline and validate the algorithms. To assess the performances and limits of the tools, it is essential to operate on a number of interesting geological structures (outcrops, layered subsurface, sedimentary rocks …).

We use a prototype of the instrument representative of the flight model to perform the measurements. The WISDOM prototype is mounted on a cart inside a case and the antenna system is 40 cm above the surface as shown on Fig.

Data Processing

For the purpose of the operations on Mars, as automatic as possible codes are developed. They allow to detect in limit time layers and buried blocks.

High resolution algorithms are used to accurately retrieve the surface topography. On smooth surfaces, the amplitude of surface echo provides a first estimate of the permittivity value which is used to convert the measured delays in distances. Entropy computation allows a local characterization of the heterogeneities. Image processing codes are developed to map the interfaces below the surface.

On-going work is done to optimize the processing of the data and a collaboration with the Ma-MISS team has recently be initiated to work on and optimize the necessary synergy with the other instruments.

1

 1

Fig. 1 :

 1 Fig.1 : WISDOM prototype during a field test in South of France

Acknowledgements

The WISDOM development has be supported by the CNES French agency and DLR German agency.