Perspective Chapter: Approximate Kalman Filter Using M-Robust Estimate Dynamic Stochastic Approximation with Parallel Adaptation of Unknown Noise Statistics by Huber’s M-Robust Parameter Estimator

Abstract

The problem of designing a feasible adaptive M-robustified Kalman filter in a case of a thick-tailed Gaussian environment, characterized by impulsive noise-inducing observation and innovation outliers, and/or errors in mathematical model-inducing structural outliers, has been considered. Firstly, the time-varying criterion is used to generate a family of dynamic stochastic approximation algorithms. The M-robust estimate stochastic approximation is derived by minimizing the minimum variance criterion, the estimates of the latter being combined with the one-step minimum mean square error prediction to design M-robust estimate Kalman filter. Finally, the latter is combined with the Huber moving window M-robust parameter estimator of the unknown noises statistics, in parallel with the M-robust state estimation to design an adaptive M-robust estimate Kalman filter. Simulated maneuvering target tracking scenario revealed that the proposed adaptive M-robust estimate-based Kalman filter improves significantly the target estimation and tracking quality, being effective in suppressing multiple outliers with contamination degrees less than thirty percent. Moreover, the achieved improvement comes with additional computational efforts. However, these efforts are usually not significant enough to prevent real-time application.

1. Introduction

An optimal estimate of a stochastic variable, or a stochastic process, using noisy measurements, is the one that maximizes, or minimizes, a suitable performance index, the examples of the latter being least squares (LS), maximum a posteriori probability (MAP), maximum likelihood (ML), minimum variance (MV), minimum mean-square error (MMSE), least absolute value (LAV), etc. [1, 2, 3, 4, 5]. Moreover, the optimization criteria usually depend on suppositions upon the statistical performance of the system parameters and/or states that have to be estimated. One of the most important contributions to the estimation field, significant from both theoretical and practical perspectives, is the Kalman filter (KF), [1, 2, 3, 4, 5]. However, if the system dynamics, and adjoin measurements, are under the influence of severe nonlinear effects that may not be linearized adequately, and/or the noises are thick-tailed Gaussian distributed, KF deteriorates its performance, and even ceases to work [6, 7]. In such situations, it may be obtained bounded estimation error, in the statistical sense, by utilizing the dynamic stochastic approximation [8]. Therefore, the dynamic stochastic approximation performs quite well in various practical problems, involving optimization, state, and parameter estimation, as well as pattern recognition and classification, [9, 10, 11, 12].

Additionally, in numerous practical situations, the real noises are thick-tailed Gaussian distributed, generating rare but high-intensity samples, named observation and innovation outliers, corrupting the mostly normally distributed measurements. The third type of outliers, named structural outliers, is caused by errors in a mathematical model, such as unmodeled system dynamics, time-varying bias, computational errors, etc. However, since KF is a linear signal processor that linearly transforms observations to calculate the system state updates, it is susceptible to outlying samples, being not robust. Therefore, it is of interest to design a robustified modification of the optimal KF, being able to manage a local not Gaussian noise vicinity, typified by impulsive disturbances inducing unavoidable outliers [13, 14, 15, 16, 17, 18]. In such situations, robust methods of mathematical statistics produce appropriate tools to cope with outliers for reducing their influence on the estimation quality [19, 20, 21, 22, 23, 24, 25, 26]. Moreover, the Huber’s approximate maximum likelihood, or M-robust, approach is commonly applied by practitioners [21]. The history of research activities in the field of robust system parameter and/or state estimation through the application of different schemes, combining the Huber M-robust estimator and the LS, or KF, techniques, is pretty long, and many articles are devoted to these topics [17, 18, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37]. Thus, the emphasized solutions can be divided into two classes. The first one contains the batch processing, off-line, or non-recursive estimation procedures, in which the KF state estimation formulation is transformed to an adjoin parameter regression problem, the latter being resolved by utilizing the Huber M-robust approach [28, 29, 30, 31, 32]. The so attained robust procedure requires an iterated numerical method, such as the classical Newton method or its appropriate modification, as well as iterated reweighted LS [24, 25, 29, 30, 31, 32]. Here, the robustness feature is obtained using simultaneous processing of predictions and observations, making the procedure efficient in reducing the effects of outliers, but at the cost of larger numerical efforts. Another class contains the recursive, sequential, or online parameter and state estimators, due to practical requirements for the fast computations in real-time signal processing. These estimators represent a good trade-off between the computational complexity and estimation quality [13, 14, 15, 16, 17, 18, 33, 34, 35, 36, 37]. Particularly, it was proposed, in the recent past, an approximate KF, where the M-robust estimate dynamic stochastic approximation is used for the measurement update [17, 18]. Unfortunately, the M-robust scheme lacks sufficient adaptability, having a lower quality upon the pure normally distributed observations, but may also degrade its performance otherwise [17, 18, 27, 33, 34, 35, 36, 37]. Therefore, an adaptation to the local stochastic environment is needed. An appropriate approach to solve this problem, utilizing a combination of both the dynamic stochastic approximation and general time-varying M-robust criterion, together with the parallel Huber’s moving window M-robust parameter estimator of the first and second-order unknown observation and state noises statistics, has been presented in this manuscript. The organization of the manuscript is the next. An overview of the KF and brief presentation of the robustness concepts are given in Chapter 2. Chapter 3 considers a design of the M-robust estimate KF. Chapter 4 is devoted to designing an adaptive M-robust estimate KF. The proposed approach combines the M-robust estimate KF and the Huber’s moving window M-robust parameter estimator of unknown noise statistics. Simulation results that illustrate theoretical derivation, and demonstrate clearly the merits of emphasized adaptive M-robust estimate KF, using the real-life maneuvering target tracking example, are given in Chapter 5. Concluding remarks are presented in Chapter 6.

2. Overview of Kalman filtering technique

The optimal KF performances hold on to the recursive linear structure and the minimum variance procedure for calculating the gain sequence. Implementation of the optimal KF assumes both the system dynamics and adjoin measurement processes to be given by the linear, discrete-time, and state-space representation

$$x_{k+1}=F_k{x}_k+G_k{w}_k;y_k=H_k{x}_k+v_k$$E1

where $x_k$ is the $n\times 1$ system state, $y_k$ is the $r\times 1$ multidimensional observation, $w_k$ is the $m\times 1$ state noise, and $v_k$ is the $r\times 1$ observation noise, at the discrete-time, $k$. Furthermore, the sequences, $\left\{v_k\right\}$ and $\left\{w_k\right\}$, are the zero-mean white stochastic disturbances, being mutually uncorrelated, as well uncorrelated with the random initial state, $x_0$, with the mean value, $m_0$, and the covariance, $P_0$, that is

$$E\left\{\left[\begin{array}{c}{w}_k\\ v_k\end{array}\right]\left[w_k^T{v}_k^T\right]\right\}=\left[\begin{array}{cc}{Q}_k{\delta }_{\mathit{kj}}& 0\\ 0& R_k{\delta }_{\mathit{kj}}\end{array}\right];E\left\{\left[\begin{array}{c}{w}_k\\ v_k\end{array}\right]{\left[x_0-m_0\right]}^T\right\}=0$$E2

where $E\left\{\cdot \right\}$ is the mathematical expectation, and ${\delta }_{\mathit{kj}}$ is the Kronecker’s delta symbol (${\delta }_{\mathit{kj}}=0$ for $k\ne j$ and ${\delta }_{\mathit{kj}}=1$ for $k=j$). Here, $F_k$ is the $n\times n$ state transition matrix, $G_k$ is the $n\times m$ state noise transition matrix, and $H_k$ is the $r\times n$ observation matrix.

Let ${\widehat{x}}_{k/j}=E\left\{x_k|Y^j\right\}$, ($j=k-1,k$), with $E\left\{\cdot |\cdot \right\}$ being the conditional mathematical expectation, represent the optimal LS estimate of the present state, $x_k,$ upon the observation set $Y^j=\left\{y_i,i\le j\right\}$, while $P_{k/j}=E\left\{\left(x_k-{\widehat{x}}_{k\mid j}\right){\left(x_k-{\widehat{x}}_{k\mid j}\right)}^T\right\}$ designating the underlying error covariances. Then, the optimal KF relations are given by [1, 2, 3, 4, 5].

time update (prediction):

$${\widehat{x}}_{k+1/k}=F_k{\widehat{x}}_{k/k};P_{k+1/k}=F_k{P}_{k/k}{F}_k^T+G_k{Q}_k{G}_k^T$$E3

measurement update (estimation, filtering):

$${\widehat{x}}_{k/k}={\widehat{x}}_{k/k-1}+K_k{\epsilon }_k;{\epsilon }_k=y_k-H_k{\widehat{x}}_{k/k-1}$$E4

$$K_k=P_{k/k-1}{H}_k^T{S}_k^{-1};S_k=H_k{P}_{k/k-1}{H}_k^T+R_k$$E5

$$P_{k/k}=\left(I-K_k{H}_k\right)P_{k/k-1};K_k=P_{k/k}{H}_k^T{R}_k^{-1}$$E6

Thus, the filter can be initialized with

$${\widehat{x}}_{0/0}=E\left\{x_0\right\}=m_0=0;P_{0/0}=E\left\{\left[x_0-m_0\right]{\left[x_0-m_0\right]}^T\right\}=P_0$$E7

Here, the measurement residual, or innovation, ${\epsilon }_k$ in (4), is a zero-mean white stochastic sequence, with the variance $S_k$, in (5).

However, since the optimal KF linearly transforms observations, the state updates are not constrained, unless the system states are independent of the observations when the state updates are identically zero. In this sense, the linear optimal KF is susceptible to outliers, being not robust.

Thus, in the case of the Gaussian distributed observations contaminated by outliers, the two estimators are frequently used by practitioners [14, 29, 30]. In this sense, if the observation noise pdf is given by the $\delta$-contaminated Gaussian pdf

$$\mathit{CN}\left(\cdot |\delta ,{\sigma }_n^2,{\sigma }_0^2\right)=\left(1-\delta \right)N\left(\cdot |0,{\sigma }_n^2\right)+\mathit{\delta N}\left(\cdot |0,{\sigma }_0^2\right);0\le \delta <1,{\sigma }_0^2\gg {\sigma }_n^2$$E8

then there are two possible procedures to handle outliers:

1. the standard KF treats the zero-mean Gaussian distributed observations with the increased known variance

   $$R=\left(1-\delta \right){\sigma }_n^2+\delta {\sigma }_o^2$$E9

2. the nominal KF ignores abnormality, or outliers, and simply treats the Gaussian distributed observations with the given nominal variance, $R={\sigma }_n^2$.

In many cases of the non-Gaussian observations, the standard KF works fairly well, while the nominal one produces bad results, and even diverges [6, 7]. However, for complicated observation pdfs, and particularly if the non-Gaussian-ness is in a thick-tailed variety, as in (8), giving rise to observation and innovation outliers, the standard KF performance may also be quite poor [13, 14, 15, 16, 17, 18, 28, 29, 30, 31, 32, 33].

Formal mathematical definitions of robustness are given in the mathematical statistics [19, 20, 21, 22, 23], and the four such definitions exist. The two of them are ad hoc and data-oriented, named resistant and efficiency robustness, being preferable by the practitioners. Furthermore, an estimate is resistantly robust if it is insensitive both to a single outlying data point and the patchy, or grouped, ones. Additionally, some estimators are efficiently robust if they produce a high performance upon both the normally distributed measurements and normal ones under contamination, involving observation, innovation, and structural outliers [19, 20, 21]. Therefore, the practical robustness involves both the efficiency and resistant properties. The other two definitions, named min-max and qualitative robustness, have strong theoretical foundations [21, 23]. Thus, the qualitative robustness utilizes the Hampel’s influence function, measuring a robust estimator’s ability to cope with outliers [23]. On the other hand, the min-max robustness holds onto minimization of the estimator worst-case performance within the given distribution family to which the real observation noise distribution is confined [21]. Such optimization task is complex, and its exact solution may be derived only for a time-invariant model when the Fisher information is minimized [21, 27]. Thus, the robust min-max estimator reduces to the optimal ML one, whose likelihood function is defined by the worst-case pdf. On contrary, the Huber M-robust estimator is not quite the optimal ML one, but it approximates the last one to achieve both the efficiency and resistant robustness performances.

A simple and efficient concept to robustify the optimal KF can be developed using the Huber M-robust procedure. A version of this approach, based on both the one-step MMSE prediction and M-robust estimate dynamic stochastic approximation, is presented in the sequel.

3. M-robust estimate Kalman filtering

Although the dynamic stochastic approximation method is primarily designed to solve parameter regression problems, it may be extended to robust estimation of the dynamic system states, represented by (1), assuming the scalar measurements. The vectorial observations can be handled by considering components of the observation vector as the scalar observation, being used one at a time. Here is assumed that the observation vector components, in (1), are mutually uncorrelated scalar random variables, yielding a diagonal covariance, $R$ in (2), A stable numerical algorithm to diagonalize a symmetric covariance matrix is the Choleskey factorization, or the UD decomposition [2]. In addition, due to simplicity, it uses the shortened notation ${\widehat{x}}_{k\mid k-1}={\overline{x}}_k$, ${\widehat{x}}_{k\mid k}={\widehat{x}}_k$, $P_{k\mid k-1}=M_k$, and $P_{k\mid k}=P_k$. In this sense, one can define the time-varying functional of the scaled innovations, representing the M-robust optimization criterion

$$J_k\left({\overline{x}}_k\right)=E\left\{\left.\rho \left(\frac{{\epsilon }_k\left({\overline{x}}_k\right)}{s_k}\right)\right|{\overline{x}}_k,Y^k\right\}$$E10

Here, $E\left\{\cdot |{\overline{x}}_k,Y^k\right\}$ is the conditional mathematical expectation given the current system state prediction, ${\overline{x}}_k$, and the measurement set, $Y^k=\left\{y_1,\dots ,y_k\right\}$, where $\rho \left(\cdot \right)$is Huber’s robust score function

$$\rho \left(z\right)=\left\{\begin{array}{l}\mathrm{\Delta }\left|z\right|-{\mathrm{\Delta }}^2/2;\left|z\right|\ge \mathrm{\Delta }\\ z^2/2;\left|z\right|<\mathrm{\Delta }\end{array}\right.;\psi \left(\cdot \right)={\rho }^{\prime }\left(\cdot \right)=min\left(\left|z\right|,\mathrm{\Delta }\right)sgn\left(z\right)$$E11

that has to cut off the outliers, while $\psi$ is the influence function, that determines the estimator’s ability to reduce the effects of different outlier types, including the observation, innovation, and structural outliers [17, 18, 19, 22, 23, 24, 25, 26]. Here, the saturation threshold, $\mathrm{\Delta }$, is a tuning parameter, providing for the required efficiency upon the pure Gaussian distributed observations, in (8) with $\delta =0$.

Using step-by-step minimization of the M-robust criterion (10), one generates a family of the recursive gradient-type algorithms

$${\widehat{x}}_k={\overline{x}}_k+{\mathrm{\Gamma }}_k{g}_k\left({\overline{x}}_k\right);g_k\left({\overline{x}}_k\right)=-{\nabla }_{\overline{x}}{J}_k\left({\overline{x}}_k\right)$$E12

where ${\nabla }_{\overline{x}}{J}_k\left(\cdot \right)$ is gradient vector of the optimization criterion in (10), while the innovation or measurement residual, $\epsilon$, is given by

$${\epsilon }_k=\epsilon \left({\overline{x}}_k\right)=y_k-H_k{\overline{x}}_k;S_k=E\left\{{\epsilon }_k{\epsilon }_k^T\right\}=H_k{M}_k{H}_k^T+R_k;s_k=S_k^{1/2}$$E13

with $S_k$ being the residual variance, with the corresponding standard deviation, $s_k$. Taking into account (10) and (11), one obtains the gradient vector expression

$${\nabla }_{\overline{x}}{J}_k\left({\overline{x}}_k\right)=\frac{{\partial }_k{J}_k\left({\overline{x}}_k\right)}{\partial {\overline{x}}_k}=-\frac{1}{s_k}E\left\{\left.\psi \left(\frac{{\epsilon }_k}{s_k}\right)\right|{\overline{x}}_k,Y^k\right\}{H}_k^T$$E14

However, the expectation, in (14), is undeterminable and may be represented approximately by the current sample, producing the stochastic gradient relation

$$m_k=-\frac{1}{s_k}\psi \left(\frac{{\epsilon }_k}{s_k}\right)H_k^T$$E15

Thus, by replacing (15) with (12), one gets

$${\widehat{x}}_k={\overline{x}}_k-{\mathrm{\Gamma }}_k{m}_k={\overline{x}}_k+\frac{1}{s_k}{\mathrm{\Gamma }}_k{H}_k^T\psi \left(\frac{{\epsilon }_k}{s_k}\right)$$E16

with $s_k$ being the normalizing, or scaling, factor, in (13).

The gain sequence, ${\mathrm{\Gamma }}_k$ in (16), has to supervise the convergence rate. Therefore, to link the M-robust dynamic stochastic approximation method (16), and the optimal KF, in (3)–(6), an additional approximate MV optimization criterion is adopted

$$J_1\left({\mathrm{\Gamma }}_k\right)=\mathit{\text{Trace}}{P}_k;P_k=E\left\{{\tilde{x}}_k\left(+\right){\tilde{x}}_k^T\left(+\right)\right\};{\tilde{x}}_k\left(+\right)=x_k-{\widehat{x}}_k$$E17

with $P_k$ being the underlying error covariance, while the matrix Trace is the sum of its diagonal components. The posed optimization problem, in (17), is a complex one that cannot be solved exactly, and a suboptimal solution has to be derived. Thus, the relation (16) can be rewritten in a recursive weighted LS form, being equal formally to the KF time-update (4), that is

$${\widehat{x}}_k={\overline{x}}_k+K_k{\epsilon }_k;K_k=s_k^{-2}{\omega }_k{\mathrm{\Gamma }}_k{H}_k^T$$E18

with the robust weighting, normalizing, or penalty, term

$${\omega }_k=\left\{\begin{array}{l}\frac{\psi \left({\epsilon }_k/s_k\right)}{{\epsilon }_k/s_k}\text{for}{\epsilon }_k\ne 0\text{and}{s}_k\ne 0\\ 1\text{for}{\epsilon }_k=0\text{and}/\text{or}{s}_k=0\end{array}\right.$$E19

The prediction error, ${\tilde{x}}_k\left(-\right)$, is given by (1) and (3), from which it follows

$${\tilde{x}}_k\left(-\right)=x_k-{\overline{x}}_k=F_{k-1}{\tilde{x}}_{k-1}\left(+\right)+G_{k-1}{w}_{k-1}$$E20

where, due to (18), (13), and (1), the estimation error, ${\tilde{x}}_k\left(+\right)$, is determined by

$${\tilde{x}}_k\left(+\right)=x_k-{\widehat{x}}_k={\tilde{x}}_k\left(-\right)-K_k{H}_k{\tilde{x}}_k\left(-\right)-K_k{v}_k$$E21

with $\left\{w_k\right\}$ and $\left\{v_k\right\}$ being the state and observation noises sequences of the zero-mean uncorrelated random variables, in (1) and (2). Under the initial guess (7), one concludes, by the mathematical induction, that the errors (20) and (21) are unbiased. Furthermore, under the assumption (2), the estimation errors and the noises are mutually uncorrelated, yielding

$$E\left\{{\tilde{x}}_k\left(-\right)\right\}=0;E\left\{{\tilde{x}}_k\left(+\right)\right\}=0;E\left\{{\tilde{x}}_k\left(+\right)w_k^T\right\}=0;E\left\{{\tilde{x}}_k\left(-\right)v_k^T\right\}=0$$E22

Taking into account (20) and (22), one concludes that the covariance, $M_k$, is defined by the KF Eq. (3), yielding

$$M_k=E\left\{{\tilde{x}}_k\left(-\right){\tilde{x}}_k^T\left(-\right)\right\}=F_{k-1}{P}_{k-1}{F}_{k-1}^T+G_{k-1}{Q}_{k-1}{G}_{k-1}^T$$E23

Moreover, it follows from (2), (21), and (22) that the error covariance, $P_k$ in (17), is given by

$$P_k=E\left\{{\tilde{x}}_k\left(+\right){\tilde{x}}_k^T\left(+\right)\right\}=M_k-K_k{H}_k{M}_k-M_k{H}_k^T{K}_k^T+R_k{K}_k{K}_k^T$$E24

By substituting (18) into (24), the relation (17) reduces to

$$J_1\left({\mathrm{\Gamma }}_k\right)=\mathit{\text{Trace}}{M}_k-2s_k^{-2}{\omega }_k\mathit{\text{Trace}}{\mathrm{\Gamma }}_k{H}_k^T{H}_k{M}_k+s_k^{-4}{\omega }_k^2{R}_k\mathit{\text{Trace}}{\mathrm{\Gamma }}_k{H}_k^T{H}_k{\mathrm{\Gamma }}_k$$E25

After applying the partial derivation, based on the rules to partial derivation of trace of matrices product, [1, 4].

$\frac{\partial }{\partial A}\mathit{\text{TraceBAC}}=B^T{C}^T;\frac{\partial }{\partial A}{\mathit{\text{TraceABA}}}^T=2\mathit{AB}\text{if}B=B^T$

one obtains from (25)

$$\frac{\partial J_1\left({\mathrm{\Gamma }}_k\right)}{\partial {\mathrm{\Gamma }}_k}=-2s_k^{-2}{\omega }_k{M}_k{H}_k^T{H}_k+2s_k^{-4}{\omega }_k^2{R}_k{\mathrm{\Gamma }}_k{H}_k^T{H}_k=0$$E26

Additionally, due to (11), the term ${\omega }_k$, in (19), can be approximated by only the zero and unity values, yielding ${{\omega }_k}^2\approx {\omega }_k$, from which it follows

$$\frac{\partial J_1\left({\mathrm{\Gamma }}_k\right)}{\partial {\mathrm{\Gamma }}_k}=-2s_k^{-2}{\omega }_k\left(M_k-s_k^{-2}{R}_k{\mathrm{\Gamma }}_k\right)H_k^T{H}_k=0$$E27

Moreover, from (27) and (13), one gets

$${\mathrm{\Gamma }}_k=s_k^2{R}_k^{-1}{M}_k=\left(R_k^{-1}{H}_k{M}_k{H}_k^T+1\right)M_k\approx M_k;s_k^2{R}_k^{-1}\approx 1$$E28

The approximation of ${\mathrm{\Gamma }}_k$ by $M_k$ is based on the following reasoning: in saturation region of the $\psi$ nonlinearity (11), indicating the outliers presence, the observation noise variance, $R_k$ in (9), has a huge value. On contrary, in linear segment of the $\psi$ nonlinearity, indicating the outliers’ absence, the M-robust filter (18) reduces to the optimal KF, (3)–(6), yielding a low prediction error covariance $M_k$. Applying (28), (18), and (24), one gets

$$P_k=M_k-2s_k^{-2}{\omega }_k{M}_k{H}_k^T{H}_k{M}_k+R_k{s}_k^{-4}{\omega }_k^2{M}_k{H}_k^T{H}_k{M}_k\approx M_k-s_k^{-2}{\omega }_k{M}_k{H}_k^T{H}_k{M}_k$$E29

Taking into account (18), the expression (29) reduces to the optimal KF Eq. (6), that is

$$P_k=\left(I-K_k{H}_k\right)M_k;{\mathrm{\Gamma }}_k=M_k;K_k=s_k^{-2}{\omega }_k{M}_k{H}_k^T;K_k={\omega }_k{P}_k{H}_k^T{R}_k^{-1}$$E30

Additionally, derivation of the fourth equation for the gain $K_k$, in (30), is based on the matrix inversion lemma [1, 4]. Thus, by substituting $S_k$ from (13) into the third equation in (30), one obtains

$$K_k={\omega }_k{M}_k{H}_k^T{\left(H_k{M}_k{H}_k^T+R_k\right)}^{-1}$$E31

Furthermore, by replacing (31) with the first equation in (30), and taking into account the approximation ${\omega }_k^2\approx {\omega }_k$, one obtains

$${\omega }_k{P}_k={\omega }_k{M}_k-\frac{{\omega }_k{M}_k{H}_k^T{H}_k{\omega }_k{M}_k}{H_k{\omega }_k{M}_k{H}_k^T+{\omega }_k{R}_k}$$E32

After application of the matrix inversion lemma, stating that the relation.

$$P_2=P_1-P_1{H}^T{\left({\mathit{HP}}_1{H}^T+R\right)}^{-1}{\mathit{HP}}_1$$

can be represented in an alternative form

$$P_2^{-1}=P_1^{-1}+H^T{R}^{-1}H$$

the relation (32) reduces to

$$P_k^{-1}=M_k^{-1}+H_k^T{R}_k^{-1}{H}_k;P_k\left(M_k^{-1}+H_k^T{R}_k^{-1}{H}_k\right)=I$$E33

where $I$ is an identity matrix. Using (33), the relation (31) can be rewritten as

$$K_k=\frac{P_k\left(M_k^{-1}+H_k^T{R}_k^{-1}{H}_k\right){\omega }_k{M}_k{H}_k^T}{H_k{M}_k{H}_k^T+R_k}={\omega }_k{P}_k{H}_k^T{R}_k^{-1}$$E34

being identical to the fourth relation, in (30). In accordance with the optimal KF, the derived M-robust estimate KF has the next recursive form.

time update (prediction), (3), (23):

$${\overline{x}}_{k+1}=F_k{\widehat{x}}_k+G_k{\overline{w}}_k;M_{k+1}=F_k{P}_k{F}_k^T+G_k{Q}_k{G}_k^T;{\overline{w}}_k=E\left\{w_k\right\}$$E35

measurement update (estimation, filtering), (11), (13), (18), (19), (30):

$${\widehat{x}}_k={\overline{x}}_k+K_k{\epsilon }_k;{\epsilon }_k=y_k-H_k{\widehat{x}}_k-{\overline{v}}_k;{\overline{v}}_k=E\left\{v_k\right\}$$E36

$$S_k=s_k^2=E\left\{{\epsilon }_k{\epsilon }_k^T\right\}=H_k{M}_k{H}_k^T+R_k;{\omega }_k=\left\{\begin{array}{l}\frac{\psi \left({\epsilon }_k/s_k\right)}{{\epsilon }_k/s_k}\text{for}{\epsilon }_k\ne 0\text{and}{s}_k\ne 0\\ 1\text{for}{\epsilon }_k=0\text{and}/\text{or}{s}_k=0\end{array}\right.$$E37

$$K_k=s_k^{-2}{\omega }_k{M}_k{H}_k^T;P_k=\left(I-K_k{H}_k\right)M_k;K_k={\omega }_k{P}_k{H}_k^T{R}_k^{-1}$$E38

where, due to the assumption (2), ${\overline{w}}_k={\overline{v}}_k=0$.

There is the same ad hoc logic in the M-robustified KF gain calculation, in (34) or (38), and calculation of the KF gain, in (6). Thus, taking into account that the weighted factor $\omega$, in (19) and (37), approximates the slope, or the first derivative, of the $\psi$ nonlinearity (11), one concludes that $\omega \left(z\right)\approx 1$ if $\left|z\right|\le \Delta$, while $\omega \left(z\right)\approx 0$ if $\left|z\right|>\Delta .$ In this sense, for those measurements being confined to the linear region of $\psi$ non-linearity, indicating the Gaussian distributed observations, value of the factor $\omega$ is closed to the unity value, and the expression (34) approximates properly the optimal KF gain, in (6). Furthermore, upon the saturation region of $\psi$ non-linearity, indicating the outliers’ existence, the factor $\omega$ is zero-valued, resulting in a low robust gain $K$, in (34), the value of the latter being produced a low value of the state update, in (18) and (28). This, in turn, reduces the effects of multiple outliers.

The application of the KF needs the state and observation noise statistics, in (1) and (2), to be known in advance. However, these requirements may be not fulfilled in practice, due to modeling errors, inducing structural outliers, as well the presence of innovation and observation outliers, [17, 18, 28, 29, 30]. A suitable approach to designing an adaptive modification of the emphasized M-robust estimate KF, in (35)–(38), based on the Huber moving window M-robust parameter estimator of unknown disturbances statistics, is considered in the next chapter.

4. Adaptive M-Robustifying Kalman filtering with unknown noise statistics

In various practical examples, components of the vector-valued measurements may be processed one at a time, so that the observations, in (1), become scalar random variables. Since the true system state is unknown, the observation noise sample, $v_k$, cannot be estimated by (1). However, an intuitive approximation of the observation noise sample, $v_k$, is given by the scalar random variable

$$r_k=y_k-H_k{\widehat{x}}_k$$E39

where ${\widehat{x}}_k$ is the M-robust state estimate, in (35)–(38). Taking into account (1), (2), and (22), one gets further

$$r_k=H_k\left(x_k-{\widehat{x}}_k\right)+v_k=H_k{\tilde{x}}_k+v_k;{\overline{r}}_k=E\left\{r_k\right\}=E\left\{v_k\right\}={\overline{v}}_k$$E40

where ${\tilde{x}}_k$ is the robust estimation error, in (17). In addition, taking into account (2), (22), and (40), the variance of the real scalar random variable, $r_k$, is given by

$$V_r\left(k\right)=E\left\{\left[r_k-{\overline{r}}_k\right]{\left[r_k-{\overline{r}}_k\right]}^T\right\}=H_{k}E\left\{{\tilde{x}}_k{\tilde{x}}_k^T\right\}{H}_k^T+E\left\{{\left[v_k-{\overline{r}}_k\right]}^2\right\}=H_k{P}_k{H}_k^T+R_k$$E41

where $P_k$ is the error covariance matrix, in (17). Therefore, if the first and second-order moments, ${\overline{r}}_k$ and $V_r\left(k\right)$, are somehow estimated, say by ${\widehat{r}}_k$ and ${\widehat{V}}_r\left(k\right),$ one can estimate further ${\overline{v}}_k$ and $R_k$, from (40) and (41), respectively. In this sense, one obtains the following estimates for the observation noise statistics (men value and variance)

$${\widehat{v}}_k={\widehat{r}}_k;{\widehat{R}}_k=\left|{\widehat{V}}_r\left(k\right)-H_k{P}_k{H}_k^T\right|$$E42

where $P_k$ is generated by the M-robustified KF, (35)–(38). Here, $\left|\cdot \right|$ denotes the absolute value to prevent the negative variance estimates, due to possible numerical errors in applications.

Let us assume further, due to simplicity and clarity, that the state noise sample, $w_k$ in (1), is a scalar random variable, as it is frequently fulfilled in tracking applications, [38, 39, 40]. In addition, if the system under consideration is a single input - single output (SISO), and the corresponding state-space model (1) is presented in the controllable canonical form, $G$ in (1) is the $n\times 1$ column vector, with zero-valued components, except the last one that is unity valued, [4]. Thus, the random input, or state noise sample, $w_k$, is a real scalar random variable. Moreover, the state noise sample, $w_k$, can be estimated from the state Eq. (1), when the unknown random states, $x_{k+1}$ and $x_k$, are replaced by the two successive robust estimates, ${\widehat{x}}_{k+1}$ and ${\widehat{x}}_k$, in (35)–(38). A such obtained state noise sample estimate represents a scalar random variable

$$q_k=T_k\left({\widehat{x}}_{k+1}-F_k{\widehat{x}}_k\right);T_k={\left(G_k^T{G}_k\right)}^{-1}{G}_k^T;G_k^T{G}_k=1$$E43

The relation (43) can be rewritten in an alternative form

$$q_k=T_k\left({\widehat{x}}_{k+1}-F_k{\widehat{x}}_k-x_{k+1}+x_{k+1}\right)$$E44

which, after replacing (1) with (44) reduces to

$$q_k=T_k\left(-{\tilde{x}}_{k+1}-F_k{\tilde{x}}_k+G_k{w}_k\right);{\tilde{x}}_k=x_k-{\widehat{x}}_k$$E45

Taking into account (1), (2), and (22), further follows that the unknown mean value, ${\overline{w}}_k$, of the state noise, $w_k$, is equal to the mean value, ${\overline{q}}_k$, of the random variable, $q_k$, that is

$${\overline{q}}_k=E\left\{q_k\right\}=T_k{G}_{k}E\left\{w_k\right\}={\overline{w}}_k;T_k{G}_k=1$$E46

Moreover, variance of the random variable, $q_k$ in (43), is defined by

$$V_q\left(k\right)=E\left\{{\left[q_k-{\overline{q}}_k\right]}^2\right\}=E\left\{\left[q_k-{\overline{q}}_k\right]{\left[q_k-{\overline{q}}_k\right]}^T\right\}$$E47

Bearing in mind (1), (2), and (22), and by substituting (45) and (46) into (47), one gets

$$\begin{array}{ll}{V}_q\left(k\right)=& T_k\left[P_{k+1}-F_{k}E\left\{{\tilde{x}}_k{\tilde{x}}_{k+1}^T\right\}-G_{k}E\left\{\left[w_k-{\overline{w}}_k\right]{\tilde{x}}_{k+1}^T\right\}\right.-E\left\{{\tilde{x}}_{k+1}{\tilde{x}}_k^T\right\}{F}_k^T\\ & \left.+F_k{P}_k{F}_k^T-E\left\{{\tilde{x}}_{k+1}\left[w_k-{\overline{w}}_k\right]\right\}{G}_k^T+G_{k}E\left\{{\left[w_k-{\overline{w}}_k\right]}^2\right\}{G}_k^T\right]T_k^T\end{array}$$E48

Furthermore, by substituting the prediction error, in (20), for the estimation error, in (21), one obtains the estimation error recursion

$${\tilde{x}}_{k+1}=\left(I-K_{k+1}{H}_{k+1}\right)F_k{\tilde{x}}_k+\left(I-K_{k+1}{H}_{k+1}\right)G_k\left[w_k-{\overline{w}}_k\right]-K_{k+1}\left[v_{k+1}-{\overline{v}}_{k+1}\right]$$E49

from which it follows

$$F_{k}E\left\{{\tilde{x}}_k{\tilde{x}}_{k+1}^T\right\}=F_k{P}_k{F}_k^T{\left(I-K_{k+1}{H}_{k+1}\right)}^T;E\left\{{\tilde{x}}_{k+1}\left[w_k-{\overline{w}}_k\right]\right\}=\left(I-K_{k+1}{H}_{k+1}\right)G_k{Q}_k$$E50

Furthermore, by substituting (50) into (48), together with using the expressions for the error covariances, $M_k$ in (23), and, $P_k$ in (30), one gets

$$V_q\left(k\right)=T_k\left(-P_{k+1}+F_k{P}_k{F}_k^T\right)T_k^T+Q_k$$E51

Therefore, if ${\widehat{q}}_k$ and ${\widehat{V}}_q\left(k\right)$ denote some estimates of the unknown mean value, ${\overline{q}}_k$ in (46), and the variance, $V_q\left(k\right)$ in (51), respectively, one can estimate the unknown first and second-order state noise statistics, ${\overline{w}}_k$ and $Q_k,$ by the following equations

$${\widehat{w}}_k={\widehat{q}}_k;{\widehat{Q}}_k=\left|{\widehat{V}}_k\left(k\right)-T_k\left(-P_{k+1}+F_k{P}_k{F}_k^T\right)T_k^T\right|$$E52

Relations (42) and (52) are still valid for the KF, in (3)–(6), since the expressions for the state prediction, ${\overline{x}}_k$, prediction error covariance, $M_k$, state estimation update, ${\widehat{x}}_k$, and estimation error covariance, $P_k$, are given by the identical equations to the M-robust estimate KF, in (35)–(38). Thus, the derived relations, (42) and (52), can be applied to designing not only an adaptive M-robustified KF but also to designing an adaptive linear optimal KF.

Next, to calculate the unknown observation noise statistics, in (42), as well as the unknown state noise statistics, in (52), a scalar constant parameter estimation problem has to be considered. Thus, let us consider a real scalar random variable, $\Re$, generating the independent and identically distributed (iid) samples, $r_i$, $i=k-L+1,\dots ,k$, in (39), being stored in a one-step moving window of the size $L$ at the present stage, $k$. Using the empirical samples, the mean value, or the location parameter, ${\overline{r}}_k=m_{\Re }$, and the variance, $V_r\left(k\right)=V_{\Re }$, related to the random variable, $\Re$, has to be calculated at each stage, $k$. Unfortunately, analogously to the linear optimal KF, the commonly used sample mean and sample variance are not robust, [19, 20, 21, 22]. Therefore, the Huber moving window M-robust parameter estimator is proposed as a robust alternative to the sample-mean and variance estimates. The Huber M-robust estimate, ${\widehat{r}}_k$, of the unknown mean value, ${\overline{r}}_k$, minimizes the robust index of goodness, being given by the experimental sum

$$J_{\mathit{ME}}\left({\widehat{r}}_k\right)=\frac{1}{L}\sum _{i=k-L+1}^k\rho \left(\frac{r_i-{\widehat{r}}_k}{{\widehat{d}}_k}\right)$$E53

where $\left\{r_i\right\}$ is the given sequence of observations, in (39), covering the last $L$ samples related to the current stage, $k$, and $\rho \left(\cdot \right)$ is the Huber’s loss function, in (11), [21]. Thus, the M-robust estimate, ${\widehat{r}}_k$ in (53), represents a solution of the nonlinear algebraic equation, obtained from the optimality condition

$${\left.\frac{{\mathit{dJ}}_{\mathit{ME}}\left(r\right)}{\mathit{dr}}\right|}_{r={\widehat{r}}_k}=\sum _{i=k-L+1}^k\psi \left(\frac{r_i-{\widehat{r}}_k}{{\widehat{d}}_k}\right)=0;\psi \left(x\right)=\frac{\mathit{d\rho }\left(x\right)}{\mathit{dx}}$$E54

where $\psi$ is the saturation nonlinearity, in (11). The scaling factor, ${\widehat{d}}_k$, provides for the scale-invariant characteristic of the M-robust location parameter estimator, in (53) and (54). Although ad hoc, a popular robust estimate, ${\widehat{d}}_k$, is the median of the absolute median deviation (MAD)

$${\widehat{d}}_k=\mathit{\text{median}}\left\{r_i-\mathit{\text{median}}\left(r_i\right)\right\}/0.6745;i=k-l+1,\dots ,k$$E55

being defined on a one-step moving window of the size $l$ [19, 21, 22, 23, 24, 25]. The divisor 0.6745 is taken since upon large enough sample size, $l$, and if the samples belong to the Gaussian pdf, then ${\widehat{d}}_k$ is equal approximately to the true variance, ${\sigma }_{\Re }^2=V_{\Re }$, [21]. Furthermore, the proposed procedure of choosing ${\widehat{d}}_k$ indicates a suitable choice of the saturation threshold, $\mathrm{\Delta }$, in (11). Since, ${\widehat{d}}_k\approx {\sigma }_{\Re }$, the free parameter, $\mathrm{\Delta }$, is commonly chosen to be some number about 1.5. Such estimate is named the Huber 1.5-M-robust parameter estimator [21]. The Eq. (54) is non-linear, and an iterated numerical algorithm is requested [21, 24, 25]. However, an approximate weighted LS approach results in a one-step suboptimal solution

$$\sum _{i=k-L+1}^k{\omega }_{i0}\left(r_i-{\widehat{r}}_k\right)\approx 0;{\widehat{r}}_k={\widehat{r}}_L=\frac{\sum _{i=k-L+1}^k{\omega }_{i0}{r}_i}{\sum _{i=k-L+1}^k{r}_i};{\omega }_{i0}=\left\{\begin{array}{l}\frac{\psi \left(\frac{r_i-{\widehat{r}}_0}{{\widehat{d}}_k}\right)}{\frac{r_i-{\widehat{r}}_0}{{\widehat{d}}_k}};{\widehat{d}}_k\ne 0\text{and}{r}_i\ne {\widehat{r}}_0\\ 1;{\widehat{d}}_k=0\text{and}/\text{or}{r}_i={\widehat{r}}_0\end{array}\right.$$E56

with ${\widehat{r}}_0$ being an arbitrary initial guess [25, 29, 30]. Particularly, when ${\widehat{d}}_k=\sqrt{V_{\Re }}$, the asymptotic variance of M-robust location parameter estimator (53), (54) is given by [21].

$$V_a=\underset{L\to \infty }{lim}E\left\{L{\left({\widehat{r}}_L-\overline{r}\right)}^2\right\}=\frac{V_{\Re }E\left\{{\psi }^2\left[\frac{r_i-\overline{r}}{\sqrt{V_{\Re }}}\right]\right\}}{E^2\left\{{\psi }^{\prime }\left[\frac{r_i-\overline{r}}{\sqrt{V_{\Re }}}\right]\right\}};\overline{r}=m_{\Re };V_r=V_{\Re }$$E57

A natural estimate of $V_a$, in (57), is obtained when the undeterminable expectation is approximated by the average sum, together with the MAD estimator (55) for the scaling factor calculation at each stage, $k$. Thus, the unknown variance, $V_r\left(k\right)$ in (42), may be estimated by the Huber’s moving window M-robust parameter estimator

$${\widehat{V}}_r\left(k\right)=\frac{{\widehat{d}}_k^2\left\{\frac{1}{L}{\sum }_{i=k-L+1}^k{\psi }^2\left[\frac{r_i-{\widehat{r}}_k}{{\widehat{d}}_k}\right]\right\}}{{\left\{\frac{1}{L}{\sum }_{i=k-L+1}^L{\psi }^{\prime }\left[\frac{r_i-{\widehat{r}}_k}{{\widehat{d}}_k}\right]\right\}}^2}$$E58

Moreover, if $\psi \left(\cdot \right)$ is the linear function, the variance estimate (58) reduces to the sample variance.

The M-robust parameter estimator, in (55), (56), and (58), is used to calculate the nuisance estimates, ${\widehat{r}}_k$ and ${\widehat{V}}_r\left(k\right)$, the estimates of the latter are used to calculate the requested observation noise mean value, ${\widehat{v}}_k$, and the variance, ${\widehat{R}}_k$, from (42). The state noise sample estimates, $q_k$ in (43), has to be managed simultaneously with the observation noise samples, $r_k$ in (39). Thus, an additional one-step sliding frame of the size $L$ has to be used, covering the last $L$ samples, $q_i$, $i=k-L+1,\dots ,k$, at each stage, $k$. Again, the Huber moving window M-robust parameter estimator, in (55), (56), and (58), is used to estimate the nuisance parameters, ${\widehat{q}}_k$ and ${\widehat{V}}_q\left(k\right)$, the estimates of the latter are applied to calculating the required state disturbances statistics, ${\widehat{w}}_k$ and ${\widehat{Q}}_k$, from (52). The proposed adaptive estimation scheme needs an additional memory and shifting operations upon the sample estimates, $r_i$ and $q_i$, within the given sliding data frames of the length L. Thus, the samples $r_{k-L}$ and $q_{k-L}$are dropped when the current samples, $r_k$ and $q_k$, are stored in the current sliding data frame.

It should be noted that the robust MAD estimator, in (55), may be also used to estimate the variances, ${\widehat{V}}_r\left(k\right)$ and ${\widehat{V}}_q\left(k\right)$, but the robust estimator (58) represents a better solution at the expense of increased computational requirements. Another alternative estimator to calculate robustly the variance may be found in the statistical literature [19, 20, 21, 22, 23, 24, 25, 26]. These solutions use primarily some influence function, being non-decreasing near the origin, but decreasing toward zero from the origin, named redescending influence function. However, the underlying M-robust estimators may not have a unique solution, requiring a brief choice of the initial conditions, being generated, for example, by the Huber’s robust estimator. Furthermore, during the filter initial steps, the disturbance sample estimates, (39) and (43), may represent bad indicators of the stochastic surroundings. Therefore, a fading memory method, weighting the successive disturbance sample estimates, $r_k$ or $q_k$, by the growing factor

$$g_k=\left(k-L\right)\left(k-L-1\right)\dots \left(k-L-\gamma \right)/k^{\gamma };k=L,L+1,\dots ;\underset{k\to \infty }{lim}{g}_k=1$$E59

can be applied to delay the individual noise sample estimates, $r_k$ and $q_k$, over the first $\gamma$ steps [15, 30]. Moreover, during the filter initialization, the first $L$ noise samples, $r_k$ and $q_k,$ may be generated by using the Gaussian random number generator, with zero-mean and the corresponding variances, ${\widehat{R}}_1$ and ${\widehat{Q}}_0$, the values of the latter being used to define the fixed scaling factors at the initial data frames. It should be noted that in most applications, the mean values of observation and state noises are zero-valued, and there is no need to estimate these noise statistics. In such a case, the mean value estimates ${\widehat{r}}_k$ and ${\widehat{q}}_k$, in (55) and (56), are used only to calculate the variance estimates, ${\widehat{V}}_k$ and ${\widehat{V}}_q$, by (58), the estimates of the latter are used to calculate the required observation and state noise variances, using (42) and (52), respectively.

A theoretical convergence analysis of the emphasized M-robustified KF algorithm, either adaptive or not, is complex in the technical sense, owing to both a nonlinear scaled innovation processing and a time-varying multivariable dynamic system model. Moreover, drawing appropriate conclusions to sensitivity on multiple outliers, involving the observation, innovation, and structural ones, as well as choice of the initial conditions and the form of nonlinear influence function, is exceedingly challenging. Therefore, comprehensive Monte Carlo simulations are needed to refine the characteristics of the proposed adaptive M-robust estimate based KF and to bring appropriate conclusions.

5. Simulation results

A requirement for simple filters in real-time applications implies the desirability of uncoupled filtering, where independent tracking is performed in each coordinate ($x,y,z$) of the Cartesian Coordinates System (CCS). Particularly, three independent KF in the CCS coordinates are utilized, each of them being based on the three-dimensional state (position, velocity, and acceleration) representation, in (1). The first step in simulations is to generate the nominal target trajectory. The kinematic equations of motion, with respect to an inertial reference system, produce the true state variables, in (1), when a desirable acceleration profile, being a piece-wise constant over the sampling intervals, is adopted [40]. The so-obtained target trajectory is used in the presented simulations, and Figure 1 depicts the actual target trajectory projections in each of the CCS planes $\mathit{xOy},$$\mathit{yOz}$ and $\mathit{xOz}$, respectively.

Figure 1.

The actual target trajectory in the CCS planes (x, 0, y), (x, 0, z), (y, 0, z).

Because of a similar behavior of the actual components of three-dimensional state vector in each of the CCS coordinates, only the $z$-coordinate is represented in Figure 2, where $z_k$, ${\dot{z}}_k$, and ${\ddot{z}}_k$ are the position, velocity, and acceleration of a target, at the k-th scan. Proceeding from a thick-tailed Gaussian behavior of the position measurement probability distribution function, associated with the target glint noise spikes, inducing innovation and observation outliers, the observation noise sequence, $\left\{v_k\right\}$, is generated as the samples from the $\delta$-contaminated normal pdf, in (8). The random noise sample, $v_k$, is produced by taking firstly a uniformly distributed sample, $u$, in the [0,1] interval. Thus, $u\le \delta$, the sample, $v_k$, is produced by the contaminating normal pdf, $N\left(\cdot |0,{\sigma }_0^2\right)$, in other ways, the sample, $v_k$, belongs to the nominal normal pdf, $N\left(\cdot |0,{\sigma }_n^2\right)$. A typical heavy-tailed glint noise record is given in Figure 3.

Figure 2.

The actual components of the three-dimensional state (position, velocity, and acceleration) in the z-CCS coordinate.

Figure 3.

Typical thick-tailed Gaussian glint noise record $\left(\delta =0.05,{\sigma }_n=1,{\sigma }_0=90\right)$.

The position measurements, in (1), are generated at each scan, $k$, by adding the previously generated observation noise sample, $v_k$, to the true target projection on the CCS coordinate of interest. For example, the noise sample, $v_k$, has to be added to the true $z_k$ position, in Figure 2, assuming that the $z$-CCS coordinate is concerned.

The filter-world equations of motion include only the linear discrete-time state-space representation with only the position measurement, in (1). Thus, the corresponding system matrices take the form

$$F=F\left(T\right)=\left[\begin{array}{ccc}1& T& T^2/2\\ 0& 1& T\\ 0& 0& 1\end{array}\right];G=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right];H=\left[\begin{array}{ccc}0& 0& 0\end{array}\right]$$E60

Here, $T$ represents the constant period of sampling, and $T=4s$ is used.

Radar observations commonly involve the range, $r$, elevation, $\beta$, and azimuth, $\alpha$, angles, because the measurement errors are not coupled in the Spherical Coordinate System (SCS). Moreover, these measurements, when transformed to CCS, are no longer uncoupled, so the use of the three independent and uncoupled three-dimensional state KF, based on the model (1) and (60), in each of the CCS coordinates, may lead to less accurate filtering than it would occur if the coupled nine-dimensional state KF is used. Thus, the emphasized procedure is a good balance between tracking ability and computational requirements, providing also a meaningful test to tracking quality in the impulsive noise vicinity inducing innovation and observation outliers, as well significant errors of dynamic system model, due to a target maneuver, inducing structural outliers. The state-space model, (1) and (60), is named the constant acceleration (CA) one [40].

The implementation of the KF, robustified or not, requires the filter initialization, and a priori knowledge of the state and observation noises statistics. The optimal KF initialization, in (7), is based on the optimal estimates of zero-mean states, in (1), when the observations are not available. Thus, the adopted initial guesses, $m_0=0$ and $P_0$ in (7), are the steady-state values. However, such initialization approach cannot be used for a tracking problem, because the state vector does not reach a steady state covariance matrix, $P_0$. Namely, the cumulative effects of many acceleration perturbations affect the state vector component variances to grow with the time index, $k$, about their nominal values [39, 40]. Thus, a suboptimal and reasonable ad-hoc initialization can be developed using the two available position sensor measurements, in (1), yielding

$${\widehat{x}}_{0\mid 0}={\widehat{x}}_0=\left[\begin{array}{c}{y}_2\\ \frac{y_2-y_1}{T}\\ 0\end{array}\right];P_{0\mid 0}=P_0=\mathit{diag}\left\{{\sigma }_1^2,{\sigma }_2^2,{\sigma }_3^2\right\}$$E61

Here, uncertainty in the initial three-dimensional state vector, in (1), is reflected in initial values of the covariance matrix, $P_0$ in (61), where ${\sigma }_1$, ${\sigma }_2$, and ${\sigma }_3$ denote the standard deviations of position, velocity, and acceleration in the three-dimensional state space model, (1) and (60). A reasonable choice is ${\sigma }_1=100m,$${\sigma }_2=50m/s$, and ${\sigma }_3=10m/s^2,$ [40].

The first-order statistics of state and observation noises are commonly zero-valued that is, $\overline{v}=\overline{w}=0$ in (35) and (36). The state noise variance, $Q$, represents a random motion, maneuver, or acceleration entering a system between the sampling intervals. Starting from the state space model, in (1) and (60), the variance $Q$ can be calculated by adopting the first-order Gauss-Markov state noise process, with the exponential correlation coefficient, assuming that $T\ll {\tau }_m$ where ${\tau }_m$ is the target maneuver time constant. Applying further the Singer’s approach, the random target acceleration can be generated from the mixed pdf, representing a combination of the uniform pdf, $f\left(u\right)$, on the interval $\left[-A_m{A}_m\right]$, with $A_m$ being the maximal target acceleration in both directions, and the three Dirac impulses $P_m\delta \left(u\pm A_m\right)$, $P_0\delta \left(u\right)$. Here, $P_m$ is the probability of maximal accelerations, $\pm A_m$, while $P_0$ is the probability of zero-valued acceleration. Starting from such mixed pdf, one can calculate the random maneuver variance, ${\sigma }_m^2$, from which it follows the state noise variance [40].

$$Q=E\left\{w_k^2\right\}=2T{\sigma }_m^2/{\tau }_m;{\sigma }_m^2=A_m^2\left(1+4P_m-P_0\right)/3$$E62

Particularly, by assuming $P_m=0.1;P_0=0.3;A_m=2g/3;T=1s;{\tau }_m=10s$ one gets ${\sigma }_m\approx g$, and $Q\approx =0.45g$, where the gravity constant $g=9.81m/s^2$.

For the limiting case, $T\ll {\tau }_m$, a suitable simplification to the two-dimensional-state KF becomes valid, since the KF gains adjoin to the acceleration estimates take low values, [29, 40]. However, the state noise must be taken to replace the omitted acceleration term, and such model is called the constant velocity (CV) one. Additionally, the limiting case, $T\ll {\tau }_m$, is not the nominal one, and usually $T\ge {\tau }_m$, [40]. Thus, the variance, $Q$, may be chosen in an ad-hoc manner, as an average Monte-Carlo value on the tracking interval of interest, [40]. In these simulations the value $Q=0.8$ is used, being obtained as the Monte-Carlo averaging over 100 runs on the tracking interval of 100 s.

If a radar system measures in the SCS coordinates $\left(r,\alpha ,\beta \right)$, and tracks in the CCS coordinates, a reasonable choice for the range sensor noise standard deviation, ${\sigma }_R$, is the one percentage of its output at the nominal range, $R$. Moreover, commonly used standard deviations, ${\sigma }_{\alpha }$ and ${\sigma }_{\beta }$, of the angular noises in the azimuth and elevation sensors, respectively, are of the order 1 deg., or 0.017 rad, [40]. By using the transformation from SCS to CCS, one obtains the following observation noise covariance in CCS, [40]

$$\begin{array}{ccc}{R}_s& =& \mathit{diag}\left[{\sigma }_R^2,{\sigma }_{\alpha }^2,{\sigma }_{\beta }^2\right];R_c={\mathit{TR}}_s{T}^T;\\ T& =& \left[\begin{array}{ccc}\cos \alpha \cos \beta & -rsin\alpha \cos \beta & -rcos\alpha sin\beta \\ sin\alpha cos\beta & -rcos\alpha cos\beta & -rsin\alpha sin\beta \\ sin\beta & 0& rcos\beta \end{array}\right]\end{array}$$E63

Thus, the position measurement noise variance, in (2), in each of the CCS coordinates is given by the corresponding diagonal component of covariance matrix, $R_c$, in (63). However, in these simulations, an alternative ad-hoc approach, based on the measurement disturbance representation, in (8) and (9), is used. Assuming the standard deviations, ${\sigma }_n=1$ and ${\sigma }_0\le 100{\sigma }_n$, as well that the observations contain from one to twenty-five outlier percentages, corresponding to the contamination degree from $\delta =0.01$ to $\delta =0.25$, one obtains the observation noise variance value $R_k=400$, as an average of the different values of the free parameters $\delta$ and ${\sigma }_0^2$, in (9).

Performances of the next algorithms are compared: optimal KF (3)–(6), designated as A1; adaptive KF, combining the A1 filter with the Huber’s moving window M-robust algorithm to estimate noises moments, in (39), (42), (43), (52), (55), (56), (58) and (59), denoted as A2; M-robust estimate KF, in (35)–(38), denoted as A3; and adaptive M-robust estimate KF, combining the filter A3 and the Huber M-robust parameter estimator of unknown noises statistics in A2, denoted as A4. A filter adaptation to the unknown noise statistics requires to adopt the lengths of the corresponding sliding frames. A reasonable ad-hoc choices are $L\in \left[50,500\right]$, $l\in \left[3,10\right]$ and the forgetting factor, in (59), $\gamma \le 10$. Particularly, the presented simulation results are generated by using $L=250$, $l=5$, and $\gamma =9$. The initial guesses of the noise statistics are chosen as ${\widehat{r}}_1=0$, ${\widehat{R}}_1=400$, ${\widehat{q}}_0=0$ and ${\widehat{Q}}_0=0.8$. These constant values of the noise statistics are also used in the non-adaptive filters A1 and A3.

Simulation results, for each of the CCS coordinates, are analyzed using the cumulative estimation error (CEE) factor of goodness, together with the estimated observation disturbance moments (mean value and variance), where the criterion

$$\mathit{CEE}\left(k\right)=\frac{1}{k}\sum _{i=1}^k\frac{‖{\widehat{x}}_i-x_i‖}{‖x_i‖};k=L,L+1$$E64

with $‖\cdot ‖$ being the Euclidian norm. Since the obtained results are similar to each of the CCS coordinates, only the results for the z-coordinate are presented in Figures 4 and 5, while the estimated observation noise mean value and variance are plotted in Figure 6. The commonly used sample variance and mean estimates, generated at moving frame of the length $L=250$ upon the Gaussian observation noise, are compared with the robustly estimated observation disturbance moments by the algorithm A4 upon the normally distributed observations corrupted by different types of outliers, involving the innovation, observation and structural ones, (Figure 6). It should be noted that the adaptive algorithms A2 and A4 provide slightly better results than the algorithm A1, and obviously better results than the algorithm A3 under the pure normally distributed observations in the presence of the target maneuver (Figure 4). Moreover, the robust algorithms, A3 and A4, provide better results than the linear non-robust ones, A1 and A2, upon the presence of multiple outliers (Figure 5).

Figure 4.

CEE criterion comparison of different estimators in the Gaussian observation noise ($\delta =0$), in the presence of a targeting maneuver.

Figure 5.

CEE criterion comparison of different estimators in the contaminated Gaussian noise ($\delta =0.1$), involving innovation, observation, and structural outliers.

Figure 6.

Estimated measurement noise statistics comparison of sample mean and variance estimates upon Gaussian noise, and the algorithm A4 estimates upon thick-tailed Gaussian noise ($\delta =0.1$): (a) mean value; (b) standard deviation.

Finally, the adaptive robustified algorithm A4 provides the best results, representing an appropriate trade-off between tracking ability and robustness performance, suppressing efficiently outliers with a contamination degree less than thirty percent, (Figures 4 and 5). Also, the estimated noise statistics by the algorithm A4 upon the presence of outliers compare equally with the sample mean and variance estimates upon the absence of outliers, (Figure 6).

6. Conclusion

Since the Kalman filter has a computationally attractive recursive structure for real-time applications, it is widely used to solve the practical problems, involving system parameter identification, dynamic system state estimation, optimal and adaptive control, statistical signal processing, etc. In addition, numerous scientific and industrial measurements are analyzed, in the statistical sense, and the obtained results have indicated that the observations contain from five to ten percent of outliers, on average, due to incomplete measurements, meter and communication errors, errors in mathematical model, etc. In this sense, a thick-tailed Gaussian noise distribution is followed by the two types of impulsive noises, inducing innovation and observation outliers. The third type of outliers, named structural outliers, appears due to modeling errors, such as unmodeled system dynamics, time-varying model bias, computational errors, etc. Particularly, the thick-tailed normally distributed observations noise is presented in a maneuvering target tracking, due to the target glint inducing the innovation and observations outliers. Two characteristics of such data can be stressed. Firstly, there exists a high-intensity spiky character of the observations and, secondly, there is a clear low frequency, named the bright spot wonder, being produced by a slow drift in a target gravity center, due to changes in the target aspect. The low frequency can be efficiently suppressed in the tracking loop, but the glint spikes, representing the observation and innovation outliers, are stuck to the tracking loop. Moreover, a linear dynamic state space model is not adequate during a target maneuver, and such modeling errors induce structural outliers. However, since the Kalman filter is a linear signal processor, its performance can be very poor upon the outliers’ existence.

The basic objective of this manuscript is to design an appropriate Kalman filter modification that performs fairly well upon multiple outliers. It has been demonstrated, by a real-life maneuvering target tracking example, that the proposed adaptive M-robust estimate-based Kalman filter improves significantly the target estimation and tracking quality, being effective in suppressing multiple outliers with contamination degree less than thirty percent. Furthermore, the achieved improvement is at expense of the additional computational efforts, but usually not so demanded to preclude a real-time application. On contrary, the emphasized approximate Kalman filter may be more efficient than the least squares, or the optimal Kalman filter. Therefore, it may be used to solve the numerous engineering problems in which the Kalman filter, or least squares, is required. Furthermore, it may be also used as an alternative to the other adaptive techniques, involving discrete noise levels, variable state dimension, and interacting multiple model approaches, as well the observation distribution estimation. Further investigations may be directed to propagation of the error covariances upon a non-linear form of the state updates, as well as to the convergence, observability, and consistency analysis of the proposed approximate filter.