## 3 State Estimation and Prediction
### Kalman Filtering  
Kalman filtering [50; 51] is a recursive algorithm that estimates the state of a system as well as the estimation uncertainty based on the prior state and the noisy measurements. The operation of the Kalman filter is described in Figure 2. In the prediction step, the state \(\hat{x}_{k|k-1}\) and the error covariance \(\hat{P}_{k|k-1}\) are projected with the state transition matrix \(A\) from the previous state \(\hat{x}_{k-1|k-1}\) and the corresponding error covariance \(\hat{P}_{k-1|k-1}\). The random variable \(w_{k}\) is the process noise, which is assumed to be normally distributed with the process noise covariance \(Q_{k}\), such that \(w_{k}~{}\sim~{}N(0,Q_{k})\). The process noise covariance \(Q_{k}\) assumed here to be constant, but it may be changed during the filter operation for adjust \begin{table}
\begin{tabular}{c c} \hline \hline
**Content** & **Description** \\ \hline Message count & Sequence number for the same type of messages originated from the same sender. \\ Temporary ID & Device identifier that is modified periodically for on-board units (OBUs). This value may be fixed for roadside units (RSUs). \\ DSRC second & Milliseconds within a minute that typically represents the moment when the position was determined. \\ Position & Geographic latitude, longitude, and height. \\ Position accuracy & Semi-major axis (length and orientation) and semi-minor axis (length) of an ellipsoid representing the position accuracy. \\ Transmission state & Vehicle transmission state (i.e., neutral, park, forward, and reverse). \\ Speed & Vehicle speed. \\ Heading & Vehicle heading. Past values may be used if the sender is stopped. \\ Steering wheel angle & Angle of the vehicle steering wheel. \\ Acceleration & Vehicle acceleration in longitudinal, lateral, and vertical axes. \\ Yaw rate & Vehicle yaw rate. \\ Brake system status & Status of the brake and other control systems (i.e., traction control, ABS, stability control, brake boost, and auxiliary brake). \\ Vehicle size & Vehicle width and length. \\ \hline \hline \end{tabular}
\end{table} 
Table 4: Data description for the basic safety message (BSM) core data frame. ment to different dynamics. In the update step, the error covariance \(\hat{P}_{k|k-1}\) is used along with the measurement matrix \(H\) and the measurement error covariance \(R_{k}\) to compute the Kalman gain \(K_{k}\). The measurement matrix \(H\) maps the state vector \(x_{k}\) to the measurement vector \(z_{k}\) such that  
\[z_{k}=Hx_{k}+v_{k}. \tag{1}\]  
The random variable \(v_{k}\) is the measurement noise, which is assumed to be normally distributed with the measurement error covariance \(R_{k}\), such that \(v_{k}\ \sim\ N(0,R_{k})\). Finally, the state \(\hat{x}_{k|k}\) and the error covariance \(\hat{P}_{k|k}\) are updated based on the Kalman gain \(K_{k}\) and the measurement vector \(z_{k}\) obtained at time step \(k\). The relative positions and the motion equations of the remote targets in the vehicle surroundings are typically given in Cartesian coordinates, and it is necessary to perform a polar-to-Cartesian transformation when the ranging measurements obtained from remote sensors (e.g., radar sensors) are in polar coordinates. An extended Kalman filter (EKF) is often utilized to handle such nonlinear systems; however, the linear approximation of a nonlinear system may result in highly unstable performance, and the derivation of the Jacobian matrices is often nontrivial in many applications [52]. More advanced nonlinear filtering approaches such as sequential Monte Carlo methods, also known as particle filters, have been introduced [53], but the computation complexity becomes enormous for high dimensional problems and the use of particle filters should be determined based on the degree of the system nonlinearity [54]. In this study, the unbiased converted measurement Kalman filter algorithm as presented in [55; 56] is employed to perform the coordinate transformation without bias and to obtain the correct covariance.