## 3 Methodology
### Trajectory Determination
#### 3.2.2 Observation Models  
The measurement update step of the Kalman Filter uses the current state of the kinematic system, the independent observations from the GPS receiver and/or the video camera. A cascading filter structure is used as both the GPS and video observations require some amount of preprocessing. The GPS receiver measurements are post-processed in Novatel's GrafNav software, which provides position \(p_{t}^{GPS}\) and velocity \(v_{t}^{GPS}\) relative to the mapping frame. The antenna reference point is offset from the origin of the body frame by a lever arm \(\bar{r}_{GPS}\). This allows the GPS to also provide an observation of orientation through the lever arm effect [33]. Compensating the GPS observations for the lever arm offset gives the following mathematical model:  
\[p_{t}^{GPS} =p_{t}+C_{b}^{n}\bar{r}_{GPS}+n_{p_{t}} \tag{13}\] \[v_{t}^{GPS} =v_{t}+C_{b}^{n}\cdot\omega_{t}\bar{r}_{GPS}+n_{p_{t}} \tag{14}\]where \(C_{b}^{n}\) represents the direction cosine matrix from the body frame to the mapping and is a function of the current quaternion attitude. \(\omega_{t}\) is the true rotational rate of the vehicle and \(n_{p_{t}}\) and \(n_{vt}\) are the stochastic measurement noise terms. The observations of orientation provided by the video camera are determined using a modification of the structure from motion (SfM) technique. The SfM technique allows the construction of the 3D structure of imaged objects as well as the estimation of the exterior camera orientation by analysing motion signals over time [38]. The technique can be applied to a set of overlapping images to obtain a sparse point cloud for a wide range of objects [29]. In creating this sparse point cloud, the technique optimally estimates the exterior orientation parameters of the camera in relative space through the use of a bundle adjustment. The 30 frames per second rate of the video camera, in conjunction with the flight dynamics of the UAV, results in relatively short baselines between the capture of video frames. This is problematic for the recovery of 3D structure within a bundle adjustment due to the poor geometry of the ray intersections used. To include a more complete set of video observations, a modified SfM algorithm was developed for use with this UAV platform. The algorithm first uses a standard SfM algorithm on key frames automatically selected based on the GPS velocity observations such that there is a constant overlap between key frames and a solution with strong geometry is achieved. Each frame is then trimmed to remove a border of 200 pixels from the edge to reduce the effects of lens distortion present in the consumer grade camera. The first stage of the SfM algorithm is then used to identify projections of the same features in space from two or more views using the Scale Invariant Feature Transform (SIFT) technique developed in [39]. SIFT key features are identified in each frame and then matched between frames using an approximate nearest neighbour kd-Tree approach [29]. The approach used in the matching of SIFT key features allows preliminary matches that are invariant to large changes in scale and rotation to be made [39]. This set of feature matches is likely to contain several incorrect correspondences or outliers. An iterative global RANSAC approach following Nister [40] is used to identify and eliminate these outliers and at the same time estimate the frame to frame homographies. The camera's exterior orientation estimates can then be derived from these homographies. The resulting set of feature correspondences and exterior orientation estimates can then be used to predict the three-dimensional locations of each feature in relative space. This information is then used along with initial camera locations within a global bundle adjustment to provide optimal estimates of 3D point location and the camera's exterior orientation within an arbitrary mapping frame.