# Tightly-Coupled GNSS/Vision Using a Sky-Pointing Camera for Vehicle Navigation in Urban Areas
## 5 Algorithm Development: GNSS/Vision Integration
### State Estimation and Data Integration  
The filter is initialized by the first epoch GNSS-only solution. Monocular visual odometry directly provides the attitude and the translation of the platform. The scale factor can be initialized by considering the two first image frames. In this case, the baseline corresponding to the two locations is synthesized and measured. This information can then be used to derive the depth information in a similar way used for stereo vision-based depth estimation [26; 29].  
The scale ambiguity was resolved in [26] by integrating the GPS carrier phase measurements with the vision system. In our case, we are using pseudorange and pseudorange rate as the measurement. For simplicity and illustration, let us consider a case where the vehicle moves forward without any rotation (translation only). The vehicle position vector and feature points ranges [26] are related as:  
\[H_{j}\cdot\begin{bmatrix}\Delta r\\ s_{j}\end{bmatrix}=\mathbf{0}_{2\times 1} \tag{37}\]  
where:  
\(\mathbf{0}_{2\times 1}\) denotes a \(2\times 1\) vector of zeros;  
\(j=1,...,M\), with \(M\) the total number for features;  
\(H_{j}\) is the matrix defining the homogeneous coordinates of the feature points from consecutive image frames;  
Figure 11: Upward-pointing camera coordinates system.  
\(\Delta r\) is the position change vector between consecutive frames; \(s_{j}\) denotes the unknown range.  
On the other hand, given the pseudorange defined as in Equation (12), the changes in pseudorange measurements between two consecutive images are related to \(\Delta r\):  
\[\Delta\rho_{j}=\Delta r+c\Delta t+\Delta\mu \tag{38}\]  
Combining Equations (37) and (38) defines a system of linear equations that can be used to unambiguously resolve the position changes, as well as the range estimates [26]. This procedure can be used for the vision system ranges and position changes' initialization. The assumption here is that the vehicle frame is aligned with the world frame.  
Once the system is initialized, the scale factor can be estimated based on the actual and previous estimated platform position. Thus, we have:  
\[\hat{s}_{k}=\sqrt{(\hat{x}_{k}-\hat{x}_{k-1})^{2}+(\hat{y}_{k}-\hat{y}_{k-1})^{ 2}+(\hat{z}_{k}-\hat{z}_{k-1})^{2}} \tag{39}\]  
where:  
\((\hat{x},\hat{y},\hat{z})\) represent the estimated platform's position.  
The filter is implemented with prediction performed at a rate of 10 Hz. This means that at an interval of 100 ms, the system checks if a new GNSS observation or visual odometry estimated parameter is available. As for the method applied in [42], the vision system's updates are processed at 10 Hz, and the update from the GNSS receiver (depicted in Figure 12) is performed at 1 Hz.  
In the implementation presented herein, the vision measurements are integrated in the form of a filter update. The platform's attitude is obtained by using the velocity estimates, i.e., by taking the arctangent of the ratio between the velocities in the east and north directions, respectively, for the azimuth for example.