Reweighted Robust Particle Filtering Approach for Target Tracking in Automotive Radar Application

Abstract

In view of the decline of filtering accuracy caused by measured outliers in target tracking application, a novel reweighted robust particle filter is proposed to acquire accurate state estimates in an automotive radar system. To infer the importance of each entry in the multidimensional contaminated measurement vector, we employ a weight vector, which follows a Gamma distribution, to model the measured noise and carry out accurate state estimates. Additionally, the particle filter method is employed to perform approximate posterior inference of state estimates in the nonlinear model. The Cramer–Rao lower bound is provided for the performance evaluation of the proposed method. Both simulation and experimental results demonstrate the superiorities of the proposed algorithm over other robust solutions.

1. Introduction

In the field of autonomous driving, automotive radars have become the core sensors due to their all-time and all-weather environmental perception capabilities. Effective target tracking is essential for the autonomous driving systems to formulate correct and timely driving strategies. By clustering the point cloud data of Millimeter Waveform (MMW) radar, the target can be detected and located in the automotive application scenario. However, the occurrence of measurement abnormalities in real-world applications is a common phenomenon that may arise due to reasons such as unexpected disturbances, temporary sensor failures, and calibration errors [1]. A typical example is to track a walking pedestrian on the road through an MMW radar sensor in action. The occlusion of roadside obstacles, such as parked cars, walls, and so on, can corrupt the range measurement, and glint, speckle, and phase noise can contaminate radar data [2,3,4]. The input of most tracking algorithms is the result of target detection. If the target is suddenly lost or temporarily occluded, the target trajectory will be interrupted [5]. Moreover, the clutter cannot be completely filtered out of radar measurements can lead to false detections and causes measurement abnormalities [6]. In addition, the non-line-of-sight (NLOS) multipath propagation of the transmitted electromagnetic wave is also one of a source of measurement abnormalities [7]. The Kalman filter (KF) offers an optimal estimation method for the linear dynamic model with a Gaussian state and measurement noise [8]. However, owing to the complex electromagnetic propagation and changeable environment, the measurement noise is a typical non-Gaussian in the MMW radar sensor and would result in measurement abnormalities, also named outliers. The presence of these outliers may degrade the performance of standard KF, thus yielding poor estimates of the state of interest [9].

A number of efforts have been devoted to addressing the issue. Outliers rejection methods are used in robust state estimation and object tracking [10]. A robust filter approach in [11] has been developed by viewing measurements as clearly distinguished outliers from normal values using Projection Statistics (PS). In [12,13,14], classical recursive approaches are converted into batch-mode regression form and iteratively reweighted least squares algorithms are used to estimate the states. A measurement-weighting-based whitening procedure is designed to distinguish outliers from normal measurements to improve the state estimation accuracy [15]. Another class of approaches is based on the heavy-tailed noise model, such as Student’s t distributions and Laplace distributions [16,17,18,19]. A scalar weight, which follows a Gamma distribution, is introduced for each observed sample such that the variance matrices of measurement noise can be reassessed in the state estimation [20,21]. Filtering and smoothing algorithms, which are much more robust yet only slightly more involved than the standard inference algorithm, are derived by assigning Wishart distribution to the noise covariance matrix [22]. By choosing the inverse Wishart priors, the measurement noise covariance matrices can be inferred based on the variational Bayes (VB) approach [23].

In this paper, a reweighted robust particle filter is proposed to acquire more accurate state estimation with robustness to outliers in the nonlinear model. Motivated by the aforementioned methods in [20,21], we generalize models to the more comprehensive case of multidimensional outlier measurements. Specifically, a weight vector, which follows conditional independent Gamma distribution, is employed to reassess the contribution of each entry in the multidimensional outlier measurement. The proposed method can be regarded as a reweighted robust filter. The particle filter (PF) method is utilized to perform the approximate posterior inference of state estimates. The Cramer–Rao bound of the proposed method is also provided for performance analysis. Both simulations and experiments demonstrate the effectiveness of the proposed method.

Notations: We use lower-case (upper-case) bold characters to denote vectors (matrices). ${(·)}^T$ denotes the transpose of a matrix or vector. $\mathrm{diag}\left(\mathbf{x}\right)$ represents a diagonal matrix that uses the elements of $\mathbf{x}$ as its diagonal elements. $p(·)$ denotes the probability density function (pdf). $x\sim \mathcal{N}(a,b)$ denotes that random variable x follows a Gaussian distribution with mean a and variance b. $\mathbf{x}\in {\mathcal{R}}^N$ denotes a real-value vector $\mathbf{x}$ with the dimension of N and $\mathbf{x}\in {\mathcal{R}}^{N\times M}$ denotes a real-value matrix $\mathbf{x}$ with the dimension of $N\times M$.

2. Proposed Model

2.1. Problem Formulation

A typical nonlinear discrete-time state-space model can be expressed as

$$\begin{array}{cc}\hfill {\mathbf{\theta }}_k& =f\left({\mathbf{\theta }}_{k-1}\right)+{\mathbf{u}}_k\hfill \\ \hfill {\mathbf{z}}_k& =h\left({\mathbf{\theta }}_k\right)+{\mathbf{n}}_k\hfill \end{array}$$

(1)

where ${\mathbf{\theta }}_k\in {\mathcal{R}}^N$ and ${\mathbf{z}}_k\in {\mathcal{R}}^M$ represent the state vector and measurement vector at time step k, $f\left(·\right)$ and $h\left(·\right)$ denote nonlinear state and measurement functions, respectively, ${\mathbf{u}}_k\in {\mathcal{R}}^N$ and ${\mathbf{n}}_k\in {\mathcal{R}}^M$ are the zero-mean Gaussian white process noises satisfying ${\mathbf{u}}_k\sim \mathcal{N}\left(\mathbf{0},\mathbf{Q}\right)$ and ${\mathbf{n}}_k\sim \mathcal{N}\left(\mathbf{0},\mathbf{R}\right)$, and $\mathbf{Q}$ and $\mathbf{R}$ are the process noise covariance matrix and measurement noise covariance matrix, respectively. The model in Equation (1) is just regarded to be a typical nonlinear system model, and the system state can be estimated by a nonlinear Kalman filter [24].

In real automotive radar applications, typical representative scenarios are often encountered in which the target of interest may be obscured by other interferences or clutters, and the acquired measurement vector, such as range, velocity, and azimuth, in the automotive radar system would fall outside a valid range and become outliers, since the acquired measurement samples at this time directly come from interferences. The weighted robust Kalman filter method has been developed by introducing a scalar $w_k$ into the measurement noise covariance matrix $\mathbf{R}$ for the improved estimates of the updated ${\mathbf{\theta }}_k$ [21]. However, the main limitation of this work is that even if it allows the treatment of multidimensional measurement, a scalar weight is considered for the entire observation vector, thus equally treating all the entries regardless of which entry is corrupted. On the one hand, the entries in the measurement vector represent diverse physical quantities respectively in the real radar system and have diverse signal-to-noise ratios. On the other hand, the variance of each entry is often influenced by the automotive radar system, such as the wider bandwidth radar system, resulting in a smaller range variance and smaller number of receiver channels, leading to a poorer azimuth measurement. Therefore, introducing a scaler in the measurement vector may be not an optimal way to treat them equally.

2.2. Generative Model

To address this issue, we introduce a novel Bayesian algorithm that treats the weights associated with each entry in the measurement vector data probabilistically. In particular, we introduce a weight vector ${\mathbf{w}}_k$ to learn the dynamical noise covariance matrix, and a reweighted robust particle filter method is proposed to acquire improved state-space estimates in a nonlinear model in the presence of contaminated measurements. We modify the measurement noise equation as follows by introducing a weight vector ${\mathbf{w}}_k$,

$$\begin{array}{c}\hfill {\mathbf{n}}_k\sim \mathcal{N}\left(\mathbf{0},{\mathbf{W}}_k^{-1}\mathbf{R}\right),\end{array}$$

(2)

where ${\mathbf{W}}_k\in {\mathcal{R}}^{M\times M}$ is a diagonal matrix with the weight vector ${\mathbf{w}}_k={\left[w_{k,1},\cdots ,w_{k,M}\right]}^T\in {\mathcal{R}}^M$ on its diagonal at time step k, and ${\mathbf{W}}_k^{-1}$ is the inverse of the matrix ${\mathbf{W}}_k$. The Gamma prior distribution is introduced for the elements of ${\mathbf{w}}_k$ to ensure that they remain positive and is written by

$$\begin{array}{cc}\hfill {\mathbf{z}}_k|{\mathbf{\theta }}_k,{\mathbf{W}}_k& \sim \mathcal{N}\left(h\left({\mathbf{\theta }}_k\right),{\mathbf{W}}_k^{-1}\mathbf{R}\right)\hfill \\ \hfill {\mathbf{\theta }}_k|{\mathbf{\theta }}_{k-1}& \sim \mathcal{N}\left(f\left({\mathbf{\theta }}_{k-1}\right),\mathbf{Q}\right)\hfill \\ \hfill w_{k,m}& \sim \mathrm{Gamma}\left(w_{k,m}|v_m/2,v_m/2\right),\hfill \end{array}$$

(3)

where $m=1,\cdots ,M$, $v_m$ is the mth hyperparameter in the vector $\mathbf{v}$ for the Gamma distribution. According to Equation (3), the modified model has the capability of dynamically learning the measurement noise covariance and adjusting the weights of each entry in the measurement vector for improved estimates of the state vector with effectively alleviating outliers.

2.3. Reweighted Robust Particle Filter

In this subsection, we develop a reweighted robust particle filter using statistical inference over the aforementioned model. The particle filter, utilizing the Monte Carlo method, is selected to provide approximate posterior estimation in the nonlinear model [25]. The state estimation generally focuses on the expectation calculation of posterior distribution,

$$\begin{array}{c}\hfill \mathbf{E}\left[g\left(\mathbf{\theta }\right)\right|{\mathbf{z}}_{1:k}]=\int g\left(\mathbf{\theta }\right)p\left(\mathbf{\theta }|{\mathbf{z}}_{1:k}\right)d\mathbf{\theta }\end{array}$$

(4)

where $g\left(\mathbf{\theta }\right)$ is the function of the variable $\mathbf{\theta }$, and $p\left(\mathbf{\theta }|{\mathbf{z}}_{1:k}\right)$ represents the posterior probability density function for $\mathbf{\theta }$ given the measurement vector data ${\mathbf{z}}_{1:k}$. However, the posterior distribution $p\left(\mathbf{\theta }|{\mathbf{z}}_{1:k}\right)$ is often difficult to analytically acquire due to the manipulation of the multiple integration. The importance sampling (IS) is often used to perform the posterior distribution by introducing a nominal distribution $\pi \left(\mathbf{\theta }|{\mathbf{z}}_{1:k}\right)$ that often utilizes a Gaussian distribution,

$$\begin{array}{cc}\hfill \mathbf{E}\left[g\left(\mathbf{\theta }\right)\left|{\mathbf{z}}_{1:k}\right.\right]& =\int g\left(\mathbf{\theta }\right)\frac{p\left(\mathbf{\theta }|{\mathbf{z}}_{1:k}\right)}{\pi \left(\mathbf{\theta }|{\mathbf{z}}_{1:k}\right)}\pi \left(\mathbf{\theta }|{\mathbf{z}}_{1:k}\right)d\mathbf{\theta }\hfill \\ \hfill & =\int g\left(\mathbf{\theta }\right)\frac{p\left({\mathbf{z}}_{1:k}|\mathbf{\theta }\right)p\left(\mathbf{\theta }\right)}{p\left({\mathbf{z}}_{1:k}\right)\pi \left(\mathbf{\theta }|{\mathbf{z}}_{1:k}\right)}\pi \left(\mathbf{\theta }|{\mathbf{z}}_{1:k}\right)d\mathbf{\theta }\hfill \\ \hfill & =\frac{\int g\left(\mathbf{\theta }\right)\frac{p\left({\mathbf{z}}_{1:k}|\mathbf{\theta }\right)p\left(\mathbf{\theta }\right)}{\pi \left(\mathbf{\theta }|{\mathbf{z}}_{1:k}\right)}\pi \left(\mathbf{\theta }|{\mathbf{z}}_{1:k}\right)d\mathbf{\theta }}{\int \frac{p\left({\mathbf{z}}_{1:k}|\mathbf{\theta }\right)p\left(\mathbf{\theta }\right)}{\pi \left(\mathbf{\theta }|{\mathbf{z}}_{1:k}\right)}\pi \left(\mathbf{\theta }|{\mathbf{z}}_{1:k}\right)d\mathbf{\theta }}.\hfill \end{array}$$

(5)

The particle weight $\omega \left(\mathbf{\theta }\right)$ is defined as

$$\begin{array}{c}\hfill \omega \left(\mathbf{\theta }\right)=\frac{p\left({\mathbf{z}}_{1:k}|\mathbf{\theta }\right)p\left(\mathbf{\theta }\right)}{\pi \left(\mathbf{\theta }|{\mathbf{z}}_{1:k}\right)}.\end{array}$$

(6)

The Equation (5) can be rewritten by introducing the $\omega \left(\mathbf{\theta }\right)$:

$$\begin{array}{cc}\hfill \mathbf{E}\left[g\left(\mathbf{\theta }\right)\left|{\mathbf{z}}_{1:k}\right.\right]& =\frac{\int g\left(\mathbf{\theta }\right)\omega \left(\mathbf{\theta }\right)\pi \left(\mathbf{\theta }|{\mathbf{z}}_{1:k}\right)d\mathbf{\theta }}{\int \omega \left(\mathbf{\theta }\right)\pi \left(\mathbf{\theta }|{\mathbf{z}}_{1:k}\right)d\mathbf{\theta }}\hfill \\ \hfill & \approx \frac{{\displaystyle \sum _{s=1}^S}g\left({\mathbf{\theta }}^{\left(s\right)}\right)\omega \left({\mathbf{\theta }}^{\left(s\right)}\right)}{{\displaystyle \sum _{s=1}^S}\omega \left({\mathbf{\theta }}^{\left(s\right)}\right)}\hfill \end{array}$$

(7)

where $s=1,\dots ,S$, and S is the total number of sampling particles. According to the Markov properties of the model, the particle weights ${\omega }_k^{\left(s\right)}$ can be deduced in the recursion form

$${\omega }_k^{\left(s\right)}\propto \frac{p\left({\mathbf{z}}_k|{\mathbf{\theta }}_k^{\left(s\right)}\right)p\left({\mathbf{\theta }}_k^{\left(s\right)}|{\mathbf{\theta }}_{k-1}^{\left(s\right)}\right)}{\pi \left({\mathbf{\theta }}_k^{\left(s\right)}|{\mathbf{\theta }}_{0:k-1}^{\left(s\right)},{\mathbf{z}}_{1:k}\right)}{\omega }_{k-1}^{\left(s\right)}.$$

(8)

Generally, $\pi \left({\mathbf{\theta }}_k|{\mathbf{\theta }}_{0:k-1},{\mathbf{z}}_{1:k}\right)=p\left({\mathbf{\theta }}_k|{\mathbf{\theta }}_{k-1}\right)$ is often used in the importance sampling, and thus the recursion in Equation (8) can be reduced into

$${\omega }_k^{\left(s\right)}\propto p\left({\mathbf{z}}_k|{\mathbf{\theta }}_k^{\left(s\right)}\right){\omega }_{k-1}^{\left(s\right)}.$$

(9)

According to Equation (9), it is obvious that the problem eventually converts to computation of $p\left({\mathbf{z}}_k|{\mathbf{\theta }}_k\right)$, which can be achieved from the marginal likelihood utilizing Bayesian inference. Considering the fact that ${\mathbf{W}}_k$ is independent of ${\mathbf{\theta }}_k$, we have

$$\begin{array}{cc}\hfill & p\left({\mathbf{z}}_k|{\mathbf{\theta }}_k\right)\hfill \\ \hfill & =\int p\left({\mathbf{z}}_k|{\mathbf{\theta }}_k,{\mathbf{W}}_k\right)p\left({\mathbf{W}}_k|{\mathbf{\theta }}_k\right)d{\mathbf{W}}_k\hfill \\ \hfill & =\prod _{m=1}^M\int p\left(z_{k,m}|{\mathbf{\theta }}_k,w_{k,m}\right)p\left(w_{k,m}\right)d{w}_{k,m}\hfill \\ \hfill & =\prod _{m=1}^M{\int }_0^{\infty }\mathcal{N}\left(z_{k,m}\mid {\mu }_{k,m},\frac{r_{m,m}}{w_{k,m}}\right)\mathrm{Gam}\left(w_{k,m}\mid \frac{v_m}{2},\frac{v_m}{2}\right)d{w}_{k,m}\hfill \\ \hfill & =\prod _{m=1}^M\frac{\Gamma \left(\frac{v_m}{2}+\frac{1}{2}\right)}{\Gamma \left(\frac{v_m}{2}\right)}{\left(\frac{{\lambda }_m}{\pi v_m}\right)}^{\frac{1}{2}}{\left[1+\frac{{\lambda }_m{\left(z_{k,m}-{\mu }_{k,m}\right)}^2}{v_m}\right]}^{-\frac{v_m}{2}-\frac{1}{2}}.\hfill \end{array}$$

(10)

where ${\mu }_{k,m}$ is the mth element of the vector $h\left({\mathbf{\theta }}_k\right)$; ${\lambda }_m=r_{m,m}^{-1}$ is the mth element on the diagonal of matrix ${\mathbf{R}}^{-1}$. The entire reweighted robust particle filter (RR-PF) method is summarized as Algorithm 1.

Algorithm 1 Reweighted Robust Particle Filter

Input:$\{{\mathbf{\theta }}_{k-1}^{\left(s\right)}:s=1,\dots ,S\}$ Output:$\langle {\mathbf{\theta }}_k\rangle$, $\{{\mathbf{\theta }}_k^{\left(s\right)}:s=1,\dots ,S\}$ 1: Draw new samples: ${\tilde{\mathbf{\theta }}}_k^{\left(s\right)}\sim p\left({\mathbf{\theta }}_k|{\mathbf{\theta }}_{k-1}^{\left(s\right)}\right),s=1,\dots ,S$ 2: Calculate particles weights: ${\tilde{\omega }}_k^{\left(s\right)}$ in Equations (9) and (10) 3: Perform resampling with ${\tilde{\mathbf{\theta }}}_k^{\left(s\right)}$ and ${\tilde{\omega }}_k^{\left(s\right)}$ to acquire ${\mathbf{\theta }}_k^{\left(s\right)}$ in Equation (7) 4: Estimate system state $〈{\mathbf{\theta }}_k〉={\sum }_{s=1}^S{\mathbf{\theta }}_k^{\left(s\right)}/S$ 5: return$\langle {\mathbf{\theta }}_k\rangle$, $\{{\mathbf{\theta }}_k^{\left(s\right)}:s=1,\dots ,S\}$

2.4. Algorithm Analysis

In this subsection, detailed analysis is provided to illustrate the reason why the proposed method is more robust to the outlier compared with the conventional heavy-tail multivariate Student’s t-distributed (CMSt) [26,27]. For comparison, we provide the probability density function (pdf) of the CMSt as

$$p\left({\mathbf{z}}_k|{\mathbf{\theta }}_k\right)\propto \frac{\Gamma \left(\frac{v}{2}+\frac{M}{2}\right)}{\Gamma \left(\frac{v}{2}\right)}{\left[1+\frac{{({\mathbf{z}}_k-\mathbf{\mu })}^T{\mathbf{R}}^{-1}({\mathbf{z}}_k-\mathbf{\mu })}{v}\right]}^{-\frac{M}{2}-\frac{v}{2}}.$$

(11)

We take 2D pdfs with $M=2$ as an example to illustrate the roles of the robustness to outliers. In this example, ${\mathbf{z}}_k={[x_k,y_k]}^T$, $\mathbf{\mu }={[0,0]}^T$, ${\mathbf{R}}^{-1}=\mathrm{diag}\left([1,1]\right)$, and $\mathbf{v}={[v,v]}^T={[0.5,0.5]}^T$ are shown in Figure 1. It is observed that both pdfs exhibit heavy-tailed properties, which indicates their robustness to outliers. The differences lie in that there exist quite prominent ridges along the x-axis and y-axis in the proposed pdf, which makes the model more adapted to the changes in the independent variable in the direction perpendicular to the ridges than CMSt.

In order to intuitively show the role of the proposed pdf in the particle filter, we illustrate the evolution of particle weights. In the particle filter, at $(k-1)\mathrm{th}$ time step, the particles are acquired with an identical weight to estimate $\langle {\mathbf{\theta }}_{k-1}\rangle$ based on the resampling technique, shown in Figure 2a. It is observed that these particle balls have the same sizes, and thus have identical weights, although the particle unreliability is different, as shown in the middle surface in Figure 2a. It is also found that the blue particle ball with a small value in the color bar denotes that this particle is highly reliable, however, the yellow ball with a large value suggests an unreliable particle. At the kth time step, when the new measurement vector data ${\mathbf{z}}_k$ are provided, particle weights in the kth time step will evolve according to Equation (9). The evolved result of particle weights in Equation (10) is shown on the top surface, and the result in Equation (11) is provided in the bottom surface.

To clearly illustrate the roles of the pdfs in Equation (10) compared with that in Equation (11), we provide slice plots of the pdfs the along y-axis. It is observed that the plot of the sharper pdf in the proposed method would encourage the contributions from the reliable particle, and it would weaken the particle weight concerning these unreliable particles, shown in the top plot in Figure 2b. It is found that the blue particle ball has a higher weight due to its high reliability in the current time step, whereas the yellow one has a lower weight in the proposed model compared with those in the bottom pdf of the CMSt. In conclusion, the proposed method encourages the contributions from the particle weights of reliable particles and weakens the contributions from the unreliable particles by discriminatively treating the observed vector data. This could be the reason why the proposed method is more robust to outliers and acquires improved state estimates compared with the CMSt method.

2.5. Cramer–Rao Bound for the Model

To further explore the performance bound of the proposed method, CRLB analysis is provided in this section. We first define a latent parameter set concerning the random parameter set p($\Theta$); the mean-square error matrix is generally served as a measure of error and is given by

$${\Sigma }_{ϵ}=\mathrm{MSE}\left(\widehat{\Theta }\right)\triangleq \mathbf{E}\left[\left(\widehat{\Theta }-\Theta \right){\left(\widehat{\Theta }-\Theta \right)}^T\right],$$

(12)

where $\Theta$ are the latent variable parameter collection, $\widehat{\Theta }$ denote estimates of the corresponding variables, and $\mathbf{E}\left(x\right)$ denotes the expectation operator for the variable x.

The Bayesian Information Matrix (BIM), which is commonly employed in time-invariant statistical models, is often used for CRLB analysis and can be written by

$$\begin{array}{c}\hfill \mathbf{J}=-\mathbf{E}\left\{{▽}_{\Theta }{\left[{▽}_{\Theta }lnp(\Theta ,\mathbf{Z})\right]}^T\right\}\triangleq \mathbf{E}\left\{\left[{▽}_{\Theta }lnp(\Theta ,\mathbf{Z})\right]{\left[{▽}_{\Theta }lnp(\Theta ,\mathbf{Z})\right]}^T\right\},\end{array}$$

(13)

where $p(\Theta ,\mathbf{Z})$ is the joint probability distribution, and ${▽}_{\Theta }$ denotes the first-order partial derivative operator concerning the variable vector $\Theta$, which can be expressed by

$${▽}_{\Theta }={\left[\frac{\partial }{\partial {\Theta }_1},\frac{\partial }{\partial {\Theta }_2},\dots \right]}^T$$

(14)

According to the Bayesian CRLB analysis, we have

$${\Sigma }_{ϵ}⩾{\mathbf{J}}^{-1}.$$

(15)

The inequality suggests that the matrix on the left is non-negative.

According to posterior Cramer–Rao bounds analysis, we have the recursive sequence ${\mathbf{J}}_k$ of posterior information submatrices for estimating the state vector ${\mathbf{\theta }}_k$ at the kth time step [28]

$${\mathbf{J}}_k={\mathbf{D}}_k^{22}-{\mathbf{D}}_k^{21}{\left({\mathbf{J}}_{k-1}+{\mathbf{D}}_k^{11}\right)}^{-1}{\mathbf{D}}_k^{12},$$

(16)

where

$${\mathbf{D}}_k^{11}=\mathbf{E}\left[-{▽}_{{\mathbf{\theta }}_{k-1}}{▽}_{{\mathbf{\theta }}_{k-1}}^{T}lnp\left({\mathbf{\theta }}_k\right|{\mathbf{\theta }}_{k-1})\right],$$

(17)

$${\mathbf{D}}_k^{12}=\mathbf{E}\left[-{▽}_{{\mathbf{\theta }}_{k-1}}{▽}_{{\mathbf{\theta }}_k}^{T}lnp\left({\mathbf{\theta }}_k\right|{\mathbf{\theta }}_{k-1})\right],$$

(18)

$${\mathbf{D}}_k^{21}=\mathbf{E}\left[-{▽}_{{\mathbf{\theta }}_k}{▽}_{{\mathbf{\theta }}_{k-1}}^{T}lnp\left({\mathbf{\theta }}_k\right|{\mathbf{\theta }}_{k-1})\right]={\left({\mathbf{D}}_k^{12}\right)}^T.$$

(19)

${\mathbf{D}}_k^{22}$ can be decomposed into

$${\mathbf{D}}_k^{22}={\mathbf{D}}_k^{22,a}+{\mathbf{D}}_k^{22,b},$$

(20)

where

$${\mathbf{D}}_k^{22,a}=\mathbf{E}\left[-{▽}_{{\mathbf{\theta }}_k}{▽}_{{\mathbf{\theta }}_k}^{T}lnp\left({\mathbf{\theta }}_k\right|{\mathbf{\theta }}_{k-1})\right],$$

(21)

$${\mathbf{D}}_k^{22,b}=\mathbf{E}\left[-{▽}_{{\mathbf{\theta }}_k}{▽}_{{\mathbf{\theta }}_k}^{T}lnp\left({\mathbf{z}}_k\right|{\mathbf{\theta }}_k)\right],$$

(22)

The initial information submatrix can be obtained from the prior probability distribution $p\left({\mathbf{\theta }}_0\right)$

$${\mathbf{J}}_0=\mathbf{E}\left[-{▽}_{{\mathbf{\theta }}_0}{▽}_{{\mathbf{\theta }}_0}^{T}lnp\left({\mathbf{\theta }}_0\right)\right],$$

(23)

where $p\left({\mathbf{\theta }}_0\right)$ denotes the initial prior distribution of the variable vector ${\mathbf{\theta }}_0$.

Using the nonlinear discrete-time state-space model in Equations (1) and (10), we have

$$\begin{array}{c}-lnp\left({\mathbf{\theta }}_k\right|{\mathbf{\theta }}_{k-1})=\frac{1}{2}\left\{{\left[{\mathbf{\theta }}_k-f\left({\mathbf{\theta }}_{k-1}\right)\right]}^T{\mathbf{Q}}^{-1}\left[{\mathbf{\theta }}_k-f\left({\mathbf{\theta }}_{k-1}\right)\right]\right\}+\mathrm{const},\hfill \end{array}$$

(24)

$$\begin{array}{cc}\hfill -& lnp\left({\mathbf{z}}_k\right|{\mathbf{\theta }}_k)=\sum _{m=1}^M\left(\frac{v_m}{2}+\frac{1}{2}\right)ln\left\{1+\frac{{\lambda }_m{\left[{\mathbf{z}}_{k,m}-h{\left({\mathbf{\theta }}_k\right)}_m\right]}^2}{v_m}\right\}+\mathrm{const}.\hfill \end{array}$$

(25)

Then,

$${\mathbf{D}}_k^{11}={\mathbf{E}}_{{\mathbf{\theta }}_{k-1}}\left\{\left[{▽}_{{\mathbf{\theta }}_{k-1}}{f}^T\left({\mathbf{\theta }}_{k-1}\right)\right]{\mathbf{Q}}^{-1}{\left[{▽}_{{\mathbf{\theta }}_{k-1}}{f}^T\left({\mathbf{\theta }}_{k-1}\right)\right]}^T\right\},$$

(26)

$${\mathbf{D}}_k^{12}=-{\mathbf{E}}_{{\mathbf{\theta }}_{k-1}}\left\{\left[{▽}_{{\mathbf{\theta }}_{k-1}}{f}^T\left({\mathbf{\theta }}_{k-1}\right)\right]\right\}{\mathbf{Q}}^{-1},$$

(27)

$${\mathbf{D}}_k^{21}={\left[{\mathbf{D}}_k^{12}\right]}^T,$$

(28)

$${\mathbf{D}}_k^{22}={\mathbf{Q}}^{-1}+{\mathbf{E}}_{{\mathbf{\theta }}_k}\left[{▽}_{{\mathbf{\theta }}_k}{▽}_{{\mathbf{\theta }}_k}^{T}lnp\left({\mathbf{z}}_k\right|{\mathbf{\theta }}_k)\right].$$

(29)

It is observed that it is difficult to carry out these integrals, the sequential Monte Carlo method is used to perform the approximations, and we define the variables ${\mathbf{\Lambda }}^{11}$, ${\mathbf{\Lambda }}^{12}$, and ${\mathbf{\Lambda }}^{22}$ as

$${\mathbf{\Lambda }}^{11}\left({\mathbf{\theta }}_{k-1}\right)=\left[{▽}_{{\mathbf{\theta }}_{k-1}}{f}^T\left({\mathbf{\theta }}_{k-1}\right)\right]{\mathbf{Q}}^{-1}{\left[{▽}_{{\mathbf{\theta }}_{k-1}}{f}^T\left({\mathbf{\theta }}_{k-1}\right)\right]}^T,$$

(30)

$${\mathbf{\Lambda }}^{12}\left({\mathbf{\theta }}_{k-1}\right)=-{▽}_{{\mathbf{\theta }}_{k-1}}{f}^T\left({\mathbf{\theta }}_{k-1}\right),$$

(31)

$${\mathbf{\Lambda }}^{22}\left({\mathbf{\theta }}_k\right)={▽}_{{\mathbf{\theta }}_k}{▽}_{{\mathbf{\theta }}_k}^{T}lnp\left({\mathbf{z}}_k\right|{\mathbf{\theta }}_k).$$

(32)

Therefore, we can rewrite Equations (26)–(29) as follows:

$$\begin{array}{cc}\hfill {\mathbf{D}}_k^{11}& =\int {\mathbf{\Lambda }}^{11}\left({\mathbf{\theta }}_{k-1}\right)p\left({\mathbf{\theta }}_{k-1}\right)d{\mathbf{\theta }}_{k-1}\hfill \\ \hfill & =\int p\left({\mathbf{z}}_{k-1}\right)\left\{\int {\mathbf{\Lambda }}^{11}\left({\mathbf{\theta }}_{k-1}\right)p\left({\mathbf{\theta }}_{k-1}\right|{\mathbf{z}}_{k-1})d{\mathbf{\theta }}_{k-1}\right\}d{\mathbf{z}}_{k-1},\hfill \end{array}$$

(33)

$$\begin{array}{cc}\hfill {\mathbf{D}}_k^{12}& =\int {\mathbf{\Lambda }}^{12}\left({\mathbf{\theta }}_{k-1}\right)p\left({\mathbf{\theta }}_{k-1}\right)d{\mathbf{\theta }}_{k-1}\hfill \\ \hfill & =\int p\left({\mathbf{z}}_{k-1}\right)\left\{\int {\mathbf{\Lambda }}^{12}\left({\mathbf{\theta }}_{k-1}\right)p\left({\mathbf{\theta }}_{k-1}\right|{\mathbf{z}}_{k-1})d{\mathbf{\theta }}_{k-1}\right\}d{\mathbf{z}}_{k-1},\hfill \end{array}$$

(34)

$$\begin{array}{cc}\hfill {\mathbf{D}}_k^{22}& ={\mathbf{Q}}^{-1}+\int {\mathbf{\Lambda }}^{22}\left({\mathbf{\theta }}_k\left)p\right({\mathbf{\theta }}_k\right)d{\mathbf{\theta }}_k\hfill \\ \hfill & ={\mathbf{Q}}^{-1}+\int p\left({\mathbf{z}}_k\right)\left\{\int {\mathbf{\Lambda }}^{22}\left({\mathbf{\theta }}_k\right)p\left({\mathbf{\theta }}_k\right|{\mathbf{z}}_k)d{\mathbf{\theta }}_k\right\}d{\mathbf{z}}_k.\hfill \end{array}$$

(35)

These integrals can be approximated by the mean of samples after having a series of samples corresponding to the density. We perform T Monte Carlo samples with the time steps of K for the calculation of the expectation instead of the integral operator over ${\mathbf{z}}_k$ and obtain the measurement sample matrix ${\mathbf{Z}}_{1:T,1:K}$. ${\mathbf{z}}_{t,k-1}$ denotes the $t\mathrm{th}$ measurement sample at the $(k-1)\mathrm{th}$ time step. Consequently, Equations (33)–(35) can be calculated by

$$\begin{array}{c}\hfill {\mathbf{D}}_k^{11}\approx \frac{1}{T}\sum _{t=1}^T\left\{\int {\Lambda }^{11}\left({\mathbf{\theta }}_{k-1}\right)p\left({\mathbf{\theta }}_{k-1}\right|{\mathbf{z}}_{t,k-1})d{\mathbf{\theta }}_{k-1}\right\}\end{array}$$

(36)

$$\begin{array}{c}\hfill {\mathbf{D}}_k^{12}\approx \frac{1}{T}\sum _{t=1}^T\left\{\int {\Lambda }^{12}\left({\mathbf{\theta }}_{k-1}\right)p\left({\mathbf{\theta }}_{k-1}\right|{\mathbf{z}}_{t,k-1})d{\mathbf{\theta }}_{k-1}\right\}\end{array}$$

(37)

$$\begin{array}{c}\hfill {\mathbf{D}}_k^{22}\approx {\mathbf{Q}}^{-1}+\frac{1}{T}\sum _{t=1}^T\left\{\int {\Lambda }^{22}({\mathbf{\theta }}_k,{\mathbf{z}}_k)p\left({\mathbf{\theta }}_k\right|{\mathbf{z}}_{t,k})d{\mathbf{\theta }}_k\right\}\end{array}$$

(38)

We further perform Monte Carlo samples over the variable ${\mathbf{\theta }}_k$ instead of an integral operator over ${\mathbf{\theta }}_k$ at the $t\mathrm{th}$ sample, and formulas inside the braces in Equations (36)–(38) can be expressed as

$$\begin{array}{c}\hfill {\mathbf{D}}_{t,k}^{11}\approx \frac{1}{L}\sum _{l=1}^L{\Lambda }^{11}\left({\mathbf{\theta }}_{t,k-1}^{\left(l\right)}\right)\end{array}$$

(39)

$$\begin{array}{c}\hfill {\mathbf{D}}_{t,k}^{12}={\left({\mathbf{D}}_{t,k}^{21}\right)}^T\cong \frac{1}{L}\sum _{l=1}^L{\Lambda }^{12}\left({\mathbf{\theta }}_{t,k-1}^{\left(l\right)}\right)\end{array}$$

(40)

$$\begin{array}{c}\hfill {\mathbf{D}}_{t,k}^{22}\approx \frac{1}{L}\sum _{l=1}^L{\Lambda }^{22}({\mathbf{\theta }}_{t,k}^{\left(l\right)},{\mathbf{z}}_{t,k})\end{array}$$

(41)

where ${\mathbf{\theta }}_{t,k-1}^{\left(l\right)}$ and ${\mathbf{\theta }}_{t,k}^{\left(l\right)}$ are, respectively, samples drawn from the posterior distributions of $p\left({\mathbf{\theta }}_{t,k-1}|{\mathbf{z}}_{t,k-1}\right)$ and $p\left({\mathbf{\theta }}_{t,k}|{\mathbf{z}}_{t,k}\right)$, and L is the number of samples. According to the approximate inference method, the Markov Chain Monte Carlo (MCMC) method is utilized to estimate ${\mathbf{\theta }}_{t,k}$ with the state transition probability $p\left({\mathbf{\theta }}_k\right|{\mathbf{\theta }}_{k-1})$ in Equation (24) and the likelihood function $p\left({\mathbf{z}}_k\right|{\mathbf{\theta }}_k)$ in Equation (25). The following Equations (33)–(35) and (39)–(41), can be rewritten by

$$\begin{array}{c}\hfill {\mathbf{D}}_k^{11}\approx \frac{1}{T}\sum _{t=1}^T\left\{\frac{1}{L}\sum _{l=1}^L{\Lambda }^{11}\left({\mathbf{\theta }}_{t,k-1}^{\left(l\right)}\right)\right\}\end{array}$$

(42)

$$\begin{array}{c}\hfill {\mathbf{D}}_k^{12}\approx {\left({\mathbf{D}}_k^{21}\right)}^T\cong \frac{1}{T}\sum _{t=1}^T\left\{\frac{1}{L}\sum _{l=1}^L{\Lambda }^{12}\left({\mathbf{\theta }}_{t,k-1}^{\left(l\right)}\right)\right\}\end{array}$$

(43)

$$\begin{array}{c}\hfill {\mathbf{D}}_k^{22}\approx {\mathbf{Q}}^{-1}+\frac{1}{T}\sum _{t=1}^T\left\{\frac{1}{L}\sum _{l=1}^L{\Lambda }^{22}({\mathbf{\theta }}_{t,k}^{\left(l\right)},{\mathbf{z}}_{t,k})\right\}\end{array}$$

(44)

When the number of samples L is large enough, the estimated results would converge to true values. Finally, we arrive at the resulting Monte-Carlo-based posterior Cramer–Rao bound (MC-PCRB).

3. Simulations and Experiments

In this simulation, we take target tracking in the automotive radar system as an example. The state process model with the constant velocity, which is a typically linear equation, can be expressed by

$${\mathbf{\theta }}_k=f\left({\mathbf{\theta }}_{k-1}\right)+{\mathbf{u}}_k=\mathbf{A}{\mathbf{\theta }}_{k-1}+{\mathbf{u}}_k$$

(45)

where ${\mathbf{\theta }}_k=\left[x_k,y_k,{\dot{x}}_k,{\dot{y}}_k\right]$, containing the two-dimensional location $\left(x_k,y_k\right)$ and the corresponding velocities $\left({\dot{x}}_k,{\dot{y}}_k\right)$ of the vehicle. The state transition matrix $\mathbf{A}$ can be written by

$$\mathbf{A}=\left[\begin{array}{cccc}1& 0& \Delta T& 0\\ 0& 1& 0& \Delta T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

(46)

where $\Delta T$ denotes the sampling interval of the radar. The nonlinear measurement model in the automotive radar system can be denoted by,

$$\left[\begin{array}{c}{R}_k\\ {\varphi }_k\\ V_k\end{array}\right]=h\left(\left[\begin{array}{c}{x}_k\\ y_k\\ {\dot{x}}_k\\ {\dot{y}}_k\end{array}\right]\right)=\left[\begin{array}{c}\sqrt{x_k^2+y_k^2}\\ arctan\left(\frac{x_k}{y_k}\right)\\ \frac{x_k{\dot{x}}_k+y_k{\dot{y}}_k}{\sqrt{x_k^2+y_k^2}}\end{array}\right].$$

(47)

where $R_k$ denotes the measured range of the target at the time step k, ${\varphi }_k$ represents the measured spatial information, and $V_k$ is its measured radial velocity.

3.1. Performance Testing on Simulated Data

In this simulation, a spiral trajectory is considered, and the spiral trajectory can be generated by a radius reduction circular motion. Assume that the radar continuously acquires the observed measurement vector, and the measurements containing outliers are generated according to the mixture model

$${\mathbf{z}}_k|{\mathbf{\theta }}_k,c_{k,m}\sim \left\{\begin{array}{cc}\mathrm{U}\left({\mathbf{z}}_k|a_m,b_m\right),& \mathrm{if}{c}_{k,m}=0\\ \mathcal{N}\left({\mathbf{z}}_k|h\left({\mathbf{\theta }}_k\right),\mathbf{R}\right),& \mathrm{if}{c}_{k,m}=1\end{array}\right.$$

(48)

where $c_{k,m}=0$ indicates a normal measurement from the mth dimension at the kth time step clutter and $U(·)$ is a uniform distribution, while $c_{k,m}=1$ indicates the measurement vector is a normal one, and noises follow Gaussian distribution with the measurement noise covariance matrix $\mathbf{R}=\mathrm{diag}\left(\right[4,0.01,1\left]\right)$. The processed noise covariance matrix is assumed to be $\mathbf{Q}=\mathrm{diag}\left([0.1,0.1,0.01,0.01]\right)$. Multidimensional outliers caused by interferences or clutters are assumed to be drawn from the uniform distribution with the following parameters: $[a_1,b_1]=[10,90]$ in the range entry in the measurement vector, $[a_2,b_2]=[-\pi /2,\pi /2]$ in the azimuth entry, and $[a_3,b_3]=[-10,10]$ in the velocity entry. The probability of outliers is assumed to be constant over time, that is, $c_{k,m}$ is a Bernoulli random variable with the probability of $p(c_{k,m}=0)=0.2$. In the particle filter method, the number of particles S is 500, and hyperparameters $\mathbf{v}$ are set to be ${[10,10,10]}^T$. The total step number K is 1000 with the sampling interval $\Delta T$ of 0.01 s. All the results are implemented on the PC with CPU (Intel Core i7-10700K) using Matlab R2020a.

For the performance evaluation of estimated target location, the root of mean square error (RMSE) and normalized mean square error (NMSE) are defined as performance indexes

$$\begin{array}{cc}\hfill {\mathrm{RMSE}}_k& =\sqrt{{\left(\langle x_k\rangle -x_k^{\mathrm{TRUE}}\right)}^2+{\left(\langle y_k\rangle -y_k^{\mathrm{TRUE}}\right)}^2}\hfill \end{array}$$

(49)

$$\begin{array}{cc}\hfill \mathrm{NMSE}& =\frac{1}{K}\sum _{k=1}^K\frac{{\mathrm{RMSE}}_k}{\sqrt{{\left(x_k^{\mathrm{TRUE}}\right)}^2+{\left(y_k^{\mathrm{TRUE}}\right)}^2}}\hfill \end{array}$$

(50)

where $\langle {\mathbf{\theta }}_k\rangle ={\left[\langle x_k\rangle ,\langle y_k\rangle ,\langle {\dot{x}}_k\rangle ,\langle {\dot{y}}_k\rangle \right]}^T$ denotes the estimated state vector at the step time k. The performance comparisons of tracking results with the conventional particle filter (PF) and other robust solutions, including the weighted robust PF (WR-PF) and the outlier robust extended KF (OR-EKF), are provided in Figure 3. It is observed that the estimated trajectory is the closest to the truth one compared with the WR-PF and OR-EKF methods, and thus the proposed method has the smallest NMSE. Note that the results in the PF method are not provided due to quite a sensitivity to outliers, yielding significantly poor performances.

The performances versus outlier ratios with $p(c_{k,m}=0)$ are also examined. The outlier ratio varies from $0\%$ to $50\%$, and all other parameters are kept in this simulation. It is reasonable that the NMSEs generally decreases with decreases in the outlier ratios. It is observed that the proposed method has the lowest NMSE across all ratios compared with other methods, as shown in Figure 4.

In addition, we also introduce average Mahalanobis distance as the performance index to evaluate the performance of the entire estimated state-space vector and defined it as

$${\mathrm{D}}_{\mathrm{M}}=\frac{1}{K}\sum _{k=1}^K\sqrt{\left(\langle {\mathbf{\theta }}_k\rangle -{\mathbf{\theta }}_k^{\mathrm{TRUE}}\right){\mathbf{Q}}^{-1}\left(\langle {\mathbf{\theta }}_k\rangle -{\mathbf{\theta }}_k^{\mathrm{TRUE}}\right)}$$

(51)

where ${\mathbf{\theta }}_k=\left[x_k,y_k,{\dot{x}}_k,{\dot{y}}_k\right]$ is the true state vector containing the two-dimensional location $\left(x_k,y_k\right)$ and the corresponding velocities $\left({\dot{x}}_k,{\dot{y}}_k\right)$ of the vehicle. $\langle {\mathbf{\theta }}_k\rangle$ denotes the estimated vector of ${\mathbf{\theta }}_k$, and $\mathbf{Q}$ is the process noise covariance matrix provided in the simulations.

The performance results versus outlier ratios are also examined in Figure 5, the proposed method has the lowest Mahalanobis distance across all ratios, and thus has better performance than those in the WR-PF and OR-EKF methods. Note that the results in the PF method are not provided due to quite a sensitivity to outliers, yielding significantly poor performances.

In the following simulations, diverse types of outliers in the multidimensional measurements are considered to examine the performances of the proposed method. First, we consider only range outliers in the multidimensional measurements, that is to say, the azimuth and velocity entries are abnormal values with $p\left(c_{k,2}\right)=p\left(c_{k,3}\right)=0$. It is found that both PF-based methods have better performances than that in the KF-based method, shown in Figure 6. The main reason may be the nonlinear approximation in the measurement function in the EKF method. These two PF-based outlier-robust methods can effectively address the range outlier issue and acquire comparable NMSEs, even though the NMSEs in the WR-PF method are a little bit smaller than those in the proposed method in the target location estimation, shown in Figure 6a. The possible reason could be that the entry of range is much larger than the other two entries in the multidimensional measurements, and thus the scalar weight in the WR-PF method is learned and estimated to accurately model the measurement noise variances, particularly in the noise variance of the range entry in the multidimensional measurements, such that the lower NMSEs in the target locations can be obtained. However, to evaluate the performance of the entire estimated state-space vector in the filtering methods, the proposed method has the lowest average Mahalanobis distance across outlier ratios, shown in Figure 6b. Therefore, it suggests that the proposed method has better estimation performances in the entire state-space vector estimation by treating the weights associated with each entry in the measurement vector data probabilistically.

Then, we consider only azimuth outliers in the multidimensional measurements, and the other two entries are normal values. The proposed method has the lowest estimated NMSE in both average Mahalanobis distance and NMSE compared with those in the OR-EKF and WR-PF methods, shown in Figure 7. Finally, Figure 8 shows the performance comparisons between the proposed method and the other two robust solutions in the only velocity outliers case, and it is observed that the proposed method has clear performance improvements and has the lowest NMSE and average Mahalanobis distance across normal measurement ratios.

Table 1. Executive time comparisons.

Method	PF	OR-EKF	WR-PF	RR-PF
Average executive time	0.0269 s	0.0096 s	0.0317 s	0.0367 s

shows the average executive comparisons of these robust solutions. It is observed that the executive time of the proposed method is comparable to these PF-based methods, whereas the proposed method has the best performance among them. It is reasonable that the OR-EKF method has the smallest executive time among all these methods owing to its analytical formulation, whereas it generally has slightly poorer performance than the PF-based robust solutions such as the WR-PF and RR-PF methods.

3.2. Cramer–Rao Bound Analysis of the Proposed Algorithm

To further illustrate the estimation performance of the algorithm, we provide the Cramer–Rao Bound for the nonlinear system to serve as the reference for the estimation error. We present the universal MC-PCRB method above for discrete-time state-space models to solve the iterative Cramer–Rao bound at each step in Equation (16). Figure 9 shows the comparison results among these methods. The outlier ratios are assigned to be $p\left(c_{k,1}\right)=p\left(c_{k,2}\right)=p\left(c_{k,3}\right)=0.2$. It is observed that the RMSEs in the outlier robust methods generally decrease with the increases in the time step, except in the conventional PF method. The proposed method is generally close to the PCRLB, and thus has the lowest RMSE. The proposed method has higher RMSEs than the WR-PF method in the early stage because of more parameters to learn. In theory, the proposed method would approach the PCRLB, as long as the number of particles approaches infinity while tracking under a stable state.

3.3. Experimental Results Based on Real Data

In this subsection, real data collected by the automotive millimeter wave (MMW) radar with linear modulated continuous waveform (LFMCW) are used to verify the algorithm’s effectiveness. In the automotive radar application, pedestrian tracking based on the MMW radar is often an issue of great importance. Therefore, we consider pedestrian tracking in these experiments. A 77 GHz millimeter wave radar, whose parameters are shown in

Table 2. Parameters for the LFMCW Radar System.

Parameters	Definition	Values
$f_c$	Carrier frequency	77 GHz
B	Bandwidth	4 GHz
$F_s$	Sampling rate	6.25 MHz
T	Pulse duration	50 $\mathsf{\mu }$s
${\mathrm{FFT}}_r$	Range FFT points	128

, is used to carry out experiments. An LFMCW waveform with a bandwidth of 4 GHz is used for the high range resolution of 0.0375 m, and the sampling rate of 6.25 MHz is used to acquire raw data. Two-dimensional (2D) Fourier Transform (FT) is firstly implemented on the raw data, and thus the constant false alarm ratio (CFAR) technique is used to acquire the estimates of the range and velocity of the walking pedestrian. Finally, the digital beamforming technique is utilized to obtain the azimuth of the pedestrian. To this end, the measurement vector is acquired for further target tracking. During the data collection of the pedestrian, several artificial blocks are place along the tracking path, and then some outlier measurements occur in the acquired measurement vector.

In the first experiment trial, the pedestrian walked from the location (1.5, 2.5) to the location (−1.5, 2.1) along a straight line. There is an artificial blocker from the location (−0.3, 1.5) to the location (0.3, 1.5). The sketch map is shown in Figure 10. The measurement vector data have some outliers from the 30th to the 37th time step because of crossing the blocker. The acquired measurement vector data are given in Figure 11. It is observed that the measurement vectors involve several obvious outliers at several time steps due to the artificial occlusion or clutters reflected from the complex environment. The tracking results and the corresponding RMSEs are, respectively, shown in Figure 12a,b. It demonstrates that the estimated trajectory in the proposed method is much closer to the true one, and thus it has the lowest RMSE compared with other methods. It is found that there exist a few fluctuations in the interval from the 20th time step and the 35th time step; a possible reason may result from the measurement precision issue of pedestrian walking trajectory in the real experiment, and the true trajectory of the walking pedestrian may be around the preassigned straight line, with some inevitable fluctuations when the pedestrian walked along the straight line in the natural walking state. Although a few fluctuations appear, the RMSEs of the proposed method reach a pretty small level, and they are almost the lowest across time steps compared with other robust solutions.

In the second experiment trial, the slightly complex trajectory, which involves curvilinear motion, turning around, and sharp turning, is considered to verify the effectiveness of the proposed method. The acquired measurement vector data are provided in Figure 13, and it is observed that the measurement vectors have obvious outliers at a few time steps due to the clutters reflected from the complex electromagnetic environment. The tracking results and the corresponding RMSEs are, respectively, shown in Figure 14a,b. They demonstrate that the estimated trajectory in the proposed method is much closer to the true one and almost has the lowest RMSE compared with other robust methods.

4. Conclusions

In this paper, we consider the nonlinear filtering problem in the presence of contaminated measurements. A novel reweighted robust particle filter method was proposed for accurate target tracking in the automotive radar system. We generalized nonlinear filtering to the more comprehensive case of multidimensional outlier measurements and enhanced state vector estimates by probabilistically learning a weight vector for each entry in the multidimension measurement vector. The particle filter technique was adopted to make an approximate posterior in this nonlinear system. We also illustrated the reason that the proposed method is more robust to outliers. Finally, the Cramer–Rao lower bound of the proposed method was also provided. Both simulations and experiments verified the superiority of the proposed method over other robust solutions.