## 2 Estimation of Sideslip Angle
### Design of Sideslip Angle Algorithm
\begin{array}{l}\hat{x}_{j,i,k|k-1}^{x}=f\left(\hat{x}_{j,i,k-1}^{x}, \hat{x}_{j,i,k-1}^{W}\right)\\ \hat{x}_{i,k|k-1}=\sum\limits_{j=1}^{M}w_{j,i}^{m}\hat{x}_{j,i,k|k-1}^{x}\\ \hat{p}_{i,k|k-1}=\sum\limits_{j=1}^{M}w_{j,i}^{c}\left(\hat{x}_{j,i,k|k-1}^{x} -\hat{x}_{i,k|k-1}\right)\times\\ \left(\hat{x}_{j,i,k|k-1}^{x}-\hat{x}_{i,k|k-1}\right)^{T}+Q\end{array} \right.. \tag{13}\]  
2 update measurement:  
\[\left\{ \begin{array}{l}z_{j,i,k|k-1}=h\left(\hat{x}_{j,i,k-1}^{x},\hat{x}_{ j,i,k-1}^{V}\right)\\ \hat{z}_{i,k|k-1}=\sum\limits_{j=1}^{M}w_{j,i}^{m}\hat{x}_{j,i,k|k-1}^{x}\\ \end{array} \right.. \tag{14}\]  
3 calculate covariance matrix:  
\[\left\{ \begin{array}{l}P_{i,zz}=\sum\limits_{j=1}^{M}w_{j,i}^{c}\left(z_{j, i,k|k-1}-\hat{z}_{i,k|k-1}\right)\times\\ \left(z_{j,i,k|k-1}-\hat{z}_{i,k|k-1}\right)^{T}+R\\ P_{i,xz}=\sum\limits_{j=1}^{M}w_{j,i}^{c}\left(\hat{x}_{j,i,k|k-1}^{x}-\hat{ x}_{i,k|k-1}\right)\times\\ \left(z_{j,i,k|k-1}-\hat{z}_{i,k|k-1}\right)^{T}\end{array} \right.. \tag{15}\]  
4 compute lower-layer Kalman gain:  
\[K_{i,k}=P_{i,xz}P_{i,z}^{-1}. \tag{16}\]  
5 update status estimated values and covariance matrix:  
\[\left\{ \begin{array}{l}\hat{x}_{i,k}=\hat{x}_{i,k|k-1}+K_{i,k}\left(z_{k}- \hat{z}_{i,k|k-1}\right)\\ \hat{p}_{i,k}=\hat{p}_{i,k|k-1}-K_{i,k}\hat{P}_{i,zz}K_{i,k}^{T}\end{array} \right.. \tag{17}\]  
(4) update upper-layer UKF. 1 update weights:  
\[\left\{ \begin{array}{l}w_{i}^{m}=w_{i}^{m}\frac{p\left(z_{k}|\hat{x}_{i,k} \right)p\left(\hat{x}_{i,k}\right)}{q\left(\hat{x}_{i,k}\right|\hat{x}_{i,k-1} \right)}\\ w_{i}^{c}=w_{i}^{c}\frac{p\left(z_{k}|\hat{x}_{i,k}\right)p\left(\hat{x}_{i,k }\right|\hat{x}_{i,k-1}\right)}{q\left(\hat{x}_{i,k}\right|\hat{x}_{i,k-1} \right)}\end{array} \right.. \tag{18}\]  
where \(p\) is the importance density function; \(q\) is the suggested distribution function. The updated weights are normalized:  
\[\left\{ \begin{array}{l}w_{i}^{m}=\frac{w_{i}^{m}}{\sum\limits_{j=1}^{M}w_{ i}^{m}}\\ w_{i}^{c}=\frac{w_{i}^{m}}{\sum\limits_{j=1}^{M}w_{j}^{c}}\end{array} \right.. \tag{19}\]2 compute the initial value and covariance at time k:  
\[\left\{ \begin{array}{l}\hat{x}_{\hat{t}}=\sum\limits_{i=1}^{N}w_{i}^{m}\hat{x}_ {i,k}\\ \hat{P}_{k}^{L}=\sum\limits_{j=1}^{M}w_{j,j}^{e}\Big{(}\hat{x}_{i,k}-\hat{x}_{ \hat{t}}\Big{)}\times\Big{(}\hat{x}_{i,k}-\hat{x}_{\hat{t}}\Big{)}^{T}+Q\\ \end{array} \right.. \tag{20}\]  
3 update measurement:  
Based on the prediction status \(\hat{x}_{\hat{t}}\) and prediction covariance matrix \(\hat{P}_{k}^{L}\) at time \(k\) determined from the above equation, \(N\) weighted sampling points \(x\) are selected by using symmetrical sampling and their measurements are updated again:  
\[\left\{ \begin{array}{l}z_{i,k}^{L}=h\Big{(}x_{i,k}^{L},x_{i,k}^{V}\Big{)}\\ z_{k}^{L}=\sum\limits_{j=1}^{N}w_{i}^{m}z_{i,k}^{L}\\ \end{array} \right.. \tag{21}\]  
4 calculate covariance matrix:  
\[\left\{ \begin{array}{l}P_{zz}=\sum\limits_{j=1}^{N}w_{i}^{e}\Big{(}z_{i,k}^ {L}-\hat{z}_{k}^{L}\Big{)}\left(z_{i,k}^{L}-\hat{z}_{k}^{L}\right)^{T}+R\\ P_{xz}^{L}=\sum\limits_{j=1}^{N}w_{i}^{e}\Big{(}x_{i,k}^{L}-\hat{x}_{\hat{t}} \Big{)}\left(z_{i,k}^{L}-\hat{z}_{k}^{L}\right)^{T}\\ \end{array} \right.. \tag{22}\]  
5 compute upper-layer Kalman gain:  
\[K_{k}^{L}=P_{xz}^{L}\Big{(}P_{zz}^{L}\Big{)}^{-1}. \tag{23}\]  
6 update status estimated values and covariance matrix:  
\[\left\{ \begin{array}{l}\hat{x}_{k}=\hat{x}_{k}+K_{k}^{L}\big{(}z_{k}-\hat{z }_{k}^{L}\big{)}\\ \hat{P}_{k}=\hat{P}_{k}^{L}-K_{k}^{L}P_{zz}^{L}\big{(}K_{k}^{L}\big{)}^{T}\\ \end{array} \right.. \tag{24}\]  
Through the above repetitions, the estimated value of the system at any time can be computed. The convergence of ADUKF is guaranteed by adjusting Q and R.