2.1 Dynamic model

The system dynamic model consists of the kinematic equations of the inertial navigation driven by the IMU measurements, which are the specific force (or the sum of dynamic acceleration of the vehicle and gravity) and angular rate. The IMU bias errors are modelled as a random walk process, while the map states are modelled as a random constant due to their stationary nature, yielding

(6)\begin{align} \dot{\mathbf{p}}^{n} &= \mathbf{v}^{n}, \end{align}

(7)\begin{align} \dot{\mathbf{v}}^{n} &= \mathbf{C}_{b}^{n} (\mathbf{{f}}^{b} - \mathbf{b}^{b}_a) - 2 \boldsymbol{\omega}^{n}_{ie} \times \mathbf{v}^{n} + \mathbf{g}^{n}(\mathbf{{p}}^{n}), \end{align}

(8)\begin{align} \dot{\boldsymbol{\psi}}^{n} &= \mathbf{E}_{b}^{n}(\boldsymbol{{\omega}}^{b} - \mathbf{{b}}^{b}_{g}), \end{align}

(9)\begin{align} \dot{\mathbf{b}}^{b}_{a} &= \mathbf{w}_a, \quad \dot{\mathbf{b}}^{b}_{g} = \mathbf{w}_g, \end{align}

(10)\begin{align} \dot{{ct}}_b &= {ct}_d, \quad \dot{{ct}}_d = w_d, \end{align}

(11)\begin{align} \dot{\mathbf{m}}^{n}_{1,\ldots,N} &= \mathbf{0}, \end{align}

where

• • $\boldsymbol {\omega }^{n}_{ie}$ is the Earth's rotation rate in the navigation frame,

• • $\mathbf {g}^{n}(\mathbf {{p}}^{n})$ is the gravitational acceleration,

• • $\mathbf {w}_a$, $\mathbf {w}_g$ and ${w}_d$ are noise processes for accelerometers, gyroscopes and receiver clock drift, respectively, and

• • $\mathbf {C}_{b}^{n}$ and $\mathbf {E}_{b}^{n}$ are the direction cosine matrix transforming a vector in the body frame to that in the navigation frame, and the matrix transforming a body rate to an Euler angle rate, respectively,

  \begin{align*} \mathbf{C}_{b}^{n}& =\left[ \begin{array}{ccc} c_\theta c_\psi & -c_\phi s_\psi + s_\phi s_\theta c_\psi & s_\phi s_\psi + c_\phi s_\theta c_\psi \\ c_\theta s_\psi & c_\phi c_\psi + s_\phi s_\theta s_\psi & -s_\phi c_\psi + c_\phi s_\theta s_\psi \\ -s_\theta & s_\phi c_\theta & c_\phi c_\theta \end{array}\right],\\ \mathbf{E}_{b}^{n} &=\left[ \begin{array}{ccc} 1 & s_\phi t_\theta & c_\phi t_\theta \\ 0 & c_\phi & -s_\phi \\ 0 & s_\phi/c_\theta & c_\phi/c_\theta \end{array}\right], \end{align*}
  where $s_{(\cdot )}$, $c_{(\cdot )}$ and $t_{(\cdot )}$ are shorthand notation for $\sin (\cdot )$, $\cos (\cdot )$ and $\tan (\cdot )$, respectively.

Please note that this utilises a simplified version of the strap-down inertial navigation equations, aiming for low-cost inertial system applications. The effects of the Earth's curvature on the attitude calculation are not included, although it will be straightforward to add the curvature terms in the model.

2.2 Observation model

In the all-source aiding strategy, the observations consist of all available aiding information, which, in this work, is a set of pseudoranges ($\rho$) and pseudorange rates ($\dot {\rho }$) measured from a GNSS receiver, and/or a set of range $r$, bearing $\phi$ and elevation $\theta$ observations from a camera sensor:

(12)\begin{align} \rho_i &= \sqrt{ (X_{i}-x)^{2} + (Y_{i}-y)^{2} + (Z_{i}-z)^{2}} + c t_{b} + v_\rho, \end{align}

(13)\begin{align} \dot{\rho}_i &= l_{xi}(V_{xi}-v_x)+ l_{yi}(V_{yi}-v_y) + l_{zi}(V_{zi}-v_z) + c \dot{t}_{b} + v_{\dot{\rho}}, \end{align}

(14)\begin{align} r_j &= \sqrt{(m_{jx}-x)^{2} + (m_{jy}-y)^{2} + (m_{jz}-z)^{2}} + v_r, \end{align}

(15)\begin{align} \phi_j &= \arctan \frac{(m_{jy}-y)}{{(m_{jx}-x)}} + v_\phi, \end{align}

(16)\begin{align} \theta_j &=\arctan \frac{(m_{jz}-z)}{\sqrt{(m_{jx}-x)^{2}+(m_{jy}-y)^{2} } } + v_\theta. \end{align}

Here $(X_i, Y_i, Z_i)$ denotes the $i$th SV's position computed from the ephemeris data, which is converted from the ECEF (Earth-Centred, Earth-Fixed) frame to the navigation frame, with $v_\rho$ being the noise process, and $(l_{xi}, l_{yi}, l_{zi})$ is the line-of-sight vector of the $i$th SV seen from the vehicle, which projects the relative velocity along the line-of-sight direction. For the range, bearing and elevation measurement, we first compute the relative position vector $((m_{jx}-x), (m_{jy}-y), (m_{jz}-z) )$ of the $j$th landmark seen from the vehicle, and then transform it to polar coordinate quantities. Finally, $(v_\rho , v_{\dot \rho }, v_r, v_\phi , v_\theta )$ represent the observation noise processes. A new landmark position is initialised by utilising the inverse function of Equations (14) to (16) with the estimated vehicle state and observation information.

4.1 Environment and sensors

A flight dataset recorded from a fixed-wing UAV platform (Kim and Sukkarieh, Reference Kim and Sukkarieh2007) is used to verify the method. An IMU sensor is a low-grade sensor with a gyroscope bias stability of $10^{\circ }/\textrm {h}$, delivering outputs at $400$ Hz. A GPS receiver from Canadian Marconi Communication (now NovAtel) was used to record the pseudorange and pseudorange rate measurements at $1$ Hz and the satellite ephemeris data. The receiver provides integrated-carrier-phase (ICP) measurements, which are used to compute the pseudorange rates. Differential GPS (DGPS) correction messages (Radio Technical Commission for Maritime Services; RTCM) were broadcast from the ground station at the time of the experiment. A Sony monochrome camera (with a frame rate of $25$ Hz) was installed in a down-looking configuration. Please refer to Kim and Sukkarieh (Reference Kim and Sukkarieh2007) for more details of the onboard sensor configurations together with a flight control computer that recorded the data.

Figures 3(a) and 3(b) show sample aerial images collected from the camera, showing a white landmark laid on the ground at the bottom right corner of Figure 3(b). These artificial visual landmarks are made of white plastic sheets ($1\ \textrm {m} \times 1\ \textrm {m}$). They are installed on the ground, whose positions are pre-surveyed using a pair of real-time-kinematic GPS receivers. An intensity-based blob-detection algorithm is used, followed by template matching to detect square-shaped landmarks. Once detected, the known size of white pixels is used to compute the distance (range) to the landmark. The resulting (range, bearing, elevation) with uncertainty are reported/recorded in the camera coordinate system as shown in Figures 3(c) and 3(d). It can be observed that landmarks are first detected from the top of the camera image (or with the minimum elevation angle in the camera coordinates), tracked and disappeared at the bottom of the image (or with the maximum elevation angle). This can be seen in the increasing elevation angles in Figure 3(d). The range also shows a ‘V’-like pattern, which stems from the fact that the distance to a landmark becomes minimum when the landmark lies directly under the vehicle. The detected landmarks are associated with the SLAM map using the joint-compatibility branch and bound (JCBB) algorithm, which can effectively match a set of landmarks with the map. In this work, however, the number of landmarks detected in each image frame is quite low (one or two), and thus the association performance is close to the nearest-neighbour (NN) method.

Figure 3. (a,b) Two aerial images seen from the UAV. In (b) an artificial/white visual landmark is seen in the bottom right corner of the image, which is a white plastic sheet of $1\ \textrm {m} \times 1\ \textrm {m}$ from which the range was estimated. (c) Extracted range and (d) bearing/elevation measurements from the detected visual landmarks. It can be observed that landmarks are detected from the top of the camera image (with a minimum elevation angle) and exit the view at the bottom of the image (with a maximum elevation angle). This also manifests in the range showing ‘V’-like patterns, which stems from the fact that the range becomes maximum when a landmark is first or last detected, while it is a minimum when it is directly under the vehicle

4.2 Flight trajectory and vertical stabilisation

The flight segment has around $6$ min of duration from taking off, along the multiple racehorse trajectories (each track is about $5$ km long). The average flight speed is around $120$ km/h, and the average altitude is $150$ m above the ground. Figure 4(a) shows the sky plot of the visible SVs in an azimuth–elevation form showing nine visible SVs. Figure 4(b) shows the change of the number of SVs during the flight, with a simulated drop to three from $250$ s and then one from $280$ s to test the performance of the system. The number of visual landmarks (in red) is also shown, which changes from zero to three during the flight. These sparse visual observations make the height aiding of the inertial navigation system particularly challenging due to the large range errors inherent to the visual sensing. In this work, we stabilise the vertical height using the additional barometric-altitude information. The absolute and relative pressure measurements from the air data system are converted to barometric height and climb rate and then used to aid the vertical inertial outputs using a simple $\alpha$–$\beta$ filter. Although this is a suboptimal approach, it simplifies the SLAM filter tuning process with reliable performance and is thus adopted in this work.

Figure 4. (a) The sky view plot of the satellite vehicle during the flight experiment. (b) A dropout of the satellites from $250$ s was simulated. The number of visual landmarks is also shown. It can be observed that the number of visual landmarks is quite low, changing from zero to three, and thus requires additional pseudorange information to calibrate the low-cost IMU

4.3 CP-SLAM performance

Figures 1 and 5 illustrate the operation of the CP-SLAM system and the estimation results, respectively. Figure 5(a) compares the estimated vehicle positions from CP-SLAM (in solid blue), non-compressed Full-SLAM (in solid red) and on-board GPS/INS solution (in dashed red). It can be seen that the CP-SLAM and Full-SLAM show almost identical accuracy. Figure 5(b) also compares the estimated map from the CP-SLAM and the ground truth (surveyed map) with a total of $85$ landmarks and uncertainty, showing consistent mapping performance.

Figure 5. The estimation results from the compressed pseudo-SLAM (CP-SLAM): (a) the vehicle trajectory compared with Full-SLAM and on-board GPS/INS solution; (b) the map with uncertainty ($10\sigma$ for visual clarity); (c) the East positional error (m) with uncertainty ($1\sigma$); (d) the roll angular error (deg) with uncertainty ($1\sigma$); (e) the receiver clock-bias error (m) with uncertainty ($1\sigma$); and (f) the $x$-gyro bias error with uncertainty ($1\sigma$). The CP-SLAM trajectory shows a consistent result compared to Full-SLAM. The navigational errors and corresponding uncertainties show larger errors when the number of SVs drops to three and one. However, thanks to the continuous SLAM aiding, the navigational/clock/sensor errors are all constrained adequately

Figure 5(c) presents the navigational errors (East position error and roll angle error) with $1\sigma$ uncertainties, and Figure 5(f) shows the receiver clock bias error and the IMU gyroscope bias error ($x$-axis) with uncertainties. It can be observed that, when the number of SVs is sufficiently large (more than four), the pseudorange measurements dominate the estimation performance. In contrast, when the number of SVs drops to three and one, the errors increase, but the errors are adequately constrained within the uncertainty bound thanks to the continuous visual aiding.

Figure 6 compares the evolution of the map covariance from CP-SLAM (in solid blue) and Full-SLAM (in dashed red). For the clarity of the plot, only the result of landmark 1 is shown for the first $10$ s from the initial registration. It can be seen that, during $90{\cdot }5$–93 s, the landmark is within the local map, thus actively updated in both CP-SLAM and Full-SLAM. However, from $93$ s onwards, the landmark becomes inactive, as it is out of the local map, making CP-SLAM's covariance unchanged. In contrast, the Full-SLAM continuously reduces the covariance. Owing to the global updates at around $95$ s and $99$ s, the whole map is corrected, making the CP-SLAM results back to optimal, thanks to the compressed fusion method. This clearly demonstrates CP-SLAM's benefits, which can run the global update less frequently without sacrificing performance. This result is consistent with the results of Guivant (Reference Guivant2017, see Figure 3 on page 13), which utilised simulated multiple mobile robots showing that the CP-SLAM methods can be generalisable to various robot navigation problems.

Figure 6. Comparison of the evolutions of covariance of CP-SLAM (in solid blue) and Full-SLAM (in dashed red), for landmark 1. Landmark 1 is first registered at around $90{\cdot }5$ s, and the covariance of CP-SLAM and Full-SLAM change in the same way as the landmark is within the active map. From $93$ s, however, the landmark becomes inactive, as it is out of the local map, making the covariance of CP-SLAM unchanged. In contrast, the Full-SLAM continuously reduces the covariance. Owing to the global updates at around $95$ s and $99$ s, the whole map is corrected, making the CP-SLAM results back to optimal, thanks to the compressed fusion method

4.4 Computational complexity

Figure 7(a) presents the change of the total number of landmarks (in blue) and the number of local landmarks (in red) registered in the CP-SLAM filter. The final state vector dimension is $272$, consisting of the vehicle states ($17$) and the landmark maps ($255$), which corresponds to $85$ landmarks with three-dimensional position for each landmark. The number of local landmarks is maintained within $20$ throughout the flight, although it increases towards the end of the flight. This increase was due to the higher altitude of the vehicle resulting in a bigger local map and thus more local landmarks.

Figure 7. (a) Comparison of the total number of landmarks registered (in blue) and the number of local landmarks in CP-SLAM (in red), and (b) comparison of the update time of the Full-SLAM (in red) and CP-SLAM (in blue). The smoothed average values of the update times clearly show the benefits of the CP-SLAM. (c) Full-SLAM's correlation coefficient matrices and (d) CP-SLAM (the vehicle states correspond to the top-left elements in the matrix). Both matrices show strong diagonal correlation patterns but with sparse off-diagonal structures due to the loop closures. CP-SLAM shows more off-diagonal elements, which are due to the frequent reshuffling of the landmarks into the local and global ones, but has a smaller band-diagonal pattern due to the lower number of local landmarks

Figure 7(b) compares the computational times of the filter update from CP-SLAM (in blue) and Full-SLAM (in red), showing the improved computational performance of CP-SLAM. The Full-SLAM update's computational complexity is $\mathcal {O}(N^{2})$, with $N$ being the total number of landmarks. At the same time, the CP-SLAM has the complexity of $\mathcal {O}(L^{2}),\ L\ll N$, with $L$ being the local map size, which is less than the total map size. We can see that the pseudorange update times are nearly constant with $1$ ms on average, while the local landmark update times increase towards $1{\cdot }5$ ms. However, the local-to-global updates sometimes show peaks during the map transitions, which was affected by the additional data association and sorting process. During the last $60$ s of the GNSS dropout period, the update time is dominated by the visual updates. This result confirms that the compressed filtering approach is suitable for the real-time processing showing on average $1{\cdot }5$ ms processing time (using MATLAB on an Intel i5-core CPU with $1{\cdot }7$ GHz).

Figures 7(c) and 7(d) further compare the correlation structures of the Full-SLAM and CP-SLAM. Owing to the sparse visual observations and landmarks, the correlation coefficient matrices become sparse (the top left corner blocks are for the vehicle states). The Full-SLAM contains a much wider band-matrix structure than the CP-SLAM, particularly towards the bottom right blocks, as expected. The CP-SLAM shows more off-diagonal elements, which are due to the frequent reshuffling of the landmarks into the local and global ones, but has a smaller band-diagonal pattern due to the lower number of local landmarks. It can be noted that Full-SLAM registers the landmarks in temporal order, while CP-SLAM reorders the map based on spatial closeness. Depending on the application, we can apply a more optimal strategy to define the local map. For example, this work utilises a moving local region along with the vehicle, while Guivant (Reference Guivant2017) used a grid-based and fixed local region resulting in more block-diagonal structures. A further investigation might lead to a more sparse structure and thus a more efficient filtering algorithm.