2.2.1 CTRV model

The CTRV model hypothesises that the vehicle is moving at a constant turn rate and velocity, which results in a circular trajectory, as shown in Figure 2 (simulated with initial state [0 m, 100 m, 0 deg, 3⋅14 m/s, −18 deg/s]^T and duration 200 s). The state vector of CTRV model at instant k is

(1)\begin{equation}{\textbf{x}_k} = {[x,y,\theta ,v,\omega ]^T}\end{equation}

where x, y represent the position; θ is the heading; v the velocity; ω the angular rate. Based on the hypothesis, the next instant's state is derived through

(2)\begin{equation}{\textbf{x}_{k + 1}} = {\textbf{x}_k} + \int_{{t_k}}^{{t_{k + 1}}} {{{\dot{\textbf{x}}}_{\tau |{t_k}}}} d\tau \end{equation}

Consequently, the CTRV model's state transition equation is

(3)\begin{equation}{\textbf{x}_{k+1}} = \left[\begin{matrix} {x_k} + \dfrac{{{v_k}}}{{{\omega_k}}}\sin ({\theta_k} + {\omega_k}T) - \dfrac{{{v_k}}}{{{\omega_k}}}\sin ({\theta_k})\\ {y_k} - \dfrac{{{v_k}}}{{{\omega_k}}}\cos ({\theta_k} + {\omega_k}T) + \dfrac{{{v_k}}}{{{\omega_k}}}\cos ({\theta_k}) \\ {{\theta_k} + {\omega_k}T}\\ {{v_k}}\\ {{\omega_k}} \end{matrix} \right]\end{equation}

where T is the prediction time interval.

2.2.2 CTRA model

Regarding the velocity derivative as constant, the CTRV model is extended to the CTRA model. The trajectory of this model follows a clothoid, as Figure 2 shows (simulated with initial state [0 m, 100 m, 0 deg, 3⋅14 m/s, 0⋅11 m/s², −1⋅8 deg/s]^T and duration 200 s). At instant k, the CTRA model's state vector is

(4)\begin{equation}{\textbf{x}_k} = {[x,y,\theta ,v,a,\omega ]^T}\end{equation}

where a denotes the tangential acceleration. Through Equation (2), the model's state transition equation is derived as follows.

(5)\begin{equation}{\textbf{x}_{k + 1}} = \left[ {\begin{matrix} {{x_k} + \dfrac{{({v_k} + {a_k}T)\sin ({\theta_k} + {\omega_k}T) - {v_k}\sin ({\theta_k})}}{{{\omega_k}}} + \dfrac{{{a_k}[\cos ({\theta_k} + {\omega_k}T) - \cos ({\theta_k})]}}{{\omega_k^2}}}\\ {{y_k} - \dfrac{{({v_k} + {a_k}T)\cos ({\theta_k} + {\omega_k}T) - {v_k}\cos ({\theta_k})}}{{{\omega_k}}} + \dfrac{{{a_k}[\sin ({\theta_k} + {\omega_k}T) - \sin ({\theta_k})]}}{{\omega_k^2}}}\\ {{\theta_k} + {\omega_k}T}\\ {{v_k} + {a_k}T}\\ {{a_k}}\\ {{\omega_k}} \end{matrix}} \right]\end{equation}

2.3.1 State space model

The discrete-time nonlinear dynamic system for vehicular state estimation is formed by

(6)\begin{align}{\textbf{x}_{k + 1}} &= F({\textbf{x}_k})+{\textbf{w}_k}\end{align}

(7)\begin{align}{\textbf{y}_k} &= H({\textbf{x}_k})+{\textbf{e}_k}\end{align}

where x_{k +1} denotes the L-dimensional state of the system at instant k + 1, and y_k is the observation. F and H are the process and observation functions. w_k and e_k are the process and observation noises, which are assumed as independent zero-mean white Gaussian noise of covariance matrices Q_k and R_k.

2.3.2 EKF

EKF follows the same prediction-correction mechanism as the standard KF. The main difference is that EKF has to linearise the nonlinear state space models [Equations (6) and (7)] at each iteration through first-order Taylor expansion around the most recent state estimate, employing the Jacobian matrices in Equation (8).

(8)\begin{equation}{\textbf{J}_F} = \frac{{\partial F(\textbf{x})}}{{\partial \textbf{x}}}|{_{\textbf{x} = {{\hat{\textbf{x}}}_k}}} \quad {\textbf{J}_H} = \frac{{\partial H(\textbf{x})}}{{\partial \textbf{x}}}|{_{\textbf{x} = \hat{\textbf{x}}_{k+1}^ - }} \end{equation}

where J_F and J_H denote the Jacobian matrices of the process and observation functions (for more details see Appendix A). EKF is initialised via

(9)\begin{equation}\begin{array}{*{20}{c}} {{{\hat{\textbf{x}}}_0} = E[{\textbf{x}_0}]} &{{\textbf{P}_0} = E[({\textbf{x}_0} - {{\hat{\textbf{x}}}_0}){{({\textbf{x}_0} - {{\hat{\textbf{x}}}_0})}^T}]} \end{array}\end{equation}

Suppose the system state has been estimated at instant k, the state and covariance at next instant are then predicted through

(10)\begin{align}\hat{\textbf{x}}_{k+1}^ - &= F({\hat{\textbf{x}}_k})\end{align}

(11)\begin{align}\textbf{P}_{k+1}^ - &= {\textbf{J}_F}{\textbf{P}_k}\textbf{J}_F^T + {\textbf{Q}_k}\end{align}

As long as the new observations are available, the prediction is corrected by

(12)\begin{align}{\textbf{G}_{k + 1}} &= \textbf{P}_{k + 1}^ - \textbf{J}_H^T{({\textbf{J}_H}\textbf{P}_{k + 1}^ - \textbf{J}_H^T + {\textbf{R}_{k + 1}})^{ - 1}}\end{align}

(13)\begin{align}{\hat{\textbf{x}}_{k + 1}} &= \hat{\textbf{x}}_{k + 1}^ -{+} {\textbf{G}_{k + 1}}({\textbf{y}_{k + 1}} - H(\hat{\textbf{x}}_{k + 1}^ - ))\end{align}

(14)\begin{align}{\textbf{P}_{k + 1}} &= (\textbf{I} - {\textbf{G}_{k + 1}}{\textbf{J}_H})\textbf{P}_{k + 1}^ - \end{align}

where G_{k +1} is Kalman gain. The EKF approximates an optimal estimate through Equations (8)–(14), recursively. The first-order linearisation and the Taylor expansion around the recent estimates (not the true state) lead to additional error. UKF, which is derivative free, is proposed to overcome the limitations.

2.3.3 UKF

Unlike EKF, UKF copes with the nonlinearities by deterministically sampling around the optimal estimate ${\hat{\textbf{x}}_k}$. Firstly, 2L + 1 sigma points ${{\bf {\cal X}}_{k,i}}$(with the weights W_i) are selected through

(15)\begin{equation}\begin{aligned} {{\bf {\cal X}}_{k,0}} & ={{\hat{\textbf{x}}}_k}\\ {{\bf {\cal X}}_{k,i}} & ={{\hat{\textbf{x}}}_k} + {(\sqrt {(L + \lambda ){\textbf{P}_k}} )_i},\quad i = 1, \ldots ,L\\ {{\bf {\cal X}}_{k,i}} & ={{\hat{\textbf{x}}}_k} - {(\sqrt {(L + \lambda ){\textbf{P}_k}} )_i},\quad i = L + 1, \ldots ,2L\\ W_0^{(m)} & =\dfrac{\lambda }{{L + \lambda }},\quad W_0^{(c)} = \dfrac{\lambda }{{L + \lambda }} + (1 - {\alpha ^2} + \beta )\\ W_i^{(m)} & =W_i^{(c)} = \dfrac{1}{{2(L + \lambda )}},\quad i = 1, \ldots ,2L \end{aligned}\end{equation}

where ${(\sqrt {(L + \lambda ){\textbf{P}_k}} )_i}$ is the i-th column of the matrix square root. $\lambda ={\alpha ^2}(L + \kappa ) - L$ is the scaling parameter; $\alpha$ determines the spread of the sigma points around ${\hat{\textbf{x}}_k}$ and is usually set to a positive value not more than 1; $\kappa$ is the second scaling parameter usually set to 0; $\beta$ is used to incorporate prior knowledge of the distribution of x; for Gaussian distribution, $\beta =2$ is optimal (Wan and Van Der Merwe, Reference Wan and Van Der Merwe2000). In the prediction step, each sigma point is propagated through

(16)\begin{equation}{{\bf {\cal X}}_{k+1}}_{,i} = F({{\bf {\cal X}}_{k,i}})\end{equation}

The state and covariance prediction is derived from the weighted mean and covariance of the transformed sigma points

(17)\begin{align}\hat{\textbf{x}}_{k+1}^ - &= \sum\limits_0^{2L} {W_i^{(m)}{{\bf {\cal X}}_{k+1,i}}} \end{align}

(18)\begin{align}\textbf{P}_{k+1}^ - &=\sum\limits_0^{2L} {W_i^{(c)}[{{\bf {\cal X}}_{k+1,i}} - \hat{\textbf{x}}_{k+1}^ - ]{{[{{\bf {\cal X}}_{k+1,i}} - \hat{\textbf{x}}_{k+1}^ - ]}^T} + {\textbf{Q}_k}} \end{align}

In the correction step, the observations are predicted through

(19)\begin{align}{{\bf {\cal Y}}_{k+1,i}} &= H({{\bf {\cal X}}_{k+1,i}}),i = 0, \ldots ,2L\end{align}

(20)\begin{align}\hat{\textbf{y}}_{k+1}^ - &= \sum\limits_0^{2L} {W_i^{(m)}{{\bf {\cal Y}}_{k+1,i}}} \end{align}

Then the predicted state and covariance are corrected via

(21)\begin{align}{\hat{\textbf{x}}_{k+1}} &= \hat{\textbf{x}}_{k+1}^ -{+} {\textbf{G}_{k+1}}({\textbf{y}_{k+1}} - \hat{\textbf{y}}_{k+1}^ - )\end{align}

(22)\begin{align}{\textbf{P}_{k+1}} &= \textbf{P}_{k+1}^ -{-} {\textbf{G}_{k+1}}{\textbf{P}_{{y_k}{y_k}}}\textbf{G}_{k+1}^T\end{align}

where Kalman gain G_{k +1} is

(23)\begin{equation}{\textbf{G}_{k+1}} = {\textbf{P}_{{x_{k+1}}{y_{k+1}}}}\textbf{P}_{{y_{k+1}}{y_{k+1}}}^{ - 1}\end{equation}

where

(24)\begin{align}{\textbf{P}_{{y_{k+1}}{y_{k+1}}}} &= \sum\limits_{i = 0}^{2L} {W_i^{(c)}[{{\bf {\cal Y}}_{k+1,i}} - \hat{\textbf{y}}_{k+1}^ - ]} {[{{\bf {\cal Y}}_{k+1,i}} - \hat{\textbf{y}}_{k+1}^ - ]^T} + {\textbf{R}_{k+1}}\end{align}

(25)\begin{align}{\textbf{P}_{{x_{k+1}}{y_{k+1}}}} &= \sum\limits_{i = 0}^{2L} {W_i^{(c)}[{{\bf {\cal X}}_{k+1,i}} - \hat{\textbf{x}}_{k+1}^ - ]} {[{{\bf {\cal Y}}_{k+1,i}} - \hat{\textbf{y}}_{k+1}^ - ]^T}\end{align}

Both UKF and EKF are based on the standard KF framework; they just differ in addressing the nonlinearities: UKF approximates the state probability distribution, while EKF approximates the nonlinear state space models. Theoretically, UKF outperforms EKF and they have equal computational complexity of O(L ³). However, UKF needs to transform each sigma point through the process and observation functions at each prediction and update process, which may cost more time for high dimensional problems.

3.1.1 Sensors of experimental vehicle

The experiments were carried out via the vehicle shown in Figure 5. Several kinds of sensors are mounted on the vehicle. In the experiments, LiDAR (Velodyne HDL-64ES3), IMU (Xsens MTi-300) and GNSS (global navigation satellite system) receiver (Trimble NetR9) were used. Only fixed GPS data were received in the test fields.

Figure 5. Test vehicle

In these experiments, Autoware just generates raw vehicle state measurements within a unified coordinate system (some sample data, see Appendix B). The LiDAR measurements were generated at 10 Hz, the GPS at 20 Hz (in two drives, GPS worked at 10 Hz) and the IMU 100 Hz. In urban areas, the measurements of LiDAR and IMU were fused by the estimators; on the highway, GPS and IMU were used instead. In KF, the prediction process is performed at 100 Hz and the update process is performed as soon as a new measurement is available. The observation functions are presented in Equation (26), where the bold subscript x and y respectively indicate the system state and observation vector. As the equation expresses, all the states are observed directly, except the velocity and acceleration.

(26)\begin{equation}\begin{aligned} {{x_\textbf{y}} = {x_\textbf{x}}}\quad {{\theta _\textbf{y}} = {\theta _\textbf{x}}} &\quad {v_\textbf{y}^x = {v_\textbf{x}}\cos {\theta _\textbf{x}}}\quad {a_\textbf{y}^x = {a_\textbf{x}}\cos {\theta _\textbf{x}}}\\ {{y_\textbf{y}} = {y_\textbf{x}}}\quad {{\omega _\textbf{y}} = {\omega _\textbf{x}}} &\quad {v_\textbf{y}^y = {v_\textbf{x}}\sin {\theta _\textbf{x}}}\quad {a_\textbf{y}^y = {a_\textbf{x}}\sin {\theta _\textbf{x}}} \end{aligned}\end{equation}

3.1.2 Configurations of EKF and UKF

For EKF and UKF, it is necessary to select the appropriate process and observation noise covariance matrix Q and R and to start the algorithm with initial ${\hat{\textbf{x}}_0}$ and P₀. In this paper, Q and R are considered to be constant. The setting of these parameters follows the approach proposed by Schneider and Georgakis (Reference Schneider and Georgakis2013), and some of them are tuned empirically. Since Q and R are system and sensor dependent, they should be determined based on the user's system and sensors. The parameters used in the experiments are listed in Tables 1 and 2. In the experiments, CTRV and CTRA use identical process noise parameters. The first aligned observation is assigned to ${\hat{\textbf{x}}_0}$ and P₀ is set by

(27)\begin{equation}{\textbf{P}_0} = {\sigma ^2}\textbf{I}\end{equation}

Table 1. Standard deviation of process noise

Table 2. Standard deviation of observation noise

In this paper $\sigma = 10$. In addition, the parameters $\alpha$, $\beta$ and $\kappa$ of UKF are set to 0⋅1, 2 and 0, respectively.

3.3.1 Relative difference

The most popular indicator for estimation accuracy is root-mean-square error (RMSE). In this paper, the relative difference evaluation was provided, instead of the absolute error evaluation. In the NU and HW experiments, the state estimates E1, E2 and E3 are respectively compared with the corresponding baseline E0 to assess their relative difference. (Both the baselines of NU and HW were generated by UKF-CTRA, integrating LiDAR-IMU and GPS-IMU respectively. The configurations are presented in Section 3.1.2.) The relative RMSE (RRMSE) is formulised as

(28)\begin{equation}\textrm{RRMSE}(x) = \sqrt {\frac{1}{n}\sum\limits_{i = 1}^n {{{({x_i} - x_i^{\textrm{baseline}})}^2}} } \end{equation}

The mean RRMSEs of all the state estimates of each route are presented in Tables 5–8. From the tables, it can be found that the four filters have not made a significant difference to state estimation (not positioning). UKF and the EKF have roughly identical performance in contrast to the simulation-based experiments where UKF shows a distinct improvement of accuracy (Tsogas et al., Reference Tsogas, Polychronopoulos and Amditis2005; Yang et al., Reference Yang, Shi and Chen2017).

Table 5. The mean RRMSE of HW1

Table 6. Mean RRMSE of HW2

Table 7. Mean RRMSE of NU1

Table 8. Mean RRMSE of NU2

A trend can be found in position estimation that E2 has the smallest relative difference, then E3 and E1 has the largest relative difference, with respect to E0. This matches the conclusions of the existing literature that UKF outperforms EKF (Yang et al., Reference Yang, Shi and Chen2017) and CTRA based filters make slightly better position estimates than CTRV based filters (Schubert et al., Reference Schubert, Richter and Wanielik2008, Reference Schubert, Adam, Obst, Mattern, Leonhardt and Wanielik2011). The heading and turn rate estimates of the four filters appear to be exactly the same, which is due to the CTRV and CTRA models having the same constant turn rate hypotheses.

Compared with other state estimates, the velocity estimates of CTRA and CTRV based filters produced relatively obvious differences, in both highway and urban scenarios. The reason is the different velocity assumptions of the two models. The CTRA model takes acceleration into account and the velocity is driven by acceleration; however, the CTRV model assumes the velocity remains constant. In velocity alone, it can be observed that the relative difference of the velocity estimate in urban driving is larger than that of highway driving, which is caused by different driving styles on highway and urban routes. On the highway, drivers prefer to drive the car at an even speed most of the time; whereas, in urban areas, many accelerating/decelerating manoeuvres have to be performed, especially at road intersections. That is why acceleration in urban driving shows higher standard deviations than highway driving (Tables 3 and 4). The changes in acceleration result in the greater relative difference in velocity estimation. In low dynamic situations (Tables 5 and 6), the performance of the two models is approximately the same.

The overall results indicate that the motion model has more effect on the filter's performance than the KF form; with the same process model, the accuracy performance of EKF and UKF is almost identical. In high dynamic situations and where the speed estimate is critical, the CTRA model based filter is recommended. Otherwise, the performance of the CTRV model based filter is also acceptable.

The negligible estimation differences among the four filters are explained by the non-severe nonlinearities of the CTRV and CTRA models, which mean that the linearisation in EKF does not cause much information loss. Besides, the low dynamics of the vehicle and the high sampling rate of the sensor make the vehicular motion quasi-linear. UKF thus does not have obvious superiority.

3.3.2 Time efficiency

Limited by the space of this paper, in this section, the results of HW1-1 and HW2-1 drives, where GPS worked at 10 Hz, arre not presented. In the experiments, when using LiDAR and IMU, a complete iteration consists of 10 predictions, 10 IMU updates and one LiDAR update; for GPS and IMU, a complete iteration consists of five predictions, five IMU updates and one GPS update. This is done by using an ROS (Robot Operating System) time synchroniser (for more information, please see http://wiki.ros.org/message_filters). The experiments were run on a normal personal computer (Inter Core i7-4790 CPU @ 3⋅6 GHz, RAM 8G, Windows 10). Each drive's experiment was repeated 10 times. The running time of each estimator was recorded and the average time cost (microsecond) per iteration was calculated and shown in Tables 9–12.

Table 9. Average time cost per iteration of HW1 (GPS@20 Hz)

Table 10. Average time cost per iteration of HW2 (GPS@20 Hz)

Table 11. Average time cost per iteration of NU1 (LiDAR@10 Hz)

Table 12. Average time cost per iteration of NU2 (LiDAR@10 Hz)

In Tables 9–12, each row records the time cost of EKF and UKF, using the same model; and each column is the time cost of the filter using identical KF form and different models. The UKF/EKF column records the time cost rate of UKF and EKF with the same model, and the CTRA/CTRV row records the time cost rate of the filters using identical KF form and different models. In particular, the bottom-right cell is the time cost rate of the slowest (UKF-CTRA) and the fastest filters (EKF-CTRV). A clear conclusion can be drawn that, when utilising the same model, EKF is faster than UKF; and when utilising the same KF form, the filter using CTRV is faster than the filter using CTRA. For a higher dimension model, EKF is obviously faster than UKF since UKF has to calculate 2L + 1 sigma points by taking a matrix square root of the state covariance matrix at each iteration and propagating these sigma points at the prediction and update steps.

In Tables 9 and 10, the average time cost of each filter is shorter than that in Tables 11 and 12. This is caused by the higher update frequency of GPS (20 Hz) compared with LiDAR (10 Hz). Despite that, the UKF/EKF and CTRA/CTRV rates are approximately equal.

It is remarkable that the EKF-CTRV is about 2⋅6 times faster than the UKF-CTRA while their accuracies are almost equal (see Tables 5–8). EKF-CTRV may not make a significant improvement in efficiency in most current applications, where just the states of several vehicles need to be estimated. When the amount of vehicles increases substantially, however, it becomes another situation, such as SENSORIS (SENSORIS, 2017) where data is exchanged between the vehicle sensors and a dedicated cloud. As Figure 7 illustrates, the data from vehicles and roadside sensors are gathered into the cloud, where vehicle state estimation is essential for mobility related applications. Such a system has urgent need of real-time processing. In this situation, the EKF-CTRV filter will show its distinguished performance. For example, in the same amount of time, the EKF-CTRV can process information from nearly 2,600 vehicles; however, the UKF-CTRA can only process information from 1,000 vehicles. The efficiency of the system can be greatly improved, with negligible accuracy loss.

Figure 7. Dedicated cloud for vehicle and road side sensor

In summary, for a specific application, an ideal filter is composed of the most appropriate motion model and KF form, taking both accuracy and efficiency into consideration. In vehicular state estimation, EKF and UKF have not made distinct differences in accuracy; however, in the efficiency aspect, EKF is faster than UKF. The state estimation performance of the CTRV and CTRA model based filters is roughly identical.