## 3 Estimation Algorithms Design for Vehicle State Estimation
### Unscented Kalman Filter  
Addressing the main drawbacks of the EKF, Jacobian matrices calculation and the first order Taylor expansion on nonlinear system, the UKF utilizes a deterministic sampling technique known as the unscented transform (UT) to pick a minimal set of sample points (called sigma points) around the mean, which is a derivative-free alternative to EKF and meanwhile avoids the expensive update of the Jacobian matrix on each iteration [33]. Additionally, UKF achieves higher-order Taylor series expansion accuracy [27,28]. Considering a nonlinear time-discrete \(\mathbf{y}=g(\mathbf{x})\) with mean \(\overline{\mathbf{x}}\) and covariance \(\mathbf{P}_{\mathbf{x}}\), to calculate the statistics of \(\mathbf{y}\), \(2L+1\) sigma points \(\chi_{i}\) with its corresponding weighting factors is formulated via the following equations:  
\[\left\{ \begin{array}{c}\chi_{0}=\overline{\mathbf{x}}\ i=0\\ \chi_{i}=\overline{\mathbf{x}}+\sqrt{(L+\lambda)\mathbf{P}_{\mathbf{x}_{i}}}\ i=1,\ldots \ \ldots,L\\ \chi_{i}=\overline{\mathbf{x}}-\sqrt{(L+\lambda)\mathbf{P}_{\mathbf{x}_{i-L}}\ i=L+1, \ldots\ \ldots,2L}\\ W_{0}^{(m)}=\lambda/(L+\lambda)\\ W_{0}^{(m)}=\lambda/(L+\lambda)+1-\alpha^{2}+\beta\\ W_{1}^{(m)}=W_{1}^{(c)}=0.5/(L+\lambda)\ i=1,2,\ldots,2L\end{array} \right. \tag{23}\]where \(L\) is the dimension of \(\mathbf{x}\); \(\lambda=\alpha^{2}(L+\kappa)-L\) is a scaling parameter. \(\alpha\) determines the spread of the sigma points around \(\overline{x}\) and is usually set to a small positive value (e.g., \(10^{-3}\)). \(\kappa\) is a secondary scaling parameter that is normally set to a non-negative value to ensure that the covariance matrix is positive definite. \(\beta\) is used to incorporate prior knowledge of the distribution of \(\mathbf{x}\), which affects the weighting of the zeroth sigma point for the calculation of the covariance. For Gaussian distribution, \(\beta=2\) is optimal [28]. These sigma vectors are propagated through a nonlinear function, \(y_{i}=g(\chi_{i})\), \(i=0\), \(1\),..., \(2L\). The mean and covariance of \(\mathbf{y}\) are estimated using the weighted sample mean and covariance of the posterior sigma points as follows:  
\[\overline{\mathbf{y}}=\sum_{i=0}^{2L}W_{i}^{(m)}\mathbf{y}_{i} \tag{24}\]  
\[\mathbf{P_{y}}=\sum_{i=0}^{2L}W_{i}^{(c)}(\mathbf{y_{i}}-\overline{\mathbf{y}})(\mathbf{y}_{i }-\overline{\mathbf{y}})^{T}. \tag{25}\]  
On the basis of unscented transform, the main steps of UKF are as in Figure 2 and put forward in the following:  
1. Initialize vehicle state and covariance matrix at time step \(k=0\) with \[\hat{\mathbf{x}}_{0}=E[\mathbf{x_{0}}]\] (26) \[\mathbf{P_{x0}}=E\Big{[}(\mathbf{x_{0}}-\hat{\mathbf{x}}_{0})\cdot(\mathbf{x_{0}}-\hat{\mathbf{x} }_{0})^{T}\Big{]}.\] (27)
2. For time step \(k=1\), \(2\)..., calculate sigma points in sigma vector: \[\chi(k-1)=\left[ \begin{array}{c}\hat{\mathbf{x}}(k-1)\\ \hat{\mathbf{x}}(k-1)+\sqrt{(L+\lambda)\mathbf{P}(k-1)}\\ \hat{\mathbf{x}}(k-1)-\sqrt{(L+\lambda)\mathbf{P}(k-1)}\end{array} \right]^{T}.\] (28)
3. Time update \(\bullet\) Propagate the sigma points through Equation (22): \[\chi(k|k-1)=f(\chi(k-1),\mathbf{u}(k-1),\mathbf{w}(k-1)).\] (29)
4. The propagated mean calculation: \[\hat{\mathbf{x}}(k|k-1)=\sum_{i=0}^{2L}W_{i}^{(m)}\cdot\chi_{i}(k|k-1).\] (30)
5. The propagated covariance calculation: \[\mathbf{P_{x}}(k|k-1)=\sum_{i=0}^{2L}W_{i}^{(c)}\cdot[\chi_{i}(k|k-1)-\hat{\mathbf{x }}(k|k-1)]\cdot\left[\chi_{i}\left(k|k-1\right)-\hat{\mathbf{x}}(k|k-1)\right]^{T}+ \mathbf{Q_{k}}.\] (31)
6. Measurement update \(\bullet\) Propagate sigma points through measurement function: \[\mathbf{y}(k|k-1)=h(\mathbf{\chi}(k-1),\mathbf{u}(k-1),\mathbf{v}(k-1))\] (32)
7.