# Mixed-Degree Cubature \(H_{\infty}\) Information Filter-Based Visual-Inertial Odometry
## 2 Model of Visual-Inertial Odometry
### Motion Model of Camera-IMU System  
A 9-dimension system was built to represent the motion of the camera-IMU system. The state vector is defined as follows:  
\[\mathbf{x}=\left[ \begin{array}{cc}\mathrm{I}\mathbf{\omega}^{\text{T}}&\mathrm{SI} \mathbf{p}^{\text{T}}&\mathrm{SI}\mathbf{v}^{\text{T}}\end{array} \right]^{\text{T}} \tag{1}\]  
where \(\mathrm{I}\mathbf{\omega}\) is the modified Rodrigues parameter (MRP) representing the rotation of the ICS relative to the SICS; \({}^{\text{SI}}\mathbf{p}\) is the position of the origin of the ICS in the SICS; and \({}^{\text{SI}}\mathbf{v}\) is the velocity of the origin of the ICS relative to the SICS. A discrete motion model was constructed according to the six degrees of the freedom kinematic model of a rigid body as follows:  
\[\mathbf{x}_{k}=\mathbf{F}\mathbf{x}_{k-1}+\mathbf{G}(\mathbf{x}_{k-1})\mathbf{u}_{k-1}+\mathbf{C} \tag{2}\]  
where \(\mathbf{u}_{k-1}=\left[ \begin{array}{cc}\mathrm{I}\mathbf{\omega}^{\text{T}}_{k-1} &\mathrm{I}\mathbf{a}^{\text{T}}_{k-1}\end{array} \right]^{\text{T}}\) is the input vector of the navigation system; \(\mathrm{I}\mathbf{\omega}_{k-1}\) and \(\mathrm{I}\mathbf{a}_{k-1}\) are the measurements of the gyroscope and the accelerator at time \(k-1\), respectively;is a constant vector; \(\Delta t\) is the sampling period; \({}^{\text{SI}}\mathbf{g}={}^{\text{SI}}_{\text{E}}\mathbf{R}^{\text{E}}\mathbf{g}\) is the gravity acceleration vector in the SICS; \({}^{\text{SI}}_{\text{E}}\mathbf{R}\) is the transformation matrix from the earth-fixed coordinate system to the SICS, of which initial value determined by a simple attitude and heading reference system (AHRS) [32]; and \(\mathbf{F}\) and \(\mathbf{G}(\mathbf{x}_{k-1})\) are the state-transition matrix and input matrix, respectively, formulated as follows:  
\[\mathbf{F}=\left[ \begin{array}{ccc}\mathbf{\mathcal{I}}_{3}&0_{3}&0_{3}\\ 0_{3}&\mathbf{\mathcal{I}}_{3}&\Delta t\mathbf{\mathcal{I}}_{3}\\ 0_{3}&0_{3}&\mathbf{\mathcal{I}}_{3}\end{array} \right] \tag{3}\] \[\mathbf{G}(\mathbf{x}_{k-1})=\left[ \begin{array}{ccc}\mathbf{A}\big{(}{}^{ \text{I}}_{\text{SI}}\mathbf{\rho}_{k-1}\big{)}&0_{3}\\ 0_{3}&0_{3}\\ 0_{3}&{}^{\text{I}}_{\text{SI}}\mathbf{R}\big{(}{}^{\text{I}}_{\text{SI}}\mathbf{\rho} _{k-1}\big{)}^{\text{T}}\end{array} \right]\]  
where \(\mathbf{\mathcal{I}}_{3}\) is the three-dimensional identity matrix; and \(0_{3}\) is the three-dimensional zero matrix. \(\mathbf{A}\big{(}{}^{\text{I}}_{\text{SI}}\mathbf{\rho}_{k-1}\big{)}\) and \({}^{\text{I}}_{\text{SI}}\mathbf{R}\big{(}{}^{\text{I}}_{\text{SI}}\mathbf{\rho}_{k-1} \big{)}\) are formulated as follows. The right subscript \(k-1\), left subscript 'SI' and left prescript \(\mathbf{1}\)' in the right side of the equations are neglected to simplify the description  
\[\mathbf{A}\big{(}{}^{\text{I}}_{\text{SI}}\mathbf{\rho}_{k-1}\big{)}=\frac{1}{4} \left[\begin{array}{ccc}\rho_{1}^{2}-\rho_{2}^{2}-\rho_{3}^{2}+1&2(\rho_{1} \rho_{2}-\rho_{3})&2(\rho_{1}\rho_{3}+\rho_{2})\\ 2(\rho_{1}\rho_{  
coordinate of the candidate pixel is constructed. We used a simple pin-hole camera model for the 3D construction. \[\prescript{SC}{\mathbf{p}}{}_{\mathrm{P}_{j}}=\left[ \begin{array}{c}\prescript{SC}{x}{ \mathrm{P}_{j}}\\ \prescript{SC}{y}{\mathrm{P}_{j}}\\ \prescript{SC}{z}{\mathrm{P}_{j}}\end{array} \right]=d\left(u_{\mathrm{P}_{j}},v _{\mathrm{P}_{j}}\right)\left[ \begin{array}{c}\frac{u_{\mathrm{P}_{j}}-c_{ \mathrm{x}}}{f_{\mathrm{x}}}\\ \frac{v_{\mathrm{P}_{j}}-c_{\mathrm{y}}}{v_{\mathrm{P}_{j}}-c_{\mathrm{y}}} \\ 1\end{array} \right] \tag{7}\]  
where \(d\left(u_{\mathrm{P}_{j}},v_{\mathrm{P}_{j}}\right)\) is the depth measurement of corresponding point; and \(f_{\mathrm{x}}\), \(f_{\mathrm{y}}\), \(c_{\mathrm{x}}\), and \(c_{\mathrm{y}}\) are the intrinsic parameters of the RGB camera. To simplify the measurement function, the constructed points were transformed into static IMU coordinate system. The transformed 3-dimensional coordinate was added into the point set of the keyframe.