## 2 Methods
### INS/Vision Tightly Coupled Integration Model
Because \(\delta\mathbf{p}_{P_{k}}^{e}\) is not the state that needs to be estimated and \(\mathbf{H}_{f,i}\) is known, we can calculate the left null space A, which satisfies the equation as follows:  
\[\mathbf{A}^{T}\mathbf{H}_{f,i}=0 \tag{22}\]  
Then, multiplying \(\mathbf{A}^{T}\) at both sides of Equation (21), the measurement model can be described as  
\[\mathbf{z}_{a,i}=\mathbf{A}^{T}\mathbf{z}_{P_{k},i}=\mathbf{A}^{T}\mathbf{H}_{\mathbf{x},i}\mathbf{x}_{i,c }+\mathbf{A}^{T}\mathbf{\varepsilon}_{P_{k},i}=\mathbf{H}_{a,x,i}\mathbf{x}_{i,c}+\mathbf{\varepsilon }_{a,i} \tag{23}\]  
where \(\mathbf{z}_{a,i}=\mathbf{A}^{T}\mathbf{z}_{P_{k},i}\), \(\mathbf{H}_{a,x,i}=\mathbf{A}^{T}\mathbf{H}_{\mathbf{x},i}\) and \(\mathbf{\varepsilon}_{a,i}=\mathbf{A}^{T}\mathbf{\varepsilon}_{P_{k},i}\), respectively. The pixel coordinate of \(P_{k}\) can be described as follows:  
\[ \begin{pmatrix}\widetilde{u}_{P_{k},i}\\ \widetilde{v}_{P_{k},i}\\ 1\end{pmatrix} =\frac{1}{z_{P_{k},i}^{c_{i}}}K\mathbf{p}_{P_{k},i}^{c_{i}} \tag{24}\]  
in which K denotes the camera intrinsic parameter. Substituting Equation (18) into (25), we have:  
\[ \begin{pmatrix}\widetilde{u}_{P_{k},i}\\ \widetilde{v}_{P_{k},i}\\ 1\end{pmatrix} =\frac{1}{z_{P_{k},i}^{c_{i}}}K\mathbf{R}_{e}^{c_{i}}\Big{(}\mathbf{p}_{ P_{k}}^{e}-\mathbf{p}_{c_{i}}^{e}\Big{)} \tag{25}\]Thus,  
\[ \begin{pmatrix}\Delta\vec{u}_{P_{k}}\\ \Delta\widehat{v}_{P_{k}}\\ 0\end{pmatrix} =K\left[\frac{1}{z_{P_{k},i+1}^{i+1}}\mathbf{R}_{e}^{c_{i+1}}\left(\bm {p}_{P_{k}}^{\varepsilon}-\mathbf{p}_{c_{i+1}}^{\varepsilon}\right)-\frac{1}{z_{P_ {k},i}^{c_{i}}}\mathbf{R}_{e}^{c_{i}}\left(\mathbf{p}_{P_{k}}^{\varepsilon}-\mathbf{p}_{c _{i}}^{\varepsilon}\right)\right] \tag{26}\]  
where \(\Delta\vec{u}_{P_{k}}=\vec{u}_{P_{k},i+1}-\vec{u}_{P_{k},i}\), \(\Delta\widehat{v}_{P_{k}}=\widehat{v}_{P_{k},i+1}-\widehat{v}_{P_{k},i}\). \(\mathbf{p}_{P_{k}}^{\varepsilon}\) is the position of \(P_{k}\) for the epoch \(\mathrm{i}+1\). \(\left(\vec{u}_{P_{k},i+1},\vec{v}_{P_{k},i+1}\right)^{T}\) is the observation of \(P_{k}\) in the \(\mathrm{i}+1\)th image. Considering that the time interval is relatively small, we assume that the pose and position of the camera have no obvious changes. Because \(z_{P_{k},i+1}^{i+1}\) is relatively large, we can make assumptions: \(\mathbf{R}_{e}^{c_{i+1}}\approx\mathbf{R}_{e}^{c_{i}}\), \(\frac{1}{z_{P_{k},i+1}^{i+1}}\approx\frac{1}{z_{P_{k},i}^{i+1}}\). For the static feature point, \(\mathbf{p}_{P_{k}}^{\varepsilon}-\mathbf{p}_{P_{k}}^{\varepsilon}=0\), thus we can obtain  
\[z_{P_{k},i+1}^{c_{i+1}} \begin{pmatrix}\Delta\vec{u}_{P_{k}}\\ \Delta\widehat{v}_{P_{k}}\\ 0\end{pmatrix} =-K\mathbf{R}_{e}^{c_{i+1}}\left(\mathbf{p}_{c_{i+1}}^{\varepsilon}-\bm {p}_{c_{i}}^{\varepsilon}\right) \tag{27}\]  
As we know, \(\mathbf{p}_{c_{i+1}}^{\varepsilon}\) and \(\mathbf{p}_{c_{i}}^{\varepsilon}\) can be obtained by PPP-RTK/INS. For dynamic objects on the road, \(\left[\left(\mathbf{p}_{P_{k}}^{\varepsilon}-\mathbf{p}_{c_{i+1}}^{\varepsilon}\right) -\left(\mathbf{p}_{P_{k}}^{\varepsilon}-\mathbf{p}_{c_{i}}^{\varepsilon}\right) \right]<\left(\mathbf{p}_{c_{i+1}}^{\varepsilon}-\mathbf{p}_{c_{i}}^{\varepsilon}\right)\). By setting a threshold for \(z_{P_{k},i+1}^{i+1}\), the dynamic objects can be removed.