## 4 Higher-Order Cubature Kalman Filter Design
### Design GHCKF/AGICK Filter  
Considering the discrete nonlinear system in the form of Equation (16), the GHCKF algorithm is used to solve the integral on the real domain \(R^{n}\):  
\[I(g)=\int_{R^{n}}g(x)\exp(-x^{T}x)dx \tag{26}\]We select the first, second, and third G-trajectories to form the generalized fifth-order cubature integral formula, transform it to the standard Gaussian distribution, and then obtain the following formula [32,33]:  
\[ \begin{array}{rl}I(g)&=\int_{R^{n}}g(x)N(x;0,I)dx\\ &=\frac{1}{\sqrt{\pi n}}\int_{R^{n}}g(\sqrt{2}x)\exp(-x^{T}x)dx\\ &=(1-\frac{(\gamma-n)n}{18})g([0])+\frac{4-n}{18}\sum\limits_{i=1}^{2n}g( \left[\sqrt{3}\right]_{i})+\\ &\frac{1}{36}\sum\limits_{i=1}^{2n(n-1)}g(\left[\sqrt{3},\sqrt{3}\right]_{i}) \\ &=\sum\limits_{i=1}^{2n^{2}+1}w_{i}g(\xi_{i})\end{array}  \tag{27}\]  
Among the values, the cubature points \(\xi_{i}\) and the weight values \(w_{i}\) are, respectively,  
\[\xi_{i}=\left\{ \begin{array}{l}\left[0\right]_{i},i=1;\\ \left[\sqrt{3}\right]_{i},i=2,\cdots,2n+1;\\ \left[\sqrt{3},\sqrt{3}\right]_{i},i=2n+2,\cdots,2n^{2}+1.\end{array} \right. \tag{28}\]  
\[w_{i}=\left\{ \begin{array}{l}\left(1-(7-n)n/18\right),i=1;\\ \left(4-n\right)/18,i=2,\cdots,2n+1;\\ 1/36,i=2n+2,\cdots,2n^{2}+1.\end{array} \right. \tag{29}\]  
In the above equations, \(n\) is the number of state variables. We place Equations (27)-(29) under the Kalman filter framework to obtain the standard GHCKF algorithm [34]. The GHCKF algorithm for vehicle state estimation is as follows:  
1. Initialize state estimated values and error covariance: \[\left\{ \begin{array}{l}\widehat{x}_{k}=E(x_{0})\\ \\ P_{k}=E(x_{0}-\widehat{x}_{k})(x_{0}-\widehat{x}_{k})^{T}\end{array} \right.\] (30)
2. Time update (1) Calculate cubature points \({x_{k}}^{i}\) (\(i=1,2,\ldots,2n^{2}+1\)): \[S_{k}=SVD(P_{k})\] (31) \[{x_{k}}^{i}=S_{k}\xi_{i}+\widehat{x}_{k}\] (32) (2) Calculate cubature points propagated by the state equation \({x_{k}}_{|k+1}{}^{i}\): \[{x_{k}}_{|k+1}{}^{i}=f(x_{k}^{i})\] (33) (3) Calculate the state estimation value at the current moment \(\widehat{x}_{k}{}_{|k+1}{}^{i}\): \[\widehat{x}_{k}{}_{|k+1}=\sum\limits_{1}^{2n^{2}+1}w_{i}{x_{k}}_{|k+1}{}^{i}\] (34) (4) Calculate the prediction error covariance \(P_{k}{}_{|k+1}\): \[P_{k}{}_{|k+1}=\sum\limits_{1}^{2n^{2}+1}w_{i}({x_{k}}_{|k+1}{}^{i}-\widehat{x }_{k}{}_{|k+1})({x_{k}}_{|k+1}{}^{i}-\widehat{x}_{k}{}_{|k+1})^{T}+Q_{k}\] (35)In the equation, \(Q_{k}\) is the process noise covariance. 3. Measurement update (1) Calculate cubature points \({x_{k}}^{i}\) (i = 1, 2, \(\ldots\), \(2n^{2}+1\)): \[S_{k+1}=SVD(P_{k|k+1})\] (36) \[{x_{k+1}}^{i}=S_{k+1}\widehat{\xi}_{i}+\widehat{x}_{k|k+1}\] (37) (2) Calculate cubature points propagated by the measurement equation \({z_{k}}_{|k+1}{}^{i}\): \[{z_{k}}_{|k+1}{}^{i}=h(x_{k+1}^{i})\] (38) (3) Calculate the measurement prediction value at the current moment \(\widehat{z}_{k}{}_{|k+1}{}^{i}\): \[\widehat{z}_{k}{}_{|k+1}=\sum\limits_{1}^{2n^{2}+1}{w_{i}}{z_{k}}_{|k+1}{}^{i}\] (39) (4) Calculate measurement error covariance \(P_{k+1}{}^{zz}\) and cross correlation covariance \(P_{k+1}{}^{zz}\): \[P_{k+1}^{zz}=\sum\limits_{1}^{2n^{2}+1}{w_{i}}({z_{k}}_{|k+1}{}^{i}-\widehat{z }_{k}{}_{|k+1})({z_{k}}_{|k+1}{}^{i}-\widehat{z}_{k}{}_{|k+1})^{T}+R_{k}\] (40) \[{x_{k}}^{i}=S_{k}\xi_{i}+\widehat{x}_{k}\]0 (41) In the equation, \(R_{k}\) is the measurement of noise covariance. 4. Update the filter gain matrix \(\mathbf{K}_{k+1}\), the state variable matrix \(\widehat{x}_{k+1}\), the error covariance matrix \(\mathbf{P}_{k+1}\): \[{x_{k}}^{i}=S_{k}\xi_{i}+\widehat{x}_{k}\]1 (42) \[{x_{k}}^{i}=S_{k}\xi_{i}+\widehat{x}_{k}\]2 (43) \[{x_{k}}^{i}=S_{k}\xi_{i}+\widehat{x}_{k}\]3 (44) In the formula, \(\mathbf{z}_{k}\) is the measured value at the current time. 5.