Hybrid Interacting Multiple Model Filtering for Improving the Reliability of Radar-Based Forward Collision Warning Systems

Abstract

Automotive forward collision warning (FCW) systems based on radar sensors attracted widespread attention in recent years. To achieve a reliable FCW, it is essential to accurately estimate the position and velocity of a preceding vehicle. To this end, this study proposed a novel estimation algorithm, which is a hybrid of interacting multiple model probabilistic data association (IMM-PDA) and finite impulse response (FIR) filters. Although the IMM-PDA filter is one of the most successful algorithm for tracking a maneuvering target in clutters, it sometimes exhibits divergence owing to modeling errors. In this study, the divergent IMM-PDA filter in the novel algorithm was reset and recovered using an assisting FIR filter. Consequently, this enabled reliable estimation for FCW. The improved reliability of the proposed algorithm was demonstrated through the simulation of preceding vehicle tracking using automotive radars.

1. Introduction

In recent years, advanced driver-assistance systems (ADAS) were developed and attracted widespread attention [1]. Among various ADAS, forward collision warning (FCW) systems are essential for the safety of drivers [2]. FCW can be implemented using various sensors, such as radar [3], vision [4], and global positioning system (GPS) [5]. Radar-based FCW is the most commonly used FCW owing to the fact that radars can provide long-range detection regardless of weather/light conditions [6,7,8].

Studies on FCW algorithms focused on the decision-making process involved in determining whether to alert a collision. In typical FCW systems [9,10,11,12], the time-to-collision (TTC) is computed using the information of the relative distance and velocity between the host and preceding vehicles, and a warning is provided if the TTC was below a certain threshold. Recent studies [13,14,15] analyzed the behaviors (driving patterns) of vehicles using artificial intelligence (AI) algorithms, and a warning is provided in situations with a high risk of collision. Although various decision-making algorithms for FCW were developed, most of them are based on information on the state (position and velocity) of preceding vehicles. Thus, it is essential to develop algorithms that can accurately estimate the state of a preceding vehicle to achieve a reliable FCW [5,9,11].

The estimation of the position and velocity of a target, known as target tracking, requires two processes: signal processing and data processing. In radar signal processing, measurements of the range, bearing, and range rate are obtained by processing the received radar signals, and this can be achieved using various signal processing methods including Fourier analysis, filtering, and AI algorithms. The measurements obtained using signal processing contain noises and clutters; hence, a postprocessing, known as data processing, is required. Stochastic filters (e.g., Kalman filter (KF)) are used in data processing to improve the accuracy of the estimation of the position and velocity of a target. In addition, data association algorithms (e.g., probabilistic data association) are used to address the problem of clutters (i.e., false measurements). Probabilistic data association filter (PDAF) [16], which is the combination of KF and probabilistic data association, is one of the most renowned and successful algorithms for single target tracking in the presence of clutters. Particularly, interacting multiple model PDAF (IMMPDAF) [17,18,19,20] is an advanced algorithm for maneuvering targets, and joint PDAF [21] is an algorithm for multiple target tracking. Recently, various multiple target tracking algorithms, such as multiple hypothesis tracking [22] and probability hypothesis density filter [23], were developed. This study focused on data processing, particularly, the problem of single vehicle tracking in the presence of clutters, for application in FCW systems. Thus, IMMPDAF was used as the main filter in the proposed algorithm.

However, as the IMMPDAF is based on the KF, it sometimes exhibits estimation divergence owing to modeling errors. For example, a significant difference in the actual movement of a target and the movement predicted by the motion model may result in the divergence of KF. This divergence could be attributed to the accumulation of errors in the KF. To overcome the filter divergence problem, stochastic filters with finite impulse response (FIR) structures were investigated and developed [24,25,26,27,28,29,30,31]. FIR stochastic filters, known as FIR filters, only utilize recent finite measurements to produce a current state estimate, which makes them robust against modeling errors. However, because they utilize finite measurements, the accuracy of their estimates are generally lower than that of KF that utilizes all past measurements. In summary, the IMMPDAF is accurate under normal conditions, but may diverge if the actual movement of a target is significantly different from that of the motion model. In contrast, FIR filters do not diverge, but are less accurate under normal conditions.

Therefore, this study proposed a novel hybrid estimation algorithm for improving the reliability of radar-based FCW. The proposed algorithm utilized IMMPDAF as the main filter, and an FIR filter as an assisting filter. The assisting FIR filter only function when the IMMPDAF diverges, and it produces a state estimate and covariance. In this situation, the IMMPDAF is reset using the information obtained from the FIR filter. Consequently, the IMMPDAF recovers from failures. Therefore, the proposed algorithm was named hybrid IMM-PDA/FIR filter (HIPFF). The performance of the proposed HIPFF was demonstrated using simulations, and the relative position and velocity of a vehicle were estimated using the HIPFF and conventional IMMPDAF. The results revealed that when a preceding vehicle decelerated rapidly, the conventional IMMPDAF often failed and diverged, whereas the proposed HIPFF did not. This indicates that HIPFF significantly improved the reliability of the state estimation for FCW systems.

The rest of this paper is organized as follows. Section 2 introduces the problem involved in the estimation of the position and velocity of a preceding vehicle in the presence of radar clutters. Section 3 introduces the IMMPDAF algorithm, and Section 4 introduces the proposed HIPFF algorithm. Section 5 presents the simulation results to demonstrate the performance of the proposed HIPFF, and the conclusions are drawn in Section 6.

2. Preceding Vehicle State Estimation Using Automotive Radars

This section describes the problem involved in the estimation of the state of a preceding vehicle using an automotive radar in the presence of clutters. The state (position and velocity) of the preceding vehicle is represented using a two-dimensional (2D) Cartesian coordinate system (Figure 1). An automotive radar for FCW is installed in the middle of the host vehicle, and the origin of the coordinate system is located at the position of the radar. The x-axis is aligned with the longitudinal direction of the host vehicle. The state of a preceding vehicle (target) is composed of 2D positions, $(x,y)$, and velocities, $(v_x,v_y)$. Consequently, the state vector is defined as $\mathbf{x}={\left[x{v}_{x}y{v}_y\right]}^T$.

Estimation algorithms require state-space models, which consist of motion and measurement models. The motion model expresses the transition of the state vector in discrete-time. Typically, the motion model used for target tracking is the discrete white noise acceleration (DWNA) model, which can be represented using:

$$\begin{array}{cc}\hfill \mathbf{x}(k+1)& =\mathbf{F}\mathbf{x}\left(k\right)+\mathbf{G}\mathbf{w}\left(k\right),\hfill \\ \hfill \mathbf{F}& =\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right],\mathbf{G}=\left[\begin{array}{cc}{T}^2/2& 0\\ T& 0\\ 0& T^2/2\\ 0& T\end{array}\right],\hfill \end{array}$$

(1)

where $\mathbf{x}\left(k\right)$ is the state vector at a discrete time k and $\mathbf{w}\left(k\right)$ is the zero-mean white Gaussian process noise vector. The covariance of $\mathbf{w}\left(k\right)$ is defined as $\mathbf{Q}\left(k\right)={\sigma }_w^2{\mathbf{I}}_2$, where ${\sigma }_w^2$ is the variance of process noise, and ${\mathbf{I}}_2$ is the $2\times 2$ identity matrix.

The process noise, $\mathbf{w}\left(k\right)$, corresponds to the acceleration of a target, and ${\sigma }_w^2$ determines the level of acceleration. This indicates that ${\sigma }_w^2$ reflects the amount of changes in velocity. When an appropriate value of ${\sigma }_w^2$ is used, the filter can produce accurate state estimates. However, if ${\sigma }_w^2$ is not sufficiently large, and there is a significant change in the velocity of a target, the filter divergence phenomenon (i.e., rapid increase in estimation error) may occur. In contrast, if ${\sigma }_w^2$ was sufficiently large and the changes in velocity was too small, the estimation accuracy is degraded. Thus, a mismatch between the model (i.e., ${\sigma }_w^2$) and the actual motion of a target can result in a degradation in the estimation accuracy or even filter divergence. However, the acceleration of a target is generally unknown, and ${\sigma }_w^2$ is determined using a rule of thumb based on the knowledge of engineers. The IMM estimation algorithm can solve the problem of determining the uncertain ${\sigma }_w^2$. The IMM algorithm utilizes multiple models that use different ${\sigma }_w^2$. A detailed algorithm for the IMM estimation is described in Section 3.

The radar reports the relative range, r, and bearing, $\theta$, between the host and preceding vehicle at a constant time interval, T, as shown in Figure 1. These radar measurements including noise can be expressed as

$$\begin{array}{cc}\hfill r_m& =r+{\overline{v}}_r,\hfill \\ \hfill {\theta }_m& =\theta +{\overline{v}}_{\theta },\hfill \end{array}$$

(2)

where r and $\theta$ are the true range and bearing, respectively; $r_m$ and ${\theta }_m$ are the measured range and bearing, respectively; and ${\overline{v}}_r$ and ${\overline{v}}_{\theta }$ are the measurement noises, which are assumed to be mutually independent and zero-mean white Gaussian. The variances of ${\overline{v}}_r$ and ${\overline{v}}_{\theta }$ are denoted as ${\sigma }_r^2$ and ${\sigma }_{\theta }^2$, respectively.

The measurement model relates the radar measurements with the state, $\mathbf{x}\left(k\right)$. However, the raw radar measurements are polar coordinates, and they should be converted to Cartesian coordinates. The conversion can be written as

$$\begin{array}{cc}\hfill x_m& =r_{m}cos\left({\theta }_m\right),\hfill \\ \hfill y_m& =r_{m}sin\left({\theta }_m\right),\hfill \end{array}$$

(3)

where $x_m$ and $y_m$ are the converted measurements, and they can also be represented as

$$\begin{array}{cc}\hfill x_m& =x+{\overline{v}}_x,\hfill \\ \hfill y_m& =y+{\overline{v}}_y,\hfill \end{array}$$

(4)

where ${\overline{v}}_x$ and ${\overline{v}}_y$ are the measurement noises in the Cartesian coordinate.

The measurement vector is defined as $\mathbf{z}\left(k\right)={\left[x_m\left(k\right)y_m\left(k\right)\right]}^T$, and the measurement model is represented as follows:

$$\begin{array}{cc}\hfill \mathbf{z}\left(k\right)& =\mathbf{H}\mathbf{x}\left(k\right)+\overline{\mathbf{v}}\left(k\right),\hfill \\ \hfill \mathbf{H}& =\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right],\hfill \end{array}$$

(5)

where $\overline{\mathbf{v}}\left(k\right)$ is the measurement noise vector. The noise covariance of converted measurement, denoted as $\mathbf{R}\left(k\right)$, is obtained as follows [17]:

$$\begin{array}{cc}\hfill \mathbf{R}\left(k\right)& =\left[\begin{array}{cc}{R}_{11}\left(k\right)& R_{12}\left(k\right)\\ R_{12}\left(k\right)& R_{22}\left(k\right)\end{array}\right],\hfill \\ \hfill R_{11}\left(k\right)& =r_m{\left(k\right)}^2{\sigma }_{\theta }^{2}si{n}^2\left({\theta }_m\left(k\right)\right)+{\sigma }_r^{2}co{s}^2\left({\theta }_m\left(k\right)\right),\hfill \\ \hfill R_{22}\left(k\right)& =r_m{\left(k\right)}^2{\sigma }_{\theta }^{2}co{s}^2\left({\theta }_m\left(k\right)\right)+{\sigma }_r^{2}si{n}^2\left({\theta }_m\left(k\right)\right),\hfill \\ \hfill R_{12}\left(k\right)& =({\sigma }_r^2-r_m{\left(k\right)}^2{\sigma }_{\theta }^2)sin\left({\theta }_m\left(k\right)\right)cos\left({\theta }_m\left(k\right)\right).\hfill \end{array}$$

(6)

Given the motion and measurement models, Equations (1) and (5), the state $\mathbf{x}\left(k\right)$ can be estimated using various estimation algorithms. However, two factors hinder this estimation: radar clutters and maneuvering of target. Radar clutters are false measurements generated by the scattering of radar reflection. Because it is unknown whether a measurement is true or false, false measurements make the estimation difficult. Nevertheless, this uncertainty in measurements can be handled by PDA algorithm. Maneuvering corresponds to changes in the direction of movement and speed. A rapid and significant maneuvering of a target results in a mismatch between the motion model (particularly ${\sigma }_w^2$) and the actual motion of the target, which can result in divergence of the estimation algorithm. This uncertainty in the motion model can be handled by IMM algorithm. In the following section, IMMPDAF, which is a combination of IMM and PDA algorithms, is introduced.

3. Interacting Multiple Model (IMM) Probabilistic Data Association (PDA) Filter

The IMMPDAF operates multiple PDA filters in parallel, and each PDA filter is matched to a model. The inputs and outputs of the multiple PDA filters were mixed using the IMM algorithm. In this section, first, the PDA filter is introduced, after which the IMM algorithm is described.

The PDA filter is a recursive estimator, indicating that the processes are repeatedly performed at each time step. The first process of the PDA filter is a time update process, in which the state estimate and estimation error covariance are updated using the motion model as follows:

$$\begin{array}{cc}\hfill \widehat{\mathbf{x}}\left(k\right|k-1)& =\mathbf{F}\widehat{\mathbf{x}}(k-1|k-1),\hfill \\ \hfill \mathbf{P}\left(k\right|k-1)& =\mathbf{F}\mathbf{P}(k-1|k-1){\mathbf{F}}^T+\mathbf{G}\mathbf{Q}\left(k\right){\mathbf{G}}^T.\hfill \end{array}$$

(7)

where the notation “$k|k-1$” corresponds to a priori estimates obtained at time k using the measurements up to time $k-1$. In addition, the notation “$k-1|k-1$” corresponds to a posteriori estimates obtained at $k-1$ using the measurements up to $k-1$.

The second process is measurement validation, in which only “valid” measurements are selected. The number of measurements obtained at time k is denoted as $m\left(k\right)$, and i-th measurement ($i=1,2,\cdots ,m\left(m\right)$) at time k is denoted as ${\mathbf{z}}_i\left(k\right)$. The measurements within the validation region (also known as the gate) are selected as follows:

$$\begin{array}{cc}\hfill {\nu }_i\left(k\right)& ={({\mathbf{z}}_i\left(k\right)-\widehat{\mathbf{z}}\left(k\right))}^T{\mathbf{S}}_i^{-1}\left(k\right)({\mathbf{z}}_i\left(k\right)-\widehat{\mathbf{z}}\left(k\right))\le \gamma ,\hfill \\ \hfill \widehat{\mathbf{z}}\left(k\right)& =\mathbf{H}\widehat{\mathbf{x}}\left(k\right|k-1),\hfill \\ \hfill {\mathbf{S}}_i\left(k\right)& =\mathbf{H}\mathbf{P}\left(k\right|k-1){\mathbf{H}}^T+{\mathbf{R}}_i\left(k\right),\hfill \end{array}$$

(8)

where $\gamma$ is the gate threshold, which can be obtained from a chi-square table, and ${\mathbf{R}}_i\left(k\right)$ is the measurement noise covariance computed using ${\mathbf{z}}_i\left(k\right)$ and (6). The number of selected (validated) measurements is denoted as $n\left(k\right)$.

The third process is the computation of association probability. The association probability, ${\beta }_i\left(k\right)$, is the probability that the measurement ${\mathbf{z}}_i\left(k\right)$ is from the true target (i.e., true measurement), which can be computed as follows:

$$\begin{array}{cc}\hfill {\beta }_i\left(k\right)& =\left\{\begin{array}{c}\frac{{\mathcal{L}}_i\left(k\right)}{1-P_D{P}_G+{\sum }_{j=1}^{n\left(k\right)}{\mathcal{L}}_j\left(k\right)}i=1,\cdots ,n\left(k\right)\hfill \\ \frac{1-P_D{P}_G}{1-P_D{P}_G+{\sum }_{j=1}^{n\left(k\right)}{\mathcal{L}}_j\left(k\right)}i=0\hfill \end{array}\right.\hfill \end{array}$$

(9)

$$\begin{array}{cc}\hfill {\mathcal{L}}_j\left(k\right)& =\frac{\exp [-0.5{({\mathbf{z}}_{\mathrm{j}}\left(\mathrm{k}\right)-\widehat{\mathbf{z}}\left(\mathrm{k}\right))}^T{\mathbf{S}}_{\mathrm{j}}^{-1}\left(\mathrm{k}\right)({\mathbf{z}}_{\mathrm{j}}\left(\mathrm{k}\right)-\widehat{\mathbf{z}}\left(\mathrm{k}\right))]}{{\left(2\pi \right)}^{n_z/2}{\left|{\mathbf{R}}_j\left(k\right)\right|}^{1/2}n\left(k\right)}{P}_D{V}_j\left(k\right)\hfill \end{array}$$

(10)

$$\begin{array}{cc}\hfill V_j\left(k\right)& =c_{n_z}{\gamma }^{n_z/2}{\left|{\mathbf{S}}_j\left(k\right)\right|}^{1/2},\hfill \end{array}$$

(11)

where $i=0$ corresponds to the case where there is no true measurement, and $V_j\left(k\right)$ is the volume of the validation region, which is computed as

$$\begin{array}{cc}\hfill V_k\left(j\right)& =c_{n_z}{\gamma }^{n_z/2}{\left|{\mathbf{S}}_k\left(j\right)\right|}^{1/2},\hfill \end{array}$$

(12)

where $n_z$ is the dimension of the measurement vector. In addition, $P_G$ and $P_D$ in (9) are the gate probability and the target detection probability, respectively.

The last process is the measurement update, in which state estimates are updated using the validated measurements as follows:

$$\begin{array}{cc}\hfill \widehat{\mathbf{x}}\left(k\right|k)& =\widehat{\mathbf{x}}\left(k\right|k-1)+\sum _{i=1}^{n\left(k\right)}{\mathbf{W}}_i\left(k\right){\beta }_i\left(k\right){\nu }_i\left(k\right),\hfill \\ \hfill {\mathbf{W}}_i\left(k\right)& =\mathbf{P}\left(k\right|k-1){\mathbf{H}}^T{\mathbf{S}}_i^{-1}\left(k\right),\hfill \\ \hfill {\nu }_i\left(k\right)& ={\mathbf{z}}_i\left(k\right)-\widehat{\mathbf{z}}\left(k\right),\hfill \\ \hfill \mathbf{P}\left(k\right|k)& =\overline{\mathbf{P}}\left(k\right|k)+\tilde{\mathbf{P}}\left(k\right|k),\hfill \\ \hfill \overline{\mathbf{P}}\left(k\right|k)& ={\beta }_0\left(k\right)\mathbf{P}\left(k\right|k-1)+\sum _{i=1}^{n\left(k\right)}{\beta }_i\left(k\right)(\mathbf{I}-{\mathbf{W}}_i\left(k\right)\mathbf{H})\mathbf{P}\left(k\right|k-1),\hfill \end{array}$$

$$\begin{array}{cc}\hfill \tilde{\mathbf{P}}\left(k\right|k)& =\sum _{i=1}^{n\left(k\right)}{\beta }_i\left(k\right){\mathbf{W}}_i\left(k\right){\nu }_i\left(k\right){\nu }_i^T\left(k\right){\mathbf{W}}_i^T\left(k\right)\hfill \\ \hfill & -\left(\sum _{i=1}^{n\left(k\right)}{\beta }_i\left(k\right){\mathbf{W}}_i\left(k\right){\nu }_i\left(k\right)\right)\left(\sum _{i=1}^{n\left(k\right)}{\beta }_i\left(k\right){\nu }_i^T\left(k\right){\mathbf{W}}_i^T\left(k\right)\right).\hfill \end{array}$$

(13)

If there is no measurement in the validation region (i.e., $n\left(k\right)=0$), the measurement update process is omitted.

Next, the IMM algorithm is introduced. In this study, the IMM algorithm uses the following models:

• $M_1$: DWNA model (1) assuming undetected target (false target)
• $M_2$: DWNA model (1) assuming true target with normal motion
• $M_3$: DWNA model (1) assuming true target performing maneuvering
• $M_4$: DWNA model (1) assuming true target with nearly constant velocity

The difference between $M_1$ and $M_2$ is the target detection probability, which can be expressed as follows:

$$\begin{array}{cc}\hfill P_D^j& =\left\{\begin{array}{c}0,\mathrm{if}j=1,\hfill \\ P_D,\mathrm{if}j=2,3,4.\hfill \end{array}\right.\hfill \end{array}$$

(14)

where j is the model (mode) number. The model $M_1$ plays a role in detecting a target loss. The differences among $M_2$–$M_4$ are the process noise variance ${\sigma }_w^2$. $M_3$ uses the highest ${\sigma }_w^2$, and $M_4$ uses the lowest ${\sigma }_w^2$. A PDA filter that uses model j is denoted as filter j ($j=1,2,3,4$).

First, IMM-PDA algorithm computes the mixing probability as

$$\begin{array}{cc}\hfill {\mu }_{h|j}(k-1|k-1)& =\frac{1}{{\overline{c}}_j}{p}_{hj}{\mu }_h(k-1),\hfill \\ \hfill {\overline{c}}_j& =\sum _{h=1}^r{p}_{hj}{\mu }_h(k-1),\hfill \end{array}$$

(15)

where ${\mu }_{h|j}(k-1|k-1)$ is the mixing probability, indicating the weight imposed on the information from filters h to i.

Using the mixing probabilities, initial estimate and covariance for the filter j are obtained as follows:

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}^{0j}(k-1|k-1)& =\sum _{h=1}^4{\widehat{\mathbf{x}}}^{0j}(k-1|k-1){\mu }_{h|j}(k-1|k-1),\hfill \end{array}$$

(16)

$$\begin{array}{cc}\hfill {\mathbf{P}}^{0j}(k-1|k-1)& =\sum _{h=1}^4{\mu }_{i|j}(k-1|k-1)\{{\mathbf{P}}^h(k-1|k-1)+[{\widehat{\mathbf{x}}}^h(k-1|k-1)\hfill \\ \hfill & -{\widehat{\mathbf{x}}}^{0j}(k-1|k-1)\left]{[{\widehat{\mathbf{x}}}^i(k-1|k-1)-{\widehat{\mathbf{x}}}^{0j}(k-1|k-1)]}^T\right\}.\hfill \end{array}$$

(17)

${\widehat{\mathbf{x}}}^{0j}(k-1|k-1)$ and ${\mathbf{P}}^{0j}(k-1|k-1)$ are fed to the filter j, which corresponds to $\widehat{\mathbf{x}}(k-1|k-1)$ and $\mathbf{P}(k-1|k-1)$ in (7), respectively. Subsequently, PDA filtering (7)–(13) is performed for each model j, and ${\widehat{\mathbf{x}}}^j\left(k\right|k)$ and ${\mathbf{P}}^j\left(k\right|k)$ are obtained

Next, the likelihoods for ${\widehat{\mathbf{x}}}^j\left(k\right|k)$ are computed as follows:

$$\begin{array}{cc}\hfill {\Lambda }_j\left[Z\left(k\right)\right]& =\frac{1-P_D^j}{V^{n\left(k\right)}}+\frac{n\left(k\right)P_D^j}{V^{n\left(k\right)-1}}\sum _{i=1}^{n\left(k\right)}\mathcal{N}[{\mathbf{z}}_i\left(k\right);{\widehat{\mathbf{z}}}^j\left(k\right|k-1),{\mathbf{S}}^j\left(k\right)],\hfill \end{array}$$

(18)

$$\begin{array}{cc}\hfill V& =\pi \gamma |{\mathbf{S}}^j{\left(k\right)|}^{1/2},\hfill \end{array}$$

(19)

$$\begin{array}{cc}\hfill {\mathbf{S}}^j\left(k\right)& =\mathbf{H}{\mathbf{P}}^j\left(k\right|k){\mathbf{H}}^T+{\mathbf{R}}_i\left(k\right),\hfill \end{array}$$

(20)

$$\begin{array}{cc}\hfill {\widehat{\mathbf{z}}}^j\left(k\right|k-1)& =\mathbf{H}{\widehat{\mathbf{x}}}^j\left(k\right|k),\hfill \end{array}$$

(21)

where $\mathcal{N}[{\mathbf{z}}_i\left(k\right);{\widehat{\mathbf{z}}}^j\left(k\right|k-1),{\mathbf{S}}^j\left(k\right)]$ corresponds to the Gaussian probability density function with the mean ${\widehat{\mathbf{z}}}^j\left(k\right|k-1)$ and covariance ${\mathbf{S}}^j\left(k\right)$; $Z\left(k\right)$ is the set of validated measurements defined as $Z\left(k\right)={\left\{{\mathbf{z}}_i\left(k\right)\right\}}_{i=1}^{n\left(k\right)}$. Using the likelihoods, mode probabilities are updated as

$$\begin{array}{cc}\hfill {\mu }_j\left(k\right)& =\frac{1}{c}{\Lambda }_j\left[Z\left(k\right)\right]{\overline{c}}_j,\hfill \end{array}$$

(22)

$$\begin{array}{cc}\hfill c& =\sum _{j=1}^4{\Lambda }_j\left[Z\left(k\right)\right]{\overline{c}}_j.\hfill \end{array}$$

(23)

Lastly, the ultimate estimate and covariance at time k are obtained by combination as follows:

$$\begin{array}{cc}\hfill \widehat{\mathbf{x}}\left(k\right|k)& =\sum _{j=1}^4{\widehat{\mathbf{x}}}^j\left(k\right|k){\mu }_j\left(k\right),\hfill \end{array}$$

(24)

$$\begin{array}{cc}\hfill \mathbf{P}\left(k\right|k)& =\sum _{j=1}^4{\mu }_j\left(k\right)\{{\mathbf{P}}^j\left(k\right|k)+[{\widehat{\mathbf{x}}}^j\left(k\right|k)-\widehat{\mathbf{x}}\left(k\right|k)]{[{\widehat{\mathbf{x}}}^j\left(k\right|k)-\widehat{\mathbf{x}}\left(k\right|k)]}^T\}.\hfill \end{array}$$

(25)

At the initial time of the IMMPDAF filtering, the mode probabilities were set equally as ${\mu }_j\left(k\right)=0.25$. The transition probabilities from mode h to j, which is denoted as $p_{hj}$, is an important design parameter that affects the performance of the IMMPDAF algorithm. If the characteristics of the target motion are known, $p_{hj}$ can be designed using the information on the mean sojourn time for each mode. However, if the target motion information is not available, $p_{hj}$ can be set equally.

The IMMPDAF can handle the problem involved in the selection of a value for uncertain ${\sigma }_w^2$ in the motion model by adopting multiple models that use different ${\sigma }_w^2$ values. However, the IMMPDAF exhibits certain limitations. First, it has a limited number of models because of computational burden. Second, the multiple models are selected based on the knowledge of engineers, and inappropriate models are sometimes selected. In target tracking problems, even IMMPDAF can fail and diverge if the changes in target velocity are significantly different from the ${\sigma }_w^2$ value in the motion model (e.g., rapid deceleration of vehicles owing to full breaking). Thus, this study proposed a novel hybrid filtering algorithm that can recover the IMMPDAF from failures. Failure of IMMPDAF under severe conditions and recovering the failed algorithm were demonstrated using simulation results.

4. Hybrid IMM-PDA/Finite Impulse Response (FIR) Filter

In this section, the proposed HIPFF algorithm is introduced. In the HIPFF algorithm, the IMMPDAF acts as the main filter, and a FIR filter was used to assist the main filter. This assisting FIR filter operates when the IMMPDAF algorithm fails. Thus, the first problem is to detect the failures of the IMMPDAF. In this study, target loss indicates that the estimated position is far away from the actual target position. If the IMMPDAF loses the target, $M_1$ exhibits the highest mode (model) probability among the four models. The mode probability of $M_1$ is denoted as ${\mu }_1\left(k\right)$. Thus, a target loss can be detected using ${\mu }_1\left(k\right)$. However, it is too late to operate the assisting FIR filter after the detection of a target loss. If there are no measurements in the validation region ($n\left(k\right)=0$), the IMMPDAF cannot perform the measurement update, which may result in a target loss. Thus, $n\left(k\right)=0$ can be considered as a symptom of failures. The proposed HIPFF algorithm operated the assisting FIR filter when $n\left(k\right)=0$, and obtained the state estimate and covariance for resetting the main filter. A minimum variance FIR filter (MVFF) was adopted for the HIPFF algorithm. The MVFF is a one-step prediction estimator, and it can produce state estimate and covariance at the current time k using the measurements on the horizon $k-N\le k\le k-1$, where N is the horizon size of the FIR filter. Moreover, because the MVFF does not use the current measurement, it is suitable for the employed assisting filter when there is no validated measurements. The equations of the MVFF can be written as follows:

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}^{*}\left(k\right|k-1)& =\mathbf{L}{\mathbf{Z}}_N,\hfill \end{array}$$

(26)

$$\begin{array}{cc}\hfill {\mathbf{P}}^{*}\left(k\right|k-1)& ={\mathbf{K}}_N{\mathbf{Q}}_N{\mathbf{K}}_N^T+\mathbf{L}{\mathbf{R}}_N{\mathbf{L}}^T,\hfill \end{array}$$

(27)

$$\begin{array}{cc}\hfill \mathbf{L}& ={\mathbf{J}}_N{\left[\begin{array}{cc}{\mathbf{M}}_{1,1}& {\mathbf{M}}_{1,2}\\ {\mathbf{M}}_{1,2}^T& {\mathbf{M}}_{2,2}\end{array}\right]}^{-1}\left[\begin{array}{c}{\overline{\mathbf{C}}}_N^T\\ {\overline{\mathbf{G}}}_N^T\end{array}\right]{\mathbf{R}}_N^{-1},\hfill \end{array}$$

(28)

$$\begin{array}{cc}\hfill {\mathbf{J}}_N& =[{\mathbf{F}}^N{\mathbf{F}}^{N-1}{\mathbf{F}}^{N-2}\cdots \mathbf{F}\mathbf{I}],\hfill \end{array}$$

(29)

$$\begin{array}{cc}\hfill {\mathbf{M}}_{1,1}& ={\overline{\mathbf{C}}}_N^T{\mathbf{R}}_N^{-1}{\overline{\mathbf{C}}}_N,\hfill \\ \hfill {\mathbf{M}}_{1,2}& ={\overline{\mathbf{C}}}_N^T{\mathbf{R}}_N^{-1}{\overline{\mathbf{G}}}_N,\hfill \\ \hfill {\mathbf{M}}_{2,2}& ={\overline{\mathbf{G}}}_N^T{\mathbf{R}}_N^{-1}{\overline{\mathbf{G}}}_N+{\mathbf{Q}}_N^{-1},\hfill \end{array}$$

(30)

$$\begin{array}{cc}\hfill {\overline{\mathbf{C}}}_N& =\left[\begin{array}{c}\mathbf{H}\\ \mathbf{H}\mathbf{F}\\ \mathbf{H}{\mathbf{F}}^2\\ ⋮\\ \mathbf{H}{\mathbf{F}}^{N-1}\end{array}\right],\hfill \end{array}$$

(31)

$$\begin{array}{cc}\hfill {\overline{\mathbf{G}}}_N& =\left[\begin{array}{ccccc}\mathbf{0}& \mathbf{0}& \dots & \mathbf{0}& \mathbf{0}\\ \mathbf{H}\mathbf{G}& \mathbf{0}& \dots & \mathbf{0}& \mathbf{0}\\ \mathbf{H}\mathbf{F}\mathbf{G}& \mathbf{H}\mathbf{G}& \dots & \mathbf{0}& \mathbf{0}\\ ⋮& ⋮& ⋮& ⋮& ⋮\\ \mathbf{H}{\mathbf{F}}^{n-1}\mathbf{G}& \mathbf{H}{\mathbf{F}}^{n-2}\mathbf{G}& \dots & \mathbf{H}\mathbf{G}& \mathbf{0}\end{array}\right],\hfill \end{array}$$

(32)

$$\begin{array}{cc}\hfill {\mathbf{R}}_N& =\left[\mathrm{diag}\left(\stackrel{N}{\overbrace{{\mathbf{R}}^{*}(k-N){\mathbf{R}}^{*}(k-N+1)\cdots {\mathbf{R}}^{*}(k-1)}}\right)\right],\hfill \end{array}$$

(33)

$$\begin{array}{cc}\hfill {\mathbf{Q}}_N& =\left[\mathrm{diag}\left(\stackrel{N}{\overbrace{{\mathbf{Q}}^{*}(k-N){\mathbf{Q}}^{*}(k-N+1)\cdots \mathbf{Q}{(k-1)}^{*}}}\right)\right],\hfill \end{array}$$

(34)

$$\begin{array}{cc}\hfill {\mathbf{Z}}_N& ={[{\overline{\mathbf{z}}}^T(k-N){\overline{\mathbf{z}}}^T(k-N+1)\cdots {\overline{\mathbf{z}}}^T(k-1)]}^T,\hfill \end{array}$$

(35)

$$\begin{array}{cc}\hfill {\mathbf{K}}_N& =[{\mathbf{F}}^{N-1}\mathbf{G}{\mathbf{F}}^{N-2}\mathbf{G}\cdots \mathbf{FG}\mathbf{G}].\hfill \end{array}$$

(36)

Equations (26) and (35) indicate that the MVFF cannot handle clutters, and the most suitable measurement should be selected at each time step on the horizon $[k-N,k-1]$. The most suitable measurement can be selected using the nearest neighbor method as follows:

$$\begin{array}{cc}\hfill \overline{\mathbf{z}}\left(l\right)& =\underset{{\mathbf{z}}_i\left(l\right)}{argmin}{({\mathbf{z}}_i\left(l\right)-\widehat{\mathbf{z}}\left(l\right))}^T{\mathbf{S}}_i^{-1}\left(l\right)({\mathbf{z}}_i\left(l\right)-\widehat{\mathbf{z}}\left(l\right)),i=1,\cdots ,m\left(l\right),\hfill \\ \hfill \widehat{\mathbf{z}}\left(l\right)& =\mathbf{H}\widehat{\mathbf{x}}\left(l\right|l),\hfill \\ \hfill {\mathbf{S}}_i\left(l\right)& =\mathbf{H}\mathbf{P}\left(l\right|l){\mathbf{H}}^T+{\mathbf{R}}_i\left(l\right),\hfill \end{array}$$

(37)

where l is the time on the horizon $[k-N,k-1]$.

Equations (33) and (34) indicate that the MVFF can handle time-varying noise covariances. In (33), ${\mathbf{R}}^{*}\left(l\right)(l=k-N,\cdots ,k-1)$ can be obtained using $\overline{\mathbf{z}}\left(l\right)$ and (6). To this end, $\overline{\mathbf{z}}$ can be converted into a polar coordinate using the relation as follows:

$$\begin{array}{cc}\hfill r_m& =\sqrt{x_m^2+y_m^2},\hfill \\ \hfill {\theta }_m& =ta{n}^{-1}\left(\frac{y_m}{x_m}\right).\hfill \end{array}$$

(38)

In (34), ${\mathbf{Q}}^{*}\left(l\right)(l=k-N,\cdots ,k-1)$ is determined by selecting one of the process noise covariances of the four models ($M_1$–$M_4$). The process noise covariance of the model producing the highest mode probability is selected as ${\mathbf{Q}}^{*}\left(l\right)$.

After the outputs of the MVFF, ${\widehat{\mathbf{x}}}^{*}\left(k\right|k-1)$ and ${\mathbf{P}}^{*}\left(k\right|k-1)$, are obtained, all the PDA filters in the main filter are reset using:

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}^j\left(k\right|k)& ={\widehat{\mathbf{x}}}^{*}\left(k\right|k-1),\hfill \end{array}$$

(39)

$$\begin{array}{cc}\hfill {\mathbf{P}}^j\left(k\right|k)& ={\mathbf{P}}^{*}\left(k\right|k-1),(j=1,2,3,4).\hfill \end{array}$$

(40)

The overall HIPFF algorithm is summarized in Algorithm 1.    

Algorithm 1: HIPFF

5. Simulation Results

In this section, the simulation results are presented to demonstrate the performance of the HIPFF. In the simulation, the 2D positions and velocities of a preceding vehicle were estimated using both the HIPFF and the IMMPDAF. The simulation scenario was as follows. Preceding and host vehicles were traveling at the same velocity on the highway. Subsequently, the preceding vehicle suddenly decelerated, and eventually stopped. The FCW system of the host vehicle estimated the position and velocity of the decelerating preceding vehicle. Various values of the initial velocities and relative distance of the vehicles were tested.

Radar clutters (false measurements) were generated independently, and were uniformly distributed in a square centered at the true measurement. The area of the square, $\overline{V}$, was computed using:

$$\begin{array}{cc}\hfill \overline{V}& =N_F/\lambda ,\hfill \\ \hfill N_F& ={\left[10\pi \gamma \right|S\left(k\right)|}^{1/2}{\lambda +1]}^{-},\hfill \end{array}$$

(41)

where $N_F$ is the number of false measurements and $\lambda$ is the clutter density. In addition, the notation ${[·]}^{-}$ corresponds to “rounded down to the nearest integer”, and $S\left(k\right)$ was obtained using a standard Kalman filter under the assumption of an ideal (no clutters) environment [17].

Automotive radars can detect objects within a field of view (FOV). In the simulation, it was assumed that a Delphi Electronically Scanning Radar (ESR) was used for the FCW system. The Delphi ESR is an automotive frequency modulated continuous wave (FMCW) radar. The maximum detection range and FOV of the Delphi ESR in the long-range mode are 174 m and ${20}^{\circ }$, respectively. In addition, the maximum detection range and FOV in the mid-range mode are 60 m and ${90}^{\circ }$, respectively.

The sampling interval, target detection probability, gate probability, and gate threshold were set as $T=0.1$ s, $P_D=0.9$, $P_G=0.99$, and $\gamma =9.21$, respectively. The noise variances of the range/bearing measurements were set as ${\sigma }_r=0.25$ m and ${\sigma }_{theta}=1.5{}^{\circ }$ [9,10]. Trajectories of preceding vehicle were generated using the DWNA model (1) with the standard deviation of process noise, ${\sigma }_w=0.08$. For the four motion models, $M_1$–$M_4$, used for IMM estimation, the standard deviations were set to 1, 1, 10, and $0.1$, respectively, and the horizon size of the MVFF was set as $N=4$.

The performance of the estimation algorithms was evaluated using the root-mean-square position error (RMSPE) and the root-mean-square velocity error (RMSVE), which can be calculated using:

$$\begin{array}{cc}\hfill \mathbf{RMSPE}\left(k\right)& =\frac{1}{N_{MC}}\sum _{i=1}^{N_{MC}}\sqrt{{(x\left(k\right)-{\widehat{x}}^i\left(k\right))}^2+{(y\left(k\right)-{\widehat{y}}^i\left(k\right))}^2},\hfill \end{array}$$

(42)

$$\begin{array}{cc}\hfill \mathbf{RMSVE}\left(k\right)& =\frac{1}{N_{MC}}\sum _{i=1}^{N_{MC}}\sqrt{{(v_x\left(k\right)-{\widehat{v}}_x^i\left(k\right))}^2+{(v_y\left(k\right)-{\widehat{v}}_y^i\left(k\right))}^2},\hfill \end{array}$$

(43)

where $N_{MC}$ is the number of Monte Carlo (MC) simulations; $\left(x\right(k),y(k\left)\right)$ and $(v_x\left(k\right),v_y\left(k\right))$ are the true 2D positions and velocities, respectively; and $({\dot{x}}^i\left(k\right),{\dot{y}}^i\left(k\right))$ and $({\widehat{v}}_x^i\left(k\right),{\widehat{v}}_y^i\left(k\right))$ are the estimated 2D positions and velocities obtained in the i-th MC run, respectively. The RMSPE and RMSVE values were computed using 1000 MC runs. In addition, the percentage of lost track among the 1000 tracks was computed. A lost track was determined if the mode probability of $M_1$ exhibited the highest value for more than 20% of the simulation time.

Figure 2 and Figure 3 show the simulation results of a successful track case when the initial distance between the preceding and host vehicles along the x-axis was $d_{0,x}$ = 100 m, and the initial velocities along the x-axis was $v_{0,x}=80$ km/h. The image revealed that the IMMPDAF did not diverge and successfully tracked the preceding vehicle under this condition. The preceding vehicle traveled at a velocity of 80 km/h for 2 s, after which it rapidly decelerated for 2 s and eventually stopped. The IMMPDAF did not always diverges and sometimes exhibited a successful track. However, under rapid deceleration, the IMMPDAF diverged often. If failures of IMMPDAF did not occur, the assisting MVFF did not operate. Subsequently, the HIPFF operated like a pure IMMPDAF and exhibited the same results as the IMMPDAF, as shown in Figure 2a–c. Figure 2d shows that a reset by the assisting MVFF did not occur. In addition, Figure 3a–d show the true/estimated positions and velocities as a function of time.

Figure 4 and Figure 5 show the simulation results of a divergent track case, where the IMMPDAF lost the preceding vehicle and the RMSPE of the IMMPDAF diverged. In this simulation, the initial distance between the preceding and host vehicles was $d_{0,x}=120$ m, and the initial velocity of the vehicles was $v_{0,x}=100$ km/h. The preceding vehicle traveled at a velocity of 100 km/h for 2 s, and rapidly decelerated for $2.5$ s and eventually stopped. In Figure 4, the HIPFF did not diverge and successfully tracked the preceding vehicle. The RMSPE and RMSVE values of the HIPFF increased initially, after which it decreased owing to the reset by the MVFF (Figure 4b,c). Figure 4d shows that the resets occurred three times. Figure 5a–d show the true/estimated positions and velocities as a function of time.

Table 1. Monte Carlo (MC) simulation results under the condition of $v_{x,0}=120$ km/h, $d_{x,0}=140$ m.

Clutter Level	Heavy		Medium
	IMMPDAF	HIPFF	IMMPDAF	HIPFF
Averaged RMSPE (m)	24.25	4.98	21.89	4.59
Averaged RMSVE (km/h)	53.98	33.94	49.39	47.23
Percentage of divergent track	95.4	0	91.7	2.7

,

Table 2. MC simulation results under condition of $v_{x,0}=100$ km/h, $d_{x,0}=120$ m.

Clutter Level	Heavy		Medium
	IMMPDAF	HIPFF	IMMPDAF	HIPFF
Averaged RMSPE (m)	22.21	4.29	19.29	3.7
Averaged RMSVE (km/h)	47.06	30.93	41.68	38.1
Percentage of divergent track	92.4	0	87.6	0.6

and

Table 3. MC simulation results under condition of $v_{x,0}=80$ km/h, $d_{x,0}=100$ m.

Clutter Level	Heavy		Medium
	IMMPDAF	HIPFF	IMMPDAF	HIPFF
Averaged RMSPE (m)	19.39	3.63	14.76	2.74
Averaged RMSVE (km/h)	39.15	28.2	31.37	29.49
Percentage of divergent track	88.9	0	75	1.7

show the MC simulation results under two different clutter levels: heavy and medium. $\lambda =5\times 10$${}^{-2}$ and $\lambda =1\times 10$${}^{-2}$ were used for the heavy and medium clutter conditions, respectively. In addition, three different initial distance and velocities were tested. Table 1, Table 2 and Table 3 show the time-averaged RMSPE and RMSVE values, and the percentages of the divergent tracks. The proposed HIPFF exhibited a higher accuracy than IMMPDAF in terms of the averaged RMSPE and RMSVE values. Moreover, the HIPFF was significantly more reliable than the IMMPDAF in terms of the percentage of divergent tracks. This demonstrates that the HIPFF is significantly more accurate and reliable than the conventional IMMPDAF under severe conditions.

6. Conclusions

This study proposed a novel state estimation algorithm, namely hybrid IMM-PDA/FIR filter (HIPFF), for forward collision warning (FCW) systems based on automotive radars. The proposed algorithm utilized interacting multiple model probabilistic data association filter (IMMPDAF) as the main filter, and a finite impulse response (FIR) filter (i.e., minimum variance FIR filter (MVFF)) for recovering the main filter from failures. Under a rapidly decelerating preceding vehicle condition, the simulation results revealed that HIPFF exhibited significantly accurate and reliable estimation of position/velocity, compared to that of the pure IMMPDAF. Therefore, the HIPFF algorithm is expected to exhibit improved performance for automotive radar-based FCW systems.