## 3 Propagation and Update of the ICEKF
### Measurement  
Let the state measurement matrix at the \(k\)-th step of the ICEKF be \(\mathbf{z}_{k}\). After the measured linear translation \(\mathbf{z}_{P}\), the attitude quaternion \(\overline{\mathbf{z}}_{q}\), the linear velocity \(\mathbf{z}_{v}\), and the angular velocity \(\mathbf{z}_{w}\) are obtained by comparing the results from the keyframes of the visual measurement with the expectation of the ICEKF, \(\mathbf{z}_{k}\) is obtained as follows:  
\[\widetilde{\mathbf{z}}_{k}=\left[\widetilde{\mathbf{z}}_{p}^{T}\overline{\mathbf{z}}_{q}^{T }\overline{\mathbf{z}}_{v}^{T}\overline{\mathbf{z}}_{w}^{T}\right]^{T}, \tag{22}\]  
For the error of the linear translation, the following holds:  
\[\widetilde{\mathbf{z}}_{p}=(\mathbf{p}_{ic}-\mathbf{R}_{ic}\mathbf{p}_{i}^{e})\lambda+\mathbf{n}_{ p}-(\hat{\mathbf{p}}_{ic}-\hat{\mathbf{R}}_{ic}\mathbf{p}_{i}^{e})\lambda\] \[=(\mathbf{p}_{ic}\lambda-\hat{\mathbf{p}}_{ic}\lambda)+\left(\hat{\mathbf{p}}_ {ic}\lambda-\hat{\mathbf{p}}_{ic}\lambda\right)+\left[\hat{\mathbf{R}}_{ic}\mathbf{p}_{i}^ {e}\lambda-\hat{\mathbf{R}}_{ic}(\mathbf{I}_{3}+\lfloor\delta\mathbf{\theta}_{ic}\times \rfloor)\mathbf{p}_{i}^{e}\lambda\right]+\left(-\hat{\mathbf{R}}_{ic}\mathbf{p}_{i}^{e} \lambda+\hat{\mathbf{R}}_{ic}\mathbf{p}_{i}^{e}\lambda\right)+\mathbf{n}_{p}\enspace, \tag{23}\]  
For the error of the linear velocity, the following holds:  
\[\widetilde{\mathbf{z}}_{p}=\mathbf{v}_{ic}-\hat{\mathbf{v}}_{ic}\] \[=\mu_{v}\Delta\hat{\mathbf{v}}_{i}\Delta t+\lambda(1-\mu_{v})\mathbf{R}_{ c}^{i}\Delta\mathbf{v}_{c}\] \[=-\mu_{v}\Delta\hat{\mathbf{R}}_{i}\hat{\mathbf{R}}_{i}\lambda\left|\delta \theta_{i}-\mu_{v}\Delta t\hat{\mathbf{R}}_{i}\Delta\mathbf{b}_{\mathbf{z}}+\lambda(1-\mu_{ v})\mathbf{R}_{c}^{i}\Delta\mathbf{v}_{c}+\mathbf{n}_{v}\enspace, \tag{24}\]Since the angular velocities in different coordinate frames cannot be directly subtracted, the measurement error can be indirectly obtained according to Equation (14) as follows:  
\[\widetilde{z}_{\omega}=\delta\hat{\theta}_{k}\Delta t+\mathbf{n}_{\omega} \tag{26}\] \[=-\Delta t\big{[}\mu_{\omega}\big{\lfloor}\big{(}\mathbf{R}_{\mu_{ \omega}c}{}^{T}\hat{\mathbf{\omega}}_{i}\big{)}\times\big{\rfloor}+(1-\mu_{\omega} )\big{\lfloor}\hat{\mathbf{\omega}}_{c}\times\big{\rfloor}\big{]}\delta\hat{\theta }_{ic}+(1-\mu_{\omega})\Delta t\Delta\mathbf{\omega}_{c}+\mu_{\omega}\Delta t\mathbf{R} _{\mu_{\omega}c}{}^{T}\Delta\omega_{i}+\mathbf{n}_{\omega}\ \,\]  
Based on the description of \(\widetilde{\mathbf{X}}\) in Equation (8) and the above expansion of the measurement process, under the small angle assumption, the measurement error is reformulated as follows:  
\[\widetilde{z}_{k}\simeq\mathbf{H}_{k}\widetilde{\mathbf{X}}_{k}+\mathbf{n}_{m}, \tag{27}\]  
where \(\mathbf{H}_{k}\) is the measurement matrix, and the measurement noise is simplified as \(\mathbf{n}_{m}=\big{[}\mathbf{n}_{p}{}^{T}\mathbf{n}_{p}{}^{T}\mathbf{n}_{\omega}{}^{T}\mathbf{n} _{\omega}{}^{T}\big{]}^{T}\). Thus, \(\mathbf{H}_{k}\) can be recovered from Equations (22)-(26) as follows:  
\[\mathbf{H}_{k}=\left[ \begin{array}{cccc}\lambda\mathbf{I}_{3\times 3}&\mathbf{0}_{3 \times 6}&\mathbf{B}_{1}&\mathbf{0}_{3\times 15}\mathbf{B}_{2}\\ \mathbf{0}_{3\times 9}&\frac{1}{2}\mathbf{I}_{3\times 3}&\mathbf{0}_{3\times 16}\\ \mathbf{0}_{3\times 3}&\mathbf{B}_{3}&\mathbf{0}_{3\times 6}\mathbf{B}_{4}&\mathbf{0}_{3\times 6}&- \mu_{\omega}\Delta t\ \mathbf{R}_{i}\ \mathbf{0}_{3\times 4}\\ \mathbf{0}_{3\times 9}&\mathbf{B}_{5}&\mathbf{0}_{3\times 3}&(1-\mu_{\omega})\Delta t\mathbf{I}_{3 \times 3}&\mu_{\omega}\Delta t\mathbf{R}_{\mu_{\omega}c}{}^{T}\mathbf{0}_{3\times 7} \end{array}