## 3 Target State Prediction with Uncertainty Estimation
### Recursive Uncertainty Estimation  
In the previous chapter, the target state prediction with the intention inference algorithm has been proposed based on the IMM approach. Because the future states are predicted based on the present and past information, the prediction uncertainty inevitably exists regardless of the prediction methods. In addition, the level of uncertainty is dependent on the prediction algorithm and the quality of the object data from the perception algorithm. Therefore, a proper algorithm for uncertainty estimation is required to increase the safety and control performance of the prediction-based motion planner. In this study, a recursive covariance estimator has been used to estimate the prediction uncertainty of the prediction results [28]. Because the prediction algorithm is based on the process update model of the EKF using IDM, the recursive covariance estimator is applicable to estimate the process noise of each state, such as the position and velocity. The basic concept of the prediction uncertainty estimation is a comparison between the current true states and predicted states derived from the true states of the previous step. The graphical representation of the relationships is described in Figure 8.  
The recursive covariance estimator assumes that the measurement noise is stationary. As mentioned in the previous chapter, the position of the target vehicle is defined in a fixed global coordinate, not a local coordinate of the autonomous vehicle. In other words, \(d_{DTC}\), the position of the state vector, is defined with respect to the conflict point on the intersection map. Therefore, the proposed prediction algorithm satisfies the assumption of the recursive covariance estimator. Since EKF is already used, the process update and measurement can be written in the linearized equation as follows:  
\[\begin{split} x_{k}&=A_{k-1}x_{k-1}+B_{k-1}w_{k-1} \\ y_{k}&=Cx_{k}+v_{k}\end{split} \tag{11}\]  
where \(w_{k-1}\) and \(v_{k}\) are the covariances of the prediction model and perception module. \(A_{k-1}\), \(B_{k-1}\), and \(C\) are the system matrix, input matrix, and output matrix, respectively. In this study, \(A_{k-1}\) is the same matrix \(F_{k}\) in the previous section. In Equation (11), \(x_{k-1}\) is the vehicle state at (\(k-1\)), and \(x_{k}\) is the predicted state using the prediction model. This means that \(x_{k}\) is the first-step prediction results from the true state \(x_{k-1}\) using the above-linearized equation.  
\(y_{k}\) can be represented in terms of \(x_{k-1}\) as follows:  
\[y_{k}=CA_{k-1}x_{k-1}+CB_{k-1}w_{k-1}+v_{k} \tag{12}\]  
Figure 8: Basic concepts of recursive covariance estimator-based prediction uncertainty estimation.  
\(x_{k-1}\) is represented in terms of \(y_{k-1}\) as follows:  
\[\begin{array}{c}x_{k-1}=\left[C^{T}C\right]^{-1}C^{T}y_{k-1}-\left[C^{T}C\right] ^{-1}C^{T}v_{k-1}\\ =My_{k-1}-Mv_{k-1}\\ \text{where, }\ M=\left[C^{T}C\right]^{-1}C^{T}\end{array} \tag{13}\]  
\(y_{k}\) can be represented as follows by substituting (13) into (12):  
\[y_{k}=CA_{k-1}My_{k-1}-CA_{k-1}Mv_{k-1}+CB_{k-1}w_{k-1}+v_{k} \tag{14}\]  
In Equation (14), the only unknown term is \(w_{k-1}\). Because \(y_{k}\) and \(y_{k-1}\) are the measurement from the perception algorithm, \(v_{k}\) and \(v_{k-1}\) are the same measurement noise under the assumption of stationary. Therefore, the recursive estimator for \(w_{k-1}\) can be defined by multiplying \(M\) to (14) and introducing the new notations \(\xi_{k}\) and \(V_{k}\):  
\[\begin{array}{c}My_{k}-AMy_{k-1}=Bw_{k-1}+Mv_{k}+AMv_{K-1}\\ \xi_{k}=Bw_{k-1}+V_{k}\\ \text{where, }\xi_{k}=My_{k}-AMy_{k-1}\\ V_{k}=Mv_{k}-AMv_{k-1}\end{array} \tag{15}\]  
The covariance of \(w_{k-1}\) is estimated as a recursive form as follows:  
\[\begin{array}{c}Cov_{k}(\xi)=\frac{k-1}{k-1}Cov_{k}(\xi)+\frac{1}{k}\xi_{k} \xi_{k}{}^{T}\\ Cov_{k}(Bw)=Cov_{k}(\xi)+Cov(V)\end{array} \tag{16}\]  
Using (16), the covariance of the process noise \(w_{k}\) is estimated.