## 3 Propagation and Update of the ICEKF
### Entire ICEKF Process  
The entire process of the ICEKF in the \(k\)-th iteration is presented as follows:  
Step 1. By calculating \(\mathbf{F}_{d}\) and \(\mathbf{Q}_{d}\) according to Equation (19), the prior covariance matrix of errors \(\mathbf{P}_{k+1|k}\) can be obtained from the following:  
\[\mathbf{P}_{k+1|k}=\mathbf{F}_{d}\mathbf{P}_{k|k}\mathbf{F}_{d}+\mathbf{Q}_{d}, \tag{29}\]  
Step 2. The Kalman gain matrix \(\mathbf{K}_{k}\) is updated as follows:  
\[\left\{ \begin{array}{l}\mathbf{S}_{k}=\mathbf{H}_{k}\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{T}+\bm {R}_{k}\\ \mathbf{K}_{k}=\mathbf{P}_{k+1|k}\mathbf{H}_{k}^{T}\mathbf{S}_{k}^{-1}\end{array} \right., \tag{30}\]  
where \(\mathbf{R}_{k}\) is the measurement noise matrix. Step 3. The current state \(\mathbf{\tilde{X}}_{k+1|k+1}=\mathbf{\tilde{X}}_{k+1|k}+\mathbf{\hat{\tilde{X}}}_{k}\) is calculated according to \(\mathbf{\hat{\tilde{X}}}_{k}=\mathbf{K}_{k}\widetilde{z}_{k}\), where \(\widetilde{z}_{k}\) is obtained via Equation (27). Step 4. The posterior covariance matrix of errors \(\mathbf{P}_{k+1|k+1}\) is updated, and Step 1 is carried out again according to the following:  
\[\mathbf{P}_{k+1|k+1}=\left(\mathbf{I}_{28\times 28}-\mathbf{K}_{k}\mathbf{H}_{k}\right)\mathbf{P}_{k+1|k }\left(\mathbf{I}_{28\times 28}-\mathbf{K}_{k}\mathbf{H}_{k}\right)^{T}+\mathbf{K}_{k}\mathbf{R}_{k}\mathbf{K}_{k}^{T}, \tag{31}\]  
During Step 2, the measurement error model of the visual measurement can be preestablished as described in [25; 26]. While updating the attitude during Step 3, the angular error of rotation in \(\mathbf{\hat{\tilde{X}}}_{k}\) is achieved with the small angle assumption. Therefore, the quaternion form of the attitude expectation \(\mathbf{\hat{\hat{\eta}}}_{k+1}\) must be recovered as follows [23]:  
\[\left\{\begin{array}{l}\delta\hat{\mathbf{q}}_{k}\approx\frac{1}{2}\hat{\sigma} \hat{\mathbf{\hat{q}}}_{k}\\ \mathbf{\hat{\tilde{q}}}_{k+1}=\left\{ \begin{array}{l}\left[\sqrt{1-\delta\hat{ q}_{k}^{T}\delta\hat{q}_{k}},\delta\hat{q}_{k}^{T}\right]^{T}\text{if}\ \delta\hat{q}_{k}^{T}\delta\hat{q}_{k}\leq 1\\ \frac{1}{\sqrt{1+\delta\hat{q}_{k}^{T}\delta\hat{q}_{k}}}\big{[}1,\delta\hat{q}_{ k}^{T}\big{]}^{T}\text{if}\ \delta\hat{q}_{k}^{T}\delta\hat{q}_{k}>1\end{array} \right., \tag{32}\]  
The measurement functions are designed as \(\mathbf{h}\left(\dot{\mathbf{X}}\right)=\left[\mathbf{h}_{1}\mathbf{1},\dots,\mathbf{h}_{8}\mathbf{1} \right]^{T}\), with \(\mathbf{h}_{1}=(\mathbf{p}_{ic}-\mathbf{R}_{ic}\mathbf{p}_{i}^{c})\lambda\), \(\mathbf{h}_{2}=\overline{\mathbf{q}}_{ic}\), \(\mathbf{h}_{3}=\overline{\mathbf{q}}_{i}\), \(\mathbf{h}_{4}=\overline{\mathbf{q}}_{ic}\mathbf{\overline{q}}_{ic}\), \(\mathbf{h}_{5}=\overline{\mathbf{q}}_{i}\mathbf{\overline{q}}_{i}\), \(\mathbf{h}_{6}=\mathbf{v}_{c}\), \(\mathbf{h}_{7}=\mathbf{\omega}_{c}\), and \(\mathbf{h}_{8}=\mathbf{\omega}_{i}\). According to the detailed deduction in Appendix C, the observability matrix of the system is constructed with the Lie derivative as follows:  
\[\mathbf{\Omega}=\left[ \begin{array}{c}\  
\end{array} \right]\quad, \tag{35}\]  
where each column corresponds to the entries in the state vector of the ICEKF, the matrices \(\mathbf{G}\) with indices represent blocks that are irrelevant to the rank analysis, and the matrices \(\mathbf{U}\) with subscripts are blocks contributing to the column rank of \(\mathbf{\Omega}\), as follows: \(\mathbf{U}_{1}=\mathbf{I}_{3}\lambda,\mathbf{U}_{4},\mathbf{U}_{5}=\mathbf{I}_{4},\mathbf{U}_{8},\mathbf{ U}_{9},\mathbf{U}_{10}=\mathbf{I}_{3},\mathbf{U}_{12}=\lambda\mu_{\mathfrak{v}}\mathbf{I}_{3}, \mathbf{U}_{14}=0.5\Xi\left(\dot{\overline{\mathbf{q}}}_{i}\right)\), \(\mathbf{U}_{15}=-\lambda\mu_{\mathfrak{v}}\mathbf{R}_{i}\), and \(\mathbf{U}_{16}=\mathbf{U}(\mathbf{R}_{i})\). To prove that \(\mathbf{\Omega}\) has full rank, block Gaussian elimination is applied. The rows of blocks can be rearranged so that only one block in each row is allowed to determine whether the corresponding column of blocks has full column rank.