# Tightly-Coupled GNSS/Vision Using a Sky-Pointing Camera for Vehicle Navigation in Urban Areas
## 5 Algorithm Development: GNSS/Vision Integration
### Tightly-Coupled GNSS/Vision  
After applying our multipath mitigation algorithm, there are still a few GNSS satellites (mostly less than four) that are available. Theoretically, at least four satellites are needed to compute the GNSS-only navigation solution [37]. This situation leads us to make a choice of the tightly-coupled GNSS/vision Kalman filter since even with fewer than four satellites, it is possible to use the raw GNSS measurement to improve the efficiency of vision-based navigation. The system model used is given by:  
\[\dot{\mathbf{x}}(t)=\mathbf{F}(t)\mathbf{x}(t)+\mathbf{G}(t)w(t) \tag{22}\]  
where:  
\(\mathbf{x}\) and \(\mathbf{F}\) are the state vector and the dynamics matrix, respectively;  
\(\mathbf{G}\) represents the shaping matrix;  
\(w\) is a vector of zero-mean, unit variance white noise. Thus, the dynamic system is expressed in the form:  
\[\dot{\mathbf{x}}(t)=\mathbf{F}(t)\mathbf{x}(t) \tag{23}\]  
The dynamic matrix is approximately constant between image frames. Its equivalent in discrete-time is called the state transition matrix \(\mathbf{\Phi}_{k,k+1}\) that converts the state from epoch \(k\) to \(k+1\), given by:  
\[\mathbf{\Phi}_{k,k+1}=\exp(\mathbf{F}\Delta t) \tag{24}\]  
Given that the Taylor series [62] of the exponential function is computed as \(\exp(A)=\mathbf{I}+A+\dfrac{A^{2}}{2}+\dfrac{A^{3}}{6}+\ldots\), the transition matrix in Equation (24) becomes:  
\[\mathbf{\Phi}_{k,k+1}=\mathbf{I}+\mathbf{F}\Delta t+\dfrac{\mathbf{F}^{2} \Delta t^{2}}{2}+\dfrac{\mathbf{F}^{3}\Delta t^{3}}{6}+\ldots \tag{25}\]  
The state vector incorporates the vehicle position, velocity, acceleration, attitude and the receiver clock as shown in Equation (26). \[\mathbf{x}= \begin{bmatrix}\phi&\lambda&h&v&a&p&\dot{p}&r&\dot{r}&A&\dot{A}&cdt &cdt\end{bmatrix} ^{\mathsf{T}} \tag{26}\]  
where:  
* \(\phi\), \(\lambda\) and \(h\) represent the position components;
* \(v\) and \(a\) stand for speed and acceleration, respectively;
* \(A\), \(p\) and \(r\) are the azimuth, pitch and roll respectively;(\(\bullet\)) represents their corresponding rates. The error states are:  
\[\delta\mathbf{x}=\left[\delta\mathbf{x}_{vehicle}\quad\delta\mathbf{x}_{vision} \quad\delta\mathbf{x}_{clock}\right]^{\mathsf{T}} \tag{27}\]  
which is equivalent to:  
\[\delta\mathbf{x}=\left[\delta\phi\quad\delta\lambda\quad\delta h\quad\delta v \quad\delta a\quad\delta p\quad\delta\dot{p}\quad\delta r\quad\delta\dot{r} \quad\delta A\quad\delta A\quad\delta A\quad\delta cdt\quad\delta cdt\right]^{ \mathsf{T}} \tag{28}\]  
and the receiver clock model is given by:  
\[\delta\mathbf{x}_{clock}(t)= \begin{bmatrix}1&\Delta t\\ 0&1\end{bmatrix} \delta\mathbf{x}_{clock}(t-1)+\mu_{clk} \tag{29}\]  
where \(\delta\mathbf{x}_{clock}=\left[\delta cdt\quad\delta cdt\right]^{\mathsf{T}}\)  
The prediction of the state and the corresponding error covariance matrix are given by Equations (30) and (31), respectively. \[\mathbf{\hat{x}}_{k+1}^{-}=\mathbf{\Phi}_{k,k+1}\mathbf{\hat{x}}_{k}^{+} \tag{30}\]  
\[\mathbf{P}_{k+1}^{-}=\mathbf{\Phi}_{k,k+1}\mathbf{\hat{P}}_{k}^{+}\mathbf{ \Phi}_{k,k+1}^{\mathsf{T}}+\mathbf{Q}_{k} \tag{31}\]  
where the superscript minus and plus respectively denote the state before and after an update; and \(\mathbf{Q}_{k}\) is the process noise matrix. The state and the corresponding error covariance matrix after the update stage are given in Equations (32) and (33). \[\mathbf{\hat{x}}_{k}^{+}=\mathbf{\hat{x}}_{k}^{-}+\mathbf{K}_{k}(\mathbf{z}_{ k}-\mathbf{H}_{k}\mathbf{\hat{x}}_{k}^{-}) \tag{32}\]  
\[\mathbf{P}_{k}^{+}=(\mathbf{I}-\mathbf{K}_{k}\mathbf{H}_{k})\mathbf{P}_{k}^{-} \tag{33}\]  
where:  
**H** denotes the design matrix, which is the derivative of the measurements with respect to the states;  
\(\mathbf{K}_{k}=\mathbf{P}_{k}^{-}\mathbf{H}_{k}^{\mathsf{T}}(\mathbf{H}_{k} \mathbf{P}_{k}^{-}\mathbf{H}_{k}^{\mathsf{T}}+\mathbf{R}_{k})^{-1}\) represents the Kalman gain. The term in the Kalman filter state update in the brackets (in Equation (32)) is called the innovation.