# Mixed-Degree Cubature \(H_{\infty}\) Information Filter-Based Visual-Inertial Odometry
## 3 Mixed-Degree Cubature \(H_{\infty}\) Information Filter-Based VIO
### Mixed-Degree Cubature Information Filter
\begin{table}
\begin{tabular}{c c} \hline \hline
**Cubature rules** & \(\mathbf{N_{\text{P}}}\) \\ \hline
3 rd-degree spherical-radial cubature rule (3-SR) & \(2n\) \\
5 th-degree spherical-radial cubature rule (5-SR) & \(2n^{2}+1\) \\
3 rd-degree spherical simplex-radial cubature rule (3-SSR) & \(2n+2\) \\
5 th-degree spherical simplex-radial cubature rule (5-SSR) & \(n^{2}+3n+3\) \\ \hline \hline \end{tabular}
\end{table} 
Table 2: Number of cubature points of different rules. Figure 2: The curves of \(\Theta\) with respect to \(n\) for different cubature rules. \[\mathbf{\gamma}_{j}^{\text{SR3}}=\left\{ \begin{array}{ccc}\mathbf{a}_{j}&,&1\leq j\leq n \\ -\mathbf{a}_{j-n}&,&n+1\leq j\leq 2n\end{array} \right. \tag{31}\]  
\[\mathbf{a}_{j}=\left[a_{j,1},a_{j,2},\cdots,a_{j,n}\right]^{\text{T}} \tag{32}\]  
Evaluate the predicted state and the covariance matrix:  
\[ \begin{array}{l}\mathbf{\hat{x}}_{k|k-1}=\frac{1}{2i}\sum\limits_{j=1}^{2n}\left( F\xi_{j}+G\big{(}\xi_{j}\big{)}u_{k-1}+c\right)\\ \mathbf{P}_{k|k-1}=\frac{1}{2i}\sum\limits_{j=1}^{2n}\left(F\xi_{j}+G\big{(}\xi_{ j}\big{)}u_{k-1}+c-\mathbf{\hat{x}}_{k|k-1}\right)\Big{(}F\xi_{j}+G\big{(}\xi_{j} \big{)}u_{k-1}+c-\mathbf{\hat{x}}_{k|k-1}\Big{)}^{\text{T}}+Q\end{array}  \tag{33}\]  
Evaluate the information matrix and the information vector:  
\[ \begin{array}{l}\mathbf{Y}_{k|k-1}=\mathbf{P}_{k|k-1}^{-1}\\ \mathbf{\hat{y}}_{k|k-1}=\mathbf{Y}_{k|k-1}\mathbf{\hat{x}}_{k|k-1}\end{array}  \tag{34}\]  
(2) Measurement-update phase  
Evaluate the cubature points based on the 5-SSR rule  
\[\xi_{k|k-1,j}^{\text{m}}=\mathbf{S}_{1,k|k-1}\mathbf{\gamma}_{j}^{\text{SSR5}}+\mathbf{ \hat{x}}_{1,k|k-1} \tag{35}\]  
where \(\mathbf{\gamma}_{j}^{\text{SSR5}}\) is formulated as follows:  
\[\mathbf{\gamma}_{j}^{\text{SSR5}}=\left\{ \begin{array}{ccc}\left[\begin{array}[ ]{ccc}0&\cdots&0\end{array}\right]^{\text{T}}&,&j=0\\ \sqrt{n_{1}+2}c_{j}&,&j=1,2,\cdots,n_{1}+1\\ -\sqrt{n_{1}+2}c_{j-(n_{1}+1)}&,&j=n_{1}+2,\cdots,2(n_{1}+1)\\ \sqrt{n_{1}+2}b_{j-2(n_{1}+1)}&,&j=2n_{1}+3,\cdots,\left(n_{1}^{2}+5n_{1}+4 \right)/2\\ -\sqrt{n_{1}+2}b_{j-(n_{1}^{2}+5n_{1}+4)/2}&,&j=\left(n_{1}^{2}+5n_{1}+6\right) /2,\cdots,n_{1}^{2}+3n_{1}+2\end{array} \right. \tag{36}\]  
where \(\{\mathbf{c}_{j}\}\) are the vertexes of the \(n_{1}\)-simplex, and  
\[ \begin{array}{l}\mathbf{c}_{j}=\left[c_{j,1},c_{j,2},\cdots,c_{j,n_{1}}\right]^ {\text{T}}\\ c_{j,i}=\left\{\begin{array}{ccc}-\sqrt{\frac{n_{1}+1}{n_{1}(n_{1}-i+2)(n_{1 }-i+1)}}&,&i<j\\ \sqrt{\frac{(n_{1}+1)(n_{1}-j+1)}{n_{1}(n_{1}-j+2)}}&,&i=j\end{array}\right. 1\leq i\leq n_{1},1\leq j\leq n_{1}+1\\ 0&,&i>j\end{array} \right.\end{array} \tag{37}\]  
\(\{\mathbf{b}_{i}\}\) are the projections of the midpoints of the edges constructed by \(\{\mathbf{c}_{j}\}\). \[\{\mathbf{b}_{i}\}=\left\{\sqrt{\frac{n_{1}}{2(n_{1}-1)}}(\mathbf{c}_{l}+\mathbf{c}_{m}):l <m,1\leq l\leq n_{1}+1,1\leq m\leq n_{1}+1\right\} \tag{38}\]  
The weights for the 5-SSR rule are as follows:  
\[w_{j}=\left\{ \begin{array}{ccc}\frac{2}{n_{1}+2}&,&j=0\\ \frac{(-7n_{1})n_{1}^{2}}{2(n_{1}+1)^{2}(n_{1}+2)^{2}}&,&j=1,\cdots,2(n_{1}+1)\\ \frac{2(n_{1}-1)^{2}}{(n_{1}+1)^{2}(n_{1}+2)^{2}}&,&j=2n_{1}+3,\cdots,n_{1}^{2} +3n_{1}+2\end{array} \right. \tag{39}\]Evaluate the predicted measurement \(\hat{\mathbf{z}}_{k|k-1}\) and the cross-covariance matrix \(\mathbf{P}_{xz}\):  
\[\hat{\mathbf{z}}_{k|k-1}=\sum\limits_{j=1}^{n_{1}^{2}+3n_{1}+3}\mathrm{w}j \textit{h}\left(\hat{\mathbf{z}}_{k|k-1,j}^{\text{m}}\right) \tag{40}\] \[\mathbf{P}_{xz}=\left[ \begin{array}{c}\sum\limits_{j=1}^{n_{1}^{2}+3 n_{1}+3}\mathrm{w}j\hat{\mathbf{z}}_{k|k-1,j}^{\text{m}}\textit{h}\left(\hat{\mathbf{z}}_{ k|k-1,j}^{\text{m}}\right)^{\text{T}}-\hat{\mathbf{x}}_{1,k|k-1}\hat{\mathbf{z}}_{k|k-1}^{ \text{T}}\\ \mathbf{P}_{21,k|k-1}\mathbf{P}_{11,k|k-1}^{-1}\left(\sum\limits_{j=1}^{n_{1}^{2}+3n_ {1}+3}\mathrm{w}j\hat{\mathbf{z}}_{k|k-1,j}^{\text{m}}\textit{h}\left(\hat{\mathbf{z}}_ {k|k-1,j}^{\text{m}}\right)^{\text{T}}-\hat{\mathbf{x}}_{1,k|k-1}\hat{\mathbf{z}}_{k|k- 1}^{\text{T}}\right)\end{array}