Supplementary Materials:
Night-Rider: Nocturnal Vision-aided Localization in Streetlight Maps Using Invariant Extended Kalman Filtering

From the Eq. (8), we need to derive the linearized model of right invariant error $\bm{\eta}_{t}^{r}$ and bias error $\bm{\zeta}_{t}$.

For the states fitting into Lie group, the derivative of right invariant error $\bm{\eta}_{t}^{r}$ associates with the log of invariant error $\bm{\xi}_{t}$ based on $\bm{\eta}_{t}^{r}=\exp(\bm{\xi}_{t}^{\wedge})$.

$$\frac{d}{dt}\bm{\eta}_{t}^{r}\approx\frac{d}{dt}(\mathbf{I}+\bm{\xi}_{t}^{\wedge})=\begin{bmatrix}\frac{d}{dt}(\mathbf{I}+(\bm{\xi}_{R_{t}})_{\times})&\frac{d}{dt}\bm{\xi}_{v_{t}}&\bm{\xi}_{p_{t}}\\ 0&0&0\\ 0&0&0\end{bmatrix}$$

(27)

With Eq. (3), the derivative of $\bm{\xi}_{t}$ is deduced as below.

	$\displaystyle\frac{d}{dt}(\mathbf{I}+(\bm{\xi}_{R_{t}})_{\times})=\frac{d}{dt}(\bm{\xi}_{R_{t}})_{\times}=\frac{d\hat{\mathbf{R}}_{b_{t}}^{w}}{dt}\mathbf{R}_{b_{t}}^{w^{\top}}+\hat{\mathbf{R}}_{b_{t}}^{w}\frac{d\mathbf{R}_{b_{t}}^{w^{\top}}}{dt}$
	$\displaystyle=\hat{\mathbf{R}}_{b_{t}}^{w}(\tilde{\bm{\omega}}_{t}-\hat{\mathbf{b}}_{\omega_{t}})_{\times}\mathbf{R}_{b_{t}}^{w^{\top}}-\hat{\mathbf{R}}_{b_{t}}^{w}(\bm{\omega}_{t})_{\times}\mathbf{R}_{b_{t}}^{w^{\top}}$
	$\displaystyle=\hat{\mathbf{R}}_{b_{t}}^{w}(\tilde{\bm{\omega}}_{t}+\mathbf{b}_{\omega_{t}}+\mathbf{n}_{\omega_{t}}-\hat{\mathbf{b}}_{\omega_{t}}-\bm{\omega}_{t})_{\times}\mathbf{R}_{b_{t}}^{w^{\top}}$
	$\displaystyle=\hat{\mathbf{R}}_{b_{t}}^{w}(\mathbf{n}_{\omega_{t}}-\bm{\zeta}_{\omega_{t}})_{\times}\hat{\mathbf{R}}_{b_{t}}^{w^{\top}}\hat{\mathbf{R}}_{b_{t}}^{w}\mathbf{R}_{b_{t}}^{w^{\top}}$
	$\displaystyle=(\hat{\mathbf{R}}_{b_{t}}^{w}(\mathbf{n}_{\omega_{t}}-\bm{\zeta}_{\omega_{t}}))_{\times}\bm{\eta}_{R_{t}}$
	$\displaystyle\approx(\hat{\mathbf{R}}_{b_{t}}^{w}(\mathbf{n}_{\omega_{t}}-\bm{\zeta}_{\omega_{t}}))_{\times}(\mathbf{I}+\bm{\xi}_{R_{t}})\approx(\hat{\mathbf{R}}_{b_{t}}^{w}(\mathbf{n}_{\omega_{t}}-\bm{\zeta}_{\omega_{t}}))_{\times}$		(28)
	$\displaystyle\frac{d}{dt}\bm{\xi}_{v_{t}}=\frac{d}{dt}{{}^{w}}\hat{\mathbf{v}}_{b_{t}}-\frac{d}{dt}(\hat{\mathbf{R}}_{b_{t}}^{w}\mathbf{R}_{b_{t}}^{w^{\top}}){{}^{w}}\mathbf{v}_{b_{t}}-\hat{\mathbf{R}}_{b_{t}}^{w}\mathbf{R}_{b_{t}}^{w^{\top}}\frac{d}{dt}{{}^{w}}\mathbf{v}_{b_{t}}$
	$\displaystyle\approx\hat{\mathbf{R}}_{b_{t}}^{w}(\tilde{\bm{a}}_{t}-\hat{\mathbf{b}}_{a_{t}})+\mathbf{g}-(\hat{\mathbf{R}}_{b_{t}}^{w}(\mathbf{n}_{\omega_{t}}-\bm{\zeta}_{\omega_{t}}))_{\times}{{}^{w}}\mathbf{v}_{b_{t}}$
	$\displaystyle\quad-\hat{\mathbf{R}}_{b_{t}}^{w}\mathbf{R}_{b_{t}}^{w^{\top}}(\mathbf{R}_{b_{t}}^{w}\bm{a}_{t}+\mathbf{g})$
	$\displaystyle=\hat{\mathbf{R}}_{b_{t}}^{w}(\bm{a}_{t}+\mathbf{b}_{a_{t}}+\mathbf{n}_{a_{t}}-\hat{\mathbf{b}}_{a_{t}}-\bm{a}_{t})$
	$\displaystyle\quad-(\hat{\mathbf{R}}_{b_{t}}^{w}(\mathbf{n}_{\omega_{t}}-\bm{\zeta}_{\omega_{t}}))_{\times}{{}^{w}}\mathbf{v}_{b_{t}}+(\mathbf{I}-\hat{\mathbf{R}}_{b_{t}}^{w}\mathbf{R}_{b_{t}}^{w^{\top}})\mathbf{g}$
	$\displaystyle\approx\hat{\mathbf{R}}_{b_{t}}^{w}(\mathbf{n}_{a_{t}}-\bm{\zeta}_{a_{t}})-(\hat{\mathbf{R}}_{b_{t}}^{w}(\mathbf{n}_{\omega_{t}}-\bm{\zeta}_{\omega_{t}}))_{\times}{{}^{w}}\mathbf{v}_{b_{t}}-(\bm{\xi}_{R_{t}})_{\times}\mathbf{g}$
	$\displaystyle=(\mathbf{g})_{\times}\bm{\xi}_{R_{t}}+({{}^{w}}\mathbf{v}_{b_{t}})_{\times}\hat{\mathbf{R}}_{b_{t}}^{w}(\mathbf{n}_{\omega_{t}}-\bm{\zeta}_{\omega_{t}})+\hat{\mathbf{R}}_{b_{t}}^{w}(\mathbf{n}_{a_{t}}-\bm{\zeta}_{a_{t}})$		(29)
	$\displaystyle\frac{d}{dt}\bm{\xi}_{p_{t}}=\frac{d}{dt}\hat{\mathbf{p}}_{b_{t}}^{w}-\frac{d}{dt}(\hat{\mathbf{R}}_{b_{t}}^{w}\mathbf{R}_{b_{t}}^{w^{\top}})\mathbf{p}_{b_{t}}^{w}-\hat{\mathbf{R}}_{b_{t}}^{w}\mathbf{R}_{b_{t}}^{w^{\top}}\frac{d}{dt}\mathbf{p}_{b_{t}}^{w}$
	$\displaystyle\approx{{}^{w}}\hat{\mathbf{v}}_{b_{t}}-(\hat{\mathbf{R}}_{b_{t}}^{w}(\mathbf{n}_{\omega_{t}}-\bm{\zeta}_{\omega_{t}}))_{\times}\mathbf{p}_{b_{t}}^{w}-\hat{\mathbf{R}}_{b_{t}}^{w}\mathbf{R}_{b_{t}}^{w^{\top}}{{}^{w}}\mathbf{v}_{b_{t}}$
	$\displaystyle=({{}^{w}}\hat{\mathbf{v}}_{b_{t}}-\hat{\mathbf{R}}_{b_{t}}^{w}\mathbf{R}_{b_{t}}^{w^{\top}}{{}^{w}}\mathbf{v}_{b_{t}})-(\hat{\mathbf{R}}_{b_{t}}^{w}(\mathbf{n}_{\omega_{t}}-\bm{\zeta}_{\omega_{t}}))_{\times}\mathbf{p}_{b_{t}}^{w}$
	$\displaystyle=\bm{\xi}_{v_{t}}+(\mathbf{p}_{b_{t}}^{w})_{\times}\hat{\mathbf{R}}_{b_{t}}^{w}(\mathbf{n}_{\omega_{t}}-\bm{\zeta}_{\omega_{t}})$		(30)

On the other hand, the differential of bias errors is calculated as

$$\frac{d}{dt}\bm{\zeta}_{t}=\begin{bmatrix}\frac{d}{dt}\hat{\mathbf{b}}_{\omega_{t}}-\frac{d}{dt}\mathbf{b}_{\omega_{t}}\\ \frac{d}{dt}\hat{\mathbf{b}}_{a_{t}}-\frac{d}{dt}\mathbf{b}_{a_{t}}\end{bmatrix}=\begin{bmatrix}\mathbf{n}_{b\omega_{t}}\\ \mathbf{n}_{ba_{t}}\end{bmatrix}$$

(31)

Therefore, the linearized model can be written as in Eq. (9).

The covariance matrix of reprojection error in Eq. (18) for the streetlight observation $B_{i}$ and cluster $L_{j}$ is derived as below:

	$\displaystyle\mathbf{\Sigma}_{t_{proj,ij}}=\mathbf{\Sigma}_{\tilde{\mathbf{p}}_{t_{i}}}+\frac{\partial\tilde{\mathbf{c}}^{m}_{t_{j}}}{\partial\begin{bmatrix}\hat{\mathbf{R}}^{w}_{b_{t}}&\hat{\mathbf{p}}^{w}_{b_{t}}\end{bmatrix}}\hat{\mathbf{P}}_{t}^{-}\left(\frac{\partial\tilde{\mathbf{c}}^{m}_{t_{j}}}{\partial\begin{bmatrix}\hat{\mathbf{R}}^{w}_{b_{t}}&\hat{\mathbf{p}}^{w}_{b_{t}}\end{bmatrix}}\right)^{\top}$		(32)
	$\displaystyle\frac{\partial\tilde{\mathbf{c}}^{m}_{t_{j}}}{\partial\hat{\mathbf{R}}^{w}_{b_{t}}}=\frac{\partial\frac{1}{\tilde{Z}_{j}^{c}}\mathbf{K}(\mathbf{R}^{c}_{b}\hat{\mathbf{R}}^{w^{\top}}_{b_{t}}(\tilde{\mathbf{C}}^{w}_{j}-\hat{\mathbf{p}}^{w}_{b_{t}})+\mathbf{t}_{b}^{c})}{\partial\hat{\mathbf{R}^{w}}_{b_{t}}}$
	$\displaystyle\quad\quad\ =\frac{\partial\frac{1}{\tilde{Z}_{j}^{c}}\mathbf{K}\tilde{\mathbf{C}}^{c}_{j}}{\partial\hat{\mathbf{R}}^{w}_{b_{t}}}=\frac{\partial\frac{1}{\tilde{Z}_{j}^{c}}\mathbf{K}\tilde{\mathbf{C}}^{c}_{j}}{\partial\tilde{\mathbf{C}}^{c}_{j}}\frac{\partial\tilde{\mathbf{C}}^{c}_{j}}{\partial\hat{\mathbf{R}}^{w}_{b_{t}}}$
	$\displaystyle\quad\quad\ =\frac{\partial\frac{1}{\tilde{Z}_{j}^{c}}\mathbf{K}\tilde{\mathbf{C}}^{c}_{j}}{\partial\tilde{\mathbf{C}}^{c}_{j}}\frac{\partial\mathbf{R}^{c}_{b}\hat{\mathbf{R}}^{w^{\top}}_{b_{t}}(\tilde{\mathbf{C}}^{w}_{j}-\hat{\mathbf{p}}^{w}_{b_{t}})+\mathbf{t}_{b}^{c}}{\partial\hat{\mathbf{R}}^{w}_{b_{t}}}$		(33)
	$\displaystyle\quad\quad\ =\begin{bmatrix}\frac{f_{x}}{\tilde{Z}^{c}_{j}}&0&-\frac{\tilde{X}^{c}_{j}f_{x}}{\tilde{Z}^{c^{2}}_{j}}\\ 0&\frac{f_{y}}{\tilde{Z}^{c}_{j}}&-\frac{\tilde{Y}^{c}_{j}f_{y}}{\tilde{Z}^{c^{2}}_{j}}\end{bmatrix}\mathbf{R}^{c}_{b}(\hat{\mathbf{R}}^{w^{\top}}_{b_{t}}(\tilde{\mathbf{C}}^{w}_{j}-\hat{\mathbf{p}}^{w}_{b_{t}}))_{\times}$
	$\displaystyle\frac{\partial\tilde{\mathbf{c}}^{m}_{t_{j}}}{\partial\hat{\mathbf{p}}^{w}_{b_{t}}}=\frac{\partial\frac{1}{\tilde{Z}_{j}^{c}}\mathbf{K}(\mathbf{R}^{c}_{b}\hat{\mathbf{R}}^{w^{\top}}_{b_{t}}(\tilde{\mathbf{C}}^{w}_{j}-\hat{\mathbf{p}}^{w}_{b_{t}})+\mathbf{t}_{b}^{c})}{\partial\hat{\mathbf{p}}^{w}_{b_{t}}}$
	$\displaystyle\quad\quad\ =\frac{\partial\frac{1}{\tilde{Z}_{j}^{c}}\mathbf{K}\tilde{\mathbf{C}}^{c}_{j}}{\partial\hat{\mathbf{p}}^{w}_{b_{t}}}=\frac{\partial\frac{1}{\tilde{Z}_{j}^{c}}\mathbf{K}\tilde{\mathbf{C}}^{c}_{j}}{\partial\tilde{\mathbf{C}}^{c}_{j}}\frac{\partial\tilde{\mathbf{C}}^{c}_{j}}{\partial\hat{\mathbf{p}}^{w}_{b_{t}}}$
	$\displaystyle\quad\quad\ =\frac{\partial\frac{1}{\tilde{Z}_{j}^{c}}\mathbf{K}\tilde{\mathbf{C}}^{c}_{j}}{\partial\tilde{\mathbf{C}}^{c}_{j}}\frac{\partial\mathbf{R}^{c}_{b}\hat{\mathbf{R}}^{w^{\top}}_{b_{t}}(\tilde{\mathbf{C}}^{w}_{j}-\hat{\mathbf{p}}^{w}_{b_{t}})+\mathbf{t}_{b}^{c}}{\partial\hat{\mathbf{p}}^{w}_{b_{t}}}$
	$\displaystyle\quad\quad\ =-\begin{bmatrix}\frac{f_{x}}{\tilde{Z}^{c}_{j}}&0&-\frac{\tilde{X}^{c}_{j}f_{x}}{\tilde{Z}^{c^{2}}_{j}}\\ 0&\frac{f_{y}}{\tilde{Z}^{c}_{j}}&-\frac{\tilde{Y}^{c}_{j}f_{y}}{\tilde{Z}^{c^{2}}_{j}}\end{bmatrix}\mathbf{R}^{c}_{b}\hat{\mathbf{R}}^{w^{\top}}_{b_{t}}$		(34)

where $\mathbf{\Sigma}_{\tilde{\mathbf{p}}_{t_{i}}}=\alpha\mathbf{I}_{2\times 2}$ and $\alpha$ is a preset parameter.

Similar to $\mathbf{\Sigma}_{t_{proj,ij}}$, the variance of angle error in Eq. (19) also originates from the covariance of detected streetlight observation and current pose.

	$\displaystyle\sigma^{2}_{t_{ang,ij}}=\frac{\partial\cos\theta_{ij}}{\partial\tilde{\mathbf{p}}_{t_{i}}}\mathbf{\Sigma}_{\bar{\tilde{\mathbf{p}}}_{t_{i}}}\left(\frac{\partial\cos\theta_{ij}}{\partial\tilde{\mathbf{p}}_{t_{i}}}\right)^{\top}$
	$\displaystyle\qquad+\frac{\partial\cos\theta_{ij}}{\partial\begin{bmatrix}\mathbf{R}^{w}_{b_{t}}&\mathbf{p}^{w}_{b_{t}}\end{bmatrix}}\hat{\mathbf{P}}_{t}^{-}\left(\frac{\partial\cos\theta_{ij}}{\partial\begin{bmatrix}\mathbf{R}^{w}_{b_{t}}&\mathbf{p}^{w}_{b_{t}}\end{bmatrix}}\right)^{\top}$		(35)
	$\displaystyle\frac{\partial\cos\theta_{ij}}{\partial\tilde{\mathbf{p}}_{t_{i}}}=\frac{\partial\frac{(\mathbf{K}^{-1}\bar{\tilde{\mathbf{p}}}_{t_{i}})^{\top}(\mathbf{R}^{c}_{b}\hat{\mathbf{R}}^{w^{\top}}_{b_{t}}(\tilde{\mathbf{C}}^{w}_{j}-\hat{\mathbf{p}}^{w}_{b_{t}})+\mathbf{t}_{b}^{c})}{\lVert\mathbf{K}^{-1}\bar{\tilde{\mathbf{p}}}_{t_{i}}\rVert_{2}\cdot\lVert\mathbf{R}^{c}_{b}\hat{\mathbf{R}}^{w^{\top}}_{b_{t}}(\tilde{\mathbf{C}}^{w}_{j}-\hat{\mathbf{p}}^{w}_{b_{t}})+\mathbf{t}_{b}^{c}\rVert_{2}}}{\partial\tilde{\mathbf{p}}_{t_{i}}}$
	$\displaystyle=\frac{\tilde{\mathbf{C}}_{j}^{c^{\top}}}{\lVert\tilde{\mathbf{C}}_{j}^{c}\rVert_{2}}\frac{\partial\frac{\tilde{\mathbf{P}}_{t_{i}}}{\lVert\tilde{\mathbf{P}}_{t_{i}}\rVert_{2}}}{\partial\tilde{\mathbf{P}}_{t_{i}}}\frac{\partial\tilde{\mathbf{P}}_{t_{i}}}{\partial\tilde{\mathbf{p}}_{t_{i}}}$
	$\displaystyle=\frac{\tilde{\mathbf{C}}_{j}^{c^{\top}}}{\lVert\tilde{\mathbf{C}}_{j}^{c}\rVert_{2}}\left(\frac{\mathbf{I}}{\lVert\tilde{\mathbf{P}}_{t_{i}}\rVert_{2}}-\frac{\tilde{\mathbf{P}}_{t_{i}}\tilde{\mathbf{P}}_{t_{i}}^{\top}}{\lVert\tilde{\mathbf{P}}_{t_{i}}\rVert_{2}^{3}}\right)\mathbf{K}^{-1}$		(36)
	$\displaystyle\frac{\partial\cos\theta_{ij}}{\partial\hat{\mathbf{R}}^{w}_{b_{t}}}=\frac{\partial\frac{(\mathbf{K}^{-1}\bar{\tilde{\mathbf{p}}}_{t_{i}})^{\top}(\mathbf{R}^{c}_{b}\hat{\mathbf{R}}^{w^{\top}}_{b_{t}}(\tilde{\mathbf{C}}^{w}_{j}-\hat{\mathbf{p}}^{w}_{b_{t}})+\mathbf{t}_{b}^{c})}{\lVert\mathbf{K}^{-1}\bar{\tilde{\mathbf{p}}}_{t_{i}}\rVert_{2}\cdot\lVert\mathbf{R}^{c}_{b}\hat{\mathbf{R}}^{w^{\top}}_{b_{t}}(\tilde{\mathbf{C}}^{w}_{j}-\hat{\mathbf{p}}^{w}_{b_{t}})+\mathbf{t}_{b}^{c}\rVert_{2}}}{\partial\hat{\mathbf{R}}^{w}_{b_{t}}}$
	$\displaystyle=\frac{\tilde{\mathbf{P}}_{t_{i}}^{\top}}{\lVert\tilde{\mathbf{P}}_{t_{i}}\rVert_{2}}\frac{\partial\frac{\tilde{\mathbf{C}}_{j}^{c}}{\lVert\tilde{\mathbf{C}}_{j}^{c}\rVert_{2}}}{\partial\tilde{\mathbf{C}}_{j}^{c}}\frac{\partial\tilde{\mathbf{C}}_{j}^{c}}{\partial\hat{\mathbf{R}}^{w}_{b_{t}}}$
	$\displaystyle=\frac{\tilde{\mathbf{P}}_{t_{i}}^{\top}}{\lVert\tilde{\mathbf{P}}_{t_{i}}\rVert_{2}}\left(\frac{\mathbf{I}}{\lVert\tilde{\mathbf{C}}_{j}^{c}\rVert_{2}}-\frac{\tilde{\mathbf{C}}_{j}^{c}\tilde{\mathbf{C}}_{j}^{c^{\top}}}{\lVert\tilde{\mathbf{C}}_{j}^{c}\rVert_{2}^{3}}\right)\frac{\partial\tilde{\mathbf{C}}_{j}^{c}}{\partial\hat{\mathbf{R}}^{w}_{b_{t}}}$
	$\displaystyle=\frac{\tilde{\mathbf{P}}_{t_{i}}^{\top}}{\lVert\tilde{\mathbf{P}}_{t_{i}}\rVert_{2}}\left(\frac{\mathbf{I}}{\lVert\tilde{\mathbf{C}}_{j}^{c}\rVert_{2}}-\frac{\tilde{\mathbf{C}}_{j}^{c}\tilde{\mathbf{C}}_{j}^{c^{\top}}}{\lVert\tilde{\mathbf{C}}_{j}^{c}\rVert_{2}^{3}}\right)\mathbf{R}^{c}_{b}(\hat{\mathbf{R}}^{w^{\top}}_{b_{t}}(\tilde{\mathbf{C}}^{w}_{j}-\hat{\mathbf{p}}^{w}_{b_{t}}))_{\times}$		(37)
	$\displaystyle\frac{\partial\cos\theta_{ij}}{\partial\hat{\mathbf{p}}^{w}_{b_{t}}}=\frac{\partial\frac{(\mathbf{K}^{-1}\bar{\tilde{\mathbf{p}}}_{t_{i}})^{\top}(\mathbf{R}^{c}_{b}\hat{\mathbf{R}}^{w^{\top}}_{b_{t}}(\tilde{\mathbf{C}}^{w}_{j}-\hat{\mathbf{p}}^{w}_{b_{t}})+\mathbf{t}_{b}^{c})}{\lVert\mathbf{K}^{-1}\bar{\tilde{\mathbf{p}}}_{t_{i}}\rVert_{2}\cdot\lVert\mathbf{R}^{c}_{b}\hat{\mathbf{R}}^{w^{\top}}_{b_{t}}(\tilde{\mathbf{C}}^{w}_{j}-\hat{\mathbf{p}}^{w}_{b_{t}})+\mathbf{t}_{b}^{c}\rVert_{2}}}{\partial\hat{\mathbf{p}}^{w}_{b_{t}}}$
	$\displaystyle=\frac{\tilde{\mathbf{P}}_{t_{i}}^{\top}}{\lVert\tilde{\mathbf{P}}_{t_{i}}\rVert_{2}}\frac{\partial\frac{\tilde{\mathbf{C}}_{j}^{c}}{\lVert\tilde{\mathbf{C}}_{j}^{c}\rVert_{2}}}{\partial\tilde{\mathbf{C}}_{j}^{c}}\frac{\partial\tilde{\mathbf{C}}_{j}^{c}}{\partial\hat{\mathbf{p}}^{w}_{b_{t}}}$
	$\displaystyle=\frac{\tilde{\mathbf{P}}_{t_{i}}^{\top}}{\lVert\tilde{\mathbf{P}}_{t_{i}}\rVert_{2}}\left(\frac{\mathbf{I}}{\lVert\tilde{\mathbf{C}}_{j}^{c}\rVert_{2}}-\frac{\tilde{\mathbf{C}}_{j}^{c}\tilde{\mathbf{C}}_{j}^{c^{\top}}}{\lVert\tilde{\mathbf{C}}_{j}^{c}\rVert_{2}^{3}}\right)\frac{\partial\tilde{\mathbf{C}}_{j}^{c}}{\partial\hat{\mathbf{p}}^{w}_{b_{t}}}$
	$\displaystyle=-\frac{\tilde{\mathbf{P}}_{t_{i}}^{\top}}{\lVert\tilde{\mathbf{P}}_{t_{i}}\rVert_{2}}\left(\frac{\mathbf{I}}{\lVert\tilde{\mathbf{C}}_{j}^{c}\rVert_{2}}-\frac{\tilde{\mathbf{C}}_{j}^{c}\tilde{\mathbf{C}}_{j}^{c^{\top}}}{\lVert\tilde{\mathbf{C}}_{j}^{c}\rVert_{2}^{3}}\right)\mathbf{R}^{c}_{b}\hat{\mathbf{R}}^{w^{\top}}_{b_{t}}$		(38)

where $\bar{(\cdot)}$ denotes the homogeneous coordinates. $\mathbf{\Sigma}_{\bar{\tilde{\mathbf{p}}}_{t_{i}}}=diag_{b}lock(\mathbf{I}_{2\times 2},0)$.

For the derivation of the Jacobian matrix $\mathbf{H}_{c_{t_{i}}}$ in Eq. (24), we transform the observation function in Eq. (23) as:

		$\displaystyle\mathbf{z}_{c_{t_{i}}}=\mathbf{y}_{t_{i}}-h_{i}(\hat{\mathbf{\Psi}}_{t_{i}})=h_{i}(\mathbf{\Psi}_{t_{i}})-h_{i}(\hat{\mathbf{\Psi}}_{t_{i}})+\mathbf{n}_{c_{t_{i}}}$
		$\displaystyle\approx h_{i}(\hat{\mathbf{\Psi}}_{t_{i}})+\mathbf{H}_{i}(\mathbf{\hat{\mathbf{\Psi}}_{t_{i}}})(\mathbf{\Psi}_{t_{i}}-\hat{\mathbf{\Psi}}_{t_{i}})-h_{i}(\hat{\mathbf{\Psi}}_{t_{i}})+\mathbf{n}_{c_{t_{i}}}$
		$\displaystyle=\mathbf{H}_{i}(\mathbf{\hat{\mathbf{\Psi}}_{t_{i}}})(\mathbf{\Psi}_{t_{i}}-\hat{\mathbf{\Psi}}_{t_{i}})+\mathbf{n}_{c_{t_{i}}}$
		$\displaystyle=\mathbf{H}_{i}(\mathbf{\hat{\mathbf{\Psi}}_{t_{i}}})\left[\mathbf{R}_{b_{t}}^{w^{\top}}(\tilde{\mathbf{C}}_{t_{j}}^{w}-\mathbf{t}^{w}_{b_{t}})-\hat{\mathbf{R}}_{b_{t}}^{w^{\top}}(\tilde{\mathbf{C}}_{t_{j}}^{w}-\hat{\mathbf{t}}^{w}_{b_{t}})\right]+\mathbf{n}_{c_{t_{i}}}$
		$\displaystyle=\mathbf{H}_{i}(\mathbf{\hat{\mathbf{\Psi}}_{t_{i}}})\left[\hat{\mathbf{R}}_{b_{t}}^{w^{\top}}(\hat{\mathbf{R}}_{b_{t}}^{w}\mathbf{R}_{b_{t}}^{w^{\top}}-\mathbf{I})\tilde{\mathbf{C}}_{t_{j}}^{w}\right.$
		$\displaystyle\qquad\qquad\qquad\qquad\qquad\left.+\hat{\mathbf{R}}_{b_{t}}^{w^{\top}}(\hat{\mathbf{t}}^{w}_{b_{t}}-\hat{\mathbf{R}}_{b_{t}}^{w}\mathbf{R}_{b_{t}}^{w^{\top}}\mathbf{t}^{w}_{b_{t}})\right]+\mathbf{n}_{c_{t_{i}}}$
		$\displaystyle\approx-\mathbf{H}_{i}(\mathbf{\hat{\mathbf{\Psi}}_{t_{i}}})\left[\hat{\mathbf{R}}_{b_{t}}^{w^{\top}}(\tilde{\mathbf{C}}_{t_{j}}^{w})_{\times}\bm{\xi}_{R_{t}}-\hat{\mathbf{R}}_{b_{t}}^{w^{\top}}\bm{\xi}_{p_{t}}\right]+\mathbf{n}_{c_{t_{i}}}$		(39)

The Jacobian matrix $\mathbf{H}_{i}(\mathbf{\hat{\mathbf{\Psi}}_{t_{i}}})$ is formulated as:

		$\displaystyle\mathbf{H}_{i}(\hat{\mathbf{\Psi}}_{t_{i}})=\frac{\partial h(\mathbf{\Psi}_{t_{i}})}{\partial\mathbf{\Psi}_{t_{i}}}\Big{|}_{\mathbf{\Psi}_{t_{i}}=\hat{\mathbf{\Psi}}_{t_{i}}}$
		$\displaystyle=-\frac{1}{\hat{Z}_{t_{j}}^{c^{2}}}(\mathbf{R}_{b}^{c}\hat{\mathbf{\Psi}}_{t_{i}}+\mathbf{t}_{b}^{c})\frac{\partial\hat{Z}_{t_{j}}^{c}}{\partial\mathbf{\Psi}_{t_{i}}}\Big{|}_{\mathbf{\Psi}_{t_{i}}=\hat{\mathbf{\Psi}}_{t_{i}}}+\frac{1}{\hat{Z}_{t_{j}}^{c}}\mathbf{R}_{b}^{c}$
		$\displaystyle\frac{\partial\hat{Z}_{t_{j}}^{c}}{\partial\mathbf{\Psi}_{t_{i}}}\Big{|}_{\mathbf{\Psi}_{t_{i}}=\hat{\mathbf{\Psi}}_{t_{i}}}=\frac{\partial\mathbf{R}_{b,3}^{c}\mathbf{\Psi}_{t_{i}}+t_{b,3}^{c}}{\partial\mathbf{\Psi}_{t_{i}}}\Big{|}_{\mathbf{\Psi}_{t_{i}}=\hat{\mathbf{\Psi}}_{t_{i}}}=\mathbf{R}_{b,3}^{c}$		(40)

where $\mathbf{R}_{b,3}^{c}$ denotes the third row of $\mathbf{R}_{b}^{c}$ and $t_{b,3}^{c}$ is the third element of $\mathbf{t}_{b}^{c}$. By substituting Eq. (-C) into Eq. (-C), the Jacobian matrix $\mathbf{H}_{c_{t_{i}}}$ of the camera-based observation model can be derived.

$\displaystyle\frac{d}{dt}\!\begin{bmatrix}\bm{\xi}_{R_{t}}\\ \bm{\xi}_{v_{t}}\\ \bm{\xi}_{p_{t}}\\ \bm{\zeta}_{\omega_{t}}\\ \bm{\zeta}_{a_{t}}\end{bmatrix}\!=\!\begin{bmatrix}\mathbf{0}&\mathbf{0}&\mathbf{0}&-\hat{\mathbf{R}}_{b_{t}}^{w}&\mathbf{0}\\ (\mathbf{g}^{w})_{\times}&\mathbf{0}&\mathbf{0}&-({{}^{w}}\hat{\mathbf{v}}_{b_{t}})_{\times}\hat{\mathbf{R}}_{b_{t}}^{w}&-\hat{\mathbf{R}}_{b_{t}}^{w}\\ \mathbf{0}&\mathbf{I}&\mathbf{0}&-(\hat{\mathbf{p}}_{b_{t}}^{w})_{\times}\hat{\mathbf{R}}_{b_{t}}^{w}&\mathbf{0}\\ \mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}\\ \mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}\end{bmatrix}\begin{bmatrix}\bm{\xi}_{R_{t}}\\ \bm{\xi}_{v_{t}}\\ \bm{\xi}_{p_{t}}\\ \bm{\zeta}_{\omega_{t}}\\ \bm{\zeta}_{a_{t}}\end{bmatrix}+$

$\displaystyle\!\begin{bmatrix}\hat{\mathbf{R}}_{b_{t}}^{w}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}\\ ({{}^{w}}\hat{\mathbf{v}}_{b_{t}})_{\times}\hat{\mathbf{R}}_{b_{t}}^{w}&\hat{\mathbf{R}}_{b_{t}}^{w}&\mathbf{0}&\mathbf{0}&\mathbf{0}\\ (\hat{\mathbf{p}}_{b_{t}}^{w})_{\times}\hat{\mathbf{R}}_{b_{t}}^{w}&\mathbf{0}&\hat{\mathbf{R}}_{b_{t}}^{w}&\mathbf{0}&\mathbf{0}\\ \mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}&\mathbf{0}\\ \mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}\end{bmatrix}\!\begin{bmatrix}\mathbf{n}_{\omega_{t}}\\ \mathbf{n}_{a_{t}}\\ \mathbf{0}_{3\times 1}\\ \mathbf{n}_{b\omega_{t}}\\ \mathbf{n}_{ba_{t}}\end{bmatrix}\!\triangleq\!\mathbf{A}_{t}\!\begin{bmatrix}\bm{\xi}_{t}\\ \bm{\zeta}_{t}\end{bmatrix}\!+\!\mathbf{Ad}_{\hat{\mathbf{X}}_{t},\hat{\mathbf{b}}_{t}}\mathbf{n}_{t}$

$$\mathbf{b}_{t}=\begin{bmatrix}\mathbf{b}_{\omega_{t}}\\ \mathbf{b}_{a_{t}}\end{bmatrix}$$

(41)

$\displaystyle\mathbf{z}_{o_{t}}=\begin{bmatrix}\mathbf{0}&\mathbf{I}&\mathbf{0}&\mathbf{0}&\mathbf{0}\end{bmatrix}\begin{bmatrix}\bm{\xi}_{t}\\ \bm{\zeta}_{t}\end{bmatrix}+\hat{\mathbf{R}}_{b_{t}}^{w}\mathbf{n}_{o_{t}}\triangleq-\mathbf{H}_{o_{t}}\begin{bmatrix}\bm{\xi}_{t}\\ \bm{\zeta}_{t}\end{bmatrix}+\hat{\mathbf{R}}_{b_{t}}^{w}\mathbf{n}_{o_{t}}$

(42)

$\displaystyle\mathbf{z}_{c_{t_{i}}}=-\mathbf{H}_{i}(\hat{\mathbf{\Psi}}_{t_{i}})\hat{\mathbf{R}}_{b_{t}}^{w^{\top}}\begin{bmatrix}(\tilde{\mathbf{C}}_{t_{j}}^{w})_{\times}&\mathbf{0}&-\mathbf{I}&\mathbf{0}&\mathbf{0}\end{bmatrix}\begin{bmatrix}\bm{\xi}_{t}\\ \bm{\zeta}_{t}\end{bmatrix}+\mathbf{n}_{c}\triangleq-\mathbf{H}_{c_{t_{i}}}\begin{bmatrix}\bm{\xi}_{t}\\ \bm{\zeta}_{t}\end{bmatrix}+\mathbf{n}_{c}$

(43)

	$\displaystyle s_{re}=\frac{1}{n_{re}}\sum_{i=1}^{n_{re}}\Delta p^{b_{re}+}_{i}+\gamma_{1}\Delta t_{b_{re}}^{b_{re}+}+\gamma_{2}\Delta\theta_{b_{re}}^{b_{re}+}+\gamma_{3}\sum_{i=1}^{n_{re}}\!neg(i)$
	$\displaystyle\Delta p^{b_{re}+}_{i}=\lVert\pi_{c}(\mathbf{R}_{b}^{c}\hat{\mathbf{R}}_{b_{re}}^{{w+}^{\top}}(\tilde{\mathbf{C}}_{re_{j}}^{w}-\hat{\mathbf{p}}^{w+}_{b_{re}})+\mathbf{t}_{b}^{c})-\tilde{\mathbf{p}}_{re_{i}}\rVert_{2}$
	$\displaystyle\Delta t_{b_{re}}^{b_{re}+}=\lVert\hat{\mathbf{p}}_{b_{re},1,2}^{w-}-\hat{\mathbf{p}}_{b_{re},1,2}^{w+}\rVert_{2}$
	$\displaystyle\Delta\theta_{b_{re}}^{b_{re}+}=yaw(\hat{\mathbf{R}}_{b_{re}}^{{w-}^{\top}}\hat{\mathbf{R}}_{b_{re}}^{w+})$