*3.2. Mixed-Degree Cubature Information Filter*

In general, the four integrals in Equations (16) and (17) cannot be solved directly. A group of numerical integration-based approximation methods named by cubature rules are used for the filter process and the resulting filters are called cubature filters. Using a cubature rule, a Gaussian weighted integral of a nonlinear function *g*(*x*) is approximated with a weighted sum as follows:

$$\int\_{\mathbb{R}^n} \operatorname{g}(\mathbf{x}) \mathcal{N} \left( \mathbf{x}; \mathbf{\hat{x}}\_{k-1|k-1}, P\_{k-1|k-1} \right) d\mathbf{x} \approx \sum\_{j=1}^{N\_p} w\_j \mathbf{g} \left( \mathbf{X}\_j \right) \tag{28}$$

where *X<sup>j</sup>* represents the cubature points; *wj* is the corresponding weight that satisfies *Np* ∑ *j*=1 *wj* = 1; and *Np* is the number of the cubature points. The formulations of the cubature points and weights are different in different cubature rules. The spherical-radial (SR) cubature rules use the spherical-radial coordinates to transform the original integral into a double integral and select the cubature points on the spherical coordinate and the radial coordinate respectively. Based on the SR rules, the spherical simplex-radial (SSR) cubature rules use the n-simplex to evaluate the cubature points on the spherical coordinate.

The degree of a cubature rule is defined based on the degree of used monomials in the Tayler polynomial of the nonlinear function. A *p*-degree cubature rule integrates all of the monomials in the Tayler polynomial of *g*(*x*) up to degree *p* exactly but not exactly for some monomials of degree *p* + 1. In general, the cubature rule with a higher degree achieves higher accuracy, but suffers from more computation with more cubature points and worse numerical stability with potential negative weights. Practically, third-degree cubature rules and fifth-degree cubature rules are commonly used in the Bayes filter. The numbers of cubature points of the four cubature rules are listed in Table 2.

**Table 2.** Number of cubature points of different rules.


A cubature rule with all weights being positive is more desirable than one with both positive and negative weights for the sake of numerical stability. A stability parameter was defined in [33] as the sum of absolute values of the weights:

$$\Theta = \sum\_{i=0}^{N\_p} |w\_i| \tag{29}$$

When Θ is larger than 1, the truncation error will decrease the accuracy of the numerical integration. The curves of Θ with respect to *n* for different cubature rules are shown in Figure 2. The stability parameters of 3-SR and 3-SSR were maintained at 1, while the stability parameter of 5-SR increased since *n* > 4, and the stability parameter of 5-SSR increased since *n* > 7.

**Figure 2.** The curves of Θ with respect to *n* for different cubature rules.

According to the previous motion and measurement models, the time-update phase involves Gaussian-weighted integrals in 9-dimensional space while the measurement-update phase involves Gaussian-weighted integrals in 6-dimensional space. To obtain high precision and numerical stability with as few cubature points as possible, we applied the 3-SR rule in the time-update phase and the 5-SSR rule in the measurement-update phase. Substituting the chosen cubature rules, we formulated the VIO process based on the mixed-degree cubature information filter as follows.
