# Research on the SSUKF Integrated Navigation Algorithm Based on Adaptive Factors
## 3 INS/GNSS Integrated Navigation Algorithm Design
### The Specific Steps of the SSUKF Algorithm  
Step 1: Initialize the State Vector \(X_{0}\) and State Covariance Matrix \(P_{0}\)  
Step 2: Calculate \(n+2\) Sampling Points  
The process of generating sampling points is one of the core steps in the filtering algorithm. Using the spherical simplex construction method, we generate \(n+2\) sampling points, where \(n\) is the dimension of the system's state vector. First, we calculate the weights \(\omega_{0}\) and \(\omega_{1}\), which determine the contribution of each sampling point in the computation. The formula for calculating the weights is as follows:  
\[\omega_{0}=1/(n+1),\ \ \omega_{1}=(1-\omega_{0})/(n+1) \tag{6}\]  
The sigma points of the spherical simplex are generated by constructing an orthogonal basis, and the distribution of the sampling points is determined by the geometric properties of the spherical simplex. Specifically, the distribution of the sampling points is as follows:  
\[s=[0,-\frac{1}{\sqrt{2}},\frac{1}{\sqrt{2}}] \tag{7}\]\[\left\{\begin{aligned} s_{1}(:,i+1)=[s(:,i+1);-\frac{1}{\sqrt{j(j+1)}}] \\ s_{1}(:,j+2)=[\mathbf{0}_{j-1};-\frac{i}{\sqrt{j(j+1)}}]\end{aligned} \right.\ i=1:j,j=2:n \tag{8}\]  
The parameters for generating the sigma points, denoted as \(s\) (with dimensions \(n\times(n+2)\)), and the weights \(w\) are as follows:  
\[s=\frac{s_{1}}{\sqrt{\omega_{1}}}\ w=[\omega_{0},\omega_{1},\cdots\omega_{1}] \tag{9}\]  
Step 3: Time Update  
First, the state vector update is computed using the Cholesky decomposition method. Then, the sampling points are used to predict the state, resulting in an updated state vector \(\mathbf{\gamma}\). Finally, a weighted average of all the sampling points is computed to obtain the updated covariance matrix \(\mathbf{P}_{k/k-1}\). The specific formulas are as follows:  
\[\mathbf{X}_{k/k-1}=X_{0}+chol(P_{0})\cdot sk=1,\ldots,n+2 \tag{10}\]  
\[y=\omega_{0}\cdot f(X_{0})\ \mathbf{Y}=[y,\ldots,y]_{2n} \tag{11}\]  
\[y=y+w(k)\cdot\mathbf{\gamma}(:,k),k=2,\ldots,n+2 \tag{12}\]  
\[\mathbf{P}_{k/k-1}=\sum_{k=1}^{n+2}w(k)\cdot(\mathbf{Y}_{k}-y)(\mathbf{Y}_{k}-y)^{T}+ \Gamma_{k}Q_{k}\Gamma_{k}^{T} \tag{13}\]  
Step 4: Measurement Update  
In this phase, we calculate the predicted measurement, \(\mathbf{\hat{Z}}_{k/k-1}\), using the measurement matrix, \(H_{k}\). The state vector is then updated based on the difference between the predicted measurement and the actual measurement. The key part of the measurement update is the calculation of the measurement covariance matrix, \(\mathbf{P}_{ZZ}\), and the covariance between the state and the measurements, \(\mathbf{P}_{XZ}\). These matrices are used to compute the Kalman gain, which is then applied to correct the state estimate based on the actual measurement data. The specific formulas are as follows:  
\[\mathbf{\hat{Z}}_{k/k-1}=H_{k}\mathbf{\hat{X}}_{k/k-1} \tag{14}\]  
\[\mathbf{P}_{ZZ}=H_{k}\mathbf{P}_{k/k-1}H_{k}^{T}+\mathbf{R}_{k} \tag{15}\]  
\[\mathbf{P}_{XZ}=\mathbf{P}_{k/k-1}H_{k}^{T} \tag{16}\]  
Step 5: Filter Update  
The filter update phase is the final step of the entire process, and its goal is to correct the state estimate based on the Kalman gain. First, the Kalman gain, \(\mathbf{K}_{k}\), is calculated. Then, using the Kalman gain, the difference between the predicted state and the actual measurement is combined to update the system's state estimate. Finally, the updated covariance matrix, \(\mathbf{P}_{k}\), is computed. The specific formulas are as follows:  
\[\mathbf{K}_{k}=\mathbf{P}_{XZ}(\mathbf{P}_{ZZ})^{-1} \tag{17}\]  
\[\mathbf{\hat{X}}_{k}=\mathbf{\hat{X}}_{k/k-1}+\mathbf{K}_{k}(\mathbf{Z}_{k}-\mathbf{\hat{Z}}_{k/k- 1}) \tag{18}\]  
\[\mathbf{P}_{k}=\mathbf{P}_{k/k-1}-\mathbf{K}_{k}\mathbf{P}_{k/k-1}\mathbf{K}_{k}^{T} \tag{19}\]  
At this point, the basic process of SSUKF has been completed.