State Estimation for Cooperative Lateral Vehicle Following Using Vehicle-to-Vehicle Communication

Abstract

A cooperative state estimation framework for automated vehicle applications is presented and demonstrated via simulations, the estimation framework is used to estimate the state of a lead and following vehicle simultaneously. Recent developments in the field of cooperative driving require novel techniques to ensure accurate and stable vehicle following behavior. Control schemes for the cooperative control of longitudinal and lateral vehicle dynamics generally require vehicle state information about the lead vehicle, which in some cases cannot be accurately measured. Including vehicle-to-vehicle communication in the state estimation process can provide the required input signals for the practical implementation of cooperative control schemes. This study is focused on demonstrating the benefits of using vehicle-to-vehicle communication in the state estimation of a lead and following vehicle via simulations. The state estimator, which uses a cascaded Kalman filtering process, takes the operating frequencies of different sensors into account in the estimation process. Simulation results of three different driving scenarios demonstrate the benefits of using vehicle-to-vehicle communication as well as the attenuation of measurement noise. Furthermore, in contrast to relying on low frequency measurement data for the input signals of cooperative control schemes, the state estimator provides a state estimate at every sample.

1. Introduction

With the implementation of communication abilities in mass production vehicles [1], the realization of cooperative driving features in commercial vehicles is near. Cooperative driving is a form of automated driving where vehicles exploit communication with each other or with infrastructure [2], via vehicle-to-vehicle (V2V) communication, vehicle-to-infrastructure (V2I) communication, or a combination of both. The most commonly known application of cooperation among road vehicles is cooperative adaptive cruise control (CACC) [3], which is an enhanced version of adaptive cruise control (ACC) using information received through V2V communication. CACC can result in improvements in safety and ride comfort [4], increased traffic throughput [3], and fuel savings up to 20% for commercial vehicles [5].

A wide variety of communication structures and control approaches have been proposed for CACC [2,3], varying from simple uni-directional to more complex multi-directional communication solutions. In addition, cooperation among vehicles can be used for exploration [6], traffic light scheduling [7], and path planning [8]. Longitudinal and lateral cooperative driving is realized via decoupled [9,10,11] and combined approaches [12], where the latter is a preferred design approach because of the coupled longitudinal and lateral vehicle dynamics [12]. Control of lateral vehicle motions can be divided into path following and trajectory following [12]. Path following is a time independent spatial approach, and trajectory following relies on a time dependent signal, imposing requirements on the velocity and acceleration. Platooning is a term that is used for vehicles driving together (cooperating), typically in the form of a string of vehicles following each other on a short distance. For platooning, where the short inter-vehicle distance can significantly limit the view [13], a vehicle following approach is preferred [12]. Cooperative driving technologies mostly use a combination of on-board sensor information (e.g., radar) and V2V communicated information. With use of V2V communication and on-board sensors, preceding vehicles can be accurately followed [14]. It is shown in [15] that the localization accuracy can be improved if more vehicles are equipped with V2V communication. However, a factor limiting the performance of automated drivign features is the available hardware [16]. For example, current ACC applications require sensor update rates from $10\mathrm{Hz}$ to $20\mathrm{Hz}$, but future safety applications may require update rates up to $50\mathrm{Hz}$ [16]. Applying a control approach similar to [12] requires detailed information of state components of the lead vehicle, which is typically not measurable from the following vehicle. In the remainder of the work, the lead vehicle is referred to as the target vehicle and the following vehicle is referred to as the host vehicle. The required state information for control approaches like presented in [12] can be obtained via V2V communication, and an estimator can be used to improve the signal quality and the continuity of the signals [15].

Common dynamic models used for vehicle tracking range from simple kinematic models to more complex models like the single-track model [16]. One of the determining factors of the model complexity is the specific state components that need to be available for the considered controller. Some of these state components may not be directly measurable and therefore need to be estimated. Bayuwindra [12] presented an orientation error observer based on an extended unicycle model. Ploeg et al. [17] presented a state estimator for the longitudinal acceleration of the preceding vehicle in order to (partly) maintain the favorable string stability properties during persistent loss of communication. A Singer acceleration model is a model where the acceleration is described as a first-order system with stochastic disturbances, and such a model has been adopted in [17], with position and velocity measurements obtained from a radar sensor [17]. A similar approach is presented in [18], but in [18] an adaptive Kalman filter has been used, improving the performance compared to using the Singer acceleration model from [17] via a continuously updated adaptive acceleration mean. Wei et al. [14] presented a longitudinal and lateral vehicle following control approach using only a radar and V2V communication, where the heading, acceleration, and yaw rate are communicated. The host vehicle dynamics are estimated using a single-track model [14]. Ess et al. [19] presented an object detection and tracking method using vision sensors. A kinematic steering model is used to track vehicles, but the low localization accuracy increases the difficulty of generating vehicle trajectories [19]. A survey paper [20] on vision based vehicle detection and tracking reveals that (extended) Kalman filtering and particle filtering are common approaches to predict vehicle motions from vision data. In addition, the use of Kalman filters applied in different applications is widely covered in the literature, e.g., the estimation of battery state of charge and capacity [21,22,23]. Finally, variations of a constant acceleration model with yaw rate have been used in [24] for tracking incoming traffic in urban scenarios.

In most sampled estimation frameworks, measurements are considered to be available at every sample. However, in practice this is often not the case, especially in multi-sensor set-ups. In the design estimation of the framework the different sensor update rates should be taken into account. In the literature, examples can be found of systems where different operating frequencies are present [25,26,27]. In some studies the issues of multi-rate sensing are discussed in combination with time delays ([28]) and inter-sample measurements ([26]). In [27], a multi-rate setup is considered in the estimation and control of a vehicle. The multi-rate Kalman filter estimate is only updated with a measurement if a new measurement is available, which in the case of [27] is on a fixed multiple of the sampling time. In between the measurement updates the estimate is solely based on the prediction step of the Kalman filter [27]. The authors of [29] discuss the fusion of radar and camera measurements; the estimates of parallel Kalman filters for the different sensors are combined via Bayesian estimation. The method presented in [29] could also be used to deal with multi-rate issues. In [30], two fusion methods are discussed; measurement fusion and state fusion. In case of a multi-rate sensor set-up, both methods could be implemented. One method of measurement fusion is based on the augmentation of the measurement vector and the observation matrix [30]. The observation matrix relates the measurements to the system state components. When multiple measurements are available the measurement vector and observation matrix can be increased in size accordingly [30]. One of the state fusion methods, which is also known as track-to-track fusion, requires multiple estimators to run simultaneously, and when multiple estimates are close together, they can be fused into one state estimate [30]. In [31], the set of measurements considered includes (time-stamped) raw radar and lidar data and bounding boxes obtained through a vision system. Therefore, the size of the measurement set is time-varying, resulting in a measured fusion based approach similar to an example discussed in [30]. In [28], a multi-sensor set-up is considered where the Kalman filter is operating at a frequency over 50 times higher than the highest measurement frequency. Similarly to methods discussed in [25,26,30,31], the estimator in [28] is only performing a measurement update when a new measurement is available. Another possibility is to introduce a multi-rate estimator as presented in [21]; two decoupled estimators are operating at a different frequency improving the model accuracy and stability.

From the material presented in Section 1 thus far, it can be concluded that the longitudinal and lateral control of platoons is widely investigated. However, in cooperative technologies the inputs are typically assumed to be continuously available and of sufficient quality, which usually is not the case in practical scenarios. Even in papers where the use of V2V communication for state estimation or control is discussed [16,32,33], practical impairments such as limited update rates and high measurement noise are not always included in the analysis.

The goal of this study is to present a cooperative state estimation framework for automotive applications. The study is focused on demonstrating the benefits of using V2V communication in the estimation process via simulations. The vehicle dynamics and (virtual) measurements considered in this study are modeled in a simulation environment. Onboard sensing is used in combination with measurements received through V2V communication to estimate the state of a host and target vehicle via a cascaded Kalman filtering approach. The state estimate resulting from the framework presented in this study can be used in a longitudinal and lateral platooning framework such as presented in [12]. The effect of the communication frequency on the estimation performance is analyzed via a root-mean-square error analysis. For this study perfect vehicle following behavior and a constant time gap are considered. The state estimator is located on the host vehicle, and onboard sensors from the host vehicle are used, such as a radar and odometer, in combination with measurements from a target vehicle received via V2V communication. The effect of different operating frequencies for the different sensors is taken into account in the estimator design. The performance of the state estimator is analyzed via simulations and error analyses, where realistic measurement noise and sensor operating frequencies are used. The framework presented in this study can easily be modified for a different sensor set-up.

The remainder of this study is organized as follows. In Section 2, the methodology is discussed, consisting of the sensor set-up, driving scenario, vehicle and measurement model, state estimator design, and the selection of the estimator setting. The simulation results and discussion are presented in Section 3 followed by the conclusions of this study in Section 4.

2. Methodology

Multi-vehicle state estimation is realized with cascaded Kalman filtering using sensors located on the host vehicle in combination with V2V communicated measurements from the preceding vehicle. First, the sensor set-up considered in this study is discussed. Second, the dynamic model representing the vehicles is discussed, including the available sensor signals. Finally, the design and selection of the process noise values of the state estimator is discussed in detail.

2.1. Sensor Set-Up

In the i-CAVE project [34] two Renault Twizys have been modified such that autonomous and cooperative driving features can be implemented and tested. Considering the implementation of the framework onto a vehicle set-up in future work, these vehicles define the available sensors and sensor signals considered in this study. The vehicles are shown in Figure 1, where the labels host and target refer to the following vehicle and lead vehicle, respectively. Each vehicle is equipped with its own set of sensors and a WiFi-P module [35], facilitating communication between the two vehicles. An overview of the relevant sensors and the respective operating frequencies is listed in

Table 1. List of sensors considered in this study, including the respective operating frequencies.

Vehicle	Sensor	Operating Frequency
Host	IMU	100 Hz
	Odometer	100 Hz
	GPS	5.0 Hz
	Radar	14.3 Hz
Target	IMU	25 Hz
	Odometer	25 Hz
	GPS	1.0 Hz

. Furthermore, the real-time operating system of both vehicles is running at a base frequency of $100\mathrm{Hz}$.

2.2. Dynamic Model and Measurements

The state estimator presented in this study is based on a Kalman filter framework [37]. Such a framework requires a model of the vehicle dynamics and the considered measurements. First, the selected model and the equations of motion in continuous time are presented, followed by the transition into state-space formulation. Next, the discretization of the model is discussed such that it can be used in a sampled application.

2.2.1. Continuous Time Equations of Motion and System Output

The dynamics of each vehicle are modeled using a unicycle model [12,38]. A schematic drawing of the two unicycle models including the relevant degrees of freedom is shown in Figure 2. The subscripts $\mathrm{t}$ and $\mathrm{h}$ represent the target and host vehicle, respectively. Furthermore, the X- and Y-coordinates define the position in a global frame, $\theta$ is the global heading angle and $vs.$ is the longitudinal vehicle velocity. The global X- and Y-coordinates considered in this study correspond with the east and north direction of an earth-centered, earth fixed coordinate system, respectively. The global heading angle is the angle between the X- and Y-axis and indicated in Figure 2. The global X- and Y-position is defined as

$${\dot{X}}_i=v_{i}cos\left({\theta }_i\right),i=\{\mathrm{t},\mathrm{h}\}$$

(1)

$${\dot{Y}}_i=v_{i}sin\left({\theta }_i\right),i=\{\mathrm{t},\mathrm{h}\}$$

(2)

respectively. Note that the lateral velocity is assumed to be zero. The longitudinal acceleration $a_i$ of the vehicle is

$$a_i={\dot{v}}_i,i=\{\mathrm{t},\mathrm{h}\}$$

(3)

and the derivative of the heading angle ${\theta }_i$ is the yaw rate ${\dot{\theta }}_i$, $i=\left\{t,h\right\}$. Finally, the longitudinal acceleration and yaw acceleration of the unicycle are considered constant, giving

$${\dot{a}}_i=\ddot{v}=0+w_{\mathrm{a},i},i=\{\mathrm{t},\mathrm{h}\}$$

(4)

$${\ddot{\theta }}_i=0+w_{\dot{\theta },i},i=\{\mathrm{t},\mathrm{h}\}$$

(5)

where $w_{\dot{\theta }}$ and $w_{\mathrm{a}}$ are zero-mean Gaussian white noise on the yaw acceleration and longitudinal jerk. These noise terms will be used in the Kalman filter, which is discussed in Section 2.3. The total system dynamics of a single unicycle is thus described with (1) to (5).

The system output (measurements) is determined by the available sensors, which are listed in Table 1. In this study indirect measurements such as the GPS velocity are not considered with the exception of the heading angle obtained from the GPS sensor. The heading angle is required for observability of the system, which is elaborated on in the end of Section 2.2.2. Except for the measurements of the radar sensor, all measurements are direct state measurements. The IMU sensor measures the longitudinal accelerations and the yaw rate of the vehicle. The odometer measures the motor speed, which is translated to a vehicle speed using a fixed gear ratio and the effecting rolling radius, and is therefore also considered as a direct state measurement. The GPS sensors measures the global X- and Y-position and the global heading angle, the latter is pre-filtered. The position measurements of the GPS sensor correspond with the centers gravity of the respective vehicles and the initial position of the host vehicle is used to null the measurements, therefore creating the origin $\left(X_{\mathrm{h}}\left(t=0\right),Y_{\mathrm{h}}\left(t=0\right)\right):=\left(0,0\right)$. The radar sensor is located on the front of the host vehicle and measures the relative speed and distance to the rear bumper of the target vehicle. The relative distance r is defined as

$$r=\sqrt{{\left(X_{\mathrm{t}}-X_{\mathrm{h}}\right)}^2+{\left(Y_{\mathrm{t}}-Y_{\mathrm{h}}\right)}^2}-L$$

(6)

where L is the length the vehicle and the X- and Y-positions of the host and target vehicle correspond with their center of gravity. Note that the expression in (6) is a simplification where the difference in heading angle between the two vehicles is ignored. The effects of this simplification can be included in the measurement noise, which also accounts for the uncertainty of the measurement location on the target vehicle’s bumper. The relative speed $\dot{r}$ is defined as

$$\dot{r}=\left(v_{\mathrm{t}}-v_{\mathrm{h}}\right)cos\left({\theta }_{\mathrm{r}}\right)$$

(7)

where ${\theta }_{\mathrm{r}}$ is the azimuth angle measured by the radar. The relative speed is derived from a constant radius cornering assumption where the vehicles drive with the same curvature and have no relative lateral offset with respect to the desired trajectory. In this study the azimuth angle ${\theta }_{\mathrm{r}}$ is not considered as a measurement because the measurement quality cannot be defined [39].

2.2.2. State-Space Formulation

In order to adopt a state-space notation as required for practical cascaded Kalman filtering, a cascading system description is considered, allowing us to transform the system dynamics into state-space form. The schematics of the cascading systems approach is shown in Figure 3, where ${\mathbf{w}}_1$ and ${\mathbf{w}}_2$ are process noise vectors, ${\mathbf{y}}_1$ and ${\mathbf{y}}_2$ measurement vectors and ${\mathbf{x}}_1$ a state vector. It should be noted that adopting the cascading system description influences the propagation of process noise through the system, hence the term cascaded Kalman filtering. The process noise ${\mathbf{w}}_1$ entering system 1, the yaw dynamics, influences the state ${\mathbf{x}}_1$ and measurements ${\mathbf{y}}_1$. As shown in Figure 3, the state ${\mathbf{x}}_1$ enters system 2, the unicycle dynamics, resulting in an indirect injection of the process noise ${\mathbf{w}}_1$ filtered by the dynamics of system 1 into system 2. Therefore, the process noise term ${\mathbf{w}}_1$ is propagated to system 2 via system 1, influencing system 2 as filtered noise. The system and output dynamics are described via the linear functions $f_1$, $f_2$, $h_1$ and non-linear function $h_2$ according to

$${\dot{\mathbf{x}}}_j=f_j\left({\mathbf{x}}_j,{\mathbf{u}}_j,{\mathbf{w}}_j\right),j=\left\{1,2\right\}$$

(8)

$$\mathbf{y}=h\left(\mathbf{x},\mathbf{u},\mathbf{v}\right),j=\left\{1,2\right\}$$

(9)

where $\mathbf{x}$ is the state vector, $\mathbf{u}$ the input vector, $\mathbf{w}$ the process noise vector, $\mathbf{y}$ the measurement vector, $\mathbf{v}$ the measurement noise vector and j refers to the system number corresponding with Figure 3. The state vectors

$${\mathbf{x}}_1={\left[\begin{array}{cccc}{\theta }_{\mathrm{t}}& {\dot{\theta }}_{\mathrm{t}}& {\theta }_{\mathrm{h}}& {\dot{\theta }}_{\mathrm{h}}\end{array}\right]}^T,$$

(10)

$${\mathbf{x}}_2={\left[\begin{array}{cccccccc}{X}_{\mathrm{t}}& Y_{\mathrm{t}}& v_{\mathrm{t}}& a_{\mathrm{t}}& X_{\mathrm{h}}& Y_{\mathrm{h}}& v_{\mathrm{h}}& a_{\mathrm{h}}\end{array}\right]}^T,$$

(11)

measurement vectors

$${\mathbf{y}}_1={\left[\begin{array}{cccc}{\theta }_{\mathrm{t}}& {\dot{\theta }}_{\mathrm{t}}& {\theta }_{\mathrm{h}}& {\dot{\theta }}_{\mathrm{h}}\end{array}\right]}^T,$$

(12)

$${\mathbf{y}}_2={\left[\begin{array}{cccccccccccc}{X}_{\mathrm{t}}& Y_{\mathrm{t}}& v_{\mathrm{t}}& a_{\mathrm{t}}& {\dot{\theta }}_{\mathrm{t}}& X_{\mathrm{h}}& Y_{\mathrm{h}}& v_{\mathrm{h}}& a_{\mathrm{h}}& {\dot{\theta }}_{\mathrm{h}}& r& \dot{r}\end{array}\right]}^T$$

(13)

and process noise vectors

$${\mathbf{w}}_1={\left[\begin{array}{cc}{w}_{\dot{\theta },\mathrm{t}}& w_{\dot{\theta },\mathrm{h}}\end{array}\right]}^T,$$

(14)

$${\mathbf{w}}_2={\left[\begin{array}{cc}{w}_{a,\mathrm{t}}& w_{a,\mathrm{h}}\end{array}\right]}^T.$$

(15)

are introduced. In state-space form, the system dynamics are described as

$${\dot{\mathbf{x}}}_j={\mathbf{A}}_j{\mathbf{x}}_j+{\mathbf{B}}_j{\mathbf{u}}_j+{\mathbf{E}}_j{\mathbf{w}}_j,j=\left\{1,2\right\}$$

(16)

$${\mathbf{y}}_j={\mathbf{C}}_j{\mathbf{x}}_j+{\mathbf{D}}_j{\mathbf{u}}_j+{\mathbf{J}}_j{\mathbf{v}}_j,j=\left\{1,2\right\}$$

(17)

where $\mathbf{A}$ is the system matrix, $\mathbf{B}$ the input matrix and $\mathbf{E}$ the process noise matrix. Furthermore, $\mathbf{C}$ is the observation matrix, $\mathbf{D}$ the observed input matrix, and $\mathbf{J}$ and $\mathbf{v}$ are the measurement noise matrix and vector. The explicit expressions for $\mathbf{A}$, $\mathbf{B}$, $\mathbf{u}$ and $\mathbf{E}$ are presented in Appendix A.1. It should be noted that via the adoption of the cascading systems approach, Figure 3, the system matrix ${\mathbf{A}}_2$ representing the unicycle dynamics is a function of the state vector ${\mathbf{x}}_1$, giving ${\mathbf{A}}_2:={\mathbf{A}}_2\left({\mathbf{x}}_1\right)$.

The system output is defined in (12) and (13) using the relative distance and speed from (6) and (7). The matrices $\mathbf{C}$, $\mathbf{D}$ and $\mathbf{J}$ are obtained by calculating the Jacobian matrices of $h\left(\mathbf{x},\mathbf{u},\mathbf{v}\right)$ with respect to $\mathbf{x}$, $\mathbf{u}$ and $\mathbf{v}$. The measurement noise vectors are described as

$${\mathbf{v}}_1={\left[\begin{array}{cccc}{\mathrm{v}}_{{\theta }_{\mathrm{t}}}& {\mathrm{v}}_{{\dot{\theta }}_{\mathrm{t}}}& {\mathrm{v}}_{{\theta }_{\mathrm{h}}}& {\mathrm{v}}_{{\dot{\theta }}_{\mathrm{h}}}\end{array}\right]}^T,$$

(18)

$${\mathbf{v}}_2={\left[\begin{array}{cccccccccc}{\mathrm{v}}_{X_{\mathrm{t}}}& {\mathrm{v}}_{Y_{\mathrm{t}}}& {\mathrm{v}}_{v_{\mathrm{t}}}& {\mathrm{v}}_{a_{\mathrm{t}}}& {\mathrm{v}}_{X_{\mathrm{h}}}& {\mathrm{v}}_{Y_{\mathrm{h}}}& {\mathrm{v}}_{v_{\mathrm{h}}}& {\mathrm{v}}_{a_{\mathrm{h}}}& {\mathrm{v}}_r& {\mathrm{v}}_{\dot{r}}\end{array}\right]}^T.$$

(19)

Again, the explicit expressions for $\mathbf{C}$, $\mathbf{D}$, $\mathbf{J}$ and $\mathbf{v}$ are presented in Appendix A.1.

As mentioned in Section 2.2.1, the heading angles obtained from pre-filtered measurement data are included in the measurement vector ${\mathbf{y}}_1$ to ensure observability. A system is observable if for any initial state and any final time the initial state can be uniquely determined from the measured input and output signals [37]. Whether a system is observable can be determined by analyzing the rank of the observability matrix $\mathcal{O}$. The observability matrix is defined as

$${\mathcal{O}}_j=\left[\begin{array}{c}{\mathbf{C}}_j\\ {\mathbf{C}}_j{\mathbf{A}}_j\\ ⋮\\ {\mathbf{C}}_j{\mathbf{A}}_j^{n-1}\end{array}\right]$$

(20)

and observability is achieved if $\mathrm{rank}\left({\mathcal{O}}_j\right)=n_j$ where $n_j$ is the length of the state vector ${\mathbf{x}}_j$ [37]. Since ${\mathbf{y}}_1$ and ${\mathbf{y}}_2$ contain all state components described in ${\mathbf{x}}_1$ and ${\mathbf{x}}_2$, respectively, it is evident that both system 1 and system 2 are observable ($\mathrm{rank}\left({\mathbf{C}}_j\right)=n_j$).

2.2.3. Model Discretization

Finally, because the vehicles are operated using a discrete-time (sampled) platform, the expressions from (16) and (17) need to be discretized. From hereon, all equations are expressed in discrete-time, and are therefore a function of sample number k. The state-space expressions from (16) and (17) are expressed in discrete-time as

$${\mathbf{x}}_{j,k+1}={\mathbf{F}}_{j,k}{\mathbf{x}}_{j,k}+{\mathbf{G}}_{j,k}{\mathbf{u}}_{j,k}+{\mathbf{M}}_{j,k}{\mathbf{w}}_{j,k},j=\left\{1,2\right\}$$

(21)

$${\mathbf{z}}_{j,k}={\mathbf{H}}_{j,k}{\mathbf{x}}_{j,k}+{\mathbf{L}}_{j,k}{\mathbf{u}}_{j,k}+{\mathbf{N}}_{j,k}{\mathbf{v}}_{j,k},j=\left\{1,2\right\}$$

(22)

In the expressions in (21) and (22) ${\mathbf{F}}_k$ is the system matrix, ${\mathbf{G}}_k$ the input matrix, ${\mathbf{M}}_k$ the process noise matrix, ${\mathbf{H}}_k$ the observation matrix, ${\mathbf{L}}_k$ the observed input matrix and ${\mathbf{N}}_k$ the measurement noise matrix. The discrete-time form of the relevant matrices are obtained according to [37]

$$\begin{array}{c}{\mathbf{F}}_k={\left.e^{\mathbf{A}{T}_{\mathrm{s}}}\right|}_{t=k{T}_{\mathrm{s}}},{\mathbf{G}}_k={\left.\left({\int }_{\tau =0}^{T_{\mathrm{s}}}{e}^{\mathbf{A}\tau }\mathrm{d}\tau \right)\mathbf{B}\right|}_{t=k{T}_{\mathrm{s}}},{\mathbf{M}}_k={\left.\left({\int }_{\tau =0}^{T_{\mathrm{s}}}{e}^{\mathbf{A}\tau }\mathrm{d}\tau \right)\mathbf{E}\right|}_{t=k{T}_{\mathrm{s}}},\hfill \\ \hfill {\mathbf{H}}_k={\left.\mathbf{C}\right|}_{t=k{T}_{\mathrm{s}}},{\mathbf{L}}_k={\left.\mathbf{D}\right|}_{t=k{T}_{\mathrm{s}}},{\mathbf{N}}_k={\left.\mathbf{J}\right|}_{t=k{T}_{\mathrm{s}}}.\end{array}$$

(23)

The matrices in (23) are evaluated at the current time $t=k{T}_{\mathrm{s}}$ where $T_{\mathrm{s}}$ is the sampling time of the estimator (in this study $T_{\mathrm{s}}=0.010\mathrm{s}$). The discrete-time state vector, input vector and measurement vector are equivalent to their continuous-time counterparts, giving

$${\mathbf{x}}_{j,k}={\left.{\mathbf{x}}_j\right|}_{t=k{T}_{\mathrm{s}}},{\mathbf{u}}_{j,k}={\left.{\mathbf{u}}_j\right|}_{t=k{T}_{\mathrm{s}}},{\mathbf{w}}_{j,k}={\left.{\mathbf{w}}_j\right|}_{t=k{T}_{\mathrm{s}}},{\mathbf{z}}_{j,k}={\left.{\mathbf{y}}_j\right|}_{t=k{T}_{\mathrm{s}}},{\mathbf{v}}_{j,k}={\left.{\mathbf{v}}_j\right|}_{t=k{T}_{\mathrm{s}}}$$

(24)

for $j=\left\{1,2\right\}$, which evidently results in

$${\mathbf{x}}_{1,k}={\left[\begin{array}{cccc}{\theta }_{\mathrm{t},k}& {\dot{\theta }}_{\mathrm{t},k}& {\theta }_{\mathrm{h},k}& {\dot{\theta }}_{\mathrm{h},k}\end{array}\right]}^T,$$

(25)

$${\mathbf{x}}_{2,k}={\left[\begin{array}{cccccccc}{X}_{\mathrm{t},k}& Y_{\mathrm{t},k}& v_{\mathrm{t},k}& a_{\mathrm{t},k}& X_{\mathrm{h},k}& Y_{\mathrm{h},k}& v_{\mathrm{h},k}& a_{\mathrm{h},k}\end{array}\right]}^T,$$

(26)

$${\mathbf{z}}_{1,k}={\left[\begin{array}{cccc}{\theta }_{\mathrm{t},k}& {\dot{\theta }}_{\mathrm{t},k}& {\theta }_{\mathrm{h},k}& {\dot{\theta }}_{\mathrm{h},k}\end{array}\right]}^T,$$

(27)

$${\mathbf{z}}_{2,k}={\left[\begin{array}{ccccccccccc}{X}_{\mathrm{t},k}& Y_{\mathrm{t},k}& v_{\mathrm{t},k}& a_{\mathrm{t},k}& X_{\mathrm{h},k}& Y_{\mathrm{h},k}& v_{\mathrm{h},k}& a_{\mathrm{h},k}& {\overline{r}}_k& {\dot{r}}_k& \end{array}\right]}^T.$$

(28)

The algebraic expressions for ${\mathbf{F}}_k$, ${\mathbf{G}}_k$, ${\mathbf{M}}_k$, ${\mathbf{H}}_k$, ${\mathbf{L}}_k$ and ${\mathbf{N}}_k$ obtained by using (23) are presented in Appendix A.2.

2.3. State Estimator Design

The state estimation of the system consisting of the host and target vehicle is realized using a cascaded Kalman filter [37,40,41]. A Kalman filter is an estimator where the gain is chosen such that the estimation error covariance is minimized. In this section, the general equations of a Kalman filter are presented in combination with the amendments following from the cascading systems formulation. In addition, the use of multi-rate sensors in the estimation process is discussed.

2.3.1. General Kalman Filtering Process

The Kalman filter considered in this study is based on a discrete-time model representing the system dynamics, which in this case is the combination of a host and target vehicle. The dynamics from (21) and (22) are modeled with

$${\widehat{\mathbf{x}}}_{j,k+1}={\mathbf{F}}_{j,k}{\widehat{\mathbf{x}}}_{j,k}+{\mathbf{G}}_{j,k}{\mathbf{u}}_{j,k},j=\left\{1,2\right\}$$

(29)

$${\widehat{\mathbf{z}}}_{j,k}={\mathbf{H}}_{j,k}{\widehat{\mathbf{x}}}_{j,k}+{\mathbf{L}}_{j,k}{\mathbf{u}}_{j,k},j=\left\{1,2\right\}$$

(30)

where ${\widehat{\mathbf{x}}}_k$ is the state estimate and ${\widehat{\mathbf{z}}}_k$ the estimated measurement. Do note that from the cascading systems approach it follows that ${\mathbf{F}}_{2,k}\left({\widehat{\mathbf{x}}}_{1,k}\right)$. Because the model presented in Section 2.2 does not include any inputs, it is evident that ${\mathbf{u}}_k=\mathbf{0}$, allowing us to simplify the estimation model to

$${\widehat{\mathbf{x}}}_{j,k+1}={\mathbf{F}}_{j,k}{\widehat{\mathbf{x}}}_{j,k},j=\left\{1,2\right\}$$

(31)

$${\widehat{\mathbf{z}}}_{j,k}={\mathbf{H}}_{j,k}{\widehat{\mathbf{x}}}_{j,k},j=\left\{1,2\right\}.$$

(32)

Generally, a Kalman filtering process consists of two steps, a prediction and a correction step. Through the use of the cascading systems approach, in this study four steps are considered. The cascaded Kalman filtering process for a single time step is summarized with the four steps [37]:

(1)

State estimation and update of the state estimation covariance matrix for system 1, the yaw dynamics

The a priori state estimate ${\widehat{\mathbf{x}}}_{1,k+1|k}$ is obtained via the modeled system dynamics from (31), resulting in

$${\widehat{\mathbf{x}}}_{1,k+1|k}={\mathbf{F}}_{1,k}{\widehat{\mathbf{x}}}_{1,k|k},$$

(33)

after which the state estimation covariance ${\mathbf{P}}_{1,k|k}$ is updated according to

$${\mathbf{P}}_{1,k+1|k}={\mathbf{F}}_{1,k}{\mathbf{P}}_{1,k|k}{\mathbf{F}}_{1,k}^T+{\mathbf{M}}_{1,k}{\mathbf{Q}}_{1,k}{\mathbf{M}}_{1,k}^T,$$

(34)

resulting in the a priori state estimation covariance ${\mathbf{P}}_{1,k+1|k}$. In (34) ${\mathbf{Q}}_{1,k}$ is the process noise covariance.

(2)

Measurement correction and update of the state estimation covariance matrix for system 1 (only executed when a new measurement is available in ${\mathbf{z}}_{1,k}$)

The estimated output is obtained via

$${\widehat{\mathbf{z}}}_{1,k}={\mathbf{H}}_{1,k}{\widehat{\mathbf{x}}}_{1,k+1|k}$$

(35)

followed by a calculation of the Kalman gain ${\mathbf{K}}_{1,k}$ according to

$${\mathbf{K}}_{1,k}={\mathbf{P}}_{1,k+1|k}{\mathbf{H}}_{1,k}^T{\left({\mathbf{H}}_{1,k}{\mathbf{P}}_{1,k+1|k}{\mathbf{H}}_{1,k}^T+{\mathbf{N}}_{1,k}{\mathbf{R}}_{1,k}{\mathbf{N}}_{1,k}^T\right)}^{-1}.$$

(36)

Using the Kalman gain, the a posteriori state estimation covariance ${\mathbf{P}}_{1,k+1|k+1}$ is obtained using

$${\mathbf{P}}_{1,k+1|k+1}=\left(\mathbf{I}-{\mathbf{K}}_{1,k}{\mathbf{H}}_{1,k}\right){\mathbf{P}}_{1,k+1|k}{\left(\mathbf{I}-{\mathbf{K}}_{1,k}{\mathbf{H}}_{1,k}\right)}^T+{\mathbf{K}}_{1,k}{\mathbf{N}}_{1,k}{\mathbf{R}}_{1,k}{\mathbf{N}}_{1,k}^T{\mathbf{K}}_{1,k}^T$$

(37)

where ${\mathbf{R}}_{1,k}$ is the measurement noise covariance. Note that (37) can be simplified to ${\mathbf{P}}_{1,k+1|k+1}=\left(\mathbf{I}-{\mathbf{K}}_{1,k}{\mathbf{H}}_{1,k}\right){\mathbf{P}}_{1,k+1|k}$, both forms are described in the literature. Finally, a measurement correction on the state estimate is performed using

$${\widehat{\mathbf{x}}}_{1,k+1|k+1}={\widehat{\mathbf{x}}}_{1,k|k+1}+{\mathbf{K}}_{1,k}\left({\mathbf{z}}_{1,k}-{\widehat{\mathbf{z}}}_{1,k}\right),$$

(38)

resulting in the a posteriori state estimate ${\widehat{\mathbf{x}}}_{1,k+1|k+1}$. If there is no new measurement available in ${\mathbf{z}}_{1,k}$ at sample k, it follows that ${\widehat{\mathbf{x}}}_{1,k+1|k+1}={\widehat{\mathbf{x}}}_{1,k+1|k}$.

(3)

State estimation and update of the state estimation covariance for system 2, the unicycle dynamics

The a priori state estimate ${\widehat{\mathbf{x}}}_{2,k+1|k}$ and state estimation error covariance ${\mathbf{P}}_{2,k+1|k}$ are obtained via

$${\widehat{\mathbf{x}}}_{2,k+1|k}={\mathbf{F}}_{2,k}\left({\widehat{\mathbf{x}}}_{1,k+1|k+1}\right){\widehat{\mathbf{x}}}_{2,k|k},$$

(39)

$${\mathbf{P}}_{2,k+1|k}={\mathbf{F}}_{2,k}\left({\widehat{\mathbf{x}}}_{1,k+1|k+1}\right){\mathbf{P}}_{2,k|k}{\mathbf{F}}_{2,k}^T\left({\widehat{\mathbf{x}}}_{1,k+1|k+1}\right)+{\mathbf{M}}_{2,k}{\mathbf{Q}}_{2,k}{\mathbf{M}}_{2,k}^T.$$

(40)

(4)

Measurement correction and update of the state estimation error covariance for system 2

Similar to step (2), this step is only executed if ${\mathbf{z}}_{2,k}$ contains a new measurement. The estimated output ${\mathbf{z}}_{2,k}$ and the Kalman gain ${\mathbf{K}}_{2,k}$ are obtained using

$${\widehat{\mathbf{z}}}_{2,k}={\mathbf{H}}_{2,k}{\widehat{\mathbf{x}}}_{2,k+1|k},$$

(41)

$${\mathbf{K}}_{2,k}={\mathbf{P}}_{2,k+1|k}{\mathbf{H}}_{2,k}^T{\left({\mathbf{H}}_{2,k}{\mathbf{P}}_{2,k+1|k}{\mathbf{H}}_{2,k}^T+{\mathbf{N}}_{2,k}{\mathbf{R}}_{2,k}{\mathbf{N}}_{2,k}^T\right)}^{-1}.$$

(42)

Subsequently, the a posteriori state estimation error covariance is calculated as

$${\mathbf{P}}_{2,k+1|k+1}=\left(\mathbf{I}-{\mathbf{K}}_{2,k}{\mathbf{H}}_{2,k}\right){\mathbf{P}}_{2,k+1|k}{\left(\mathbf{I}-{\mathbf{K}}_{2,k}{\mathbf{H}}_{2,k}\right)}^T+{\mathbf{K}}_{2,k}{\mathbf{N}}_{2,k}{\mathbf{R}}_{2,k}{\mathbf{N}}_{2,k}^T{\mathbf{K}}_{2,k}^T$$

(43)

followed by the measurement correction

$${\widehat{\mathbf{x}}}_{2,k+1|k+1}={\widehat{\mathbf{x}}}_{2,k|k+1}+{\mathbf{K}}_{2,k}\left({\mathbf{z}}_{2,k}-{\widehat{\mathbf{z}}}_{2,k}\right)$$

(44)

where ${\widehat{\mathbf{x}}}_{2,k+1|k+1}={\widehat{\mathbf{x}}}_{2,k|k+1}$ if no new measurement is located in ${\mathbf{z}}_{2,k}$.

The process noise covariance and measurement noise covariance matrices are determined using the expected state and measurement noise:

$${\mathbf{Q}}_{1,k}=E\left\{{\mathbf{w}}_n{\mathbf{w}}_m^T\right\}=\left[\begin{array}{cc}{\sigma }_{w_{{\dot{\theta }}_{\mathrm{t}}}}^2& 0\\ 0& {\sigma }_{w_{{\dot{\theta }}_{\mathrm{h}}}}^2\end{array}\right]{\delta }_{\mathit{nm}}={\mathbf{Q}}_1,$$

(45)

$${\mathbf{Q}}_{2,k}=E\left\{{\mathbf{w}}_n{\mathbf{w}}_m^T\right\}=\left[\begin{array}{cc}{\sigma }_{w_{a_{\mathrm{t}}}}^2& 0\\ 0& {\sigma }_{w_{a_{\mathrm{h}}}}^2\end{array}\right]{\delta }_{\mathit{nm}}={\mathbf{Q}}_2,$$

(46)

$${\mathbf{R}}_{1,k}=E\left\{{\mathbf{v}}_n{\mathbf{v}}_m^T\right\}=\left[\begin{array}{cccc}{\sigma }_{{\theta }_{\mathrm{t},k}}^2& 0& 0& 0\\ 0& {\sigma }_{{\dot{\theta }}_{\mathrm{t},k}}^2& 0& 0\\ 0& 0& {\sigma }_{{\theta }_{\mathrm{t},k}}^2& 0\\ 0& 0& 0& {\sigma }_{{\dot{\theta }}_{\mathrm{h},k}}^2\end{array}\right]{\delta }_{\mathit{nm}}={\mathbf{R}}_1,$$

(47)

(48)

where ${\sigma }^2$ is the standard deviation of the respective signals defined in $\mathbf{w}$ and $\mathbf{v}$. Zero-mean Gaussian white noise is considered for both the process noise and measurement noise (however, note the previous comment about filtered process noise entering system 2), and because of the Kroneckerdelta function ${\delta }_{nm}$ the noise matrices do not depend on time (there is no correlation between different samples), giving ${\mathbf{Q}}_{1,k}={\mathbf{Q}}_1$, ${\mathbf{Q}}_{2,k}={\mathbf{Q}}_2$, ${\mathbf{R}}_{1,k}={\mathbf{R}}_1$ and ${\mathbf{R}}_{2,k}={\mathbf{R}}_2$ [37]. The power of the process and measurement noise is considered to be constant, yielding constant $\mathbf{Q}$ and $\mathbf{R}$ matrices. The measurement noise values described in ${\mathbf{R}}_1$ and ${\mathbf{R}}_2$ are obtained through experiments performed prior to this study. In

Table 2. List of the available measurements considered in this chapter. The noise levels are obtained via experiments and are expressed in terms of standard deviation (SD).

Sensor	Measurement Signal	Noise Level (SD)
IMU (host)	Longitudinal acceleration $a_{\mathrm{h}}$	$0.189\mathrm{m}/{\mathrm{s}}^2$
	Yaw rate ${\dot{\theta }}_{\mathrm{h}}$	$0.0138\mathrm{rad}/\mathrm{s}$
Odometer (host)	Longitudinal velocity $v_{\mathrm{h}}$	$0.0721\mathrm{m}/\mathrm{s}$
GPS (host)	Global X-position $X_{\mathrm{h}}$	$0.702\mathrm{m}$
	Global Y-position $Y_{\mathrm{h}}$	$0.702\mathrm{m}$
	Global heading angle ${\theta }_{\mathrm{h}}$	$0.0347\mathrm{rad}$
Radar (host)	Relative distance r	$0.0106\mathrm{m}$
	Relative speed $\dot{r}$	$0.138\mathrm{m}/\mathrm{s}$
IMU (target)	Longitudinal acceleration $a_{\mathrm{t}}$	$0.294\mathrm{m}/{\mathrm{s}}^2$
	Yaw rate ${\dot{\theta }}_{\mathrm{t}}$	$0.0139\mathrm{rad}/\mathrm{s}$
Odometer (target)	Longitudinal velocity $v_{\mathrm{t}}$	$0.0814\mathrm{m}/\mathrm{s}$
GPS (target)	Global X-position $X_{\mathrm{t}}$	$0.493\mathrm{m}$
	Global Y-position $Y_{\mathrm{t}}$	$0.493\mathrm{m}$
	Global heading angle ${\theta }_{\mathrm{t}}$	$0.0910\mathrm{rad}$

, a list of the measurements signals together with their respective noise levels is provided. The values for the noise levels in Table 2 are expressed as standard deviations. The matrices $\mathbf{Q}$ and $\mathbf{R}$ considered in this study are square and diagonal, giving $\mathbf{Q}={\mathbf{Q}}^T$ and $\mathbf{R}={\mathbf{R}}^T$. Furthermore, $\mathbf{Q}$ and $\mathbf{R}$ are considered positive definite.

2.3.2. Multi-Rate Sensing

Set-ups including a variety of sensors typically deal with different sensor update rates. In Table 1, it is shown that this is also the case for the sensor set-up considered in this study. Taking the different sensor update rates into account in the estimator design can increase the estimation performance. In this study it is assumed that all measurements are a multitude of the base frequency of the state estimator, and therefore the effect of inter-sample measurements is not considered. Furthermore, the effect of time delays is not considered in this work. From the literature presented in Section 1 it can be concluded that performing measurement updates only when new measurements are available is a common approach. Although this methodology results in a size-varying measurement vectors, observation matrices and measurement noise matrices, it is a straightforward approach which ensures measurements are only used once to update the state estimator. This method is implemented by removing the elements of ${\mathbf{z}}_k$, ${\mathbf{H}}_k$, and ${\mathbf{N}}_k$ which do not correspond with new measurements [25]. This is realized with the introduction of ${\mathbf{S}}_k$, which is a matrix of dimension $m_k\times n$ with $m_k$ the number of new measurements at sample k and n the total number of rows of ${\mathbf{H}}_k$ [25]. The elements of ${\mathbf{S}}_k$ are 1 at the positions $\left(1,s_1\right),\dots ,\left(m_k,s_{m_k}\right)$ and 0 elsewhere, with $s_1$ to $s_{m_k}$ corresponding with the row numbers of ${\mathbf{H}}_k$ belonging to the new measurements. Using the matrix ${\mathbf{S}}_k$ results in modified versions of the observation vectors ${\mathbf{z}}_k$, observation matrices ${\mathbf{H}}_k$, and measurement noise matrices ${\mathbf{N}}_k$:

$${\mathbf{z}}_{j,k}^{*}={\mathbf{S}}_{j,k}{\mathbf{z}}_{j,k},{\mathbf{H}}_{j,k}^{*}={\mathbf{S}}_{j,k}{\mathbf{H}}_{j,k},{\mathbf{N}}_{j,k}^{*}={\mathbf{S}}_{j,k}{\mathbf{N}}_{j,k},j=\left\{1,2\right\}.$$

(49)

Note that in (33) to (44) the vectors and matrices ${\mathbf{z}}_k,{\mathbf{H}}_k,{\mathbf{N}}_k$ should be replaced with ${\mathbf{z}}_k^{*},{\mathbf{H}}_k^{*},{\mathbf{N}}_k^{*}$. This methodology is similar to what is presented in [26], where all measurements which are unavailable are set to the measurement prediction, thus not resulting in any measurement corrections. However, setting the unavailable measurement equal to the prediction does result in a modified estimation error covariance matrix (see (35) to (37) and (41) and (43)).

One of the issues that may arise using the method resulting in (49), is that the majority of the measurement corrections, Equations (38) and (44), is based on one or a limited number of sensors. For example, considering a system with two equivalent sensors, of which sensor 1 operates at a significantly higher frequency in comparison to sensor 2, the state estimate will be mostly based on the information coming from sensor 1. This might lead to drift and/or large estimation errors of state components which are measured less frequently, either direct or indirect. In order to prevent large estimation errors, the weight of the measurement is adjusted with the ratio between the sampling frequency of Kalman filter $f_0$ and the sampling frequency of the sensor $f_s$. In essence, this approach normalizes the weight of a measurement based on the sensor sampling frequency. The weighting factor is implemented in the Kalman filter by multiplying the sensor standard deviation $\sigma$ with the ratio between the sampling frequency of the specific sensor and of the operating system, resulting in

$${\sigma }^{*}=\frac{f_0}{f_s}\sigma$$

(50)

where $f_s$ is the sampling frequency of the corresponding sensor and $f_0$ the sampling frequency of the operating system. Note that for the measurements obtained through V2V communication, the communication frequency may influence the weighting factor from (50). For all sensors located on the host vehicle it follows that $f_s$ is upperbounded by the communication frequency. Implementing this approach can decrease the sensitivity to sensor faults and errors of sensors with a relatively high sampling frequency. The effect of the weighting factor on the estimation performance is demonstrated in Appendix B.

2.4. Selection of the Process Noise Values

The performance of a Kalman filter depends on the specific setting, which is generally determined by the $\mathbf{Q}$ and $\mathbf{R}$ matrices. More specifically, the ratio between $\mathbf{Q}$ and $\mathbf{R}$ is of importance. In the literature various methods to acquire the process and/or measurement noise values (hence $\mathbf{Q}$ and $\mathbf{R}$) can be found, examples are [42,43,44,45,46]. Bian et al. [47] presented a two-step parameter optimization method for low-order model based estimation, where a particle swarm optimization algorithm is used to optimize the model parameters and to tune the error covariance matrices of an extended Kalman filter. Saha et al. [42] presented a selection procedure based on a robustness and sensitivity metric, allowing the estimator to be tuned via a weighted trade-off between the two metrics. Gelen and Atasoy [43] presented a selection procedure based on three metrics, and two parameter were defined in order to test consistency of the Kalman filter (the normalized estimation error squared and the normalized innovation squared). Shu et al. [44] presented a detailed error analysis and optimize the estimation error with respect to the process noise covariance. Åkesson et al. [45] presented a generalized autocovariance method for tuning of estimators with correlated noise. Note that noise might become correlated after discretizing continuous-time systems [45]. Correlated noise results in the Kalman filter to lose its optimality, yielding sub-optimal results and invalidity of (a part of) the stability and convergence proofs of Kalman filters. Lastly, Matisko and Havlena [46] presented a method for noise covariance estimation using a Bayesian approach next to Monte Carlo numerical methods.

Although numerous procedures used to determine the estimator setting can be found in the literature, the methods are often relatively complex to implement because of the large number of equations, assumptions and parameters, and are subject to manual selection of relevant parameters. In this study, the setting of the estimator is solely determined by optimization of the estimator performance through the $\mathbf{Q}$ matrices, since the values in the $\mathbf{R}$ matrices are available from prior experiments. The estimator setting is determined by the minimization of a global error function for different levels of process noise covariances ${\mathbf{Q}}_1$ and ${\mathbf{Q}}_2$, which in essence is similar to the approach from [44]. Because each unicycle model contains two noise terms, $w_{\mathrm{a}}$ for the longitudinal jerk in (4) and $w_{\dot{\theta }}$ for the yaw acceleration in (5), the process noise covariance for each unicycle consists of two parameters that can be selected. Note that it is assumed that process noise is uncorrelated as is described with (45) and (46). Furthermore, the process noise on the target vehicle is assumed to be equal to the process noise on the host vehicle, reducing the total optimization to a two-parameter problem. Similarly to the method presented in [42], the process noise covariance is varied according to

$${\mathbf{Q}}_1=\left[\begin{array}{cc}{10}^{P_{\dot{\theta }}}& 0\\ 0& {10}^{P_{\dot{\theta }}}\end{array}\right]{\mathbf{Q}}_0,{\mathbf{Q}}_2=\left[\begin{array}{cc}{10}^{P_a}& 0\\ 0& {10}^{P_a}\end{array}\right]{\mathbf{Q}}_0$$

(51)

by adapting $P_{\mathrm{a}}$ and $P_{\dot{\theta }}$ and where ${\mathbf{Q}}_0=\mathbf{I}$. The performance of the state estimator can be analyzed for different settings of the process noise covariance, and based on an error function a preferred setting can be selected. The error function E, which has to be minimized for $P_{\mathrm{a}}$ and $P_{\dot{\theta }}$, is defined as

$$E=\sum _{n=1}^{N_1}\sqrt{\frac{1}{K}\sum _{k=1}^K{g}_{1,n}{\left(x_{1,n,k}^{\mathrm{gt}}-{\widehat{x}}_{1,n,k}\right)}^2}+\sum _{n=1}^{N_2}\sqrt{\frac{1}{K}\sum _{k=1}^K{g}_{2,n}{\left(x_{2,n,k}^{\mathrm{gt}}-{\widehat{x}}_{2,n,k}\right)}^2}$$

(52)

where ${\mathbf{g}}_1$ and ${\mathbf{g}}_2$ describe the relative weights for the different states and $N_1$ and $N_2$ are the number of elements in ${\mathbf{x}}_{1,k}$ and ${\mathbf{x}}_{2,k}$ as well as ${\mathbf{g}}_1$ and ${\mathbf{g}}_2$. Furthermore, K is the total number of samples used in the calculation of E and the superscript $\mathrm{gt}$ refers to the ground truth signals. The error function is a weighted summation of the root-mean-square (RMS) errors of all the states. The weight vectors $\mathbf{g}$ are defined as

$${\mathbf{g}}_1=\left[\begin{array}{cccc}{g}_{\mathrm{h}}^2& g_{\mathrm{y}}^2& g_{\mathrm{h}}^2& g_{\mathrm{y}}^2\end{array}\right],{\mathbf{g}}_2=\left[\begin{array}{cccccccc}{g}_{\mathrm{p}}^2& g_{\mathrm{p}}^2& g_{\mathrm{v}}^2& g_{\mathrm{a}}^2& g_{\mathrm{p}}^2& g_{\mathrm{p}}^2& g_{\mathrm{v}}^2& g_{\mathrm{a}}^2\end{array}\right]$$

(53)

where the subscripts $\mathrm{h},\mathrm{y},\mathrm{p},\mathrm{v},\mathrm{a}$ are short for the heading, yaw rate, position, velocity and acceleration, respectively. Note that the weights in $\mathbf{g}$ are also used to make the individual state components dimensionless, allowing for summation of the errors. The final setting ${\mathbf{Q}}_1,\mathrm{final}$ and ${\mathbf{Q}}_2,\mathrm{final}$ of the estimator is obtained by minimizing E for ${\mathbf{Q}}_1$ and ${\mathbf{Q}}_2$. Note that using the methodology presented here, in Section 2.4, locating a global minimum is not guaranteed.

3. Simulation Results

The performance of the state estimator is analyzed through simulations based on a multi-body vehicle model and different driving scenarios. All simulations presented in this study are realized with Matlab/Simulink R2019b [48]. After setting the process noise covariance values of the estimator via the method presented in Section 2.4, the performance is analyzed via a RMS error analysis, where the effect of the V2V communication frequency is investigated in more detail.

3.1. Multi-Body Vehicle Model

In order to ensure the simulations are a realistic representation, a detailed multi-body vehicle model is used. The model is developed as part of the i-CAVE project and can be used to investigate the vehicle behavior in presence of sensor and actuator faults, indicating the high level of detail [34,49]. The model is validated through measurements and experiments in previous work [49], and serves as a suitable representation for the vehicles from Figure 1. For more information about this model the reader is referred to [49]. It is assumed that the host vehicle is accurately following the target vehicle via a longitudinal and lateral cooperative control approach. The distance between the vehicle is defined by a constant time gap h. In the simulations it is assumed the host vehicle follows the target vehicle perfectly. The simulation data obtained with the multi-body model is used as a ground truth in the analysis of the estimation performance. All ground truth data is provided as a file supplementary to this study.

3.2. Driving Scenarios

The performance of the state estimator is analyzed for different driving scenarios, such that it can be shown that the state estimator is suited for multiple scenarios. In this study, three different scenarios are considered:

• Straight line driving
• Constant curvature cornering
• 8-shape cornering

In each scenario a constant forward velocity is considered. The ground truth trajectories for all driving scenarios are shown in Figure 4, with the x-axis truncated to maintain visibility of scenario 1 and 2. The first scenario is the most simple one, where all states except for the global X-position are constant. In scenario 2 a constant radius cornering maneuver is considered, resulting in valid modeling assumptions of a constant yaw rate and acceleration. Scenario 3 contains a varying road curvature which results in a non-constant yaw rate, thus making the constant yaw rate assumption in the model invalid. In all driving scenarios the lateral acceleration remains limited, yielding linear tyre characteristics [50], and each simulation is terminated after $30\mathrm{s}$.

3.3. Process Noise Covariance Selection

The setting of the state estimator is obtained by minimizing E from (52) for ${\mathbf{Q}}_1$ and ${\mathbf{Q}}_2$ for one driving scenario. By using only one scenario to select the process noise covariance values of the state estimator, the other two scenarios provide more objective results of the performance. Furthermore, this gives an indication of the estimation performance for different scenarios. Scenario 3, the 8-shape driving maneuver, is used to tune the estimator. This scenario is the most complex where the model assumption of a constant yaw rate is not valid, therefore being most representative for a regular driving scenario. The default process noise covariance matrices are chosen as ${\mathbf{Q}}_{1,0}={\mathbf{Q}}_{2,0}=\mathbf{I}$. The error function E from (52) is minimized for $P_{\mathrm{a}}$ and $P_{\dot{\theta }}$. The weights that have been used in this study are summarized in

Table 3. Weighting values for the different system states.

Weight	$g_{\mathrm{h}}$	$g_{\mathrm{y}}$	$g_{\mathrm{p}}$	$g_{\mathrm{v}}$	$g_{\mathrm{a}}$
Value	$\frac{1}{0.1}1/\mathrm{rad}$	$\frac{1}{0.1}\mathrm{s}/\mathrm{rad}$	$\frac{1}{0.05}1/\mathrm{m}$	$\frac{1}{0.5}\mathrm{s}/\mathrm{m}$	$\frac{1}{0.1}{\mathrm{s}}^2/\mathrm{m}$

. The effect of the weighting values in Table 3 on the estimation behavior is not considered and can be analyzed in future work. The error is calculated in the time interval $\left[5,30\right]\mathrm{s}$, such that initialization errors do not affect the error E and thus the eventual setting of the estimator.

In Figure 5, the error function is plotted for different values of $P_{\mathrm{a}}$ and $P_{\dot{\theta }}$, and the minimum is indicated with a black dot. The results are generated by varying $P_{\mathrm{a}}$ and $P_{\dot{\theta }}$ with a step-size of $0.5$. The resulting process noise covariance matrices ${\mathbf{Q}}_1,\mathrm{final}$ and ${\mathbf{Q}}_2,\mathrm{final}$ are obtained for $P_{\mathrm{a}}=-3.5$ and $P_{\dot{\theta }}=0.0$. As can be observed in Figure 5, the surface of E is relatively flat around the minimum, and therefore the estimator has a low sensitivity for (limited) variations in $\mathbf{Q}$. Figure 5 shows that variations of $P_{\dot{\theta }}$ with $P_{\dot{\theta }}>=1.0$ do not visibly influence the error E, which is caused by the low measurement noise of the yaw rate measurement (Table 2).

3.4. Estimation Performance

Now that the setting of the state estimator is selected via a RMS error analysis, the performance of the estimator can be analyzed in more detail. First the estimated trajectories are presented in combination with the ground truth trajectories. Next, the performance is analyzed via RMS and absolute maximum errors, and finally the effect of the V2V communication frequency is investigated. In the latter, the values of the process noise matrices ${\mathbf{Q}}_1$ and ${\mathbf{Q}}_2$ is considered to be fixed.

3.4.1. Trajectory Estimation

The estimated spatial trajectories are shown in Figure 6 and serve as a graphical way of indicating the estimation performance. Quantitative results corresponding with the spatial trajectories from Figure 6 are summarized in

Table 4. Mean values and standard deviations corresponding to the results presented in Figure 6 (*: scenario that is used to select the estimator setting).

	(Scenario 1)		(Scenario 2)		(Scenario 3) *
Error Signal	Mean (m)	St.d. (m)	Mean (m)	St.d. (m)	Mean (m)	St.d. (m)
$X_{\mathrm{t}}^{\mathrm{gt}}-X_{\mathrm{t}}$	$-0.0215$	$0.415$	$-0.0215$	$0.415$	$-0.0215$	$0.415$
$X_{\mathrm{t}}^{\mathrm{gt}}-{\widehat{X}}_{\mathrm{t}}$	$0.0448$	$0.187$	$-0.123$	$0.604$	$-0.0788$	$0.432$
$Y_{\mathrm{t}}^{\mathrm{gt}}-Y_{\mathrm{t}}$	$-0.0961$	$0.476$	$-0.0961$	$0.476$	$-0.0961$	$0.476$
$Y_{\mathrm{t}}^{\mathrm{gt}}-{\widehat{Y}}_{\mathrm{t}}$	$0.846$	$0.577$	$-0.0542$	$0.6278$	$-0.346$	$0.536$
$X_{\mathrm{h}}^{\mathrm{gt}}-X_{\mathrm{h}}$	$0.0823$	$0.713$	$0.0823$	$0.713$	$0.0823$	$0.713$
$X_{\mathrm{h}}^{\mathrm{gt}}-{\widehat{X}}_{\mathrm{h}}$	$-0.111$	$0.253$	$-0.0591$	$0.354$	$0.141$	$0.414$
$Y_{\mathrm{h}}^{\mathrm{gt}}-Y_{\mathrm{h}}$	$-0.0447$	$0.723$	$-0.0447$	$0.723$	$-0.0447$	$0.723$
$Y_{\mathrm{h}}^{\mathrm{gt}}-{\widehat{Y}}_{\mathrm{h}}$	$-0.136$	$0.0657$	$0.0118$	$0.495$	$-0.0518$	$0.527$

via mean values and standard deviations. The estimator setting obtained from Figure 5, which is solely based on driving scenario 3, is used for all three scenarios. The estimated trajectories visually correspond relatively well with the ground truth trajectories for all three scenarios. The mean values and standard deviations of the error signals listed in Table 4 demonstrate that the mean values of the estimation error ($\mathrm{groundtruth}-\mathrm{estimation}$) are generally higher than the mean values of the measurement error ($\mathrm{groundtruth}-\mathrm{measurement}$). Looking at the results from Figure 6 this is as expected, since mean error values are affected by offsets. Furthermore, since the measurement noise is generated in simulations, the mean value should go to zero for $t\to \infty$. The standard deviations from Table 4 demonstrate that the estimation of the host vehicle outperforms the target vehicle for all scenarios, with an reduction of around 25 to 65% for different scenarios. In interesting observation is that the improvement are the smallest for the scenario which is used to obtain the estimator settings, but considering that scenario (number 3) is most complex this is a reasonable result. For the target vehicle, the standard deviation of the estimation error increases with approximately 5 to 45%, this increase is most likely caused by delayed estimator behavior clearly demonstrated in Figure 6b,c. This effect is caused by the limited update frequency of the GPS sensor located on the target vehicle in combination with the communication device limiting the update rates of all measurements belonging to the target vehicle. It is expected that the standard deviation of the estimation error can become smaller than the standard deviation of the measurement error by increasing the relative weight of the GPS measurements of the target vehicle, for example by modifying the measurement noise of the measured position $\left(X_{\mathrm{t}},Y_{\mathrm{t}}\right)$. The results in Figure 6 and Table 4 confirm that the trajectory of the host vehicle has smaller errors with respect to the ground truth compared to the trajectory of the target vehicle. The errors in the estimated Y-positions in Figure 6a remain below $2\mathrm{m}$. Furthermore, all results in Figure 6 show limited estimation errors considering the quality of the available measurements. It should be noted that the initial conditions of the state estimator correspond exactly with the initial states of the vehicles. The effect of the initial conditions might be investigated in future work. Figure 6b reveals that during the time in between the GPS measurements, the estimator is able to follow the curvature of the cornering maneuver.

The value of the error function E for the different scenarios is shown in Figure 7. The results show that the value of E is similar for scenario 2 and scenario 3, where the error for scenario 3 is slightly smaller. This may be caused by the fact that scenario 3 is used to select the setting of the estimator. The error E for scenario 1 is smaller compared to the values for the other scenarios, most likely caused by the limited complexity of the driving scenario, i.e., in scenario 1 no cornering maneuver is considered.

The ground truth and estimation results over time are presented in Figure 8 for the global heading of both the target and host vehicle. The mean values and standard deviations of the error signals corresponding with the results from Figure 8 are listed in

Table 5. Mean values and standard deviations corresponding to the results presented in Figure 8 (*: scenario that is used to select the estimator setting).

	(Scenario 1)		(Scenario 2)		(Scenario 3) *
Error Signal	Mean (rad)	St.d. (rad)	Mean (rad)	St.d. (rad)	Mean (rad)	St.d. (rad)
${\theta }_{\mathrm{t}}^{\mathrm{gt}}-{\theta }_{\mathrm{t}}$	$0.0293$	$0.0845$	$0.0293$	$0.0845$	$0.0293$	$0.0845$
${\theta }_{\mathrm{t}}^{\mathrm{gt}}-{\widehat{\theta }}_{\mathrm{t}}$	$0.0845$	$0.0076$	$0.0292$	$0.0641$	$0.0292$	$0.0641$
${\theta }_{\mathrm{h}}^{\mathrm{gt}}-{\theta }_{\mathrm{h}}$	$-0.000731$	$0.0333$	$-0.000731$	$0.0333$	$-0.000731$	$0.0333$
${\theta }_{\mathrm{h}}^{\mathrm{gt}}-{\widehat{\theta }}_{\mathrm{h}}$	$-0.00217$	$0.00218$	$-0.00130$	$0.0124$	$-0.00122$	$0.0124$

. The results in Figure 8 demonstrate that the estimated signals correspond well with the ground truth. This is confirmed with Table 5, which also demonstrates that the errors in the heading angles can be significantly reduced using the proposed estimator. Although the mean values show an increase comparing the estimation errors with the measurement errors, the standard deviations of the error are reduced by roughly 25 to 65% during cornering maneuvers and over 90% for straight line driving. Interestingly, comparing the results from Table 5 for the three scenarios shows that the presence of cornering behavior influences the estimation errors, but the complexity of the cornering maneuver seems to have minimal effect on the estimation errors (scenario 2 vs. scenario 3). Finally, Figure 8b,c clearly show that the results for the host vehicle have a constant lag with respect to the target vehicle. This is caused by the constant time gap, ensuring a constant inter-vehicle distance.

3.4.2. State and Measurement Errors

The estimator performance can be analyzed numerically by calculating the errors between the estimation and ground truth signals, and compare these with the errors between the ground truth and the measurements. The RMS and absolute maximum errors between the estimated state components and ground truth are listed in

Table 6. Maximum and RMS errors between the ground truth, the estimates and the measurements for all three driving scenarios (*: scenario for which the state estimator setting is selected). The top part contains the errors between the ground truth and estimation results, the middle part contains the errors between the ground truth and the measurements of direct state components, and the bottom part contains errors related to the radar sensor (**: reconstructed via (6) and (7)).

	(Scenario 1)		(Scenario 2)		(Scenario 3) *
Error Signal	RMS	Abs. max	RMS	Abs. max	RMS	Abs. max
$X_{\mathrm{t}}^{\mathrm{gt}}$−${\widehat{X}}_{\mathrm{t}}\left(\mathrm{m}\right)$	$1.75$× 10${}^{-1}$	$5.60$× 10${}^{-1}$	$6.63$× 10${}^{-1}$	$13.8$× 10${}^{-1}$	$4.63$× 10${}^{-1}$	$12.9$× 10${}^{-1}$
$Y_{\mathrm{t}}^{\mathrm{gt}}$−${\widehat{Y}}_{\mathrm{t}}\left(\mathrm{m}\right)$	$11.2$× 10${}^{-1}$	$18.4$× 10${}^{-1}$	$6.80$× 10${}^{-1}$	$17.6$× 10${}^{-1}$	$6.89$× 10${}^{-1}$	$17.7$× 10${}^{-1}$
$v_{\mathrm{t}}^{\mathrm{gt}}$−${\widehat{v}}_{\mathrm{t}}\left(\mathrm{m}/\mathrm{s}\right)$	$7.63$× 10${}^{-2}$	$19.7$× 10${}^{-2}$	$30.6$× 10${}^{-2}$	$73.1$× 10${}^{-2}$	$25.0$× 10${}^{-2}$	$48.0$× 10${}^{-2}$
$a_{\mathrm{t}}^{\mathrm{gt}}$−${\widehat{a}}_{\mathrm{t}}\left(\mathrm{m}/{\mathrm{s}}^2\right)$	$2.81$× 10${}^{-2}$	$8.32$× 10${}^{-2}$	$11.4$× 10${}^{-2}$	$24.3$× 10${}^{-2}$	$8.94$× 10${}^{-2}$	$23.2$× 10${}^{-2}$
${\theta }_{\mathrm{t}}^{\mathrm{gt}}$−${\widehat{\theta }}_{\mathrm{t}}\left(\mathrm{rad}\right)$	$6.47$× 10${}^{-2}$	$15.2$× 10${}^{-2}$	$6.47$× 10${}^{-2}$	$15.2$× 10${}^{-2}$	$6.47$× 10${}^{-2}$	$15.2$× 10${}^{-2}$
${\dot{\theta }}_{\mathrm{t}}^{\mathrm{gt}}$−${\widehat{\dot{\theta }}}_{\mathrm{t}}\left(\mathrm{rad}/\mathrm{s}\right)$	$13.6$× 10${}^{-3}$	$42.3$× 10${}^{-3}$	$13.6$× 10${}^{-3}$	$42.3$× 10${}^{-3}$	$14.0$× 10${}^{-3}$	$44.4$× 10${}^{-3}$
$X_{\mathrm{h}}^{\mathrm{gt}}$−${\widehat{X}}_{\mathrm{h}}\left(\mathrm{m}\right)$	$2.80$× 10${}^{-1}$	$7.95$× 10${}^{-1}$	$3.77$× 10${}^{-1}$	$7.93$× 10${}^{-1}$	$4.66$× 10${}^{-1}$	$9.60$× 10${}^{-1}$
$Y_{\mathrm{h}}^{\mathrm{gt}}$−${\widehat{Y}}_{\mathrm{h}}\left(\mathrm{m}\right)$	$1.61$× 10${}^{-1}$	$2.62$× 10${}^{-1}$	$5.41$× 10${}^{-1}$	$12.1$× 10${}^{-1}$	$5.80$× 10${}^{-1}$	$11.7$× 10${}^{-1}$
$v_{\mathrm{h}}^{\mathrm{gt}}$−${\widehat{v}}_{\mathrm{h}}\left(\mathrm{m}/\mathrm{s}\right)$	$8.70$× 10${}^{-2}$	$27.3$× 10${}^{-2}$	$20.4$× 10${}^{-2}$	$49.6$× 10${}^{-2}$	$24.9$× 10${}^{-2}$	$51.0$× 10${}^{-2}$
$a_{\mathrm{h}}^{\mathrm{gt}}$−${\widehat{a}}_{\mathrm{h}}\left(\mathrm{m}/{\mathrm{s}}^2\right)$	$3.23$× 10${}^{-2}$	$9.88$× 10${}^{-2}$	$9.02$× 10${}^{-2}$	$27.3$× 10${}^{-2}$	$8.79$× 10${}^{-2}$	$20.2$× 10${}^{-2}$
${\theta }_{\mathrm{h}}^{\mathrm{gt}}$−${\widehat{\theta }}_{\mathrm{h}}\left(\mathrm{rad}\right)$	$1.26$× 10${}^{-2}$	$3.38$× 10${}^{-2}$	$1.26$× 10${}^{-2}$	$3.38$× 10${}^{-2}$	$1.26$× 10${}^{-2}$	$3.38$× 10${}^{-2}$
${\dot{\theta }}_{\mathrm{h}}^{\mathrm{gt}}$−${\widehat{\dot{\theta }}}_{\mathrm{h}}\left(\mathrm{rad}/\mathrm{s}\right)$	$8.74$× 10${}^{-3}$	$42.1$× 10${}^{-3}$	$8.74$× 10${}^{-3}$	$42.1$× 10${}^{-3}$	$8.87$× 10${}^{-3}$	$42.3$× 10${}^{-3}$
$X_{\mathrm{t}}^{\mathrm{gt}}$−$X_{\mathrm{t}}\left(\mathrm{m}\right)$	$3.72$× 10${}^{-1}$	$9.97$× 10${}^{-1}$	$3.72$× 10${}^{-1}$	$9.97$× 10${}^{-1}$	$3.72$× 10${}^{-1}$	$9.97$× 10${}^{-1}$
$Y_{\mathrm{t}}^{\mathrm{gt}}$−$Y_{\mathrm{t}}\left(\mathrm{m}\right)$	$5.00$× 10${}^{-1}$	$12.0$× 10${}^{-1}$	$5.00$× 10${}^{-1}$	$12.0$× 10${}^{-1}$	$5.00$× 10${}^{-1}$	$12.0$× 10${}^{-1}$
$v_{\mathrm{t}}^{\mathrm{gt}}$−$v_{\mathrm{t}}\left(\mathrm{m}/\mathrm{s}\right)$	$7.87$× 10${}^{-2}$	$24.4$× 10${}^{-2}$	$7.87$× 10${}^{-2}$	$24.4$× 10${}^{-2}$	$7.87$× 10${}^{-2}$	$24.4$× 10${}^{-2}$
$a_{\mathrm{t}}^{\mathrm{gt}}$−$a_{\mathrm{t}}\left(\mathrm{m}/{\mathrm{s}}^2\right)$	$30.0$× 10${}^{-2}$	105 × 10${}^{-2}$	$30.0$× 10${}^{-2}$	105 × 10${}^{-2}$	$30.0$× 10${}^{-2}$	105 × 10${}^{-2}$
${\theta }_{\mathrm{t}}^{\mathrm{gt}}$−${\theta }_{\mathrm{t}}\left(\mathrm{rad}\right)$	$8.20$× 10${}^{-2}$	$17.9$× 10${}^{-2}$	$8.20$× 10${}^{-2}$	$17.9$× 10${}^{-2}$	$8.20$× 10${}^{-2}$	$17.9$× 10${}^{-2}$
${\dot{\theta }}_{\mathrm{t}}^{\mathrm{gt}}$−${\dot{\theta }}_{\mathrm{t}}\left(\mathrm{rad}/\mathrm{s}\right)$	$13.9$× 10${}^{-3}$	$42.8$× 10${}^{-3}$	$13.9$× 10${}^{-3}$	$42.8$× 10${}^{-3}$	$13.9$× 10${}^{-3}$	$42.8$× 10${}^{-3}$
$X_{\mathrm{h}}^{\mathrm{gt}}$−$X_{\mathrm{h}}\left(\mathrm{m}\right)$	$7.31$× 10${}^{-1}$	$20.7$× 10${}^{-1}$	$7.31$× 10${}^{-1}$	$20.7$× 10${}^{-1}$	$7.31$× 10${}^{-1}$	$20.7$× 10${}^{-1}$
$Y_{\mathrm{h}}^{\mathrm{gt}}$−$Y_{\mathrm{h}}\left(\mathrm{m}\right)$	$7.36$× 10${}^{-1}$	$21.2$× 10${}^{-1}$	$7.36$× 10${}^{-1}$	$21.2$× 10${}^{-1}$	$7.36$× 10${}^{-1}$	$21.2$× 10${}^{-1}$
$v_{\mathrm{h}}^{\mathrm{gt}}$−$v_{\mathrm{h}}\left(\mathrm{m}/\mathrm{s}\right)$	$7.18$× 10${}^{-2}$	$26.4$× 10${}^{-2}$	$7.18$× 10${}^{-2}$	$26.4$× 10${}^{-2}$	$7.18$× 10${}^{-2}$	$26.4$× 10${}^{-2}$
$a_{\mathrm{h}}^{\mathrm{gt}}$−$a_{\mathrm{h}}\left(\mathrm{m}/{\mathrm{s}}^2\right)$	$18.7$× 10${}^{-2}$	$69.6$× 10${}^{-2}$	$18.7$× 10${}^{-2}$	$69.6$× 10${}^{-2}$	$18.7$× 10${}^{-2}$	$69.6$× 10${}^{-2}$
${\theta }_{\mathrm{h}}^{\mathrm{gt}}$−${\theta }_{\mathrm{h}}\left(\mathrm{rad}\right)$	$3.38$× 10${}^{-2}$	$8.09$× 10${}^{-2}$	$3.38$× 10${}^{-2}$	$8.09$× 10${}^{-2}$	$3.38$× 10${}^{-2}$	$8.09$× 10${}^{-2}$
${\dot{\theta }}_{\mathrm{h}}^{\mathrm{gt}}$−${\dot{\theta }}_{\mathrm{h}}\left(\mathrm{rad}/\mathrm{s}\right)$	$13.8$× 10${}^{-3}$	$46.8$× 10${}^{-3}$	$13.8$× 10${}^{-3}$	$46.8$× 10${}^{-3}$	$13.8$× 10${}^{-3}$	$46.8$× 10${}^{-3}$
$r^{\mathrm{gt}}$−$\widehat{r}$ ** (m)	$12.1$× 10${}^{-3}$	$34.8$× 10${}^{-3}$	$25.7$× 10${}^{-3}$	$61.3$× 10${}^{-3}$	$14.1$× 10${}^{-3}$	$37.8$× 10${}^{-3}$
$r^{\mathrm{gt}}$−r (m)	$15.3$× 10${}^{-3}$	$46.8$× 10${}^{-3}$	120 × 10${}^{-3}$	154 × 10${}^{-3}$	$60.9$× 10${}^{-3}$	146 × 10${}^{-3}$
${\dot{r}}^{\mathrm{gt}}$−$\widehat{\dot{r}}$ ** (m/s)	$5.95$× 10${}^{-2}$	$15.2$× 10${}^{-2}$	$36.2$× 10${}^{-2}$	$74.3$× 10${}^{-2}$	$30.5$× 10${}^{-2}$	$62.6$× 10${}^{-2}$
${\dot{r}}^{\mathrm{gt}}$−$\dot{r}\left(\mathrm{m}/\mathrm{s}\right)$	$13.9$× 10${}^{-2}$	$47.7$× 10${}^{-2}$	$13.9$× 10${}^{-2}$	$47.7$× 10${}^{-2}$	$13.9$× 10${}^{-2}$	$47.7$× 10${}^{-2}$

, where the superscript $\mathrm{gt}$ refers to ground truth. In addition, the errors between the measurement and ground truth signals are also included in Table 6. For the expressions of the RMS and maximum errors the estimated heading of the target vehicle is selected as an example, resulting in

$$\mathrm{RMS}=\sqrt{\frac{1}{K}\sum _{k=1}^K{\left({\theta }_{\mathrm{t},k}^{\mathrm{gt}}-{\widehat{\theta }}_{\mathrm{t},k}\right)}^2}$$

(54)

$$\mathrm{Max}=\sqrt{{\left({\theta }_{\mathrm{t}}^{\mathrm{gt}}-{\widehat{\theta }}_{\mathrm{t}}\right)}^2}$$

(55)

where K is the number of samples considered.

First, let us focus on the top part of Table 6. Similarly to Section 3.3, the errors are calculated over the time interval $\left[5,30\right]\mathrm{s}$ to limit the effect of initialization. The difference in errors between the states of the host vehicle and target vehicle are small for the individual scenarios. Comparing the scenarios with each other reveals that all errors are similar in magnitude and the results for scenario 1 show smaller errors compared to scenario 2 and 3. Considering the overall agreement in errors for the different scenarios, it can be concluded that the estimator has a small sensitivity to the complexity of the driving scenario. The results do indicate that including a cornering maneuver increases the errors, which is in line with the expectation. The setting of the estimator is based on scenario 3 only, and the maximum and RMS errors are similar or smaller for the other scenarios, which implies the setting is suitable for a variety of scenarios. Considering the implementation onto the vehicle set-up in future work, these results are positive, since the setting of the estimator is independent of the driving maneuver. The results listed in Table 6 are in line with the results from Figure 5.

By continuing to the middle and bottom part of Table 6, the estimation performance can be compared with the quality of the measurements, providing a more objective performance analysis. The RMS and maximum errors for measurements of direct state components are equal for all scenarios, because the measurement noise is created in simulation via pre-specified noise levels (Table 2). Furthermore, in simulation the measurements are not influenced by external disturbances. Comparing the errors between the measurements and the ground truth, and the estimates and the ground truth, allows us to analyze the attenuation of measurement noise. For the majority of the state components the measurement noise is attenuated. Noteworthy is the increase RMS and maximum error for the longitudinal velocity, which is caused by the corrections using the GPS measurements. Furthermore, the errors in the position estimate of the target vehicle are larger compared with the measurements, this is caused by estimation errors in the heading angle. For the host vehicle this is not the case, because the measured noise on the host vehicle’s heading angle is smaller, the position estimation is more accurate. Furthermore, it should be noted that the results depend on the weights described in Table 3, the sensor operating frequencies from Table 1 and the noise levels from Table 2. The results from Table 6 reveal that while using the state estimator, measurement noise can be reduced to over 50%, depending on the state component. From Table 6 it can be concluded that the estimator can significantly reduce the measurement noise, which confirms the benefits of using a state estimator for cooperative applications. However, the results also indicate the measurement noise on the heading angles have a significant influence on the quality of the position estimate. In contrast to solely relying on limited frequency measurement data, using the estimator results in a state estimate which is both available and updated every sample.

3.4.3. V2V Communication Frequency

The effect of the communication frequency on the estimation performance is analyzed by varying the communication frequency $f_{\mathrm{c}}$ while keeping all other parameters fixed, including the setting of the estimator. Note that varying $f_{\mathrm{c}}$ does have an effect on the specific weighting of the communicated measurements through (50). The communication frequency is varied by taking multitudes of the sampling time of the operating frequency. The error function for different sampling times of the V2V communication is shown in Figure 9 for all three scenarios. The results show that the shape of the error function is somewhat similar for all scenarios. The trend of the error functions shown in Figure 9 reveals that the error increases for an increasing sampling time of the V2V communication with the exception of the results for scenario 1. This is most likely caused by constant velocity and the lack of cornering maneuvers in scenario 1, reducing the sensitivity to delays caused by limited sensor update rates.

It is evident that the results presented in Figure 9 depend on the sensor operating frequencies and thus the relative weights, (50), the measurement noise levels and the weights used in the error function (Table 3). This is demonstrated with Figure 10, which displays the RMS error between the ground truth and estimate of the yaw rate and longitudinal acceleration for different communication frequencies. The shapes of the curves in Figure 10a,b show clear differences. Because the yaw rate estimate of the target vehicle is predominantly based on the yaw rate measurement, it is expected that the estimation error decreases for an increasing communication frequency which is confirmed with Figure 10a. The estimate of the longitudinal acceleration, however, is also influenced by velocity and position measurements as well as the relative measurements from the radar sensor. The weight of the these measurements depends on the communication frequency through (50). A smaller sampling time for the V2V communication results in a relatively higher weight of the GPS measurements of the target vehicle, visibly affecting the longitudinal acceleration estimates. For communication samplings times above approximately $0.10\mathrm{s}$ most curves in Figure 10 show a constant or increasing trend, which is in agreement with the general expectation that the error increases for a larger sampling time.

3.5. Discussion

The presented framework can be used for the state estimation of a host and target vehicle and thus cooperative lateral vehicle following. For selected state components, measurement noise is significantly attenuated. Moreover, using the presented approach results in availability of the state of the target vehicle on every sample, in contrast to methods which rely solely on V2V communication [14], with potentially a limited number of samples. This contribution relaxes the high update rate requirements of future safety applications [16]. Relating this work to previous works on state estimation for cooperative automotive applications such as [17] shows that the use of V2V communication in state estimation can significantly improve the estimation performance. It has to be noted that the approach presented in [17] serves as a fall-back in case communication is lost, rather than a general solution. The framework presented in this study provides accurate information of the state components of the host and target vehicle, which can be used in cooperative control frameworks. Moreover, depending on the required input signals for the considered cooperative control approach, the use of V2V communication might become a necessity to ensure observability of the system. With the presented approach it is possible to estimate state components which are otherwise difficult to estimate, for example the yaw rate of the target vehicle. Comparing the obtained results with other works, mostly control techniques ([32,33,51]), reveals that the presented framework does not solely rely on one or two sensors of which the measurements are communicated wireless, but instead uses a combination of on-board sensors and V2V communication, and takes different update rates into account. Therefore, the presented approach is potentially more robust to sensor faults and missing information due to for example packet loss. However, studies as [52,53] consider these kind of communication specifications in more detail. The presented approach is potentially more robust compared to approaches using only on-board information such as [13]. The statements presented in this paragraph clearly define the benefits of using V2V communication for state estimation, especially considering the application in a cooperative control framework, providing detailed information of the relevant state components.

Regarding the simulation results, the estimation errors have the same level of magnitude for all scenarios, implying a small sensitivity to the specific scenario. In other words, the estimator is likely to display a similar performance throughout different scenarios of varying complexity. The results also show that during cornering maneuvers the estimation errors increase compared to straight line driving. Furthermore, the effect of measurement noise can be significantly reduced, depending on the setting of the estimator and the measurement noise of relevant sensors. In general, increasing the V2V communication frequency decreases the estimation errors. However, the results depend on the weighting values defined in Table 3 as well as the operating frequencies of the different sensors.

Limitations of the presented approach include the effect of time delays, the assumptions on the measurement noise, the requirements in measurement quality, and the effect of vehicle dynamics and external disturbances, as is explained further in the remainder of this section. Typically, sensors are subject to time delays, which results in measurement obtained at t actually correspond with $t-\tau$ where $\tau$ is the delay. In this study, the effect of time delays is not considered and therefore set to zero for all sensors. The values for the measurement noise are considered to be known exactly and they are assumed to be constant. Although one can calculate the measurement noise through experiments, as is done in this study, it remains difficult to determine the noise level exactly, like in any other practical set-up. The wide variety of automotive sensors, each with their own signal quality and operating frequency, makes it difficult to present a fixed or optimal setting. For that reason, the properties of the sensors already located on the vehicle set-up are considered fixed except for the operating frequency of the V2V communication. Furthermore, excited vehicle dynamics and external disturbances also influence the measurements in practice. Regarding the required measurement quality, in the preparation of the simulations it was found that the quality of the measurement signals can have a significant effect on the estimation performance. Specifically the quality of the measured heading angles is of importance, high measurement noise in the measured heading angles leads to poor estimation results. This is in part caused by the adaptation of the cascaded systems approach. With this approach some relations between the different state components are ignored, for example those between the heading angle and position (the heading angle estimates are not corrected using the position measurements). Finally, the effect of sensor offsets is also not considered in this study.

Several opportunities for future work are identified, in part based on the results presented in this study. First and foremost is the implementation of the presented approach onto a full-size vehicle setup, such that the methodology can be verified and demonstrated with experiments. With the implementation onto a vehicle setup, the effect of external disturbances, e.g., driving on a poor quality road, on the performance of the methodology presented in this study can be analyzed. In addition, investigating the effect of sensor time delays on the estimation performance is considered an interesting route for future work. Furthermore, the effect of relevant V2V communication properties, including packet loss and communication delays, is a compelling direction for future work. The effect of the weighting values listed in Table 3 on the estimation performance as mentioned in Section 3.3 as well as the effect of the initial conditions as mentioned in Section 3.4.1 are items that can also be analyzed in future work. The effect of measurement noise and process noise can be investigated in future work as well. Although measurement noise can be determined from experiments, in the preparation of the work presented in this study, the specific noise levels demonstrated to play a significant role in the estimation performance. Finally, in Section 1 it is noted that future safety applications may require high update rates, and therefore it would be interesting to research the extent to which the methodology presented in this study can provide a solution for the increasing safety requirements.

4. Conclusions

A framework for state estimation using V2V communication for cooperative driving applications is presented and demonstrated by means of simulations. The main contribution of this study is the successful demonstration of a cooperative state estimation framework, which has good potential for a diverse selection of cooperative driving applications. The framework can be implemented on a wide variety of vehicles with different sensor and communication setups, given that observability of the system is maintained. The estimator provides detailed information of relevant state components, which can be used as input for longitudinal and lateral cooperative driving control schemes. Via simulations it is shown that the estimator has a small sensitivity to the specific setting as well as the complexity of the driving scenario. The effect of measurement noise can be significantly reduced, although the results vary for the different state components, which is presented by means of both quantitative and qualitative results. The operating frequencies of different sensors are taken into account in the estimator design. The analysis of the effect of the V2V communication frequency on the estimation performance indicates the estimation quality increases for higher communication frequencies. Limitations of this work include the assumptions on the measurement noise, the magnitude of the measurement noise and the exclusion of external disturbances. The effect of sensor time delays and implementation onto a vehicle set-up is considered in future work. Practical implementation and performing experiments will automatically include the effect of external disturbances.