Feedback Robust Cubature Kalman Filter for Target Tracking Using an Angle Sensor

Abstract

The direction of arrival (DOA) tracking problem based on an angle sensor is an important topic in many fields. In this paper, a nonlinear filter named the feedback M-estimation based robust cubature Kalman filter (FMR-CKF) is proposed to deal with measurement outliers from the angle sensor. The filter designs a new equivalent weight function with the Mahalanobis distance to combine the cubature Kalman filter (CKF) with the M-estimation method. Moreover, by embedding a feedback strategy which consists of a splitting and merging procedure, the proper sub-filter (the standard CKF or the robust CKF) can be chosen in each time index. Hence, the probability of the outliers’ misjudgment can be reduced. Numerical experiments show that the FMR-CKF performs better than the CKF and conventional robust filters in terms of accuracy and robustness with good computational efficiency. Additionally, the filter can be extended to the nonlinear applications using other types of sensors.

1. Introduction

For the last several decades, the target tracking problem has been the subject of constant and intensive interest in many fields, such as navigation, military applications, and sensors networks [1]. The aim of the target tracking problem is to estimate the position and the velocity of a target from a set of measurements collected by sensors [2]. According to the types of sensors, target tracking can be separated into range-only tracking, direction of arrival (DOA) tracking, and Doppler effects tracking [3,4,5]. Among these methods, DOA tracking is a typical nonlinear problem whose measurements are the basic information from an angle sensor [6,7]. Accordingly, this paper focuses on the DOA tracking with a single angle sensor.

Various nonlinear filtering algorithms have been applied to the DOA tracking problem, such as the extended Kalman filter (EKF) [8], the particle filter (PF) [9,10], the unscented Kalman filter (UKF), and the cubature Kalman filter [11]. The UKF and the CKF are called the deterministically-sampled filters (DSFs) which approximate the Gaussian posterior distribution by means of a set of deterministically sampled points. When the discretization errors of practical systems are not considered, they perform better than the EKF in terms of the accuracy and show better than the PF in terms of the computational cost [11]. However, the UKF and the CKF are effective only under the Gaussian and white assumption of the measurement noise. Actually, measurement outliers are almost inevitable from sensors in practical applications. Owing to their potentially non-Gaussian or non-symmetric properties, measurement outliers may lead to unstable, or even divergent performance. On the other hand, the tracking performance is also concerned with the discretization of the practical system [12]. In this paper, we focus on dealing with outliers, and the discretization errors are not considered.

The hard decision method [13] may prevent outliers to a certain degree. However, it may lead to misjudgment easily owing to the nonlinear property of the DOA tracking. Moreover, once the misjudgment appears, the performance will be degraded significantly. Huber’s M-estimation [14] is an important method to deal with measurement outliers from sensors. It combines the minimum l₁ and l₂ norm estimation technique, which exhibits robustness for contaminated measurements. Unfortunately, the M-estimation is generally limited in linear cases. Indeed, it can be directly extended to nonlinear systems with the help of the first-order Taylor expansion, but the accuracy is difficult to be ensured because of enormous linearized errors in highly nonlinear cases. Recently, combining the DSFs with the M-estimation has been taken into account. They usually reformulate the measurement update stage, and the M-estimation method can be introduced according to a linear or nonlinear regression [15,16,17,18,19]. Nevertheless, the linear or nonlinear regression induces some complex operations such as the matrix inversion and decomposition, which may lead to poor computational efficiency. Moreover, the outliers’ judgment variable in regression-based robust DSFs includes not only measurement errors, but also predicted errors and nonlinear errors in nonlinear systems. Hence the outliers’ judgment threshold is difficult to be determined. Additionally, the robust DSFs do not perform as well as their non-robust versions such that the UKF and the CKF when no outliers appear from sensors, because the weights of the normal measurements may be reduced improperly under Gaussian assumption.

Considering the problems above, the feedback M-estimation based robust cubature Kalman filter (FMR-CKF) is proposed for the single sensor based DOA tracking. Unlike regression-based methods, we introduce the M-estimation to the CKF by means of the equivalent weight function, and the complex computational operations can be avoided. A feedback strategy is also proposed to improve the misjudgment in different cases. When an outlier from the angle sensor is detected, the filter is split into two sub-filters: the standard CKF and the robust CKF. They are simultaneously updated and the sub-filter whose outliers’ judgment variable is smaller at the next time index can be chosen. The FMR-CKF takes full advantage of the accuracy of the CKF and the robustness of the M-estimation and simulation results show that the proposed algorithm performs better than the conventional algorithms whether outliers appear. It should be noted that the proposed filter is different from the gating method in multi-target tracking problem [20,21]. A gating method is used in multi-target tracking problem because the measurement origin is uncertain and it is a binary decision method. On the contrary, the proposed filter is to reduce the influence of the outliers according to the equivalent weight function.

The rest of the paper is organized as follow. Section 2 describes the nonlinear system model of DOA tracking based on an angle sensor. The nonlinear robust filter which is called the MRCKF is presented in Section 3. Section 4 introduces the feedback strategy for DOA tracking. Section 5 yields the performance comparison of the proposed algorithm with conventional algorithms. Conclusions are drawn in Section 6.

2. System Model and Cubature Kalman Filter

The target is assumed to be in linear motion with nearly constant velocity for convenience. Of course, the CKF-based method can also handle the condition when the velocity varies. The state vector of the target at time index $k$ ($k=1,2,\cdots ,n$) can be given by ${\mathit{x}}_k^t ={[x_k^t,y_k^t,{\dot{x}}_k^t,{\dot{y}}_k^t]}^T$, where $(x_k^t,y_k^t)$ is the position coordinate and $({\dot{x}}_k^t,{\dot{y}}_k^t)$ is the velocity coordinate. Similarly, the state vector of the moving angle sensor is expressed by ${\mathit{x}}_k^o ={\left[x_k^o,y_k^o,{\dot{x}}_k^o,{\dot{y}}_k^o\right]}^T$. Thus, the state vector in the relative own-ship reference at time index $k$ is defined as ${\mathit{x}}_k={\mathit{x}}_k^t-{\mathit{x}}_k^o={[x_k,y_k,{\dot{x}}_k,{\dot{y}}_k]}^T$.

The dynamic model of DOA tracking can be written as:

$${\mathit{x}}_k=\mathit{F}{\mathit{x}}_{k-1}-{\mathit{u}}_{k-1,k}+{\mathit{v}}_k$$

(1)

where $\mathit{F}$ is the transformed matrix, ${\mathit{u}}_{k-1,k}$ is a vector of deterministic inputs that accounts for the effects of observer accelerations [22], and ${\mathit{v}}_k$ represents the process noise, which is generally assumed to be Gaussian distributed such that $N\left(0,\mathit{Q}\right)$. $\mathit{F}$, ${\mathit{u}}_{k-1,k}$, and the covariance matrix $\mathit{Q}$ are given by [2]:

$${\mathit{u}}_{k-1,k}=\left[\begin{array}{c}{x}_k^o-x_{k-1}^o-\mathsf{\Delta }{\dot{x}}_{k-1}^o\\ y_k^o-y_{k-1}^o-\mathsf{\Delta }{\dot{y}}_{k-1}^o\\ {\dot{x}}_k^o-{\dot{x}}_{k-1}^o\\ {\dot{y}}_k^o-{\dot{y}}_{k-1}^o\end{array}\right], \mathit{F}=\left[\begin{array}{cccc}1& 0& \mathsf{\Delta }& 0\\ 0& 1& 0& \mathsf{\Delta }\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right], \mathit{Q}=\left[\begin{array}{cccc}{\mathsf{\Delta }}^3/3& 0& {\mathsf{\Delta }}^2/2& 0\\ 0& {\mathsf{\Delta }}^3/3& 0& {\mathsf{\Delta }}^2/2\\ {\mathsf{\Delta }}^2/2& 0& \mathsf{\Delta }& 0\\ 0& {\mathsf{\Delta }}^2/2& 0& \mathsf{\Delta }\end{array}\right]\cdot q$$

where $\mathsf{\Delta }$ is the measurement interval and q represents the intensity of the process noise.

The measurement model is:

$$z_k=h({\mathit{x}}_k)=\arctan \frac{x_k}{y_k}+w_k$$

(2)

where $z_k$ represents the angle measurement at time index k that is obtained by a single angle sensor. $w_k$ represents the measurement noise which is generally assumed to be independent and identically Gaussian with a covariance matrix $R_k$.

The CKF uses the third-degree spherical-radial rule to numerically approximate the multi-dimensional integral involved in nonlinear Bayesian filtering. The cubature points can be expressed as: ${\xi }_j=\sqrt{n_x}{\left[\begin{array}{cc}{\mathit{I}}_{n_x}& -{\mathit{I}}_{n_x}\end{array}\right]}_j$, where ${\mathit{I}}_{n_x}$ is the $n_x\times n_x$ dimensional identity matrix, and ${[\cdot ]}_j$ represents the $j^{th}$ column of $[\cdot ]$ ($j=1,2,\cdots ,2n_x$).

At the time update stage, the dynamic model of DOA tracking is typically linear; thus, the mean and associated covariance of the Gaussian predictive density can be directly written as:

$${\widehat{\mathit{x}}}_{k|k-1}=\mathit{F}{\widehat{\mathit{x}}}_{k-1}-{\mathit{u}}_{k-1,k}$$

(3)

$${\mathit{P}}_{k|k-1}=\mathit{F}{\mathit{P}}_{k-1}{\mathit{F}}^T+\mathit{Q}$$

(4)

At the measurement update stage, the predicted covariance is first factorized as ${\mathit{P}}_{k|k-1}={\mathit{S}}_{k|k-1}{\mathit{S}}_{k|k-1}^T$. We then evaluate the cubature points:

$${\mathit{x}}_{k|k-1,j}^{*}={\mathit{S}}_{k|k-1}{\mathit{\xi }}_j+{\widehat{\mathit{x}}}_{k|k-1}$$

(5)

The cubature points are propagated through the nonlinear measurement equation:

$$z_{k|k-1,j}^{*}=h({\mathit{x}}_{k|k-1,j}^{*})$$

(6)

Accordingly, the predicted measurement can be estimated through:

$${\widehat{z}}_{k|k-1}=\frac{1}{2n_x}{\displaystyle \sum _{j=1}^{2n_x}{z}_{k|k-1,j}^{*}}$$

(7)

The Kalman gain is given by:

$${\mathit{K}}_k={\mathit{P}}_{xz,k|k-1}{\mathit{P}}_{zz,k|k-1}^{-1}$$

(8)

where the innovation covariance matrix ${\mathit{P}}_{zz,k|k-1}$ and the cross covariance matrix ${\mathit{P}}_{xz,k|k-1}$ are:

$${\mathit{P}}_{zz,k|k-1}=\frac{1}{2n_x}{\displaystyle \sum _{j=1}^{2n_x}(z_{k|k-1,j}^{*}-{\widehat{z}}_{k|k-1,j})}{(z_{k|k-1,j}^{*}-{\widehat{z}}_{k|k-1,j})}^T+R_k$$

(9)

$${\mathit{P}}_{xz,k|k-1}=\frac{1}{2n_x}{\displaystyle \sum _{j=1}^{2n_x}({\widehat{\mathit{x}}}_{k|k-1}-{\mathit{x}}_{k|k-1,j}^{*})}{({\widehat{z}}_{k|k-1}-z_{k|k-1,j}^{*})}^T$$

(10)

Therefore, the state estimate and the corresponding covariance can be updated as:

$${\widehat{\mathit{x}}}_{k|k}={\widehat{\mathit{x}}}_{k|k-1}+{\mathit{K}}_k(z_k-{\widehat{z}}_{k|k-1})$$

(11)

$${\mathit{P}}_{k|k}={\mathit{P}}_{k|k-1}-{\mathit{K}}_k{\mathit{P}}_{zz,k|k-1}{\mathit{K}}_k^T$$

(12)

3. Robust Cubature Kalman Filter Based on the M-Estimation

Without regard to the discretization errors, the CKF will be accurate and stable if the measurement noise follows Gaussian distribution exactly. However, measurements are easily contaminated by the outliers in practical applications [17]. The outliers will lead to inaccurate performance if we do nothing about them. Accordingly, the contaminated distribution is introduced:

$$G=(1-\epsilon )D+\epsilon M$$

(13)

where $D$ is the dominated distribution, $M$ is the interference distribution, and $\epsilon$ is the contaminated rate. In general, the Gaussian distribution is widely adopted to be the dominated distribution $D$ because it is tractable. The outlier can be regarded as the measurement noise from the interference distribution $M$. In order to obtain accurate and robust results, both $D$ and $M$ should be considered.

The key of the M-estimation is to keep the weights of normal measurements and to reduce the weights of contaminated measurements. Generally, the weight of the normal measurement is considered the inverse matrix of the measurement covariance such that $p_k={\left(R_k\right)}^{-1}$. Thus, the innovation covariance matrix of the CKF ${\mathit{P}}_{zz,k|k-1}$ can be rewritten as:

$${\mathit{P}}_{zz,k|k-1}=\frac{1}{2n_x}{\displaystyle \sum _{j=1}^{2n_x}(z_{k|k-1,j}^{*}-{\widehat{z}}_{k|k-1})}{(z_{k|k-1,j}^{*}-{\widehat{z}}_{k|k-1})}^T+p_k^{-1}$$

(14)

The covariance of the outlier which comes from the unknown interference distribution is denoted as ${\overline{R}}_k$, and the equivalent weight matrix of the contaminated measurement can be expressed as ${\overline{p}}_k={\left({\overline{R}}_k\right)}^{-1}$. Consequently, the equivalent innovation covariance matrix ${\overline{\mathit{P}}}_{zz,k|k-1}$ is obtained by:

$${\overline{\mathit{P}}}_{zz,k|k-1}=\frac{1}{2n_x}{\displaystyle \sum _{j=1}^{2n_x}(z_{k|k-1,j}^{*}-{\widehat{z}}_{k|k-1,j})}{(z_{k|k-1,j}^{*}-{\widehat{z}}_{k|k-1,j})}^T+{\overline{p}}_k^{-1}$$

(15)

According to Equation (13), the outliers can be regarded as the data coming from the interference distribution, where the covariance is ${\overline{p}}_k^{-1}$.

The corresponding equivalent Kalman gain ${\overline{\mathit{K}}}_k$ is given by:

$${\overline{\mathit{K}}}_k={\mathit{P}}_{xz,k|k-1}{\overline{\mathit{P}}}_{zz,k|k-1}^{-1}$$

(16)

Therefore, the estimate state and the corresponding covariance can be updated as:

$${\widehat{\mathit{x}}}_{k|k}={\widehat{\mathit{x}}}_{k|k-1}+{\overline{\mathit{K}}}_k(z_k-{\widehat{z}}_{k|k-1})$$

(17)

$${\mathit{P}}_{k|k}={\mathit{P}}_{k|k-1}-{\overline{\mathit{K}}}_k{\overline{\mathit{P}}}_{zz,k|k-1}{\overline{\mathit{K}}}_k^T$$

(18)

It can be seen from Equations (14) and (15) that the only difference between the CKF and the M-estimation based robust CKF (MRCKF) is the weight matrix, which implies that handling measurement outliers is actually transformed into designing of the equivalent weight matrix ${\overline{p}}_k$, i.e., the Huber’s weight function can be expressed as [14]:

$${\overline{p}}_k=\{\begin{array}{l}{R}_k^{-1},\left|{\tilde{r}}_k\right|\le c\\ \frac{R_k^{-1}\cdot c}{\left|{\tilde{r}}_k\right|},\left|{\tilde{r}}_k\right|>c\end{array}$$

(19)

where ${\tilde{r}}_k$ is the standardized residual at time index $k$ and the threshold $c$ is usually chosen as $c\in \left[1.3, 2\right]$ [23]. When ${\overline{p}}_k=R_k^{-1}$, the MRCKF turns into the CKF, which means the CKF is a special case of the MRCKF with $\epsilon =0$.

It is noted that the outliers’ judgment threshold $c$ is difficult to be determined because the standardized residual includes not only measurement errors, but also predicted errors and nonlinear errors in the DOA tracking problem. In this respect, we consider the standardized innovation:

$$e_k=\left(z_k-{\widehat{z}}_{k|k-1}\right)/S_{zz,k|k-1}$$

(20)

where $S_{zz,k|k-1}$ is a square factor of the innovation covariance matrix such that $P_{zz,k|k-1}=S_{zz,k|k-1}{S}_{zz,k|k-1}^T$.

An outliers’ judgment variable can be constructed as:

$${\lambda }_k=\left(z_k-{\widehat{z}}_{k|k-1}\right)P_{zz,k|k-1}^{-1}\left(z_k-{\widehat{z}}_{k|k-1}\right)=e_k^2$$

(21)

${\lambda }_k$ is the Mahalanobis distance [24] between the actual measurement $z_k$ and the predicted measurement ${\widehat{z}}_{k|k-1}$ at time index k. From a statistical perspective, the Mahalanobis distance provides an inconsistency degree between the actual measurement and the predicted measurement. When the process noise ${\mathit{v}}_{k-1}$ and the measurement noise $w_k$ are both Gaussian, the predicted measurement will be statistically close to the actual measurement. Otherwise, when a measurement outlier appears, ${\lambda }_k$ will become abnormal. It is difficult to verify the real distribution of $e_k$ due to nonlinear property of the measurement equation. Here, $e_k$ is approximately considered Gaussian and ${\lambda }_k$ will approximately follow Chi-square distribution, such that:

$${\lambda }_k={\chi }^2\left(1\right)$$

(22)

where ${\chi }^2\left(m\right)$ represents the Chi-square distribution with m degrees of freedom.

Denoting the significance level as $\alpha$, we obtain:

$$p\left({\lambda }_k>{\gamma }_{\alpha }\right)=1-\alpha$$

(23)

where $p(\cdot )$ is probabilistic computation operator and ${\gamma }_{\alpha }$ represents the $\alpha$-quantile.

When a measurement outlier appears, ${\lambda }_k$ will generally deviate from Chi-square distribution. In other words, if ${\lambda }_k>{\gamma }_{\alpha }$ and $1-\alpha$ is chosen as a small value, the measurement can be regarded as an abnormal measurement which is contaminated by the outlier. Consequently, the outliers’ judgment threshold ${\gamma }_{\alpha }$ can be chosen according to the significance level $\alpha$ from Chi-square distribution tables.

When the filter is modified by the equivalent weight, the new judgment variable should be not exceeded ${\gamma }_{\alpha }$, otherwise the robust method will be failure. i.e.:

$${\overline{\lambda }}_k=\left(z_k-{\widehat{z}}_{k|k-1}\right){\overline{P}}_{zz,k|k-1}^{-1}\left(z_k-{\widehat{z}}_{k|k-1}\right)\le {\gamma }_{\alpha }$$

(24)

Substituting Equations (14) and (15) into Equation (24), we have:

$${\overline{p}}_k^{-1}\ge \frac{{\lambda }_k{P}_{zz,k|k-1}}{{\gamma }_{\alpha }}-P_{zz,k|k-1}+R_k$$

(25)

Therefore, a new equivalent weight function can be described as:

$${\overline{p}}_k=\{\begin{array}{cc}{R}_k^{-1},& {\lambda }_k\le {\gamma }_{\alpha }\hfill \\ {\left(\frac{{\lambda }_k{P}_{zz,k|k-1}}{{\gamma }_{\alpha }}-P_{zz,k|k-1}+R_k\right)}^{-1},& {\lambda }_k>{\gamma }_{\alpha } \hfill \\ \end{array}$$

(26)

Compared with Equation (19), the outliers’ judgment threshold of Equation (26) is determined naturally according to confidence level of ${\lambda }_k$, and improper experiential value can be avoided. From a computational complexity perspective, iterations and regression procedure are not required, which implies that the MRCKF shows better than the regression-based robust DSFs in terms of the computational efficiency. Although the M-estimation is introduced to the CKF based on the DOA problem, the MRCKF can be extended to other nonlinear filtering system.

4. Feedback Strategy for DOA Tracking

If there are no measurement outliers from the angle sensor (the measurement noise follows the Gaussian distribution exactly), the performance of the CKF is generally better than that of its robust versions (such as the MRCKF). This is because the MRCKF do not always minimize the $l_2$ norm in that case, and the weights of normal measurements may be reduced improper during the filtering update. What is worse, nonlinear property of the DOA tracking system would make ${\lambda }_k$ deviate from Chi-square distribution, which further increases the misjudgment rate. Of course, increasing the number of the measurements in each time index may improve the problem above. Unfortunately, there is only one measurement in the DOA tracking problem at each time index, which implies that the DOA filters are more sensitive to the measurement outliers and the misjudgment occurs easily.

Here, we do not consider continuous outliers and propose a feedback strategy to solve the problem. When the outliers’ judgment variable ${\lambda }_k$ exceeds the threshold at time index k, the filter is split into two sub-filters: the standard CKF and the MRCKF. Then they are updated according to their solution frameworks. At time index k + 1, the outliers’ judgment variables of the sub-filters can be obtained. If ${\lambda }_{k+1,MRCKF}<{\lambda }_{k+1,CKF}$, the MRCKF is chosen, otherwise the CKF is chosen. The new filtering algorithm with the feedback strategy is named the feedback MRCKF (FMR-CKF). The flow diagram of the FMR-CKF can be summarized in Figure 1.

Smaller ${\lambda }_{k+1}$ means the predicted information which is obtained according to the corresponding sub-filter at time index k is closer to the actual measurement at time index k + 1. That is, the sub-filter with a smaller ${\lambda }_{k+1}$ performs better at time index k. The better sub-filter can be chosen according to the judgment variables at the next time index. Meanwhile, the probability of misjudgment can be reduced.

5. Numerical Experiments

The algorithms are compared in this section. System model is described as Equations (1) and (2). We assume that the measurement interval Δ = 1 min and the process noise intensity q = 10⁻¹⁰ km²/s³. To satisfy the target observability requirements, a single observer is assumed to be maneuvering where the start coordinate is (0,0). From 0 min to 12 min, the observer is in uniform linear motion, where the speed is 4 knots and the course is 140°. From 13 min to 17 min, the observer is in uniform turning motion, where the speed is 4 knots and the turn rate is 30°. From 18 min to 30 min, the observer is in uniform linear motion, where the speed is 4 knots and the course is 290°.

The initialization method is the same as [2,25]. For fair comparison, the same initial condition are set for different algorithms. The initial range from the target to the angle senor r = 5 km and the estimated standard deviation σ_r = 1.5 km. The standard deviation of the angle estimate σ_z = 1°; Similarly, the initial velocity of the target s = 4 knots and the estimated standard deviation σ_s = 2 knots. The true target course c = −120°, and the estimated standard deviation σ_c = π/$\sqrt{12}$. The initial state and covariance of the algorithms can be estimated according to [2]. In the nonlinear regression based novel robust UKF (NRUKF), the scale parameter κ = 0 and the other parameters are initialized according to [17]. In the MRCKF and FMR-CKF, α = 95% and Υ_a is 3.841.

The root mean square error in position (RMSE_pos) at time index , and the mean squared error in position (MSE_pos) for each Monte Carlo runs are, respectively, defined as:

$${\text{RMSE}}_{\text{pos},k}=\sqrt{\frac{1}{L}{\displaystyle \sum _{i=1}^L{({\widehat{x}}_{k,i}^t-x_{k,i}^t)}^2+}{({\widehat{y}}_{k,i}^t-y_{k,i}^t)}^2}$$

(27)

$${\text{MSE}}_{\text{pos},i}=\frac{1}{n}{\displaystyle \sum _{k=1}^n\left[{\left(x_k^t-{\widehat{x}}_{k|k,i}^t\right)}^2+{\left(y_k^t-{\widehat{y}}_{k|k,i}^t\right)}^2\right]}$$

(28)

where L = 500 is Monte-Carlo times and $i=1,2,\cdots ,L$.

Two cases are considered for illustrating the performance of the algorithms.

Case 1: No outliers in angle measurements. The measurement noise follows Gaussian distribution exactly, and contaminated rate $\epsilon =0$.

Case 2: The dominated distribution is Gaussian. The outliers exist at 20 min, 25 min, 30 min, and the true values of the outliers are $6{\sigma }_z$. A large outlier exists at 35 min, and the true value is $30{\sigma }_z$.

The RMSE_pos of the various filtering algorithms in Case 1 and Case 2 are described in Figure 2 and Figure 3, respectively.

The EKF and gating rule-based method often fails to yield meaningful results. Hence, we do not present the results of the EKF and the gating rule-based method. As shown in Figure 2, when the measurement noise follows Gaussian distribution exactly, the NRUKF and the MRCKF is inferior to that of the CKF, because the normal measurement errors may be misjudged as the outliers in the NRUKF and the MRCKF under Gaussian assumption. On the contrary, the FMR-CKF outperforms the CKF because the FMR-CKF can select the better sub-filter (the CKF or the MRCKF) according to the feedback strategy in each time index. For instance, when a large measurement error appears (follows Gaussian distribution), the FMR-CKF selects the MRCKF to reduce the influence of the large measurement error, but the CKF neglects it and then the performance is degraded. Hence the FMR-CKF performs the best in this case.

As shown in Figure 3, when measurement outliers appear, the results of the CKF become unreliable due to non-robust property. Among the robust filters, the MRCKF outperforms the NRUKF because the MRCKF introduces the M-estimation without linearization and approximate regression procedures, which makes better use of the accuracy of the CKF and the robustness of the M-estimation. Meanwhile, the outliers’ judgment threshold of the MRCKF is naturally obtained according to Chi-square distribution, but that of the NRUKF is generally experiential, which may lead to more misjudgments. Compared with the MRCKF, the FMR-CKF further avoids the misjudgment based on the feedback strategy and, hence, it performs better than the MRCKF.

The average MSEs in position of the filters are shown in Figure 4a and the corresponding variances are shown in Figure 4b.

As can be seen in Figure 4, the average MSEs_pos and the corresponding variances of the FMR-CKF change smaller in different cases and are obviously lower than those of the other algorithms. It demonstrates that the FMR-CKF is more numerically stable and consistent than the other algorithms. The judgment variable of the NRUKF, i.e., the measurement residual, includes both predicted error and nonlinear error, which leads to unstable performance and, hence, the variances of the NRUKF are significantly higher than those of the other algorithms in different cases.

To compare the computational cost, the computational time of the algorithms investigated relative to that of the CKF in Case 2 are shown [2]. Of course, some errors may exist because coding of each method may not be equivalently optimized in the simulation, but it can be seen that FMR-CKF significantly outperforms the NRUKF in terms of the computational time.

Table 1. Relative computation times of the algorithms.

	CKF	NRUKF	MRCKF	FMR-CKF
Relative computation times	1	2.41	1.13	1.32

shows that the NRUKF runs much slower than the other existing algorithms as it requires iterations and complex operations, such as matrix decomposition and inversion in the regression procedure. The FMR-CKF introduces computational time in the equivalent weight computation and the splitting procedure, which shows a major computational advantage over the NRUKF.

6. Conclusions

We have proposed the FMR-CKF to deal with the DOA tracking problem when measurement outliers appear from the angle sensor. Compared with the DSFs such as the CKF and UKF, the FMR-CKF can solve measurement outliers effectively. Compared with the regression-based robust DSFs, the new filter takes fully advantage of the robustness of the M-estimation and the accuracy of the CKF by means of the equivalent weight function and the feedback strategy. Additionally, the CKF solution framework is retained, which implies that the FMR-CKF is a derivative-free and computationally-efficient filter. Simulation results show that the proposed algorithm exhibits the best performance (both in filter accuracy and stability) whether the measurement outliers appear. While the example was the 2-dimensional DOA problem in this paper, the FMR-CKF has potential applications in higher dimensional problems or other nonlinear situations.