## 3 Design of Vehicle State Observer
### Square Root Cubature Kalman Filter Algorithm  
The SCKF algorithm is based on the traditional CKF algorithm framework and updates the square root of the covariance matrix directly in the form of Cholesky decomposition, which ensures non-negative characterization of the covariance matrix and improves numerical stability while reducing the algorithm's computational effort [26].  
Based on the above vehicle discrete system equations, the SCKF algorithm is calculated as follows:  
Initialization:  
\[\hat{x}_{0}=E[x_{0}] \tag{20}\]  
\[P_{0}=E[(x_{0}-\hat{x}_{0})(x_{0}-\hat{x}_{0})]^{\mathrm{T}}] \tag{21}\]  
Time update:  
Cholesky decomposition of the error covariance matrix:  
\[P_{k-1|k-1}=S_{k-1|k-1}S_{k-1|k-1}^{T} \tag{22}\]  
Calculate the set of vehicle state cubature points:  
\[X_{k-1|k-1}^{i}=S_{k-1|k-1}\zeta_{i}+\hat{x}_{k-1|k-1} \tag{23}\]  
Calculate the set of vehicle state cubature points: where \(\zeta_{i}=\sqrt{m/2}[I]_{i_{i}}\,[I]_{i}\) represents the \(i\)-th column vector of \([I]\), \(i=1,2,\,\ldots,m=2n\), and \([I]\) is the point set generated by the unit vector of n-dimensional space that is fully arranged or inverted. The generated point  
\[\text{set meets the}\,[I]=\left\{\begin{array}{c}\left[\begin{array}{c} 1\\ 0\\ \vdots\\ 0\end{array}\right]\left[\begin{array}{c}0\\ 1\\ \vdots\\ 0\end{array}\right]\cdots\left[\begin{array}{c}0\\ 0\\ \vdots\\ 1\end{array}\right]\left[\begin{array}{c}-1\\ 0\\ \vdots\\ 0\end{array}\right]\left[\begin{array}{c}0\\ -1\\ \vdots\\ 0\end{array}\right]\cdots\left[\begin{array}{c}0\\ 0\\ \vdots\\ 0\end{array}\right]\\ \vdots\\ 0\end{array}\right\}\text{condition}.\right.\]  
Cubature point propagation:  
\[X_{k|k-1}^{*}=f(X_{k-1|k-1}^{i}) \tag{24}\]  
State one-step prediction:  
\[\hat{x}_{k|k-1}=\frac{1}{2n}{\sum_{i=1}^{2n}X_{i,k|k-1}^{*}} \tag{25}\]  
Calculate the square root of the prediction error covariance matrix:  
\[S_{k|k-1}=\text{Tria}[\chi_{k|k-1}^{*},\text{Chol}(Q_{k-1})] \tag{26}\]  
The weighted central moment \(\chi_{k|k-1}^{*}\) is defined as  
\[\chi_{k|k-1}^{*}=\frac{1}{\sqrt{m}}[X_{1,k|k-1}^{*}-\hat{x}_{k|k-1},X_{2,k|k-1 }^{*}-\hat{x}_{k|k-1},X_{m,k|k-1}^{*}-\hat{x}_{k|k-1}] \tag{27}\]  
where \(\text{Tria}(\cdot)\) denotes the matrix's QR decomposition and \(S\) the upper triangular matrix after the decomposition.  
Measurement update:  
Further calculation of cubature measurement points:  
\[X_{i,k|k-1}=S_{k|k-1}\zeta_{i}+\hat{x}_{k|k-1} \tag{28}\]  
Cubature point propagation:  
\[Z_{k|k-1}^{i}=h(X_{k|k-1}^{i}) \tag{29}\]Calculate the prediction matrix of measurement:  
\[\hat{z}_{k|k-1}=\frac{1}{m}\sum_{i=1}^{m}Z_{i,k|k-1} \tag{30}\]  
Calculate the square root of the self-covariance matrix:  
\[S_{k|k-1}^{zz}=\text{Tria}[Z_{k|k-1},\text{Chol}(R_{k})] \tag{31}\]  
The weighted central moment \(Z_{k|k-1}\) is defined as  
\[Z_{k|k-1}=\frac{1}{\sqrt{m}}[Z_{1,k|k-1}-\hat{z}_{k|k-1},Z_{2,k|k-1}-\hat{z}_{ k|k-1},Z_{m,k|k-1}-\hat{z}_{k|k-1}] \tag{32}\]  
Calculate the self-covariance matrix and the cross-covariance matrix:  
\[P_{k|k-1}^{zz}=S_{k|k-1}^{zz}(S_{k|k-1}^{zz})^{\text{T}} \tag{33}\]  
\[P_{k|k-1}^{xz}=X_{k|k-1}Z_{k|k-1}^{\text{T}} \tag{34}\]  
The weighted central moment \(X_{k|k-1}\) is defined as  
\[X_{k|k-1}=\frac{1}{\sqrt{m}}[X_{1,k|k-1}-\hat{x}_{k|k-1},X_{2,k|k-1}-\hat{x}_{ k|k-1},X_{m,k|k-1}-\hat{x}_{k|k-1}] \tag{35}\]  
Calculating the Kalman gain matrix:  
\[K=P_{k|k-1}^{xz}(P_{k|k-1}^{zz})^{-1} \tag{36}\]  
Update the posterior prediction of state:  
\[\hat{x}_{k|k}=\hat{x}_{k|k-1}+K(z_{k}-\hat{z}_{k|k-1}) \tag{37}\]  
Calculate the square root factor of the error covariance matrix:  
\[S_{k|k}=\text{Tria}([x_{k|k-1}-KZ_{k|k-1},K\text{chol}(R_{k})]) \tag{38}\]  
where \(Q_{k}\) and \(R_{k}\) are the system noise and measurement noise variance, respectively.