Dealing With Measurement Delay

The fusing of delayed measurements with current best estimation of a system states must be done with a mathematically coherent approach. The following figure illustrates a system with multiple sensors which some introduce measurement delays:

Figure 2.31: System With Delayed Measurements

At time 𝑘, this system receives two kinds of measurements: non-delayed measurements, 𝒚̃_𝑘, which are related to the current system states 𝒙𝑘 and delayed measurements, 𝒚̃𝑘∗, corresponding to the

system states at time 𝑙 = 𝑘 − 𝑑, where 𝑑 is the measurement delay of the sensors. From an estimation point of view, the measurement 𝒚̃_𝑘∗ _{should be fused at time 𝑙, causing a correction to the}

states 𝒙̂𝑙 and a decrease of its covariance. The non-delayed measurements acquired in the interval 𝑙

to 𝑘 is fused non optimally if 𝒚̃_𝑘∗ _{is omitted and updated later in time.}

To address this problem, the first method, presented in [103] consists in simply neglecting the delay by performing a normal measurement update when the measurements become available. This method is obviously sub-optimal and may lead to filter divergence if the delay is large.

The second technique, summarized in [103], is to recompute the whole estimated state trajectory during the delayed period. This requires the saving of all measurements (delayed and not delayed) during the complete latency duration as well as the estimated state vector and its covariance when the delayed measurements are acquired. When the delayed measurements are available, the state trajectory is recomputed until the current time. Despite the fact that this solution provides an

Real states Filter states Measurements 𝒙̂𝑘 𝒙̂𝑙 𝒙𝑙 𝒙𝑘 𝒚̃𝑘∗ 𝒚̃𝑘 𝒚̃𝑙

optimal estimation, this technique is practically never used because it requires a lot of computational power and memory particularly if the delay is large.

The third method uses extrapolation of the delayed measurement from time 𝑙 to 𝑘. In order words, this approach makes that the measurements related to states at time 𝑙 will corresponds to the states at time 𝑘 after the extrapolation. This technique is often used due to its simplicity. However it is sub- optimal because it introduces very difficult-to-estimate extrapolation noise in the measurements and the update is performed at the wrong time (the fact that the measurement is available earlier in time is neglected and may decrease the estimation accuracy).

The fourth technique is adapted for the KF and the EKF. It is presented in [103]. This technique can be presented considering the following linear dynamic model (for simplicity, but the technique can be extended to nonlinear model with local linearization):

𝒙𝑘= 𝑨𝑘𝒙𝑘−1 (2.59)

𝒚𝑘= 𝑪𝑘𝒙𝑘 (2.60)

The states corresponding to the latency-lagged measurements, denoted 𝒙𝑙, must be stored in a

buffer and when the lagged measurement 𝒚̃𝑘∗ become available, the filter is updated by computing

the residue as follows:

𝒓𝑘 = 𝒚̃𝑘∗− 𝑪𝑙𝒙𝑙 (2.61)

Since the correct innovation is fused with the wrong estimated states, the filter is sub-optimal and comparable with the third technique, but performs better than the first approach. The performance of this method was improved by [119]. In fact, these papers present a solution, named the Larsen’s method, which addresses the sub-optimality fusion problem. However, the sensitive matrix 𝑪𝑙 and

the measurements noise covariance matrix 𝑹𝑙 must be known when the delayed measurement is

acquired (it is often the case). If these requirements are met, the covariance of the filter can be update at the time 𝑙 as if the measurement 𝒚̃_𝑘∗ is already available. By doing that, the states of the filter can be easily updated using the following equation when 𝒚̃_𝑘∗ become available:

𝒙𝑘|𝑘 = 𝒙𝑘|𝑘−1+ 𝑴𝑲𝑙(𝒚̃𝑘∗− 𝑪𝑙𝒙𝑙) (2.62)

𝑴 = ∏(𝑰 − 𝑲𝑘−𝑖𝑪𝑘−𝑖) 𝑁−1

𝑖=0

𝑨𝑘−𝑖−1 (2.63)

where 𝑲𝑖 is the Kalman gain computed to perform the updates of non-delayed measurements

occurred in the time interval 𝑙 to 𝑘. This improvement still has a small problem: the state covariance is wrong and non-delayed measurement fusions are sub-optimal during the interval 𝑙 to 𝑘 (the state covariance has been updated following the delayed measurement but not the states). To avoid this problem, the author of [119] proposes the utilisation of two filters in parallel. The first filter works as presented previously and provides an optimal estimate only at time 𝑘, when the delayed measurements become available. Between the time 𝑙 and 𝑘, the optimal estimate is provided by a second filter which does not consider the delayed measurement. At time 𝑘, the optimally fused delayed measurement state estimate of the first filter and its covariance are used to reinitialize the second filter. This strategy, called the Larsen’s modified two-filters method, makes that optimal estimate is available at all time at the cost of the computational complexity.

The fifth technique consists in augmenting the state vector of the navigation filter with past state variables fixed in time and corresponding to the time at which the delayed measurement has been acquired [8]. Only the cross covariance between these augmented state variables and the current state variables evolves. When the delayed measurement becomes available, these past state variables are used in the measurement update instead of the current states. In other words, the measurement model becomes a function of the past state variables. Updating these past state variables indirectly corrects the estimation of the current states through the cross covariance. This technique keeps the estimation optimal. However, it cannot work when the time at which the measurement has been taken is known a posteriori. In fact, the measurement delay is often known only when the measurement is available using its timestamp. In this case, it is too late to save the states and their corresponding covariance at the time of the measurement. A typical application in which this approach could be used is mainly when the measurement is triggered by the navigation software. In addition to starting the acquisition of the measurement, the trigger signal starts, at the same time, the state augmentation operation. Another limitation of this technique is that it requires the augmentation of the state vector for each delayed measurements, which might result in a computationally intensive filter.