# Mixed-Degree Cubature \(H_{\infty}\) Information Filter-Based Visual-Inertial Odometry
## 3 Mixed-Degree Cubature \(H_{\infty}\) Information Filter-Based VIO
### Bayes Filter for Nonlinear Visual-Inertial Navigation System  
Consider the following nonlinear system  
\[ \begin{split}\mathbf{x}_{k}&=\mathbf{f}(\mathbf{x}_{k-1})+\mathbf{w }_{k-1}\\ \mathbf{z}_{k}&=\mathbf{h}(\mathbf{x}_{k})+\mathbf{n}_{k}\end{split}  \tag{15}\]  
where \(\mathbf{f}(\cdot)\) and \(\mathbf{h}(\cdot)\) are arbitrary nonlinear functions and \(\mathbf{w}_{k-1}\) and \(\mathbf{n}_{k}\) are the process noise and the measurement noise, respectively. Under the Gaussian assumption of noises, the Bayes filter for nonlinear system described by the information vector and information matrix was formulated as an iterated process of a time-update phase and a measurement-update phase. (1) Time-update phase  
\[ \begin{split}\hat{\mathbf{x}}_{k|k-1}&=\int_{\mathbb{R}^ {n}}f(x)\boldsymbol{\mathcal{N}}\Big{(}x;\hat{\mathbf{x}}_{k-1|k-1},P_{k-1|k-1 }\Big{)}dx\\ \boldsymbol{P}_{k|k-1}&=\int_{\mathbb{R}^{n}}f(x)f^{ \mathrm{T}}(x)\boldsymbol{\mathcal{N}}\Big{(}x;\hat{\mathbf{x}}_{k-1|k-1},P_{k- 1|k-1}\Big{)}dx-\hat{\mathbf{x}}_{k|k-1}\hat{\mathbf{x}}_{k|k-1}^{\mathrm{T}}\\ \boldsymbol{Y}_{k|k-1}&=\boldsymbol{P}_{k|k-1}^{-1}\\ \boldsymbol{y}_{k|k-1}&=\boldsymbol{Y}_{k|k-1}\hat{ \mathbf{x}}_{k|k-1}\end{split}  \tag{16}\]  
(2) Measurement-update phase  
\[ \begin{split}\hat{\mathbf{z}}_{k|k-1}&=\int_{\mathbb{R }^{n}}h(x)\boldsymbol{\mathcal{N}}\Big{(}x;\hat{\mathbf{x}}_{k|k-1},P_{k|k-1} \Big{)}dx\\ \boldsymbol{P}_{xz}&=\int_{\mathbb{R}^{n}}xh^{ \mathrm{T}}(x)\boldsymbol{\mathcal{N}}\Big{(}x;\hat{\mathbf{x}}_{k|k-1},P_{k|k -1}\Big{)}dx-\hat{\mathbf{x}}_{k|k-1}\hat{\mathbf{z}}_{k|k-1}^{\mathrm{T}}\\ \boldsymbol{H}_{k}&=\Big{(}\boldsymbol{Y}_{k|k-1} \boldsymbol{P}_{xz}\Big{)}^{\mathrm{T}}\\ \hat{\boldsymbol{y}}_{k|k}&=\boldsymbol{\hat{y}}_{k|k -1}+\boldsymbol{H}_{k}^{\mathrm{T}}\boldsymbol{R}_{k}^{-1}\Big{(}\mathbf{z}_{k }-\hat{\mathbf{x}}_{k|k-1}\Big{)}+\boldsymbol{H}_{k}\hat{\mathbf{x}}_{k|k-1}\\ \boldsymbol{Y}_{k|k}&=\boldsymbol{Y}_{k|k-1}+ \boldsymbol{H}_{k}^{\mathrm{T}}\boldsymbol{R}_{k}^{-1}\boldsymbol{H}_{k}\end{split}  \tag{17}\]  
The state estimation could be recovered from the information state  
\[ \begin{split}\boldsymbol{P}_{k|k}&=\boldsymbol{Y}_{k|k }^{-1}\\ \hat{\mathbf{x}}_{k|k}&=\boldsymbol{P}_{k|k}\hat{ \boldsymbol{y}}_{k|k}\end{split}  \tag{18}\]  
According to the measurement model in Equation (14), only a part of the state vector appeared explicitly in the measurement equation. We defined the state vector explicitly shown in the measurement model as follows:  
\[\boldsymbol{x}_{1}=\left[ \begin{array}{cc}\mathbf{1}&\mathbf{1}\\ \mathbf{SI}&\boldsymbol{p}\\ \boldsymbol{p}&\end{array} \right]\in\mathbb{R}^{n_{1}} \tag{19}\]  
where \(n_{1}=6\). The rest state vector is  
\[\boldsymbol{x}_{2}=\mathbf{SI}\boldsymbol{v}\in\mathbb{R}^{n_{2}}. \tag{20}\]  
Correspondingly, the covariance matrix \(\boldsymbol{P}\) of the state vector is composed of four blocks as follows:  
\[\boldsymbol{P}=\left[ \begin{array}{cc}\boldsymbol{P}_{11}&\boldsymbol{P}_{12 }\\ \boldsymbol{P}_{21}&\boldsymbol{P}_{22}\\ \end{array} \right] \tag{21}\]  
Applying the above partition of the state vector to the evaluation of the predicted measurements (the first equation in Equation (17)), one can obtain. \[ \begin{array}{rl}\hat{\mathbf{z}}_{k|k-1}&=\int_{\mathbb{R}^{n}}h(x) \boldsymbol{\mathcal{N}}\Big{(}x;\hat{\mathbf{x}}_{k|k-1},P_{k-1|k-1}\Big{)}dx \\ &=\int_{\mathbb{R}^{n1}}h(x)\boldsymbol{\mathcal{N}}\Big{(}x_{1};\hat{ \mathbf{x}}_{1,k|k-1},P_{11,k|k-1}\Big{)}dx_{1}\end{array}  \tag{22}\]  
As shown by Equation (22), the original 9-dimensional integral is simplified to a 6-dimensional integral. The evaluation of the cross-covariance matrix (the second equation in Equation (17)) can also be simplified in a similar way. \[