# Autonomous Navigation Based on the Earth-Shadow Observation near the Sun-Earth L2 Point
## 3 Design of the Navigation Filter Algorithm
### Fifth-Degree Cubature Kalman Filter  
Consider the general nonlinear state space model of the discrete-time system  
\[\mathbf{x}_{k+1}=\mathbf{f}(\mathbf{x}_{k})+\mathbf{w}_{k} \tag{32}\]  
\[\mathbf{z}_{k+1}=\mathbf{h}(\mathbf{x}_{k+1})+\mathbf{v}_{k+1}. \tag{33}\]  
where \(\mathbf{x}_{k}\in\mathbb{R}^{n}\) is the system state vector at time \(k\), \(\mathbf{f}(\cdot)\) is the nonlinear state function, \(\mathbf{z}_{k}\in\mathbb{R}^{m}\) is the measurement vector at time \(k\), and \(\mathbf{h}(\cdot)\) is the nonlinear measurement function. \(\mathbf{w}_{k}\) and \(\mathbf{v}_{k}\) are the system process noise and measurement noise at time \(k\), and their statistical characteristics meet the following formulas:  
\[\begin{array}{l}E[\mathbf{w}_{k}]=0,\text{Cov}\left(\mathbf{w}_{k},\mathbf{w}_{j}\right) =\mathbf{Q}_{k}\delta_{i,j}\\ E[\mathbf{v}_{k}]=0,\text{Cov}\left(\mathbf{v}_{k},\mathbf{v}_{j}\right)=\mathbf{R}_{k}\delta_ {i,j}\\ \text{Cov}\left(w_{i},v_{j}\right)=0,\end{array} \tag{34}\]  
where \(\mathbf{Q}_{k}\in\mathbb{R}^{n\times n}\) is the noise covariance matrix of the positive-semidefinite process, \(\mathbf{R}_{k}\in\mathbb{R}^{n\times m}\) is the noise covariance matrix of positive-definite process, and \(\delta_{ij}\) is the Kronecker product.  
In the framework of nonlinear Kalman filtering, the Gauss integral is approximated by the fifth-order Spherical-Radial rule in Bayesian estimation. Therefore, the fifth-degree CKF can be constructed. According to Section 3.1, it can be seen that the key step of CKF is to convert the Gauss integral into the spherical radial integral domain because it is easier to calculate. In the Gauss integral problem, we are to approximate the Gauss integral through the combination of spherical integral and radial integral. The general steps of nonlinear Kalman filtering in the fifth-degree CKF will be omitted below. The core part is to explain the selection of integral points and the set of weights.  
The integral point \(\zeta_{i}\) is obtained from the following equation:  
\[\zeta_{i}=\left\{\begin{array}{l}\mathbf{0}_{n+1},i=0\\ \sqrt{n+2}\cdot\mathbf{y}_{1}^{+},i=1,\ldots,n(n-1)/2\\ -\sqrt{n+2}\cdot\mathbf{y}_{1-n(n-1)/2}^{+},i=n(n-1)/2+1,\ldots,n(n-1)\\ \sqrt{n+2}\cdot\mathbf{y}_{1-n(n-1)}^{-},i=n(n-1)+1,\ldots,3n(n-1)/2\\ -\sqrt{n+2}\cdot\mathbf{y}_{1-3n(n-1)2}^{-},i=3n(n-1)/2+1,\ldots,2n(n-1)\\ \sqrt{n+2}\cdot\mathbf{e}_{i-2n(n-1)},i=2n(n-1)+1,\ldots,n(2n-1)\\ \sqrt{n+2}\cdot\mathbf{e}_{i-n(2n-1)},i=n(2n-1)+1,\ldots,2n^{2}.\end{array}\right. \tag{35}\]  
The corresponding weight \(\mathbf{w}_{i}\) of each integral point is as follows:  
\[w_{i}=\left\{\begin{array}{l}\frac{2}{n+2},i=0\\ \frac{4}{(n+2)^{2}},i=1,\ldots,2n(n-1)\\ \frac{4-n}{(n+2)^{2}},i=2n(n-1)+1,\ldots,2n^{2}.\end{array}\right. \tag{36}\]  
The fifth-degree CKF proposed in this paper selects \(2n^{2}+1\) sigma points. According to the last term of Equation (36), there will be a negative weight when the dimension of the system is greater than four. However, the negative weight in the fifth-degree CKF tends to be zero, which will not cause the divergence problem in the high-dimensional system.