## 2 Methods
### INS/Vision Tightly Coupled Integration Model  
By denoting the error state of the camera as \(\left(\delta\mathbf{p}^{e}_{c_{i}}\mathbf{\phi}_{c_{i}}\right)\) for the \(i\)th image, the error state vector of INS/vision tightly coupled integration with the MSCKF at the time when the \(k\)th image is captured is expressed as  
\[\mathbf{x}_{i,c}=\left(\mathbf{x}_{INS}\quad|\quad\delta\mathbf{p}^{e}_{c_{j}}\quad\mathbf{ \phi}_{c_{j}}\quad\ldots\quad\delta\mathbf{p}^{e}_{c_{k}}\mathbf{\phi}_{c_{k}}\right)^ {T} \tag{17}\]  
in which \(\delta\mathbf{p}^{e}_{c_{i}}\) and \(\mathbf{\phi}_{c_{i}}\) (\(i=j,j+1,\,\ldots,\,k\)) indicate the error states of the camera position and attitude for epoch \(i\). The above error state vector is augmented when new camera data is introduced. Visual measurements of the same feature point from multiple camera views are used to construct the geometric constraints. At the time of taking the \(i\)th (\(i<k+1\)) image, the transformation of the static feature point \(P_{k}\) can be expressed as  
\[\mathbf{p}^{c_{i}}_{P_{k},i}=\mathbf{R}^{c_{i}}_{e}\left(\mathbf{p}^{e}_{P_{k}}-\mathbf{p}^{e}_ {c_{i}}\right) \tag{18}\]in which, \(p_{R,i}^{c_{i}}=\left(x_{p_{k},i}^{c_{i}}\quad y_{p_{k},i}^{c_{i}}\quad z_{p_{k},i}^ {c_{i}}\right)\) indicates the position of \(P_{k}\) in the camera frame; \(\mathbf{R}_{e}^{c_{i}}\) and \(\mathbf{p}_{e}^{e}\) are the attitude rotation matrix and position vector against the global frame (\(e\)-frame), respectively; \(\mathbf{p}_{P_{k}}^{e}\) is the estimated position of \(P_{k}\) in the \(e\)-frame, which can be calculated by triangulation. By differentiating Equation (18), the equation of the camera state can be obtained as follows:  
\[\delta\mathbf{p}_{P_{k},i}^{c_{i}}=-\mathbf{R}_{e}^{c_{i}}\Big{[}\left(\mathbf{p}_{P_{k}}^ {e}-\mathbf{p}_{c_{i}}^{e}\right)\times\Big{]}\mathbf{\phi}_{c_{i}}-\mathbf{R}_{e}^{c_{i}} \delta\mathbf{p}_{c_{i}}^{e}+\mathbf{R}_{e}^{c_{i}}\delta\mathbf{p}_{P_{k}}^{e} \tag{19}\]  
in which \(\delta\mathbf{p}_{P_{k}}^{e}\) indicates the error of the approximate position of the feature point. Concerning the camera measurement residual vector, it can be written as  
\[z_{P_{k},i}= \begin{pmatrix}u_{P_{k},i}^{0}-\widetilde{u}_{P_{k},i}\\ v_{P_{k},i}^{0}-\widetilde{v}_{P_{k},i}\end{pmatrix} ,\ \ \varepsilon_{P_{k},i}= \begin{pmatrix}\varepsilon_{\widetilde{u}_{P_{k},i}}\\ \varepsilon_{\widetilde{v}_{P_{k},i}}\end{pmatrix}  \tag{20}\]  
where \(\left(u_{P_{k},i}^{0},v_{P_{k},i}^{0}\right)^{T}\) is the estimated pixel coordinate of \(P_{k}\) by back projection and \(\left(\widetilde{u}_{P_{k},i},\widetilde{v}_{P_{k},i}\right)^{T}\) is the observation of \(P_{k}\) in the \(i\)th image and \(\mathbf{\varepsilon}_{P_{k},i}\) is the measurement noise. Then, based on the chain rule, the residual formula can be expressed as [27]  
\[\mathbf{z}_{P_{k},i}=\delta\big{(}u_{P_{k},i},v_{P_{k},i}\big{)}^{T}=\frac{\partial \big{(}u_{P_{k},i},v_{P_{k},i}\big{)}^{T}}{\partial\mathbf{p}_{P_{k},i}^{c_{i}}} \delta\mathbf{p}_{P_{k},i}^{c_{i}}=\mathbf{H}_{x,i}\mathbf{x}_{i,c}+\mathbf{H}_{f,i}\delta\bm {p}_{P_{k}}^{e}+\mathbf{\varepsilon}_{P_{k},i} \tag{21}\]  
where  
\[\mathbf{H}_{x,i}=\begin{pmatrix}0_{2\times 21}&0_{2\times 6}&\ldots&-\mathbf{J}\mathbf{R}_{e}^{c _{i}}&-\mathbf{J}\mathbf{R}_{e}^{c_{i}}\Big{[}\Big{(}\mathbf{p}_{P_{k}}^{e}-\mathbf{p}_{c_{i}} ^{e}\Big{)}\times\Big{]}&\ldots\Big{)}\mathbf{H}_{f,i}=\mathbf{J}\mathbf{R}_{e}^{c_{i}}\bm {I}=\frac{\partial\big{(}u_{P_{k},i},v_{P_{k},i}\big{)}^{T}}{\partial\mathbf{p}_{P _{k},i}^{c_{i}}}= \begin{pmatrix}\frac{f_{x}}{z_{P_{k},i}^{c_{i}}}&0&-\frac{f_{ x}x_{P_{k},i}^{c_{i}}}{z_{P_{k},2}^{c_{i}}}\\ 0&\frac{f_{y}}{z_{P_{k},i}^{c_{i}}}&-\frac{f_{y}y_{P_{k},i}^{c_{i}}}{z_{P_{k},i }^{c_{i}}}\end{pmatrix} \]  
in which \((f_{x},f_{y})\) means the focal length and \(\left(x_{P_{k},i}^{c_{i}}\quad y_{P_{k},i}^{c_{i}}\quad z_{P_{k},i}^{c_{i}}\right)\) means the position of a feature point in the camera frame.