Robust LCEKF for Mismatched Nonlinear Systems with Non-Additive Noise/Inputs and Its Application to Robust Vehicle Navigation

Abstract

It is well known that the standard state estimation technique performance is particularly sensitive to perfect system knowledge, where the underlying assumptions are: (i) Process and measurement functions and parameters are known, (ii) inputs are known, and (iii) noise statistics are known. These are rather strong assumptions in real-life applications; therefore, a robust filtering solution must be designed to cope with model misspecifications. A possible way to design robust filters is to exploit linear constraints (LCs) within the filter formulation. In this contribution we further explore the use of LCs, derive a linearly constrained extended Kalman filter (LCEKF) for systems affected by non-additive noise and system inputs, and discuss its use for model mismatch mitigation. Numerical results for a robust tracking and navigation problem are provided to show the performance improvement of the proposed LCEKF, with respect to state-of-the-art techniques, that is, a benchmark EKF without mismatch and a misspecified EKF not accounting for the mismatch.

1. Introduction

The design and use of state estimation techniques is fundamental in a plethora of applications, such as robotics, tracking, guidance and navigation systems [1,2,3,4]. For a linear dynamic system, the Kalman filter (KF) is the the best linear minimum mean square error (MSE) estimator. The most widespread solution for nonlinear systems is to resort to system linearisations, leading to the so-called linearised or extended KF (EKF) [3]. In both cases, as well as for more advanced techniques such as sigma-point filters [5], the main assumption is perfect system knowledge: (i) Known process and measurement functions, and their parameters, (ii) known inputs, and (iii) known noise statistics (i.e., first and second order moments for the KF and EKF). However, these are rather strong assumptions in real-life applications, where the noise statistics’ parameters may be unknown to a certain extent, inputs may be uncertain and system parameters may be misspecified. The performance degradation of minimum MSE estimators under model mismatch has been widely reported in the literature [6,7,8,9,10], which is a reason why there exists a need to develop robust filtering techniques able to cope with mismatched systems.

Notice that in the robust filtering literature, a lot of effort has been devoted to the mitigation of possible noise statistics’ parameters misspecification, that is, to coping with non-nominal or unexpected noise behaviours. In that perspective, we may cite the robust KFs able to counteract the presence of outliers [11], the ones that estimate Gaussian noise covariances [12,13], or alternative formulations to deal with heavy-tailed distributions [14,15]. In contrast, few contributions explored how to counteract a mismatch on system matrices/functions or the filter initialisation. Within the KF framework, the latter can be solved by either the information filter form of the KF ([3] §6.2) or the so-called Fisher initialisation [16], which can be generalised by imposing initial distortionless constraints [17], i.e., the so-called minimum variance distortionless response (MVDR) estimators. In order to further generalise these MVDR results, how to incorporate non-stationary constraints within the KF has been recently proposed by Villà-Valls et al. [18], leading to a general linearly constrained KF (LCKF) formulation, which has also been shown concerning linear systems to generalise the results proposed by Teixeira et al. [19]. Indeed, the use of linear constraints (LCs) has been shown to be a promising robust filter design solution in order to mitigate the impact of misspecified system matrices [18], which complements the existing results cited above for noise parameter mismatch mitigation. Notice that other alternatives exist to estimate unknown system parameters/inputs, such as augmenting the filter state, which are not discussed in this contribution.

Once the general LCKF has been established, it is of interest to extend its use to more general nonlinear settings. A first attempt in the context of additive nonlinear systems was recently proposed by Hrustic et al. [20], where a linearly constrained EKF (LCEKF) was introduced, together with its use to mitigate parametric misspecifications on both system functions. Notice that this approach is fundamentally different from state constrained solutions [21,22], where LCs are imposed on the state and not on the filter. Overall, the use of non-stationary linear constraints for robust filtering in general nonlinear systems is still an open issue. In this contribution we further explore the use of LCs to mitigate the impact of model misspecifications in nonlinear dynamic systems with both non-additive noise and system inputs. (Notations: Italic, lower case boldface and upper case boldface indicate scalar, vector and matrix quantities; $\mathbb{E}\left[.\right]$ denotes the expectation operator; the filter estimates based on measurements up to time k are denoted by ${\widehat{\mathbf{x}}}_{k|k}$; $\mathcal{N}(\mu ,\mathbf{R})$ is the normal distribution of mean $\mu$ and covariance $\mathbf{R}$; ${\mathbf{m}}_{\mathbf{z}}$ and ${\mathbf{C}}_{\mathbf{z}}$ denote the mean and covariance of a given variable $\mathbf{z}$.) The main contributions of this article are:

• The derivation of a LCEKF for general non-additive nonlinear systems.

• A demonstration of how to exploit LCs for robust filter design towards the mitigation of parametric modelling errors in both system functions.

• The LCEKF performance improvement with respect to state-of-the-art EKF solutions is validated and discussed for an illustrative tracking and navigation problem.

Notice that the proposed methodology, even if not directly stated throughout the article, can also be used to mitigate a mismatch on both process and measurement noise means (constant or time-varying), which can be regarded as inputs.

2. Background

Consider a linear discrete state–space model, where the state vector ${\mathbf{x}}_k\in {\mathbb{C}}^{P_k}$ must be estimated from the available measurements ${\mathbf{y}}_k\in {\mathbb{C}}^{N_k}$ (for $k\ge 1)$,

$$\begin{array}{cc}\hfill & {\mathbf{x}}_k={\mathbf{F}}_{k-1}{\mathbf{x}}_{k-1}+{\mathbf{w}}_{k-1},{\mathbf{y}}_k={\mathbf{H}}_k{\mathbf{x}}_k+{\mathbf{v}}_k,\hfill \end{array}$$ (1)

with ${\mathbf{F}}_{k-1}\in {\mathbb{C}}^{P_k\times P_{k-1}}$ and ${\mathbf{H}}_k\in {\mathbb{C}}^{N_k\times P_k}$ are known system model matrices, ${\mathbf{w}}_k\in {\mathbb{C}}^{P_k}$ and ${\mathbf{v}}_k\in {\mathbb{C}}^{N_k}$ are the process and measurement noise with zero mean and known covariance. If a minimum set of uncorrelation conditions holds [17], the recursive linear estimator of ${\mathbf{x}}_k$, which minimises the MSE (for $k\ge 1$) is the KF (the superscript ${(·)}^b$ stands for the best solution in the MSE sense),

$$\begin{array}{cc}\hfill & {\widehat{\mathbf{x}}}_{k|k}^b=\left(\mathbf{I}-{\mathbf{K}}_k^b{\mathbf{H}}_k\right){\mathbf{F}}_{k-1}{\widehat{\mathbf{x}}}_{k-1|k-1}^b+{\mathbf{K}}_k^b{\mathbf{y}}_k,\hfill \end{array}$$ (2)

where the optimal gain ${\mathbf{K}}_k^b\in {\mathbb{C}}^{P_k\times N_k}$ is the one that minimises the MSE,

$${\mathbf{K}}_k^b=arg\underset{{\mathbf{K}}_k}{min}\left\{{\mathbf{P}}_{k|k}\left({\mathbf{K}}_k\right)\right\},$$ (3)

$${\mathbf{P}}_{k|k}\left({\mathbf{K}}_k\right)=\mathbb{E}\left[({\widehat{\mathbf{x}}}_{k|k}\left({\mathbf{K}}_k\right)-{\mathbf{x}}_k){({\widehat{\mathbf{x}}}_{k|k}\left({\mathbf{K}}_k\right)-{\mathbf{x}}_k)}^H\right].$$

A possible way to robustify the KF is to incorporate LCs. How to use non-stationary constraints within the KF framework, leading to the so-called LCKF, has been recently proposed in [18]. In this case, the filter is

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}_{k|k}^b& =\left(\mathbf{I}-{\mathbf{L}}_k^b{\mathbf{H}}_k\right){\mathbf{F}}_{k-1}{\widehat{\mathbf{x}}}_{k-1|k-1}^b+{\mathbf{L}}_k^b{\mathbf{y}}_k,\hfill \end{array}$$ (4)

$$\begin{array}{cc}\hfill {\mathbf{L}}_k^b& =arg\underset{{\mathbf{L}}_k}{min}\left\{{\mathbf{P}}_{k|k}\left({\mathbf{L}}_k\right)\right\}\mathrm{s}.\mathrm{t}.{\mathbf{L}}_k{\Delta }_k={\mathbf{T}}_k,\hfill \end{array}$$ (5)

computed from a “constrained” KF recursion [18], with ${\mathbf{L}}_k^b\in {\mathbb{C}}^{P_k\times N_k}$. Such LCKF is fully adaptive and allows to incorporatenew LCs at every time k (${\Delta }_k\in {\mathbb{C}}^{N_k\times Q_k}$ and ${\mathbf{T}}_k\in {\mathbb{C}}^{P_k\times Q_k}$). The use of such LCKF to robustify the filter under a mismatched model was also discussed in [18].

If we consider now an additive noise nonlinear discrete state–space model (NLDSSM),

$$\begin{array}{cc}\hfill {\mathbf{x}}_k& ={\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1}\right)+{\mathbf{m}}_{{\mathbf{w}}_{k-1}}+d{\mathbf{w}}_{k-1},\hfill \end{array}$$ (6)

$$\begin{array}{cc}\hfill {\mathbf{y}}_k& ={\mathbf{h}}_k\left({\mathbf{x}}_k\right)+{\mathbf{m}}_{{\mathbf{v}}_k}+d{\mathbf{v}}_k,\hfill \end{array}$$ (7)

where ${\mathbf{f}}_{k-1}(·)$ and ${\mathbf{h}}_k(·)$ are the known system model (process and measurement) functions, and $\mathbb{E}\left[d{\mathbf{w}}_{k-1}\right]=\mathbf{0}$, $\mathbb{E}\left[d{\mathbf{v}}_k\right]=\mathbf{0}$; a standard approach to derive a nonlinear filter of ${\mathbf{x}}_k$ is to assume that Equations (6) and (7) can be linearised at the vicinity of a nominal trajectory [3] yielding the standard EKF,

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}_{k|k}^b& ={\widehat{\mathbf{x}}}_{k|k-1}^b+{\mathbf{K}}_k^b({\mathbf{y}}_k-{\widehat{\mathbf{y}}}_{k|k-1}^b),\hfill \end{array}$$ (8)

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}_{k|k-1}^b& \simeq {\mathbf{f}}_{k-1}\left({\widehat{\mathbf{x}}}_{k-1|k-1}^b\right)+{\mathbf{m}}_{{\mathbf{w}}_{k-1}},\hfill \end{array}$$ (9)

$$\begin{array}{cc}\hfill {\widehat{\mathbf{y}}}_{k|k-1}^b& \simeq {\mathbf{h}}_k\left({\widehat{\mathbf{x}}}_{k|k-1}^b\right)+{\mathbf{m}}_{{\mathbf{v}}_k},\hfill \end{array}$$ (10)

where ${\mathbf{K}}_k^b$ is computed as in the unconstrained KF with ${\mathbf{F}}_{k-1}\simeq \frac{\partial {\mathbf{f}}_{k-1}\left({\widehat{\mathbf{x}}}_{k-1|k-1}^b\right)}{\partial {\mathbf{x}}_{k-1}^T},{\mathbf{H}}_k\simeq \frac{\partial {\mathbf{h}}_k\left({\widehat{\mathbf{x}}}_{k|k-1}^b\right)}{\partial {\mathbf{x}}_k^T}$. As has been recently proposed in [20], the corresponding LCEKF for additive noise systems is ${\widehat{\mathbf{x}}}_{k|k}^b={\widehat{\mathbf{x}}}_{k|k-1}^b+{\mathbf{L}}_k^b({\mathbf{y}}_k-{\widehat{\mathbf{y}}}_{k|k-1}^b)$, where the constrained gain ${\mathbf{L}}_k^b$ is computed as in the original LCKF but using the linearised matrices, as done in the EKF. Notice that the LCKF in [18] is not applicable to nonlinear systems, and the LCEKF in [20] is only valid for additive systems. The goal of this article is to further analyse these methods and extend their use to more general settings.

3. An LCEKF with Non-Additive Noise and System Inputs, and Its Use in Robust Filtering

We consider now a more general NLDSSM, represented by the following state and measurement equations ($k\ge 1$)

$$\left\{\begin{array}{c}{\mathbf{x}}_k={\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1},{\mathbf{w}}_{k-1}\right)\hfill \\ {\mathbf{y}}_k={\mathbf{h}}_k\left({\mathbf{x}}_k,{\mathbf{v}}_k\right)\hfill \end{array}\right.,{\mathbf{x}}_k\in {\mathbb{C}}^{P_k},{\mathbf{y}}_k\in {\mathbb{C}}^{N_k},$$ (11)

with ${\mathbf{f}}_{k-1}(·)$ and ${\mathbf{h}}_k(·)$ being known system model functions, and both noises ${\mathbf{w}}_k$ and ${\mathbf{v}}_k$ with known mean and covariance. First, we want to obtain the EKF-type linear MMSE filter of ${\mathbf{x}}_k$ that can be written as in the previous cases. If $tr\left({\mathbf{C}}_{{\mathbf{x}}_0}\right)\ll 1$, $tr\left({\mathbf{C}}_{{\mathbf{w}}_{k-1}}\right)\ll 1$, $tr\left({\mathbf{C}}_{{\mathbf{x}}_{k-1}}\right)\ll 1$ and $tr\left({\mathbf{C}}_{{\mathbf{v}}_k}\right)\ll 1$, we can resort to a first order Taylor expansion of ${\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1},{\mathbf{w}}_{k-1}\right)$ at the vicinity of $\left({\mathbf{m}}_{{\mathbf{x}}_{k-1}},{\mathbf{m}}_{{\mathbf{w}}_{k-1}}\right)$, and of ${\mathbf{h}}_k\left({\mathbf{x}}_k,{\mathbf{v}}_k\right)$ at the vicinity of $\left({\mathbf{m}}_{{\mathbf{x}}_k},{\mathbf{m}}_{{\mathbf{v}}_k}\right)$. In such a case, if we restrict ourselves, for legibility, to the usual uncorrelation conditions, i.e., ${\mathbf{C}}_{{\mathbf{x}}_0,{\mathbf{w}}_k}=\mathbf{0}$, ${\mathbf{C}}_{{\mathbf{x}}_0,{\mathbf{v}}_k}=\mathbf{0}$, ${\mathbf{C}}_{{\mathbf{w}}_l,{\mathbf{w}}_k}={\mathbf{C}}_{{\mathbf{w}}_k}{\delta }_k^l$, ${\mathbf{C}}_{{\mathbf{v}}_l,{\mathbf{v}}_k}={\mathbf{C}}_{{\mathbf{v}}_k}{\delta }_k^l$, ${\mathbf{C}}_{{\mathbf{w}}_l,{\mathbf{v}}_k}=\mathbf{0}$, the general form of the EKF becomes

$$\begin{array}{cc}\hfill & {\widehat{\mathbf{x}}}_{k|k}^b={\widehat{\mathbf{x}}}_{k|k-1}^b+{\mathbf{K}}_k^b({\mathbf{y}}_k-{\widehat{\mathbf{y}}}_{k|k-1}^b),\hfill \end{array}$$ (12)

$$\begin{array}{cc}\hfill & {\widehat{\mathbf{x}}}_{k|k-1}^b\simeq {\mathbf{f}}_{k-1}({\widehat{\mathbf{x}}}_{k-1|k-1}^b,{\mathbf{m}}_{{\mathbf{w}}_{k-1}}),\hfill \end{array}$$ (13)

$${\widehat{\mathbf{y}}}_{k|k-1}^b\simeq {\mathbf{h}}_k({\widehat{\mathbf{x}}}_{k|k-1}^b,{\mathbf{m}}_{{\mathbf{v}}_k}),$$ (14)

$$\{\begin{array}{c}{\mathbf{P}}_{k|k-1}^b={\mathbf{F}}_{k-1}{\mathbf{P}}_{k-1|k-1}^b{\mathbf{F}}_{k-1}^H+{\mathbf{N}}_{k-1}{\mathbf{C}}_{{\mathbf{w}}_{k-1}}{\mathbf{N}}_{k-1}^H,\hfill \\ {\mathbf{S}}_{k|k-1}^b={\mathbf{H}}_k{\mathbf{P}}_{k|k-1}^b{\mathbf{H}}_k^H+{\mathbf{M}}_k{\mathbf{C}}_{{\mathbf{v}}_k}{\mathbf{M}}_k^H,\hfill \\ {\mathbf{K}}_k^b={\mathbf{P}}_{k|k-1}^b{\mathbf{H}}_k^H{\left({\mathbf{S}}_{k|k-1}^b\right)}^{-1},\hfill \\ {\mathbf{P}}_{k|k}^b=(\mathbf{I}-{\mathbf{K}}_k^b{\mathbf{H}}_k){\mathbf{P}}_{k|k-1}^b,\hfill \end{array}$$

with ${\mathbf{F}}_{k-1}$, ${\mathbf{N}}_{k-1}$, ${\mathbf{H}}_k$, ${\mathbf{M}}_k$ approximated as: ${\mathbf{F}}_{k-1}\simeq \frac{\partial \left({\widehat{\mathbf{x}}}_{k-1|k-1}^b,{\mathbf{m}}_{{\mathbf{w}}_{k-1}}\right)}{\partial {\mathbf{x}}_{k-1}^T}$, ${\mathbf{N}}_{k-1}\simeq \frac{\partial {\mathbf{f}}_{k-1}\left({\widehat{\mathbf{x}}}_{k-1|k-1}^b,{\mathbf{m}}_{{\mathbf{w}}_{k-1}}\right)}{\partial {\mathbf{w}}_{k-1}^T}$, ${\mathbf{H}}_k\simeq \frac{\partial {\mathbf{h}}_k\left({\widehat{\mathbf{x}}}_{k|k-1}^b,{\mathbf{m}}_{{\mathbf{v}}_k}\right)}{\partial {\mathbf{x}}_k^T}$ and ${\mathbf{M}}_k\simeq \frac{\partial {\mathbf{h}}_k\left({\widehat{\mathbf{x}}}_{k|k-1}^b,{\mathbf{m}}_{{\mathbf{v}}_k}\right)}{\partial {\mathbf{v}}_k^T}$. Notice that we do not explicitly exhibit the dependence on the system inputs in the previous EKF, which can be embedded in the noise means ${\mathbf{m}}_{{\mathbf{w}}_{k-1}}$ and ${\mathbf{m}}_{{\mathbf{v}}_k}$.

3.1. Mismatched and True System Models

As previously stated, it is unlikely that the practitioner has a full knowledge of the system, and therefore we want to cope with the situation where there is a true ${\mathcal{M}}_T$ and a mismatched/assumed ${\mathcal{M}}_A$ NLDSSM as follows,

$$\begin{array}{cc}\hfill & {\mathcal{M}}_A:\left\{\begin{array}{c}{\mathbf{x}}_k^{\prime }={\mathbf{f}}_{k-1}({\mathbf{x}}_{k-1}^{\prime },{\mathbf{m}}_{{\mathbf{w}}_{k-1}}+d{\mathbf{w}}_{k-1},\widehat{!})\hfill \\ {\mathbf{y}}_k={\mathbf{h}}_k({\mathbf{x}}_k^{\prime },{\mathbf{m}}_{{\mathbf{v}}_k}+d{\mathbf{v}}_k,\widehat{‘})\hfill \end{array}\right.\hfill \end{array}$$ (15)

$$\begin{array}{cc}\hfill & {\mathcal{M}}_T:\left\{\begin{array}{c}{\mathbf{x}}_k={\mathbf{f}}_{k-1}({\mathbf{x}}_{k-1},{\mathbf{m}}_{{\mathbf{w}}_{k-1}}+d{\mathbf{w}}_{k-1},!)\hfill \\ {\mathbf{y}}_k={\mathbf{h}}_k({\mathbf{x}}_k,{\mathbf{m}}_{{\mathbf{v}}_k}+d{\mathbf{v}}_k,‘)\hfill \end{array}\right.\hfill \end{array}$$ (16)

with ${\mathbf{w}}_{k-1}={\mathbf{m}}_{{\mathbf{w}}_{k-1}}+d{\mathbf{w}}_{k-1}$, ${\mathbf{v}}_k={\mathbf{m}}_{{\mathbf{v}}_k}+d{\mathbf{v}}_k$, $\mathbb{E}\left[d{\mathbf{w}}_{k-1}\right]=\mathbf{0}$ and $\mathbb{E}\left[d{\mathbf{v}}_k\right]=\mathbf{0}$. Since the EKF of ${\mathbf{x}}_k$ is based on the measurements and our knowledge of the model dynamics, any mismatch between the true model dynamics ${\mathcal{M}}_T$ and the assumed one ${\mathcal{M}}_A$ leads to a suboptimal filter, and possibly to a filter with bad performance, as the discrepancy between the two models increases.

The existence of uncertainty on system nonlinear functions, either because of a parametric model error or a mismatch on system inputs, is taken into account as ${\mathbf{f}}_{k-1}\left(.\right)\triangleq {\mathbf{f}}_{k-1}\left(.,!\right)$ and ${\mathbf{h}}_k\left(.\right)\triangleq {\mathbf{h}}_k\left(.,‘\right)$, where $!$ and $‘$ are deterministic vector values or system inputs, and the possible parametric model mismatch is given by $\widehat{!}=!+d\widehat{!}$ and $\widehat{‘}=‘+d\widehat{‘}$. If errors $d\widehat{!}$ and $d\widehat{‘}$ are small, then the true state and measurement nonlinear functions differ from the assumed ones via first order Taylor series as follows,

$${\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1},{\mathbf{w}}_{k-1},!\right)-{\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1},{\mathbf{w}}_{k-1},\widehat{!}\right)\simeq \frac{\partial {\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1},{\mathbf{w}}_{k-1},\widehat{!}\right)}{\partial {!}^T}\left(!-\widehat{!}\right),$$ (17)

$${\mathbf{h}}_k\left({\mathbf{x}}_k,{\mathbf{v}}_k,‘\right)-{\mathbf{h}}_k({\mathbf{x}}_k,{\mathbf{v}}_k,\widehat{‘})\simeq \frac{\partial {\mathbf{h}}_k({\mathbf{x}}_k,{\mathbf{v}}_k,\widehat{‘})}{\partial {‘}^T}(‘-\widehat{‘}).$$

At time $k\ge 1$, the EKF of ${\mathbf{x}}_k$ is obtained from the Kalman-like recursion Equation (12) computed with the assumed model ${\mathcal{M}}_A$,

$$\begin{array}{cc}\hfill & {\widehat{\mathbf{x}}}_{k|k}\left({\mathbf{L}}_k\right)={\widehat{\mathbf{x}}}_{k|k-1}^b+{\mathbf{L}}_k({\mathbf{y}}_k-{\widehat{\mathbf{y}}}_{k|k-1}^b),\hfill \end{array}$$ (18)

$$\begin{array}{cc}\hfill & {\widehat{\mathbf{x}}}_{k|k-1}^b={\mathbf{f}}_{k-1}({\widehat{\mathbf{x}}}_{k-1|k-1}^b,{\mathbf{m}}_{{\mathbf{w}}_{k-1}},\widehat{!}),\hfill \end{array}$$ (19)

$$\begin{array}{cc}\hfill & {\widehat{\mathbf{y}}}_{k|k-1}^b={\mathbf{h}}_k({\widehat{\mathbf{x}}}_{k|k-1}^b,{\mathbf{m}}_{{\mathbf{v}}_k},\widehat{‘}),\hfill \end{array}$$ (20)

with the gain ${\mathbf{L}}_k$ obtained from the MSE minimisation, and also computed with ${\mathcal{M}}_A$.

3.2. Impact of System Parametric Modelling Errors

The first step towards the system model mismatch mitigation is to compute the estimation error induced by the use of the assumed model ${\mathcal{M}}_A$. Among the possible estimation error breakdowns, we look for the one that makes the terms in (17) appear, and which reduces to the error analysed in [18] when the system becomes linear (as shown in Section 3.5). An iterative approach was taken to integrate these two requirements and led to breaking down the error as

$$\begin{array}{cc}\hfill & {\widehat{\mathbf{x}}}_{k|k}\left({\mathbf{L}}_k\right)-{\mathbf{x}}_k={\widehat{\mathbf{x}}}_{k|k-1}^b-{\mathbf{x}}_k+{\mathbf{L}}_k\left({\mathbf{y}}_k-{\widehat{\mathbf{y}}}_{k|k-1}^b\right)\hfill \\ \hfill & ={\mathbf{f}}_{k-1}\left({\widehat{\mathbf{x}}}_{k-1|k-1}^b,{\mathbf{m}}_{{\mathbf{w}}_{k-1}},\widehat{!}\right)-{\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1},{\mathbf{m}}_{{\mathbf{w}}_{k-1}}+d{\mathbf{w}}_{k-1},\widehat{!}\right)\hfill \\ \hfill & +{\mathbf{L}}_k{\mathbf{h}}_k\left({\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1},{\mathbf{m}}_{{\mathbf{w}}_{k-1}}+d{\mathbf{w}}_{k-1},\widehat{!}\right),{\mathbf{m}}_{{\mathbf{v}}_k}+d{\mathbf{v}}_k,\widehat{‘}\right)\hfill \\ \hfill & -{\mathbf{L}}_k{\mathbf{h}}_k\left({\mathbf{f}}_{k-1}\left({\widehat{\mathbf{x}}}_{k-1|k-1}^b,{\mathbf{m}}_{{\mathbf{w}}_{k-1}},\widehat{!}\right),{\mathbf{m}}_{{\mathbf{v}}_k},\widehat{‘}\right)+{”}_k\left({\mathbf{L}}_k\right),\hfill \end{array}$$ (21)

with the additional error term,

$$\begin{array}{cc}\hfill & {”}_k\left({\mathbf{L}}_k\right)={\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1},{\mathbf{w}}_{k-1},\widehat{!}\right)-{\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1},{\mathbf{w}}_{k-1},!\right)\hfill \\ \hfill & +{\mathbf{L}}_k\left({\mathbf{h}}_k\left({\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1},{\mathbf{w}}_{k-1},!\right),{\mathbf{v}}_k,‘\right)-{\mathbf{h}}_k({\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1},{\mathbf{w}}_{k-1},\widehat{!}\right),{\mathbf{v}}_k,\widehat{‘})\right).\hfill \end{array}$$ (22)

Indeed, if we assume that ${\widehat{\mathbf{x}}}_{k-1|k-1}^b$ is a good enough unbiased estimate of ${\mathbf{x}}_{k-1}$, that is,

$$\begin{array}{cc}\hfill & \mathbb{E}\left[{\widehat{\mathbf{x}}}_{k-1|k-1}^b\right]=\mathbb{E}\left[{\mathbf{x}}_{k-1}\right]={\mathbf{m}}_{{\mathbf{x}}_{k-1}}\hfill \end{array}$$ (23)

$$\begin{array}{cc}\hfill & tr\left({\mathbf{P}}_{k-1|k-1}^b\right)=\mathbb{E}\left[\right||{\widehat{\mathbf{x}}}_{k-1|k-1}^b-{\mathbf{x}}_{k-1}{\left|\right|}^2]\ll 1,\hfill \end{array}$$ (24)

then the first order approximations of Equations (21) and (22) are

$$\begin{array}{cc}\hfill & {\widehat{\mathbf{x}}}_{k|k}\left({\mathbf{L}}_k\right)-{\mathbf{x}}_k\simeq (\mathbf{I}-{\mathbf{L}}_k{\widehat{\mathbf{H}}}_k)\left({\widehat{\mathbf{F}}}_{k-1}({\widehat{\mathbf{x}}}_{k-1|k-1}^b-{\mathbf{x}}_{k-1})-{\widehat{\mathbf{N}}}_{k-1}d{\mathbf{w}}_{k-1}\right)+{\mathbf{L}}_k{\widehat{\mathbf{M}}}_{k}d{\mathbf{v}}_k+{”}_k\left({\mathbf{L}}_k\right),\hfill \end{array}$$ (25)

$$\begin{array}{cc}\hfill & {”}_k\left({\mathbf{L}}_k\right)\simeq {\mathbf{L}}_k\frac{\partial {\mathbf{h}}_k({\mathbf{x}}_k,{\mathbf{v}}_k,\widehat{‘})}{\partial {‘}^T}(‘-\widehat{‘})-(\mathbf{I}-{\mathbf{L}}_k{\widehat{\mathbf{H}}}_k)\frac{\partial {\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1},{\mathbf{w}}_{k-1},\widehat{!}\right)}{\partial {!}^T}\left(!-\widehat{!}\right),\hfill \end{array}$$ (26)

where the linearised matrices for the assumed model are now ${\widehat{\mathbf{F}}}_k=\frac{\partial {\mathbf{f}}_k\left({\mathbf{m}}_{{\mathbf{x}}_k},{\mathbf{m}}_{{\mathbf{w}}_k},\widehat{!}\right)}{\partial {\mathbf{x}}_k^T},{\widehat{\mathbf{N}}}_k=\frac{\partial {\mathbf{f}}_k\left({\mathbf{m}}_{{\mathbf{x}}_k},{\mathbf{m}}_{{\mathbf{w}}_k},\widehat{!}\right)}{\partial {\mathbf{w}}_k^T}$, ${\widehat{\mathbf{H}}}_k=\frac{\partial {\mathbf{h}}_k\left({\widehat{\mathbf{m}}}_{{\mathbf{x}}_k},{\mathbf{m}}_{{\mathbf{v}}_k},\widehat{‘}\right)}{\partial {\mathbf{x}}_k^T},{\widehat{\mathbf{M}}}_k=\frac{\partial {\mathbf{h}}_k\left({\widehat{\mathbf{m}}}_{{\mathbf{x}}_k},{\mathbf{m}}_{{\mathbf{v}}_k},\widehat{‘}\right)}{\partial {\mathbf{v}}_k^T}$, with mean terms given by ${\mathbf{m}}_{{\mathbf{x}}_k}\simeq {\mathbf{f}}_{k-1}\left({\mathbf{m}}_{{\mathbf{x}}_{k-1}},{\mathbf{m}}_{{\mathbf{w}}_{k-1}},!\right)$ and ${\widehat{\mathbf{m}}}_{{\mathbf{x}}_k}\simeq {\mathbf{f}}_{k-1}\left({\mathbf{m}}_{{\mathbf{x}}_{k-1}},{\mathbf{m}}_{{\mathbf{w}}_{k-1}},\widehat{!}\right)$.

3.3. Mitigation of Parametric Modelling Errors through LCs

Once we have established the additional error induced by the model mismatch, the practitioner needs an efficient way to mitigate it. Notice that

$$\mathbb{E}[{\widehat{\mathbf{x}}}_{k-1|k-1}^b-{\mathbf{x}}_{k-1}]=\mathbf{0}\Rightarrow \mathbb{E}\left[{\widehat{\mathbf{x}}}_{k|k}\left({\mathbf{L}}_k\right)-{\mathbf{x}}_k\right]=\mathbb{E}\left[{”}_k\left({\mathbf{L}}_k\right)\right],$$ (27)

$$\mathbb{E}\left[{”}_k\left({\mathbf{L}}_k\right)\right]\simeq {\mathbf{L}}_k\mathbb{E}\left[\frac{\partial {\mathbf{h}}_k({\mathbf{x}}_k,{\mathbf{v}}_k,\widehat{‘})}{\partial {‘}^T}\right](‘-\widehat{‘})-(\mathbf{I}-{\mathbf{L}}_k{\widehat{\mathbf{H}}}_k)\mathbb{E}\left[\frac{\partial {\mathbf{f}}_{k-1}({\mathbf{x}}_{k-1},{\mathbf{w}}_{k-1},\widehat{!})}{\partial {!}^T}\right](!-\widehat{!}),$$

therefore, we can conclude that

$$\begin{array}{cc}\hfill & \forall (!-\widehat{!}),\forall ((‘-\widehat{‘}),\mathbb{E}\left[{”}_k\left({\mathbf{L}}_k\right)\right]=\mathbf{0}\iff \left\{\begin{array}{c}\hfill {\mathbf{L}}_k\mathbb{E}\left[\frac{\partial {\mathbf{h}}_k({\mathbf{x}}_k,{\mathbf{v}}_k,\widehat{‘})}{\partial {‘}^T}\right]=\mathbf{0}\\ \hfill (\mathbf{I}-{\mathbf{L}}_k{\widehat{\mathbf{H}}}_k)\mathbb{E}\left[\frac{\partial {\mathbf{f}}_{k-1}({\mathbf{x}}_{k-1},{\mathbf{w}}_{k-1},\widehat{!})}{\partial {!}^T}\right]=\mathbf{0}\end{array}\right.,\hfill \end{array}$$ (28)

which defines a sensible set of constraints in order to mitigate at first order, which mainly consists of the bias, the error introduced by parametric modelling errors in the nonlinear system functions. However, expectations are probably not computable in several applications, and

$$\begin{array}{c}d{\mathbf{H}}_k\triangleq \mathbb{E}\left[\frac{\partial {\mathbf{h}}_k\left({\mathbf{x}}_k,{\mathbf{v}}_k,\widehat{‘}\right)}{\partial {‘}^T}\right]\simeq \frac{\partial {\mathbf{h}}_k\left({\widehat{\mathbf{x}}}_{k|k-1}^b,{\mathbf{m}}_{{\mathbf{v}}_k},\widehat{‘}\right)}{\partial {‘}^T},\hfill \\ d{\mathbf{F}}_k\triangleq \mathbb{E}\left[\frac{\partial {\mathbf{f}}_k\left({\mathbf{x}}_k,{\mathbf{w}}_k,\widehat{!}\right)}{\partial {!}^T}\right]\simeq \frac{\partial {\mathbf{f}}_k\left({\widehat{\mathbf{x}}}_{k|k}^b,{\mathbf{m}}_{{\mathbf{w}}_k},\widehat{!}\right)}{\partial {!}^T}.\hfill \end{array}$$ (29)

These LCs provide a non degenerate solution [18] only if $rank\left(d{\mathbf{F}}_{k-1}\right)=R_{k-1}<P_{k-1}$. Then, Equation (28) can be recast as $\{{\mathbf{L}}_{k}d{\mathbf{H}}_k=\mathbf{0},{\mathbf{L}}_k\left({\widehat{\mathbf{H}}}_{k}d{\mathbf{F}}_{k-1}\right)=d{\mathbf{F}}_{k-1}\}$. Let $d{\mathbf{F}}_{k-1}={\mathbf{U}}_{k-1}d{\mathsf{\Phi }}_{k-1}$ be the singular value decomposition (SVD) of $d{\mathbf{F}}_{k-1}$, where ${\mathbf{U}}_{k-1}\in {\mathbb{C}}^{P_k\times R_k}$ has full rank $R_k<P_k$ and $d{\mathsf{\Phi }}_{k-1}\in {\mathbb{C}}^{R_k\times P_{k-1}}$. Then Equation (28) becomes,

$$\{{\mathbf{L}}_{k}d{\mathbf{H}}_k=\mathbf{0},{\mathbf{L}}_k\left({\widehat{\mathbf{H}}}_k{\mathbf{U}}_{k-1}\right)={\mathbf{U}}_{k-1}\}.$$ (30)

3.4. Exploiting LCs for Robust Filtering under Mismatch

Notice that by imposing LCs in Equation (30), the estimate obtained with the mismatched NLDSSM Equation (15) is matched to the true observation in Equation (16). Indeed, the LCKF minimises the MSE associated to the true state ${\mathbf{x}}_k$, matching the true observations to the assumed model. We detail the robust LCEKF methodology in the sequel. At every time k,

Prediction

$${\widehat{\mathbf{x}}}_{k|k-1}^b\simeq {\mathbf{f}}_{k-1}({\widehat{\mathbf{x}}}_{k-1|k-1}^b,{\mathbf{m}}_{{\mathbf{w}}_{k-1}},\widehat{!}),$$ (31)

$${\mathbf{P}}_{k|k-1}^b={\widehat{\mathbf{F}}}_{k-1}{\mathbf{P}}_{k-1|k-1}^b{\widehat{\mathbf{F}}}_{k-1}^H+{\widehat{\mathbf{N}}}_{k-1}{\mathbf{C}}_{{\mathbf{w}}_{k-1}}{\widehat{\mathbf{N}}}_{k-1}^H,$$

$${\widehat{\mathbf{y}}}_{k|k-1}^b\simeq {\mathbf{h}}_k({\widehat{\mathbf{x}}}_{k|k-1}^b,{\mathbf{m}}_{{\mathbf{v}}_k},\widehat{‘}),$$ (32)

$${\mathbf{S}}_{k|k-1}^b={\widehat{\mathbf{H}}}_k{\mathbf{P}}_{k|k-1}^b{\widehat{\mathbf{H}}}_k^H+{\widehat{\mathbf{M}}}_k{\mathbf{C}}_{{\mathbf{v}}_k}{\widehat{\mathbf{M}}}_k^H,$$

Unconstrained Gain

$${\mathbf{K}}_k={\mathbf{P}}_{k|k-1}^b{\widehat{\mathbf{H}}}_k^H{\left({\mathbf{S}}_{k|k-1}^b\right)}^{-1},$$ (33)

Linear Constraints

$${\Delta }_k=\left[d{\mathbf{H}}_k{\widehat{\mathbf{H}}}_k{\mathbf{U}}_{k-1}\right],{\mathbf{T}}_k=\left[\mathbf{0}{\mathbf{U}}_{k-1}\right]$$

$${\Gamma }_k={\mathbf{T}}_k-{\mathbf{K}}_k{\Delta }_k,{\mathsf{\Psi }}_k={\Delta }_k^H{\left({\mathbf{S}}_{k|k-1}^b\right)}^{-1}{\Delta }_k$$

Constrained Gain

$${\mathbf{L}}_k^b={\mathbf{K}}_k+{\Gamma }_k{\mathsf{\Psi }}_k^{-1}{\Delta }_k^H{\left({\mathbf{S}}_{k|k-1}^b\right)}^{-1}$$ (34)

Update

$${\widehat{\mathbf{x}}}_{k|k}^b={\widehat{\mathbf{x}}}_{k|k-1}^b+{\mathbf{L}}_k^b({\mathbf{y}}_k-{\widehat{\mathbf{y}}}_{k|k-1}^b),$$ (35)

$${\mathbf{P}}_{k|k}^b=\left(\mathbf{I}-{\mathbf{K}}_k{\widehat{\mathbf{H}}}_k\right){\mathbf{P}}_{k|k-1}^b+{\Gamma }_k{\mathsf{\Psi }}_k^{-1}{\Gamma }_k^H$$

with the matrices ${\widehat{\mathbf{F}}}_{k-1}$, ${\widehat{\mathbf{N}}}_{k-1}$, ${\widehat{\mathbf{H}}}_k$, ${\widehat{\mathbf{M}}}_k$ evaluated at ${\widehat{\mathbf{x}}}_{k-1|k-1}^b$ and ${\widehat{\mathbf{x}}}_{k|k-1}^b$ instead of at the unknown state means.

3.5. Special Cases: Linear and Additive Systems

If we assume that the NLDSSMs Equations (15) and (16) become linear, we have that,

$$\begin{array}{cc}\hfill {\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1},{\mathbf{w}}_{k-1},!\right)& ={\mathbf{F}}_{k-1}(!){\mathbf{x}}_{k-1}+{\mathbf{w}}_{k-1},\hfill \end{array}$$ (36)

$$\begin{array}{cc}\hfill {\mathbf{h}}_k\left({\mathbf{x}}_k,{\mathbf{v}}_k,‘\right)& ={\mathbf{H}}_k\left(‘\right){\mathbf{x}}_k+{\mathbf{v}}_k,\hfill \end{array}$$ (37)

with ${\mathbf{F}}_{k-1}(!)=[{\mathbf{f}}_{k-1}^1(!)\dots {\mathbf{f}}_{k-1}^{P_{k-1}}(!)]$ and ${\mathbf{H}}_k(‘)=[{\mathbf{h}}_k^1\left(‘\right)\dots {\mathbf{h}}_k^{P_k}\left(‘\right)]$, leading to ${\widehat{\mathbf{F}}}_{k-1}={\mathbf{F}}_{k-1}(\widehat{!})$, ${\widehat{\mathbf{N}}}_{k-1}=\mathbf{I}$, ${\widehat{\mathbf{H}}}_k={\mathbf{H}}_k(\widehat{‘})$ and ${\widehat{\mathbf{M}}}_k=\mathbf{I}$. Moreover, in this case,

$$\frac{\partial {\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1},{\mathbf{w}}_{k-1},\widehat{!}\right)}{\partial {!}^T}\left(!-\widehat{!}\right)\simeq \left({\mathbf{F}}_{k-1}\left(!\right)-{\mathbf{F}}_{k-1}\left(\widehat{!}\right)\right){\mathbf{x}}_{k-1}=d{\mathbf{F}}_{k-1}{\mathbf{x}}_{k-1},$$ (38)

$$\frac{\partial {\mathbf{h}}_k({\mathbf{x}}_k,{\mathbf{v}}_k,\widehat{‘})}{\partial {‘}^T}(‘-\widehat{‘})\simeq ({\mathbf{H}}_k\left(‘\right)-{\mathbf{H}}_k\left(\widehat{‘}\right)){\mathbf{x}}_k=d{\mathbf{H}}_k{\mathbf{x}}_k,$$

then, Equation (26) reduces to the linear system error analysed in [18], which confirms the relevance of Equations (25) and (26) as a first order approximation of Equations (21) and (22) and the choice of the form of Equation (21).

If we assume the intermediate case where the NLDSSMs Equations (15) and (16) are still nonlinear but with additive noise, then,

$$\begin{array}{cc}\hfill {\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1},{\mathbf{w}}_{k-1},!\right)& ={\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1},!\right)+{\mathbf{w}}_{k-1},\hfill \end{array}$$ (39)

$$\begin{array}{cc}\hfill {\mathbf{h}}_k\left({\mathbf{x}}_k,{\mathbf{v}}_k,‘\right)& ={\mathbf{h}}_k\left({\mathbf{x}}_k,‘\right)+{\mathbf{v}}_k,\hfill \end{array}$$ (40)

leading to ${\widehat{\mathbf{F}}}_k=\frac{\partial {\mathbf{f}}_k\left({\mathbf{m}}_{{\mathbf{x}}_k},\widehat{!}\right)}{\partial {\mathbf{x}}_k^T}$, ${\widehat{\mathbf{N}}}_k=\mathbf{I}$, ${\widehat{\mathbf{H}}}_k=\frac{\partial {\mathbf{h}}_k\left({\widehat{\mathbf{m}}}_{{\mathbf{x}}_k},\widehat{‘}\right)}{\partial {\mathbf{x}}_k^T}$ and ${\widehat{\mathbf{M}}}_k=\mathbf{I}$, with ${\mathbf{m}}_{{\mathbf{x}}_k}\simeq {\mathbf{f}}_{k-1}\left({\mathbf{m}}_{{\mathbf{x}}_{k-1}},!\right)$ and ${\widehat{\mathbf{m}}}_{{\mathbf{x}}_k}\simeq {\mathbf{f}}_{k-1}\left({\mathbf{m}}_{{\mathbf{x}}_{k-1}},\widehat{!}\right)$. Because the noise terms do not depend on the system parameters or inputs, we simply have that,

$$\begin{array}{cc}\hfill & \frac{\partial {\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1},{\mathbf{w}}_{k-1},\widehat{!}\right)}{\partial {!}^T}=\frac{\partial {\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1},\widehat{!}\right)}{\partial {!}^T},\hfill \\ \hfill & \frac{\partial {\mathbf{h}}_k({\mathbf{x}}_k,{\mathbf{v}}_k,\widehat{‘})}{\partial {‘}^T}=\frac{\partial {\mathbf{h}}_k({\mathbf{x}}_k,\widehat{‘})}{\partial {‘}^T}.\hfill \end{array}$$

Finally, if we consider nonlinear additive inputs of the form, ${\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1},{\mathbf{w}}_{k-1},!\right)={\mathbf{f}}_{k-1}\left({\mathbf{x}}_{k-1}\right)+{\mathbf{g}}_{k-1}\left(!\right)+{\mathbf{w}}_{k-1}$, then $\partial {\mathbf{f}}_{k-1}({\mathbf{x}}_{k-1},{\mathbf{w}}_{k-1},\widehat{!})/\partial {!}^T=\partial {\mathbf{g}}_{k-1}(\widehat{!})/\partial {!}^T$ and we recover the results in [20].

4. Illustrative Example: Robust Vehicle Tracking and Navigation

To illustrate the validity of the proposed LCEKF and its performance improvement with respect to standard (not robust) EKF solutions, we analyse a two vehicle tracking and navigation problem. We consider for both vehicles a tricycle dynamic model (i.e., two rear wheels provide the speed, and the front one controls the direction), also known as the Ackerman model [23], which is used to describe the kinematic behavior (2D position and orientation) of most parts of vehicles with three and four wheels (see Figure 1). In the sequel subscript ${(·)}_T$ refers to the tracking vehicle, and ${(·)}_B$ to the tracked one. Refer to Appendix A for details.

Figure 1.

Three-wheeled vehicle model in movement.

As shown in Figure 2, we consider a tracking vehicle at $(x_T,y_T)$ with linear velocity $V_T$, orientation $\alpha$ and steering angle $\psi$, and a tracked vehicle at $(x_B,y_B)$ with linear velocity $V_B$, orientation $\theta$ and steering angle $\varphi$. The state at discrete time k is ${\mathbf{x}}_k^T=[{\left(x_B\right)}_k,{\left(y_B\right)}_k,{\theta }_k,{\left(x_T\right)}_k,{\left(y_T\right)}_k,{\alpha }_k]$ (in the global frame (xOy)), to be estimated from measurements ${\mathbf{y}}_k^T=[{\left(x_B^{\prime }\right)}_k,{\left(y_B^{\prime }\right)}_k,{\beta }_k,{\left(x_T\right)}_k,{\left(y_T\right)}_k,{\alpha }_k]$, where $({\left(x_B^{\prime }\right)}_k,{\left(y_B^{\prime }\right)}_k)$ and ${\beta }_k$ are the tracked vehicle position and orientation in the tracker (x’O’y’) coordinate frame. Velocities, $V_T$ and $V_B$, and steering information, $\psi$ and $\varphi$, are obtained from an odometer and steering angle sensor. We consider a cooperative tracking process where the tracked vehicle communicates its navigation information, i.e., $V_B$ and $\varphi$, to the tracking vehicle. Therefore, we have an input parameter vector ${\mathbf{u}}^T=[V_B,\varphi ,D_B,V_T,\psi ,D_T]$, with $D_B$ and $D_T$ being the distance from the front wheel to the rear axle.

Figure 2.

Two vehicle tracker (T) and tracked (B) scenario, and the different vehicle coordinate systems.

4.1. State Model with Both Additive and Non-Additive Noise

We consider a nonlinear state model of the form ${\mathbf{x}}_k={\mathbf{f}}_{k-1}({\mathbf{x}}_{k-1},\mathbf{u},{\mathbf{w}}_{k-1})+{\mathbf{w}}_{k-1}^{\prime }$. The additive noise ${\mathbf{w}}_{k-1}^{\prime }\sim \mathcal{N}({\mathbf{m}}_{\mathbf{w}}^{\prime },{\mathbf{C}}_{\mathbf{w}}^{\prime })$ models deviations on the vehicles’ position and orientation, due to wind perturbations or unmodeled accelerations, with ${\mathbf{C}}_{\mathbf{w}}^{\prime }=\mathrm{diag}({\sigma }_{x_B}^2,{\sigma }_{y_B}^2,{\sigma }_{\theta }^2,{\sigma }_{x_T}^2,{\sigma }_{y_T}^2,{\sigma }_{\alpha }^2)$. The non-additive noise ${\mathbf{w}}_{k-1}={[w_v,w_{\psi }]}_{k-1}^T\sim \mathcal{N}({\mathbf{m}}_{\mathbf{w}},{\mathbf{C}}_{\mathbf{w}})$ models perturbations on the input parameters $V_T$ and $\psi$ (i.e., the odometer may be affected by tire pressure and road conditions, and the steering information is subject to noise due to friction and wind), with ${\mathbf{C}}_{\mathbf{w}}=\mathrm{diag}({\sigma }_v^2,{\sigma }_{\psi }^2)$,

$$\begin{array}{cc}\hfill & {\mathbf{x}}_k={\mathbf{x}}_{k-1}+{\mathbf{g}}_{k-1}({\mathbf{x}}_{k-1},\mathbf{u},{\mathbf{w}}_{k-1})+{\mathbf{w}}_{k-1}^{\prime },\hfill \end{array}$$ (41)

$$\begin{array}{cc}\hfill & {\left[{\mathbf{g}}_{k-1}(·)\right]}_1=\frac{2D_B}{tan\left(\varphi \right)}sin\left(\frac{V_{B}dttan\left(\varphi \right)}{2D_B}\right)cos\left({\theta }_{k-1}+\frac{V_{B}dttan\left(\varphi \right)}{2D_B}\right),\hfill \end{array}$$ (42)

$$\begin{array}{cc}\hfill & {\left[{\mathbf{g}}_{k-1}(·)\right]}_2=\frac{2D_B}{tan\left(\varphi \right)}sin\left(\frac{V_{B}dttan\left(\varphi \right)}{2D_B}\right)sin\left({\theta }_{k-1}+\frac{V_{B}dttan\left(\varphi \right)}{2D_B}\right),\hfill \end{array}$$ (43)

$$\begin{array}{cc}\hfill & {\left[{\mathbf{g}}_{k-1}(·)\right]}_3=\frac{V_{B}dt}{D_B}tan\left(\varphi \right),\hfill \end{array}$$ (44)

$$\begin{array}{cc}\hfill & {\left[{\mathbf{g}}_{k-1}(·)\right]}_4=\frac{2D_T}{tan\left(\widehat{\psi }\right)}sin\left(\frac{{\widehat{V}}_{T}dttan\left(\widehat{\psi }\right)}{2D_T}\right)cos\left({\alpha }_{k-1}+\frac{{\widehat{V}}_{T}dttan\left(\widehat{\psi }\right)}{2D_T}\right),\hfill \end{array}$$ (45)

$$\begin{array}{cc}\hfill & {\left[{\mathbf{g}}_{k-1}(·)\right]}_5=\frac{2D_T}{tan\left(\widehat{\psi }\right)}sin\left(\frac{{\widehat{V}}_{T}dttan\left(\widehat{\psi }\right)}{2D_T}\right)sin\left({\alpha }_{k-1}+\frac{{\widehat{V}}_{T}dttan\left(\widehat{\psi }\right)}{2D_T}\right),\hfill \end{array}$$ (46)

$$\begin{array}{cc}\hfill & {\left[{\mathbf{g}}_{k-1}(·)\right]}_6=\frac{{\widehat{V}}_{T}dt}{D_T}tan\left(\widehat{\psi }\right),\hfill \end{array}$$ (47)

with ${\widehat{V}}_T=V_T+w_v$ and $\widehat{\psi }=\psi +w_{\psi }$.

4.2. Nonlinear Measurement Model and Mismatch

The measurement principle is illustrated in Figure 2, where the tracker locates itself at point $O^{\prime }$ and the tracked vehicle at point B in its coordinate frame (x’O’y’),

$$\begin{array}{cc}\hfill & {\mathbf{y}}_k={\mathbf{h}}_k\left({\mathbf{x}}_k\right)+{\mathbf{n}}_k,{\mathbf{n}}_k\sim \mathcal{N}(\mathbf{0},{\mathbf{C}}_n),\hfill \end{array}$$ (48)

$$\begin{array}{cc}\hfill & {\mathbf{C}}_n=\mathrm{diag}({\sigma }_{x^{\prime }}^2,{\sigma }_{y^{\prime }}^2,{\sigma }_{\beta }^2,{\sigma }_{x_T^{\prime }}^2,{\sigma }_{y_T^{\prime }}^2,{\sigma }_{{\alpha }^{\prime }}^2),\hfill \end{array}$$ (49)

and ${\mathbf{h}}_k\left({\mathbf{x}}_k\right)$ is given by

$$\begin{array}{c}\hfill {\mathbf{h}}_k\left({\mathbf{x}}_k\right)=\left[\begin{array}{cc}\begin{array}{c}\left[\begin{array}{cc}cos(-{\alpha }_k)& -sin(-{\alpha }_k)\\ sin(-{\alpha }_k)& cos(-{\alpha }_k)\end{array}\right]\end{array}& \mathbf{O}\\ \mathbf{O}& {\mathbf{I}}_4\end{array}\right]\left[\begin{array}{c}\begin{array}{c}{\left(x_T\right)}_k-{\left(x_B\right)}_k\\ {\left(y_T\right)}_k-{\left(y_B\right)}_k\\ {\theta }_k-{\alpha }_k\\ {\left(x_T\right)}_k\\ {\left(y_T\right)}_k\\ {\alpha }_k\end{array}\end{array}\right].\end{array}$$ (50)

We consider the case where we may have an imperfect knowledge on the system inputs. In practice, the distance $D_B$ and $D_T$ are not accurate, i.e., there exists a deviation between the true values, $D_B$ and $D_T$, and the assumed ones, ${\widehat{D}}_B$ and ${\widehat{D}}_T$, $d{D}_B=D_B-{\widehat{D}}_B$ and $d{D}_T=D_T-{\widehat{D}}_T$. In order to mitigate the possible impact of such error, we resort to the LCEKF in Section 3.4, incorporating the constraint

$$\left(\mathbf{I}-{\mathbf{L}}_k{\widehat{\mathbf{H}}}_k\right)d{\mathbf{F}}_{k-1}=\mathbf{0},{\left.d{\mathbf{F}}_k=\frac{\partial {\mathbf{f}}_k({\mathbf{x}}_k,\mathbf{u},{\mathbf{w}}_k)}{\partial {\mathbf{a}}^T}\right|}_{{\mathbf{x}}_k={\widehat{\mathbf{x}}}_{k|k},\mathbf{u}=\widehat{\mathbf{u}},{\mathbf{w}}_k={\mathbf{m}}_{\mathbf{w}}},$$ (51)

with the input vector given by $\widehat{\mathbf{u}}={[V_B,\varphi ,{\widehat{D}}_B,V_T,\psi ,{\widehat{D}}_T]}^T$ in the additive noise case, or $\widehat{\mathbf{u}}={[V_B,\varphi ,{\widehat{D}}_B,{\widehat{V}}_T,\widehat{\psi },{\widehat{D}}_T]}^T$ in the non-additive noise one, and $\mathbf{a}={[D_B,D_T]}^T$.

4.3. Scenarios

We consider the following setup: $dt=1$ ms, ${\mathbf{C}}_{{\mathbf{x}}_0}=\mathbf{0}$, ${\mathbf{x}}_0={\left(10,20,1^{\circ },8,19,{1.5}^{\circ }\right)}^T$, and a measurement noise with ${\sigma }_{x^{\prime }}=1$ m, ${\sigma }_{y^{\prime }}=1$ m, ${\sigma }_{\beta }={0.1}^{\circ }$, ${\sigma }_{x_T^{\prime }}=1$ m, ${\sigma }_{y_T^{\prime }}=1$ m and ${\sigma }_{{\alpha }^{\prime }}={0.1}^{\circ }$. Three scenarios are of interest:

• S1) Only additive noise (${\mathbf{w}}_{k-1}=\mathbf{0}$): ${\mathbf{m}}_{\mathbf{w}}^{\prime }=\mathbf{0}$, ${\sigma }_{x_B}={\sigma }_{y_B}={\sigma }_{x_T}={\sigma }_{y_T}=0.1$ m and ${\sigma }_{\theta }={\sigma }_{\alpha }={0.1}^{\circ }$.

• S2) Only non-additive noise (${\mathbf{w}}_{k-1}^{\prime }=\mathbf{0}$): ${\mathbf{m}}_{\mathbf{w}}=\mathbf{0}$, ${\sigma }_v=0.1$ m/s and ${\sigma }_{\psi }={0.1}^{\circ }$.

• S3) Both additive and non-additive noise with the values in S1 and S2.

In the three scenarios we consider different mismatches $dD=d{D}_T=d{D}_B=0.1,0.5$ and 1 m, and true values $D_B=3$ m and $D_T=3$ m. We compare five filters:

• (i)EKF${}_{ref}$ is the benchmark using the true system ($d{D}_T=d{D}_B=0$).
• (ii)EKF stands for the mismatched filter, that is, using the mismatched system input values ${\widehat{D}}_B$ and ${\widehat{D}}_T$.
• (iii)LCEKF${}_{DT}$ is a linearly constrained filter only accounting for an erroneous value of $D_T$ ($\mathbf{a}=D_T$ in Equation (51)).
• (iv)LCEKF${}_{DB}$ is a linearly constrained filter only mitigating the impact of $d{D}_B$ ($\mathbf{a}=D_B$ in Equation (51)).
• (v)LCEKF${}_{DB,DT}$ accounts for both mismatched inputs ($\mathbf{a}={[D_B,D_T]}^T$).

4.4. Results

Numerical results for the three scenarios are summarised in Figure 3, where we show the position MSE vs time for the five filters obtained over 100 Monte Carlo runs. First, notice that in each scenario (S1, S2 and S3) and regardless of the mismatch value $dD$ the standard EKF always diverges and is not able to correctly estimate the states of interest. That is, even a minor mismatch on the distance from the front wheel to the rear axle induces a performance breakdown on the standard misspecified EKF; therefore, such a mismatch must be accounted for. In addition, the larger the mismatch the faster the performance breakdown. Second, the LCEKF${}_{DB}$ accounting only for a mismatched input on the tracked vehicle ($D_B$) does not improve the standard EKF performance (results are superimposed in the plots). Third, the LCEKF${}_{DT}$, which mitigates the tracker input mismatch ($D_T$), slightly improves the performance w.r.t. the EKF and LCEKF${}_{dB}$ but does not prevent a performance breakdown. Then, we can conclude that with a mismatch on both vehicles’ distance D, and regardless of whether the system is affected by either additive or non-additive noise, either we correctly deal with both mismatched inputs or the filters are not useful anymore.

Figure 3.

Results for the different sceanrios: (Top) Additive noise S1, (Middle) non-additive noise S2, and (Bottom) both additive and non-additive noise S3. MSE results for the optimal EKF${}_{ref}$, mismatched EKF, and three LCEKF implementations: LCEKF${}_{DB}$ (mitigation of $d{D}_B$), LCEKF${}_{DT}$ (mitigation of $d{D}_T$), and LCEKF${}_{DB,DT}$ (mitigation of both $d{D}_B$ and $d{D}_T$).

In contrast to the previous filters, the LCEKF${}_{DB,DT}$, which properly accounts for both input mismatched values, is able to correctly estimate the states and avoid the performance breakdown exhibited by the mismatched filters, regardless of the mismatched value $dD$, and therefore it is a powerful solution to coping with a possible system model mismatch. Notice that the LCEKF${}_{DB,DT}$ performance is almost superimposed with the optimal EKF${}_{ref}$ in the plots, which again proves the good behaviour of the LCEKF for both additive and non-additive noises. It is worth pointing out that the non-additive noise has a larger impact than the additive one on the system dynamics, which can be seen from the optimal MSE results (i.e., MSE EKF${}_{ref}$$\approx {10}^{-4}$ for the additive noise case S1 and MSE EKF${}_{ref}$$\approx 0.5\times {10}^{-2}$ for both non-additive noise cases S2 and S3); this drives the estimation error (MSE EKF${}_{ref}$ S2 ≈ MSE EKF${}_{ref}$ S3). This further supports the need for filters able to cope with non-additive noise and system inputs, and the interest of the robust LCEKF formulation proposed in this contribution.

5. Conclusions

In this contribution we explored the use of linear constraints to design robust nonlinear KF-type filtering strategies. This was shown in previous contributions to be a promising solution to mitigate parametric modelling errors in both system functions, a problem of interest in real-life applications. In that perspective, we derived a linearly constrained EKF for systems affected by non-additive noise and system inputs, which generalises previous solutions only able to cope with either linear systems or additive noises. Within this framework, it was shown how to exploit linear constraints to mitigate possible nonlinear system model parametric misspecifications on both process and measurement functions. This approach encompasses both errors on system function parameters and system inputs, as well as on noise mean values. A two vehicle robust tracking and navigation problem was used to show the validity and performance improvement provided by the robust LCEKF with respect to state-of-the-art solutions. In addition, it was shown that the non-additive noise has a larger impact than the additive one on the system dynamics, and drives the estimation error, which further supports the need for filters able to cope with non-additive noise and system inputs, and the interest of the robust LCEKF formulation.

As future work, it would be of interest to analyse the use of linear constraints within the context of sigma-point filtering, to better cope with the system nonlinearity and avoid the EKF linearisation errors. Moreover, the proposed solution should be compared to alternative system identification techniques, which, rather than mitigating the impact of mismatched parameters, estimate them together with the states of the system. Another research line would be the generalisation to more general mismatched systems, for instance, considering a possible mismatch on covariance matrices, as well as the comparison or combination with noise statistics’ parameter estimation techniques.

Appendix A. Details on the Robust Vehicle Tracking and Navigation Example

Appendix A.1. Tricycle Dynamic Model

The tricycle model (also known as Ackerman model) can be used to model the kinematic behavior of most part of vehicles with three and four wheels. As it is shown in Figure 1, it consists of two main wheels which provide the vehicle speed, and a third wheel which controls the vehicle direction. The turn radius $\rho$ is given by

$$\rho =\frac{D}{\tan \left(\psi \right)}$$ (A1)

where D denotes the distance from the front wheel to the rear axle. We consider the following equations

$$\begin{array}{c}\hfill \left\{\begin{array}{cc}dx\hfill & =x^{\prime }-x=S\cos (\theta +\frac{\Delta \theta }{2})\hfill \\ dy\hfill & =y^{\prime }-y=S\sin (\theta +\frac{\Delta \theta }{2})\hfill \\ d\theta \hfill & ={\theta }^{\prime }-\theta =L/\rho \hfill \end{array}\right.,\end{array}$$ (A2)

where,

$$\begin{array}{c}\hfill \left\{\begin{array}{cc}L\hfill & =Vdt\hfill \\ \Delta \theta \hfill & =L/\rho =\frac{Vdt\tan \left(\psi \right)}{D}\hfill \\ S\hfill & =2\rho \sin \left(\frac{\Delta \theta }{2}\right)=\frac{2Vdt\tan \left(\psi \right)}{D}\sin \left(\frac{Vdt\tan \left(\psi \right)}{2D}\right)\hfill \\ \hfill & =Vdt\mathrm{sinc}\left(\frac{Vdt\tan \left(\psi \right)}{2D}\right)\hfill \end{array}\right.,\end{array}$$ (A3)

and then,

$$\begin{array}{c}\hfill \left\{\begin{array}{cc}dx\hfill & =Vdt\mathrm{sinc}\left(\frac{Vdt\tan \left(\psi \right)}{2D}\right)\cos \left(\theta +\frac{Vdt\tan \left(\psi \right)}{2D}\right)\hfill \\ dy\hfill & =Vdt\mathrm{sinc}\left(\frac{Vdt\tan \left(\psi \right)}{2D}\right)\sin \left(\theta +\frac{Vdt\tan \left(\psi \right)}{2D}\right)\hfill \\ d\theta \hfill & =\frac{Vdt\tan \left(\psi \right)}{D}\hfill \end{array}\right..\end{array}$$ (A4)

Appendix A.2. Nonlinear Discrete State Model

Considering the state at discrete time k, ${\mathbf{x}}_k^T=[{\left(x_B\right)}_k,{\left(y_B\right)}_k,{\theta }_k,{\left(x_T\right)}_k,{\left(y_T\right)}_k,{\alpha }_k]$ (in the global frame (xOy)), and the previous equations, we can obtain the state equation for the general case with additive and non-additive noise terms, ${\mathbf{x}}_k={\mathbf{f}}_{k-1}({\mathbf{x}}_{k-1},\mathbf{u},{\mathbf{w}}_{k-1})+{\mathbf{w}}_{k-1}^{\prime }$, detailed in Equations (41)–(47). For the three scenarios of interest, we give in the sequel the corresponding approximations (i.e., we omit the dependency with discrete time k for the sake of simplicity):

• (S1) Only additive noise (${\mathbf{w}}_{k-1}=\mathbf{0}$, ${\widehat{\mathbf{N}}}_{k-1}=\mathbf{0}$):

  Let

  $$\begin{array}{cc}\hfill q\left(\mathbf{x}\right)& =V_{B}dttan\left(\varphi \right)/\left(2D_B\right),g\left(\mathbf{x}\right)=\theta +q\left(\mathbf{x}\right),\hfill \end{array}$$ (A5)

  $$\begin{array}{cc}\hfill q_T\left(\mathbf{x}\right)& =V_{T}dttan\left(\psi \right)/\left(2D_T\right),g_T\left(\mathbf{x}\right)=\alpha +q_T\left(\mathbf{x}\right),\hfill \end{array}$$ (A6)

  then

  $$\begin{array}{cc}\hfill & {\widehat{\mathbf{F}}}_{k-1}={\left.\frac{\partial {\mathbf{f}}_{k-1}({\mathbf{x}}_{k-1},\mathbf{u})}{\partial {\mathbf{x}}_{k-1}^T}\right|}_{{\mathbf{x}}_{k-1}={\widehat{\mathbf{x}}}_{k-1|k-1},\mathbf{u}=\widehat{\mathbf{u}}},\hfill \\ \hfill & \frac{\partial \mathbf{f}(\mathbf{x},\mathbf{u})}{\partial {\mathbf{x}}^T}=\left[\begin{array}{cccccc}1& 0& -V_{B}dt\mathrm{sinc}\left(q\left(\mathbf{x}\right)\right)sin\left(g\left(\mathbf{x}\right)\right)& 0& 0& 0\\ 0& 1& V_{B}dt\mathrm{sinc}\left(q\left(\mathbf{x}\right)\right)cos\left(g\left(\mathbf{x}\right)\right)& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& 0& -V_{T}dt\mathrm{sinc}\left(q_T\left(\mathbf{x}\right)\right)\sin \left(g_T\left(\mathbf{x}\right)\right)\\ 0& 0& 0& 0& 1& V_{T}dt\mathrm{sinc}\left(q_T\left(\mathbf{x}\right)\right)\cos \left(g_T\left(\mathbf{x}\right)\right)\\ 0& 0& 0& 0& 0& 1\end{array}\right].\hfill \end{array}$$ (A7)

• (S2) Only non-additive noise (${\mathbf{w}}_{k-1}^{\prime }=\mathbf{0}$):

  Let

  $$\begin{array}{cc}\hfill q\left(\mathbf{x}\right)& =V_{B}dttan\left(\varphi \right)/\left(2D_B\right),g\left(\mathbf{x}\right)=\theta +q\left(\mathbf{x}\right)\hfill \end{array}$$ (A8)

  $$\begin{array}{cc}\hfill q_T\left(\mathbf{x}\right)& =(V_T+w_v)dttan(\psi +w_{\psi })/\left(2D_T\right),g_T\left(\mathbf{x}\right)=\alpha +q_T\left(\mathbf{x}\right),\hfill \end{array}$$ (A9)

  then

  $$\begin{array}{cc}\hfill & {\widehat{\mathbf{F}}}_{k-1}={\left.\frac{\partial {\mathbf{f}}_{k-1}({\mathbf{x}}_{k-1},\mathbf{u},{\mathbf{w}}_{k-1})}{\partial {\mathbf{x}}_{k-1}^T}\right|}_{{\mathbf{x}}_{k-1}={\widehat{\mathbf{x}}}_{k-1|k-1},\mathbf{u}=\widehat{\mathbf{u}},{\mathbf{w}}_{k-1}={\mathbf{m}}_{\mathbf{w}}=\mathbf{0}},\hfill \end{array}$$ (A10)

  $$\begin{array}{cc}\hfill & \frac{\partial \mathbf{f}\left(\mathbf{x},\mathbf{u},\mathbf{w}\right)}{\delta {\mathbf{x}}^T}=\hfill \\ \hfill & \left[\begin{array}{cccccc}1& 0& -V_{B}dt\mathrm{sinc}\left(q\left(\mathbf{x}\right)\right)\sin \left(g\left(\mathbf{x}\right)\right)& 0& 0& 0\\ 0& 1& V_{B}dt\mathrm{sinc}\left(q\left(\mathbf{x}\right)\right)\cos \left(g\left(\mathbf{x}\right)\right)& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& 0& -(V_T+w_v)dt\mathrm{sinc}\left(q_T\left(\mathbf{x}\right)\right)\sin \left(g_T\left(\mathbf{x}\right)\right)\\ 0& 0& 0& 0& 1& (V_T+w_v)dt\mathrm{sinc}\left(q_T\left(\mathbf{x}\right)\right)\cos \left(g_T\left(\mathbf{x}\right)\right)\\ 0& 0& 0& 0& 0& 1\end{array}\right],\hfill \end{array}$$

  $$\begin{array}{cc}\hfill & {\widehat{\mathbf{N}}}_{k-1}={\left.\frac{\partial {\mathbf{f}}_{k-1}({\mathbf{x}}_{k-1},\mathbf{u},{\mathbf{w}}_{k-1})}{\partial {\mathbf{w}}_{k-1}^T}\right|}_{{\mathbf{x}}_{k-1}={\widehat{\mathbf{x}}}_{k-1|k-1},\mathbf{u}=\widehat{\mathbf{u}},{\mathbf{w}}_{k-1}={\mathbf{m}}_{\mathbf{w}}=\mathbf{0}},\hfill \end{array}$$ (A11)

  $$\begin{array}{cc}\hfill & \frac{\partial \mathbf{f}(\mathbf{x},\mathbf{u},\mathbf{w})}{\partial {\mathbf{w}}^T}=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ \frac{\partial {\mathbf{f}}_4\left(\mathbf{x},\mathbf{u},\mathbf{w}\right)}{\partial w_v}& \frac{\partial {\mathbf{f}}_4\left(\mathbf{x},\mathbf{u},\mathbf{w}\right)}{\partial w_{\psi }}\\ \frac{\partial {\mathbf{f}}_5\left(\mathbf{x},\mathbf{u},\mathbf{w}\right)}{\delta w_v}& \frac{\partial {\mathbf{f}}_5\left(\mathbf{x},\mathbf{u},\mathbf{w}\right)}{\partial w_{\psi }}\\ \frac{\partial {\mathbf{f}}_6\left(\mathbf{x},\mathbf{u},\mathbf{w}\right)}{\partial w_v}& \frac{\partial {\mathbf{f}}_6\left(\mathbf{x},\mathbf{u},\mathbf{w}\right)}{\partial w_{\psi }}\end{array}\right],\hfill \end{array}$$ (A12)

  where

  $$\begin{array}{cc}\hfill \frac{\partial {\mathbf{f}}_4\left(\mathbf{x},\mathbf{u},\mathbf{w}\right)}{\partial w_v}& =dt\mathrm{sinc}\left(q_T\left(\mathbf{x}\right)\right)\cos \left(g_T\left(\mathbf{x}\right)\right)-dt{q}_T\left(\mathbf{x}\right)\mathrm{sinc}\left(q_T\left(\mathbf{x}\right)\right)\sin \left(g_T\left(\mathbf{x}\right)\right)\hfill \\ \hfill & +dt\left(\cos \left(q_T\left(\mathbf{x}\right)\right)-\mathrm{sinc}\left(q_T\left(\mathbf{x}\right)\right)\right)\cos \left(g_T\left(\mathbf{x}\right)\right),\hfill \\ \hfill \frac{\partial {\mathbf{f}}_4\left(\mathbf{x},\mathbf{u},\mathbf{w}\right)}{\partial w_{\psi }}& =-\frac{{(V_T+w_v)}^{2}d{t}^2}{2D_T}\left(1+{\tan }^2(\psi +w_{\psi })\right)\mathrm{sinc}\left(q_T\left(\mathbf{x}\right)\right)\sin \left(g_T\left(\mathbf{x}\right)\right)\hfill \\ \hfill & +\frac{(V_T+w_v)dt\left(1+{\tan }^2(\psi +w_{\psi })\right)}{\tan (\psi +w_{\psi })}\cos \left(g_T\left(\mathbf{x}\right)\right)\cos \left(q_T\left(\mathbf{x}\right)\right)\hfill \\ \hfill & -\frac{(V_T+w_v)dt\left(1+{\tan }^2(\psi +w_{\psi })\right)}{\tan (\psi +w_{\psi })}\cos \left(g_T\left(\mathbf{x}\right)\right)\sin \mathrm{c}\left(q_T\left(\mathbf{x}\right)\right),\hfill \\ \hfill \frac{\partial {\mathbf{f}}_5\left(\mathbf{x},\mathbf{u},\mathbf{w}\right)}{\partial w_v}& =dt\mathrm{sinc}\left(q_T\left(\mathbf{x}\right)\right)\sin \left(g_T\left(\mathbf{x}\right)\right)+dt{q}_T\left(\mathbf{x}\right)\mathrm{sinc}\left(q_T\left(\mathbf{x}\right)\right)\cos \left(g_T\left(\mathbf{x}\right)\right)\hfill \\ \hfill & +dt\left(\cos \left(q_T\left(\mathbf{x}\right)\right)-\mathrm{sinc}\left(q_T\left(\mathbf{x}\right)\right)\right)\sin \left(g_T\left(\mathbf{x}\right)\right),\hfill \\ \hfill \frac{\partial {\mathbf{f}}_5\left(\mathbf{x},\mathbf{u},\mathbf{w}\right)}{\partial w_{\psi }}& =\frac{{(V_T+w_v)}^{2}d{t}^2}{2D_T}\left(1+{\tan }^2(\psi +w_{\psi })\right)\mathrm{sinc}\left(q_T\left(\mathbf{x}\right)\right)\cos \left(g_T\left(\mathbf{x}\right)\right)\hfill \\ \hfill & +\frac{(V_T+w_v)dt\left(1+{\tan }^2(\psi +w_{\psi })\right)}{\tan \left(\psi \right)}\sin \left(g_T\left(\mathbf{x}\right)\right)\cos \left(q_T\left(\mathbf{x}\right)\right)\hfill \\ \hfill & -\frac{(V_T+w_v)dt\left(1+{\tan }^2(\psi +w_{\psi })\right)}{\tan (\psi +w_{\psi })}\sin \left(g_T\left(\mathbf{x}\right)\right)\mathrm{sinc}\left(q_T\left(\mathbf{x}\right)\right),\hfill \\ \hfill \frac{\partial {\mathbf{f}}_6\left(\mathbf{x},\mathbf{u},\mathbf{w}\right)}{\partial w_v}& =\frac{dt}{D_T}\tan \left(\psi +w_{\psi }\right),\hfill \\ \hfill \frac{\partial {\mathbf{f}}_6\left(\mathbf{x},\mathbf{u},\mathbf{w}\right)}{\partial w_{\psi }}& =\frac{(V+w_v)dt}{D_T}\left(1+{\tan }^2\left(\psi +w_{\psi }\right)\right).\hfill \end{array}$$

• (S3) Both additive and non-additive noise: in this case ${\widehat{\mathbf{F}}}_{k-1}$ and ${\widehat{\mathbf{N}}}_{k-1}$ are computed as in S2)

Appendix A.3. Nonlinear Measurement Model

The nonlinear measurement model is given in Equation (48). To use the EKF and LCEKF we need to compute the approximation ${\widehat{\mathbf{H}}}_k={\left.\frac{\partial {\mathbf{h}}_k\left({\mathbf{x}}_k,{\mathbf{v}}_k,\widehat{‘}\right)}{\partial {\mathbf{x}}_k^T}\right|}_{{\mathbf{x}}_k={\widehat{\mathbf{x}}}_{k|k-1}^b}={\left.\frac{\partial {\mathbf{h}}_k\left({\mathbf{x}}_k\right)}{\partial {\mathbf{x}}_k^T}\right|}_{{\mathbf{x}}_k={\widehat{\mathbf{x}}}_{k|k-1}^b}$, which is given by

$${\widehat{\mathbf{H}}}_k={\left.\left[\begin{array}{cccccc}-cos\left({\alpha }_k\right)& -sin\left({\alpha }_k\right)& 0& cos\left({\alpha }_k\right)& sin\left({\alpha }_k\right)& \left(\begin{array}{c}-sin\left({\alpha }_k\right)({\left(x_T\right)}_k-{\left(x_B\right)}_k)\\ +cos\left({\alpha }_k\right)(\left(y_T\right)-{\left(y_B\right)}_k)\end{array}\right)\\ sin\left({\alpha }_k\right)& -cos\left({\alpha }_k\right)& 0& -sin\left({\alpha }_k\right)& cos\left({\alpha }_k\right)& \left(\begin{array}{c}-cos\left({\alpha }_k\right)(x_T-x_B)\\ -sin\left({\alpha }_k\right)({\left(y_T\right)}_k-{\left(y_B\right)}_k)\end{array}\right)\\ 0& 0& 1& 0& 0& -1\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]\right|}_{{\mathbf{x}}_k={\widehat{\mathbf{x}}}_{k|k-1}^b}.$$ (A13)

Notice that in this case ${\widehat{\mathbf{M}}}_k=\mathbf{0}$.

Appendix A.4. Linear Constraints

As already stated, the goal is to mitigate the possible mismatch impact by exploiting the constraints

$$\left(\mathbf{I}-{\mathbf{L}}_k{\widehat{\mathbf{H}}}_k\right)d{\mathbf{F}}_{k-1}=\mathbf{0},{\left.d{\mathbf{F}}_k=\frac{\partial {\mathbf{f}}_k({\mathbf{x}}_k,\mathbf{u},{\mathbf{w}}_k)}{\partial {\mathbf{a}}^T}\right|}_{{\mathbf{x}}_k={\widehat{\mathbf{x}}}_{k|k},\mathbf{u}=\widehat{\mathbf{u}},{\mathbf{w}}_k={\mathbf{m}}_{\mathbf{w}}=\mathbf{0}},$$ (A14)

with $\widehat{\mathbf{u}}={[V_B,\varphi ,{\widehat{D}}_B,V_T,\psi ,{\widehat{D}}_T]}^T$ and $\mathbf{a}={[D_B,D_T]}^T$. Omitting the dependency with discrete time k for the sake of simplicity, such constraint is computed as follows. Let

$$\begin{array}{cc}\hfill q\left(\mathbf{x}\right)& =V_{B}dttan\left(\varphi \right)/\left(2D_B\right),g\left(\mathbf{x}\right)=\theta +q\left(\mathbf{x}\right),\hfill \\ \hfill q_T\left(\mathbf{x}\right)& =(V_T+w_v)dttan(\psi +w_{\psi })/\left(2D_T\right),g_T\left(\mathbf{x}\right)=\alpha +q_T\left(\mathbf{x}\right),\hfill \end{array}$$

$$\begin{array}{cc}\hfill & \frac{\partial \mathbf{f}(\mathbf{x},\mathbf{u})}{\partial {\mathbf{a}}^T}=\left[\begin{array}{cc}\frac{\partial {\mathbf{f}}_1(\mathbf{x},\mathbf{u})}{\partial D_B}& \frac{\partial {\mathbf{f}}_1(\mathbf{x},\mathbf{u})}{\partial D_T}\\ \frac{\partial {\mathbf{f}}_2(\mathbf{x},\mathbf{u})}{\partial D_B}& \frac{\partial {\mathbf{f}}_2(\mathbf{x},\mathbf{u})}{\partial D_T}\\ \frac{\partial {\mathbf{f}}_3(\mathbf{x},\mathbf{u})}{\partial D_B}& \frac{\partial {\mathbf{f}}_3(\mathbf{x},\mathbf{u})}{\partial D_T}\\ \frac{\partial {\mathbf{f}}_4(\mathbf{x},\mathbf{u})}{\partial D_B}& \frac{\partial {\mathbf{f}}_4(\mathbf{x},\mathbf{u})}{\partial D_T}\\ \frac{\partial {\mathbf{f}}_5(\mathbf{x},\mathbf{u})}{\partial D_B}& \frac{\partial {\mathbf{f}}_5(\mathbf{x},\mathbf{u})}{\partial D_T}\\ \frac{\partial {\mathbf{f}}_6(\mathbf{x},\mathbf{u})}{\partial D_B}& \frac{\partial {\mathbf{f}}_6(\mathbf{x},\mathbf{u})}{\partial D_T}\end{array}\right]=\left[\begin{array}{cc}\frac{\partial {\mathbf{f}}_1(\mathbf{x},\mathbf{u})}{\partial D_B}& 0\\ \frac{\partial {\mathbf{f}}_2(\mathbf{x},\mathbf{u})}{\partial D_B}& 0\\ \frac{\partial {\mathbf{f}}_3(\mathbf{x},\mathbf{u})}{\partial D_B}& 0\\ 0& \frac{\partial {\mathbf{f}}_4(\mathbf{x},\mathbf{u})}{\partial D_T}\\ 0& \frac{\partial {\mathbf{f}}_5(\mathbf{x},\mathbf{u})}{\partial D_T}\\ 0& \frac{\partial {\mathbf{f}}_6(\mathbf{x},\mathbf{u})}{\partial D_T}\end{array}\right],\hfill \end{array}$$ (A15)

$$\begin{array}{cc}\hfill \frac{\partial {\mathbf{f}}_1(\mathbf{x},\mathbf{u})}{\partial D_B}& =\frac{V_{B}dt}{D_B}\mathrm{sinc}\left(q\left(\mathbf{x}\right)\right)\cos \left(g\left(\mathbf{x}\right)\right)-\frac{V_{B}dt}{D_B}\cos \left(q\left(\mathbf{x}\right)\right)\cos \left(g\left(\mathbf{x}\right)\right)\hfill \\ \hfill & +\frac{V_{B}dt}{D_B}q\left(\mathbf{x}\right)\mathrm{sinc}\left(q\left(\mathbf{x}\right)\right)\sin \left(g\left(\mathbf{x}\right)\right),\hfill \\ \hfill \frac{\partial {\mathbf{f}}_2(\mathbf{x},\mathbf{u})}{\partial D_B}& =-\frac{V_{B}dt}{D_B}\mathrm{sinc}\left(q\left(\mathbf{x}\right)\right)\cos \left(g\left(\mathbf{x}\right)\right)-\frac{V_{B}dt}{D_B}\cos \left(q\left(\mathbf{x}\right)\right)\sin \left(g\left(\mathbf{x}\right)\right)\hfill \\ \hfill & -\frac{V_{B}dt}{D_B}q\left(\mathbf{x}\right)\mathrm{sinc}\left(q\left(\mathbf{x}\right)\right)\cos \left(g\left(\mathbf{x}\right)\right),\hfill \\ \hfill \frac{\partial {\mathbf{f}}_3(\mathbf{x},\mathbf{u})}{\partial D_B}& =\frac{2q\left(\mathbf{x}\right)}{D_B},\hfill \\ \hfill \frac{\partial {\mathbf{f}}_4(\mathbf{x},\mathbf{u})}{\partial D_T}& =\frac{V_{T}dt}{D_T}\mathrm{sinc}\left(q_T\left(\mathbf{x}\right)\right)\cos \left(g_T\left(\mathbf{x}\right)\right)-\frac{V_{T}dt}{D_T}\cos \left(q_T\left(\mathbf{x}\right)\right)\cos \left(g_T\left(\mathbf{x}\right)\right)\hfill \\ \hfill & +\frac{V_{T}dt}{D_T}{q}_T\left(\mathbf{x}\right)\mathrm{sinc}\left(q_T\left(\mathbf{x}\right)\right)\sin \left(g_T\left(\mathbf{x}\right)\right),\hfill \\ \hfill \frac{\partial {\mathbf{f}}_5(\mathbf{x},\mathbf{u})}{\partial D_T}& =-\frac{V_{T}dt}{D_T}\mathrm{sinc}\left(q_T\left(\mathbf{x}\right)\right)\sin \left(g_T\left(\mathbf{x}\right)\right)-\frac{V_{T}dt}{D_T}\cos \left(q_T\left(\mathbf{x}\right)\right)\sin \left(g_T\left(\mathbf{x}\right)\right)\hfill \\ \hfill & -\frac{V_{T}dt}{D_T}{q}_T\left(\mathbf{x}\right)\mathrm{sinc}\left(q_T\left(\mathbf{x}\right)\right)\cos \left(g_T\left(\mathbf{x}\right)\right),\hfill \\ \hfill \frac{\partial {\mathbf{f}}_6(\mathbf{x},\mathbf{u})}{\partial D_T}& =\frac{2q_T\left(\mathbf{x}\right)}{D_T}.\hfill \end{array}$$

Author Contributions

Conceptualization, E.C. and J.V.-V.; D.V. designed the model; R.B.A. and E.C. developed the theory, performed the computations and carried out the implementation; E.C., J.V.-V. and G.P. verified the analytical methods; writing—original draft preparation, R.B.A. and J.V.-V.; writing—review and editing, J.V.-V., E.C. and G.P.; funding acquisition, J.V.-V., E.C., D.V. and G.P. All authors have read and agreed to the published version of the manuscript.

Funding

This work has been partially supported by the DGA/AID projects 2018.60.0072.00.470.75.01 and 2019.65.0068.00.470.75.01, and the Agence Nationale de la Recherche project ANR-17-CE22-0001-01.

Institutional Review Board Statement

All subjects gave their informed consent for inclusion before they participated in the study. The study was conducted in accordance with the Declaration of Helsinki, and the protocol was approved by the Ethics Committee of 2018.60.0072.00.470.75.01 and 2019.65.0068.00.470.75.01.

Informed Consent Statement

Informed consent was obtained from all subjects involved in the study.

Conflicts of Interest

The authors declare no conflict of interest.