Research ArticleOriginal Article
Open Access

A Robust Approach to Vision-Based Terrain-Aided Localization

Dan Navon, Ehud Rivlin, and Hector Rotstein
NAVIGATION: Journal of the Institute of Navigation March 2025, 72 (1) navi.683; DOI: https://doi.org/10.33012/navi.683

Abstract

Terrain-aided navigation, which combines radar altitude with a digital terrain map (DTM), was developed before the era of the Global Positioning System to prevent error growth resulting from inertial navigation. Recently, cameras and substantial computational power have become ubiquitous in flying platforms, prompting interest in studying whether the radar altimeter can be replaced by a visual sensor.

This paper presents a novel approach to vision-based terrain-aided localization by revisiting the correspondence and DTM (C-DTM) problem. We demonstrate that we can simplify the C-DTM problem by dividing it into a structure-from-motion (SFM) problem and then anchoring the solution to the terrain. The SFM problem can be solved using existing techniques such as feature detection, matching, and triangulation wrapped with a bundle adjustment algorithm. Anchoring is achieved by matching the point cloud to the terrain using ray-tracing and a variation of the iterative closest point method. One of the advantages of this two-step approach is that an innovative outlier filtering scheme can be included between the two stages to enhance overall robustness.

The resulting algorithm consistently demonstrated high precision and statistical independence in the presence of initial errors across various simulations. The impact of different filtering methods was also studied, showing an improvement of 50% compared with the unfiltered case.

The new algorithm has the potential to improve localization in real-world scenarios, making it a suitable candidate for pairing with an inertial navigation system and a Kalman f ilter to construct a comprehensive navigation system.

Keywords

1 INTRODUCTION

The relatively high sensitivity of global navigation satellite systems (GNSSs) to intentional or unintentional signal denial or spoofing (Jafarnia-Jahromi et al., 2012) has prompted researchers to explore alternative navigation schemes that achieve near - GNSS performance. Examples of these techniques include integrated inertial navigation system (INS)/celestial navigation (Gou et al., 2019), localization using geomagnetic anomalies (Canciani & Raquet, 2016), and alternatives to satellite navigation (Hein, 2020). Additionally, vision-based localization, based on variations of the structure-from-motion (SFM) problem in computer vision (Hartley & Zisserman, 2004), can be employed.

Vision-assisted navigation (VAN) is becoming popular in various settings, as small and inexpensive cameras have become pervasive in autonomous and semi-autonomous aerial vehicles. VAN typically does not require additional hardware to provide a navigation solution. Despite its advantages, vision-based navigation has two major drawbacks:

  1. Ambiguity: Motion recovered from a sequence of images intrinsically lacks information regarding scale; hence, unless additional information or valid heuristics are available, the results will be ambiguous and translation (and, consequently, velocity) will be recovered up to scale.

  2. Local nature: Images are referred to the local coordinate system attached to the imaging sensors and, hence, lack the global quality usually required for absolute navigation. Consequently, vision-only localization behaves as other dead-reckoning methods, with position and orientation accuracy degrading as the motion proceeds.

To overcome these drawbacks, “absolute” data that anchor the local visual frame to some external world coordinate system are required. For instance, natural or artificial landmarks have been extensively used to geo-reference the vision system, as described by Sala et al. (2006). Hosseini et al. (2020) incorporated an elevation model coupled with matched features to obtain a more accurate position for unmanned aerial vehicles, whereas Kim (2021) applied a higher-level approach by extracting object labels using a semantic segmentation deep neural network (DNN) and matched the labels with a preloaded object database. Although image comparison results in a low positional error, these methods tend to suffer from differences between the databases and the captured images, owing to changes in the scene such as weather, dynamic objects, and construction, making the matching process more difficult. Turek et al. (2021) reviewed the issues and techniques related to the processing of image information and its association with terrain reference images.

This paper considers using images as the basis of terrain-aided navigation (TAN), sometimes called terrain-referenced navigation, an approach initially proposed before the era of the Global Positioning System to assist cruise missile guidance. In traditional TAN, a radar altimeter, an INS, and a digital terrain map (DTM) are combined to match radar measurements against the DTM to correct a missile’s position using the TERCOM/SITAN algorithm (Fellerhoff, 1988; Golden, 1980). More recent methods have attempted to improve the matching accuracy between the DTM and the vehicle using a variety of sensors such as an interferometric radar altimeter (Kim et al., 2018), steerable laser (Carroll & Canciani, 2021; Livshitz & Idan, 2020), lidar (Hemann et al., 2016; Leines & Raquet, 2015), or sonar in underwater environments (Kedong et al., 2006). Although improvements have been achieved compared with legacy algorithms, active distance measurement may be problematic for space- or energy-conscious applications, prompting interest in passive vision-based methods.

Vision-based TAN uses a DTM and information extracted from a sequence of images to generate a navigation solution that does not diverge over time. In works by Lerner et al. (2006), Kim and Bang (2018), Geva et al. (2015), Zhang et al. (2011), and Scaramuzza and Fraundorfer (2011), the authors added a constraint to the visual odometery (VO) problem. They matched two-dimensional (2D) visual features between successive images and reconstructed the features while minimizing the mean distance error from the reconstructed points to the DTM. Although these methods have been shown to work successfully in several scenarios, they are not robust and tend to fail when the DTM differs from the constructed model or when outliers contaminate the measurement process. Outliers are always present in vision-based methods; hence, they must be treated systematically to achieve reliable localization.

The algorithm proposed in this work follows a two-step approach. First, the motion problem up to scale and absolute pose are solved using any of the many existing VO algorithms. Then, the point cloud computed from VO is anchored to a DTM using an algorithm resembling the point-to-plane iterative closest point (ICP) algorithm (Rusinkiewicz & Levoy, 2001). It is worth noting that the problem is not reduced to a three-dimensional (3D) reconstruction step followed by 3D matching, because the second step, used to estimate the position, orientation, and scale, is intrinsically related to the first step. As will be explained later, between the two steps, each 2D visual feature is associated with its corresponding 3D location on the DTM via ray-tracing. In practice, the feature association may be incorrect because of errors in the VO solution, inaccurate terrain anchoring, or objects in the image that inaccurately represent the terrain. One of the primary purposes of the research presented here is to develop methods and criteria to remove these outliers from the set of constraints before computing a pose solution. Wrong assignments can cause the solution to diverge; thus, outlier filtering must be performed after the ray-tracing process by using all available cues to identify problematic points. The cues considered here include scale statistics to detect deviated points, local curvature analysis to remove complex terrain patches, and geometric considerations to reject features with significant sensitivity.

2 CORRESPONDENCE AND DTM REVISITED

This section presents an alternative formulation to the correspondence and DTM (C-DTM) constraint first introduced by Lerner et al. (2006). This constraint was proposed for computing the relative motion and pose of a camera mounted on an air vehicle using two consecutive images and a DTM. This new proof provides further insight into the two-frame problem constrained by a DTM and suggests alternative ways of obtaining an efficient computational algorithm. Moreover, this proof shows how the approach can be extended to other VO algorithms, including the bundle adjustment (BA) algorithm or any of its variations.

2.1 Deriving the C-DTM Constraint

Consider the projections q1i,q2i of the i-th feature point onto an image plane for two time instants t1,t2. Using the pin-hole model, we obtain the following:

λ2iq2i=λ1iKR12K−1q1i−KT122-1

In this equation, R12 and T12 denote the relative rotation and translation of the camera from t1 to t2, respectively. Moreover, K is the matrix of intrinsic parameters of the camera, and λ1i and λ2i denote the depth of the i-th feature point on the first and second images, respectively. From this equation, the epipolar constraint can be obtained by first computing the vector product by K · T12 and then the scalar product with the projection at t2:

(q2i)T(KT12)∧KR12K−1q1i=02-2

Here, (x)∧ denotes the cross operator:

x∧=[x1x2x3]=[0−x3x2x30−x1−x2x10]2-3

The true location of the i-th feature point on the terrain is as follows:

QTi=p1+λ1iR1K−1q1i2-4

where p1 is the true location of the camera at time t1 and R1 is the rotation from the camera to world coordinates at time t1. Because of positioning and orientation errors, QTi cannot be computed precisely. Instead, using ray-tracing and intersection with the DTM, one can compute the estimated feature location QEi:

QEi=p¯1+μ1iR¯1K−1q1i2-5

where:

p1=p¯1−Δp2-6

R1=ΔΨR¯12-7

Here, Δp and ΔΨ model the position and orientation errors, respectively, and ΔΨ can usually be considered as a “small” rotation matrix, “close” to the identity matrix. Assuming that the true features lie on a planar approximation to the DTM at QEi with normal NiT, we obtain the following:

NiT(QTi−QEi)=02-8

Thus, applying Equation (2-4) yields the following:

λ1iNiTR1K−1q1i=NiiT(QEi−p1)2-9

We then obtain the following relationship:

λ1i=NiT(QEi−p1)NiTR1K−1q1i2-10

By substituting Equation (2-10) in Equation (2-1), we have the following:

λ2iq2i=KR12K−1q1iNiTNiTR1K−1q1i(QEi−p1)−KT122-11

from which:

q2i×[KR12K−1q1iNiTNiTR1K−1q1i(QEi−p1)−KT12]=02-12

Equation (2-12) was originally derived by Lerner et al. (2006) via a re-projection argument and algebra. This equation was referred to as the C-DTM constraint, as it combines correspondence with a DTM model. The above derivation is simpler and similar to the work of Faugers and Lustman (1988). The C-DTM includes all of the variables in the pose (absolute) and motion (relative) problem, and it has been shown that by extracting a (large) number of feature points and formulating a collection of such equations, all of the unknown variables can be computed. From a computational viewpoint, Equation (2-12) is a nonlinear constraint that can be solved, e.g., by performing Newton–Raphson iterations. However, this approach can be problematic for practical applications.

2.2 Alternative DTM Constraint

Instead of solving Equation (2-12) as proposed by Lerner et al. (2006), one can solve the epipolar constraint and then address Equation (2-9). In other words, one could solve the SFM problem up to the well-known seven degrees of freedom and then use the DTM to remove ambiguities. Let T^12 and R^12 denote the estimated solutions to the epipolar constraint. Then, assuming that there are enough feature points and that they have been chosen correctly, R^12≈R12 and T12≈κT^12 for some positive scale factor κ. Moreover, taking the cross product of Equation (2-1) with q2i on the left yields the following:

0=λ^1i(q2i×KR^12K−1q1i)−q2i×KT^122-13

from which the parameters λ^1i can be found. Note that, owing to the ambiguity between translation and depth, we have the following:

λ1i≈κλ^1i ∀i2-14

Applying this expression together with Equations (2-6) and (2-7) in Equation (2-4) yields the following:

QTi=p¯−Δp+κλ^1iR1K−1q1i2-15

Hence, by using Equation (2-8), we obtain the following:

NiT(QEi−p¯)=−NiTΔp+κλ^1iNiTR1K−1q1i2-16

We can combine this result with Equation (2-5):

μ1iNiTR¯1K−1q1i=−NiTΔp+κλ^1iNiTR1K−1q1i2-17

Because κ is independent of i, for any camera view, we can take a set of M different feature points and define the fitting error ϵ as follows:

ϵ≐∑i=1M[NiT(μ1iR¯1K−1q1i+Δp−κλ^1iR1K−1q1i)]22-18

The rotation R1 and location error Δp could, in principle, be solved by minimizing this fitting error. Because an analytic solution to this problem is relatively complex, we discuss below a numerical approximation that transforms this problem into a linear least-squares problem by using a small-angle approximation.

It is interesting to compare this equation with the formulation presented by Low (2004) for the point-to-plane ICP problem. In Low’s approach, given a collection of point pairs such as (ui, vi) and normal vectors Ni, the optimal rotation and translation to be applied to the ui points to align them with the vi points are obtained by minimizing the so-called alignment error:

ϵA=∑i=1M[NiT(Rui+t−vi)]22-19

The fitting error problem is then an unscaled version of the point-to-plane solution. By comparison, the first-order approximation to the DTM can then be considered as a way to formulate and solve the ICP. Hence, the outer part of the algorithm of Lerner et al. (2006) provides an iterative solution to the problem with two major differences: First, the scaling is unknown, and second, the vi points are computed via ray-tracing.

The solution to the above-mentioned equation assumes that the rotation matrix R1 is approximately known and, as shown next, results in a linear system of equations.

2.3 Approximate Solution

Under the assumption that the initial solution is close to the true solution, the rotation error in Equation (2-7) can be replaced by the small-angle approximation:

ΔΨ≈I−Δψ∧2-20

with:

Δψ=[δϕδθδψ]2-21

where δϕ,δθ, and δψ are small angles (i.e., less than a few degrees). With this approximation, we re-write Equation (2-17) as follows:

μ1iNiTR¯1K−1q1i=−NiTΔp+κλ^1iNiT(I−Δψ∧)R¯1K−1q1i2-22

We can divide by κ and apply the property that for two vectors x and y , x∧y = –y∧x:

μ1iNiTR¯1K−1q1i1κ=−NiTΔp^+λ^1iNiTR¯1K−1q1i+λ^1iNiT(R¯1K−1q1i)∧Δψ2-23

where Δp^=1κΔp. Then, after some re-ordering, we obtain the following:

NiT[I−λ^1i(R¯1K−1q1i)∧μ1iR¯1K−1q1i][Δp^Δψ1κ]=λ^1iNiTR¯1K−1q1i2-24

This is a linear equation with seven unknowns; thus, in principle, seven feature points may suffice to solve for an absolute position, orientation, and scale. This will yield a more precise estimate of R1 and of the scale κ. The latter can also be used to de-scale Δp^ and then solve for the true position. Note that in order to update R1 as in Equation (2-7), ΔΨ should be formed as the rotation matrix associated with Δψ, and not by using its respective small-angle approximation.

Equation (2-24) denotes the relationship for a single feature point as aix = bi, with ai representing a row vector of dimension 7 and bi representing a scalar. By considering a (large) number of feature points and packing all of these terms into a matrix A and a vector b, we obtain an expression of the form Ax = b that can be solved using linear least squares.

3 OUTLIER DETECTION AND REMOVAL

Outlier detection and removal are the primary challenges encountered when dealing with real-life data in computer vision. This is especially true when considering the ray-tracing step, as position and orientation errors may affect the intersection point on the DTM, particularly when considering small slant angles. Under these conditions, the coplanar assumption in Equation (2-8) may not hold even approximately. Consequently, the system of linear equations in Equation (2-24) becomes sensitive to outliers, and each incorrect entry may lead to divergence of the solution. Therefore, identifying outliers and removing or reducing their weights is essential for C-DTM to achieve robust performance in real-world applications. To effectively address the outlier detection problem, all available cues must be taken into account.

3.1 Scale Statistics

As seen above, two estimates for the depth of each point are available before we proceed to Step 2. The first estimate is the un-scaled estimate λ^1i, and the second estimate is μ1i, computed via ray-tracing. Under the assumption that both estimates are approximately correct, one must have κ≈ηji=μjiλ^ji, ∀i, j, namely, they are equal up to a constant, i.e., the scale factor. A histogram of the quotient can thus be used to remove points for which the relationship between the estimates is far from the average. To illustrate this, consider the histogram obtained using the simulated data (to be presented in Section 5) shown in Figure 2(a). Note that the values ηji in the left histogram are close to unity, but some deviate from the population mean. When these outlier candidates are removed, as in the right histogram, the distribution becomes more consistent with the basic assumption of the C-DTM. Depending on the quality of the data, one may want to filter out a prespecified quantity using the median absolute deviation (MAD) threshold factor. Figure 2(b) presents a scene with different depth relations. Here, p1 and λ1i are the true pose and depth of the i-th feature, and p^1, λ^1i, and μ1i are the estimated pose and depths. It can be seen that the ray (μ1i) has missed the right intersection point and should be filtered. While η1j,k≈2 will be at the mean area of the histogram, η1i≈4 will be at the extreme and, hence, should be eliminated.

FIGURE 1
FIGURE 1

This figure illustrates the actual (estimated) pose, feature location, and depth in blue (red). The depth is estimated via ray-tracing to the DTM. The unscaled depth estimation is shown in green.

FIGURE 2
FIGURE 2

Example of scale statistics (details given in Section 3.1) (a) ηji distribution before and after outlier removal (b) Example of different depth estimations

3.2 Slant-Angle Condition

The effect of position and orientation errors is minor when the angle between the ray direction and the local normal is relatively small. As the angle approaches 90°, the sensitivity of terrain intersection computations to pose errors increases, and therefore, the distance between QT and QE increases, compromising the assumption that both points lie on a plane. This property is illustrated for a simple 2D example in Figure 3. Thus, the angle between the normal and the ray direction is another parameter that must be monitored in C-DTM: the weight of the corresponding constraint must be reduced in inverse proportion to the angles between the normal and the ray direction.

FIGURE 3
FIGURE 3

Viewing a terrain with different slant angles

As the angle between the ray direction and the normal to the terrain grows, the effect of pose errors on the terrain increases.

3.3 Patch Curvature

Numerical implementation of the constraint in Equation (2-12) requires that an approximation for the normal to the terrain at QE be computed using grid points from the DTM. To highlight a bad example, Figure 4 shows a terrain patch that cannot be closely approximated by a plane at QEi. This condition, necessary for the validity of the planar assumption of the terrain patch, can be tested by estimating the normal from different combinations around grid points in a larger neighborhood of the intersection point.

FIGURE 4
FIGURE 4

Example of the patch curvature problem

When viewing the i-th point, the terrain patch does not look locally planar. At this point, the normals to the terrain differ at the grid points, suggesting that the normal at the center cannot be approximated by computing grid-point altitude differences and should be eliminated. The angle between Nj and its surrounding grid points is close to 90°; thus, this point is labeled as reliable.

3.4 Residual Monitoring

Let ei = Ax – b denote the residual obtained when solving Equation (2-24) for a relatively large quantity of feature points. Then, a standard procedure for eliminating outliers in linear equations is to examine the normalized residual:

e¯i≐eiσei3-25

FIGURE 5
FIGURE 5

Features with deviated residuals (e.g., MAD larger than 1.5) are marked in red and filtered out. Equation (2-24) is resolved without these outliers.

Features that result in a high residual can be filtered out using a MAD threshold. In the equation, σei denotes the standard deviation (STD) of the collection of residuals ei. A more systematic approach for this is the M-estimator proposed by Li and Swetits (2006), which was also used by Lerner et al. (2006) to reduce the effect of outliers in the nonlinear C-DTM solution.

4 IMPLEMENTATION DETAILS

The data required for implementing the C-DTM algorithm include a sequence of consecutive images with sufficient overlap (e.g., 50%), a DTM of the scene, and a sufficiently accurate initial pose. As previously discussed, the first step of the algorithm is to reconstruct the structure of the scene (e.g., computing approximate locations of the feature points) by using 2D projections onto the image plane and tracking corresponding points across the images. This process has been intensely studied in many computer vision articles (Scaramuzza & Fraundorfer, 2011). For instance, speeded-up robust features (SURF), as proposed by Bay et al. (2008), can be extracted from two consecutive images and matched according to a distance metric between the two feature vectors, similar to the approach described by Lowe (2004).

Next, the essential matrix can be estimated using, e.g., the normalized eight-point algorithm (Fan et al., 2023; Hartley & Zisserman, 2004) coupled with random sample consensus (RANSAC). The essential matrix can be decomposed into the relative rotation and the normalized translation motion matrices R and T, respectively.

After the camera motion has been recovered, multi-point triangulation, as described by Hartley and Zisserman (2004), can be used to recover the scene structure by triangulating corresponding 2D features into 3D world points. While not strictly required, one can refine the point cloud and improve the matching with the DTM by computing more accurate estimates to λ^1i using the BA algorithm. The above steps are referred to as SFM in our Algorithm 1 pseudocode.

ALGORITHM 1
ALGORITHM 1

The New C-DTM Algorithm

Next, the ray-tracing procedure (see Section 4.1) consists of prolonging the rays originating from the center of the focus and passing through given pixels until they intersect the terrain at QEi. These intersections yield the second depth estimation μ1i and, hence, NiT at QEi. Subsequently, one can solve the system of equations using Equation (2-24), correct the initial pose in Equations (2-6) and (2-7), and utilize κ to solve the translation-depth ambiguity. The estimation of the actual feature locations, indicated by Equation (2-4), can now be adjusted using Equation (2-14). Future camera poses can be determined using the perspective-three-point (P3P) method, as described by Gao et al. (2003).

To address the possibility that the coplanar assumption in Equation (2-8) may not hold, as explained in Section 3, one can iterate the algorithm to refine the approximation of Equation (2-8). The underlying idea is that each iteration moves the solution toward the true solution by reducing the approximation error of the coplanar assumption. However, if the initial 3D assignment is too far from the correct location or if the terrain cannot be approximated by a plane, then multiple iterations may cause the algorithm to diverge or converge to a local minimum and produce incorrect results.

To decide whether a solution has converged or diverged, a termination criterion may be placed on the size of the correction between iterations, i.e., one can monitor whether Δp increases or decreases between two iterations up to some predefined threshold. If the size of the correction decreases, then the relative residuals r¯=‖ei‖‖b‖, where ei and b are as defined in Section 3.4, can be used to study the convergence. Lower r¯ values usually indicate convergence to the global minimum, whereas higher values indicate convergence to a local minimum. If a local minimum is detected, then one can use heuristic perturbation techniques in an attempt to help the algorithm escape these local minima.

FIGURE 6
FIGURE 6

A pictorial illustration of the algorithm pipeline

Triangulation is applied to corresponding features to compute an unscaled point cloud (above). Then, ray-tracing provides an initial scaling, deforming the point cloud (below, left). Finally, the C-DTM algorithm restores the point cloud structure with the correct scale.

4.1 Ray-Tracing

Ray-tracing techniques are generally used in computer graphics (Sun, 2023) to trace the path of rays or particles through a system to calculate the light intensity from different objects. In the context of this work, ray-tracing relates the unscaled SFM point cloud with a scaled model. Essentially, this approach can be used to determine the intersection of a visual ray with the DTM, providing the second depth estimation μ1i. Specifically, the following algorithm was used in our implementation:

  1. Compute the ray direction: d1i→=R1K−1q1i.

  2. Extend the ray l=p1+d1i¯·max_dist from the camera position until it reaches a plane guaranteed to be below the DTM or until the DTM bounds are reached.

  3. Find the closest intersection point li such that liz – DTM(lix,liy) ≈ 0 by, e.g., using a variation of a binary search.

  4. Calculate the depth as μ1i=‖li−p1‖‖d1i→‖.

This process effectively incorporates real-world spatial information into our vision-based localization algorithm.

5 PERFORMANCE EVALUATION

This section presents the performance of the new C-DTM algorithm and shows the improvements achieved by applying the filtering procedures introduced in Section 3. The results show that the algorithm performs well under errors and measurement noise common in a practical application. Moreover, because the algorithm will eventually be used as part of a navigation filter, the statistical properties of the localization and orientation errors are studied. Specifically, we examine whether errors can be considered to be unbiased and statistically, or at least linearly, independent of the input errors. This last property is often overlooked when new vision-based algorithms are tested but, in our experience, is critical when assessing an overall navigation scheme.

Inputs to the algorithm were generated by adding random position and orientation errors to the true data of the camera with the following characteristics:

Δp=[δxδyδz]∼N(0,σpos)Δψ=[δϕδθδψ]∼N(0,σrot)5-26

where ϕ, θ, and ψ denote the roll, pitch, and yaw, respectively.

Four different values were selected for each of the STDs in Equation (5-26), giving rise to sixteen different Gaussian noise combinations:

σpos={15,25,35,45}[m], σrot={1.5,2,2.5,3}[deg]5-27

The experiments were numbered 1 to 16 as detailed in Table 1.

View this table:
TABLE 1

Labels for the 16 Different Experiments For instance, experiment number 11 was run with σpos = 35[m] and σrot = 2.5°.

For each of these experiments, the performance was estimated by using N = 500 Monte Carlo runs. Three different properties of the algorithm were studied: the STD of the output errors, the mean error, and the Pearson correlation coefficients. The STD of the errors for each axis was computed as a function of the input error measured using the Euclidean norm (position) and the geodesic distance α(ΔΨ)=cos−1(trace(ΔΨ)−12) (attitude). The mean value of the errors per axis was considered to verify that the results were unbiased. Lastly, the Pearson correlation coefficient matrix indicates the linear independence of the algorithm with respect to input errors. Having conducted 16 experiments, each with its own correlation coefficient matrix, we present the average correlation coefficient value calculated from these 16 matrices.

5.1 Testing Environments

Two simulation environments were used to test the performance of the algorithm: a fully synthetic simplified simulation written in MATLAB and a more realistic simulation that includes high-fidelity image rendering. The former aimed to debug the algorithm and show its low sensitivity to various input errors, allowing complete control of the errors, including ground-truth data. The latter is closer to field experiments and can be used to verify that the performance is not degraded when real-life errors and noise are introduced.

5.2 Synthetic Scene Setup

The first scenario uses a MATLAB synthetic simulation, for which the DTM was simulated using a sum of Gaussian functions with the following form:

Z=∑i=1nhi2πe−(x−μi)22σi25-28

The sum of Gaussians, denoted by Z, models a DTM, where x represents map coordinates and each Gaussian function is characterized by its mean, STD, and relative altitude (μi, σi, and hi). Here, hi is not normalized by σi, as this choice ensures that each Gaussian directly contributes to the terrain’s elevation profile, allowing for a varied and realistic DTM representation.

Note that in this simulation, ground-truth data are available, and we have the ability to perform many experiments by injecting errors to assess the overall performance. In reality, DTMs are not continuous functions, but are instead given in the form of the altitude of the terrain tabulated on a discrete grid. Correspondingly, in our simulations, the sum of each Gaussian was discretized by selecting the spacing between x,y nodes in each direction to be 25 m. This value was selected for ease of use, but it is also close to the level standardized in level-2 digital terrain elevation data maps (30 m) for commercially available DTMs.

FIGURE 7
FIGURE 7

DTM and camera locations at the eight different locations on which features were projected onto the image planes (shown in pink)

Red dots represent randomly generated world points. Blue lines show the ray-tracing process.

FIGURE 8
FIGURE 8

STD comparison on the synthetic scene (a) Position STD error (b) Rotation STD error

The STDs of each position and angular axis are estimated as a function of the size and rotation of the input perturbation.

Simulations were performed by randomly selecting the initial pose approximately 100 m above the ground and performing a relatively short straight-and-level flight. The camera orientation was set to θ = −35° while moving in a direction normal to the optical axis. Eight images were considered by taking steps of 25 m each between frame. Under these conditions, randomly generated features lying on the field of view were at a distance of 150–350 m. These world points were projected onto each one of the eight image planes and rounded to the nearest pixel. We simulated a camera system featuring a 60° field of view and a resolution of 1920 × 1080 pixels. Projections were conducted utilizing the intrinsic camera parameters of this sensor, denoted by the matrix K:

K=[1662.8096001662.8540001]

5.2.1 Localization Results

As shown next, the algorithm performs consistently well on the synthetic scene. Figure 12 shows the position (a) and angular errors (b) per axis as a function of the input perturbation norm and rotation, respectively. The results are surprisingly accurate and appear to be independent of the size of the input errors (note the scale). For the worst case of input errors, the STD of the position error was reduced from 50 m to less than 65 cm on each axis. Correspondingly, the angular error per axis decreased from 5° to less than 0.07° for the yaw axis.

Table 2 presents the mean value of the position and orientation errors for each of the 16 selected cases (see definitions next to Equation (5-26)). Note that the mean values are relatively small and appear to be independent of the size of the input errors, which supports our unbiased assumption.

View this table:
TABLE 2

Mean Output Error for Each of the Experiments Detailed Above Equation (5-26)

See Table 1.

Table 3 shows the Pearson correlation coefficient matrix, quantifying the cross-correlation between each input and each output. Following the argument of Schober and Schwarte (2018), these values suggest a negligible relationship between the input and output, supporting the claim that the algorithm is statistically linearly independent of the input.

View this table:
TABLE 3

Pearson Correlation Coefficient Values for the Synthetic Scene

Each cell represents the average correlation between the input and output errors across all 16 experiments.

As mentioned in Section 4, as the input error increases, a moderately higher number of iterations is required in order to achieve the best possible performance. This is especially true for localization errors because iterations are required to guarantee the point cloud alignment, as shown in Figure 9(a). Furthermore, as illustrated in Figure 9(b), the algorithm may actually fail to converge to the global minimum for relatively large initial errors. This is a direct consequence of the linear patch approximation on which the algorithm is based and is important to consider when combining the algorithm with a full navigation filter.

FIGURE 9
FIGURE 9

Algorithm performance (experiments numbered as in Table 1) (a) Mean number of iterations until convergence (b) Number of unsuccessful experiments out of N = 500

5.3 Simulation Test

To test the algorithm in a more realistic environment, a simulation was built using the Airsim/Unreal Engine (UE) tool. Airsim has built-in capabilities for rendering realistic images while generating ground-truth navigation data and a DTM. One of UE’s most popular scenarios is “Landscape Mountains,” which has a high DTM resolution of 1.28 m. Usually, such a high-resolution DTM is rarely accessible for free and consumes more computation power when computing the ray intersection points with the model. Hence, we sampled the DTM to obtain a 25-m resolution.

Two scenarios were constructed to examine the performance of the algorithm. First, the camera was simulated to be pointing downward with a pitch of θ = −80°. The camera moves in a short straight-and-level flight with a 30-m distance between frames. In the second scenario, trees were added to the scene, and θ = −40°. Typical images are shown in Figure 10. In the first case, we view the scene from a top-view position. In this case, there are fewer slant-angle or scale statistics errors, making the matching process more straightforward. Furthermore, most features lie directly on the DTM; thus, the main error arises from the difference between the inherent error of the DTM model and the actual structure of the terrain. To stress our algorithm in this simple case, we injected high initial error values:

σpos={40,50,60,70}[m], σrot={2,3,4,5}[deg]5-29

The second case is more challenging not only because of the angle of the camera, but also because of the presence of some features lying on the trees, constituting outliers that must be handled by the algorithm.

FIGURE 10
FIGURE 10

Example Airsim simulation frames (a) Top view (b) Slant angle (pitch = −40°) with trees

5.3.1 Results

As shown in Figures 11 and 12, the algorithm performs well, even in these more realistic scenarios. Surprisingly, the performance in the Airsim simulation cases appears to be better than the performance in the synthetic scenario. This result is most likely due to the fact that the terrain modeled by the UE is more realistic, has more variations, and is therefore more suitable for TAN.

FIGURE 11
FIGURE 11

Comparison of position STD error (a) Airsim: Top-view scene (b) Airsim: Slantangle-view scene

The x-axis represents the input error as single values (Equation (5-27)), and the y-axis shows the output error versus ground truth for each translation axis.

FIGURE 12
FIGURE 12

Comparison of rotation STD error (a) Airsim: Top-view scene (b) Airsim: Slantangle-view scene

The x-axis represents the input error as single values (Equation (5-27)), and the y-axis represents the output error versus ground truth for each rotation axis.

Figures 9(a) and 13 demonstrate that the higher complexity of natural scenes translates into a significant reduction in the number of iterations required for convergence. By comparing Figures 13(a) and 13(b), we see that more challenging scenarios giv e rise to outliers, resulting in a higher number of iterations that aim to improve the coplanar approximation (Equation (2-8)), so as to achieve good performance.

FIGURE 13
FIGURE 13

Mean number of iterations until convergence (experiments numbered as in Table 1) (a) Airsim: Top-view scene (b) Airsim: Slant-angle-view scene

Outliers not only increase the computational cost; they can result in a greater number of failures, e.g., divergence or local minimum solutions. Figure 14 shows that in the slanted-angle case, the number of failures is higher than in the top-view scenario. This observation is consistent with our main claim: data filtering is required to achieve the best possible performance and to aid in convergence of the algorithm.

FIGURE 14
FIGURE 14

Number of unsuccessful experiments out of N = 500 (experiments numbered as in Table 1) (a) Airsim: Top-view scene (b) Airsim: Slant-angle-view scene

Furthermore, both scenes demonstrated a negligible correlation between the input and output of the algorithm, as shown in Table 4. The algorithm displayed a near-constant mean error regardless of the applied error. The inherent error of the DTM could explain the existence of this mean error due to height approximation errors among the DTM grid nodes. In the synthetic scenario, the DTM is represented by a sum of Gaussians (Section (5.2)), where the height between nodes is approximated via a cubic interpolation, which results in a good approximation. In contrast, in real-world DTMs, there will always be a gap between the terrain seen by the images and the DTM. This average distance between the SFM point cloud and the DTM is embedded in the residuals of the solution to Equation (2-24).

View this table:
TABLE 4

Pearson Correlation Coefficient Values (a) Airsim: Top-view scene (b) Airsim: Slant-angle-view scene

View this table:

5.4 Comparison of Filtering Methods

We conducted a series of six simulations to assess the influence of various filtering methods on the performance of the algorithm. For each simulation, the algorithm used one of the four distinct filtering methods described in Sections 3.1, 3.2, 3.3, and 3.4. Additionally, we tested scenarios in which either all of these methods were used together or none were applied. The slant-angle scene was selected for this investigation, given its inherently challenging nature for the algorithm. The test parameters were drawn from Equation (5-26), with σpos = 45[m], σrot =3[deg], ensuring a consistent input error of |Δp|2 = 70[m], α(ΔΨ) = 5[deg] across all experiments.

Through a comparative analysis of the application of all, none, or a single filter, a trade-off between STD error reduction, as depicted in Figure 15, and the number of unsuccessful experiments, shown in Figure 16(b), is discernible. Utilizing all filters leads to an optimal decrement in both STD error and the number of unsuccessful experiments.

FIGURE 15
FIGURE 15

A comparison of filters based on of STD output errors for the tree scene; input errors sampled with σpos = 45[m], σrot = 3[deg] (a) Position STD error (b) Rotation STD error

FIGURE 16
FIGURE 16

Comparison of filter performance (a) Mean number of iterations until convergence (b) Number of unsuccessful experiments out of N = 500

View this table:
TABLE 5

Airsim: Mean Error Values (a) Airsim: Top-view scene (b) Airsim: Slant-angle-view scene

View this table:

Examining the outcomes obtained by applying all or none of the filters provides a practical comparison to the work of Lerner et al. (2006), who directly addressed the C-DTM problem as described by Equation (2-12). In contrast, the approach detailed in this paper separates the C-DTM into an SFM problem before matching the SFM’s point cloud with the terrain while filtering outliers between the stages. As a result, the number of unsuccessful experiments is reduced by half, as shown in Figure 16(b), thereby underscoring the importance of the filtering process.

Two filters that warrant further exploration are the scale-statistics filter (Section 3.1) and the residuals filter (Section 3.4). Application of the scale-statistics filter alone increased the number of successful experiments at the cost of increasing the STD of the errors. Indeed, as shown in Figure 16(b), the filter is capable of converging to a “good” solution in cases where using all of the filters does not, albeit at the cost of a higher error rate, i.e., increased STD error, as shown in Figure 15.

We can infer from Figure 16(a) that the residuals filter often reaches the maximum number of iterations (30). This suggests that although the filter aids in approaching the global solution, it tends to oscillate around the global solution, which results in a higher STD error.

These insights provide valuable direction for further refining the filter selection and application process. Nonetheless, the results emphasize the role of filtering techniques in mitigating errors and increasing success rates, even in situations of high initial error, thus validating the algorithm’s robustness in complex scenarios.

6 CONCLUSIONS

This work revisits the C-DTM problem, introducing a new perspective that significantly contributes to the field of TAN. The main contributions of this paper are as follows:

  • We divide the C-DTM problem into two separate stages by recovering motion and structure up to scale and then solving a special point cloud alignment problem that anchors the solution to the terrain. This approach allows us to replace the nonlinear solution of the original problem with two linear solutions.

  • We propose a novel method for outlier detection that enhances the robustness of the localization process in diverse terrains and conditions.

  • We show that the navigation algorithm has desirable statistical properties, making it adequate for combining with a navigation filter.

Empirical results illustrated the resilience and effectiveness of the proposed vision-based terrain-aided localization algorithm. Despite substantial noise and uncertainty, the algorithm consistently produced precise outcomes, making it attractive for real-world localization processes.

One of the assumptions on which the proposed approach is based is that the DTM provides a good model for the actual terrain. This assumption can be challenged whenever the features used for the computations do not lie on the terrain but on natural or man-made structures above it. Because such features might affect the accuracy of the algorithm, it is important to not only detect features but to also analyze their nature. Recent advancements in semantic segmentation can provide a solution for this issue, for instance, by weighting constraints according to the semantics of the corresponding feature. We believe that these findings can open a promising direction for further developing and refining vision-based TAN designs.

HOW TO CITE THIS ARTICLE

Navon, D., Rivlin, E., & Rotstein, H. (2025). A robust approach to vision-based terrain-aided localization. NAVIGATION, 72(1). https://doi.org/10.33012/navi.683

This is an open access article under the terms of the Creative Commons Attribution License, which permits use, distribution and reproduction in any medium, provided the original work is properly cited.

REFERENCES

  1. ↵
    1. Bay, H.,
    2. Ess, A.,
    3. Tuytelaars, T., &
    4. Van Gool, L.
    (2008). Speeded-up robust features (SURF). Computer Vision and Image Understanding, 110(3), 346–359. https://doi.org/10.1016/j.cviu.2007.09.014
    CrossRef
  2. ↵
    1. Canciani, A., &
    2. Raquet, J.
    (2016). Absolute positioning using the earth’s magnetic anomaly field. NAVIGATION, 63(2), 111–126. https://doi.org/10.1002/navi.138
  3. ↵
    1. Carroll, J. D., &
    2. Canciani, A. J.
    (2021). Terrain-referenced navigation using a steerable-laser measurement sensor. NAVIGATION, 68(1), 115–134. https://doi.org/10.1002/navi.406
  4. ↵
    1. Fan, B.,
    2. Dai, Y.,
    3. Seo, Y., &
    4. He, M.
    (2023). A revisit to the normalized eight-point algorithm and a self-supervised deep solution. ArXiv. https://doi.org/10.48550/arXiv.2304.10771
  5. ↵
    1. Faugers, O., &
    2. Lustman, F.
    (1988). Motion and structure from motion in a piecewise planar environment. International Journal in Pattern Recognition and Artificial Intelligence, 2(3), 485–508. https://doi.org/10.1142/S0218001488000285
  6. ↵
    1. Fellerhoff, J. R.
    (1988). AFTI/SITAN (Advanced fighter technology integration/Sandia Inertial terrain-aided navigation) final report (Tech. Rep.). Sandia National Laboratories (SNL). Retrieved from http://www.osti.gov/servlets/purl/6873200/
  7. ↵
    1. Gao, X. S.,
    2. Hou, X. R.,
    3. Tang, J., &
    4. Cheng, H. F.
    (2003). Complete solution classification for the perspective-three-point problem. IEEE Transactions on Pattern Analysis and Machine Intelligence, 25(8), 930–943. https://doi.org/10.1109/TPAMI.2003.1217599
    CrossRef
  8. ↵
    1. Geva, A.,
    2. Briskin, G.,
    3. Rivlin, E., &
    4. Rotstein, H.
    (2015). Estimating camera pose using bundle adjustment and digital terrain model constraints. Proc. of the 2015 International Conference on Robotics and Automation (ICRA), Seattle, WA, 4000–4005. https://doi.org/10.1109/ICRA.2015.7139758
  9. ↵
    1. Golden, J. P.
    (1980). Terrain contour matching (TERCOM): A cruise missile guidance aid. Proc. of the 24th Annual Technical Symposium, San Diego, CA, 10–18. https://doi.org/10.1117/12.959127
  10. ↵
    1. Gou, B.,
    2. Cheng, Y.-m., &
    3. de Ruiter, A. H. J.
    (2019). INS/CNS navigation system based on multi-star pseudo measurements. Aerospace Science and Technology, 95, 105506. https://doi.org/10.1016/j.ast.2019.105506
  11. ↵
    1. Hartley, R. I., &
    2. Zisserman, A.
    (2004). Multiple view geometry in computer vision (2nd ed.). Cambridge University Press. https://doi.org/10.1017/CBO9780511811685
  12. ↵
    1. Hein, G. W.
    (2020). Status, perspectives and trends of satellite navigation. Satellite Navigation, 1(1), 1–12. https://doi.org/10.1186/s43020-020-00023-x
  13. ↵
    1. Hemann, G.,
    2. Singh, S., &
    3. Kaess, M.
    (2016). Long-range GPS-denied aerial inertial navigation with LIDAR localization. Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Daejeon, Korea (South). 1659–1666. https://doi.org/10.1109/IROS.2016.7759267
  14. ↵
    1. Hosseini, K.,
    2. Ebadi, H., &
    3. Farnood Ahmadi, F.
    (2020). Determining the location of UAVs automatically using aerial or remotely sensed high-resolution images for intelligent navigation of UAVs at the time of disconnection with GPS. Journal of the Indian Society of Remote Sensing, 48(12), 1675–1689. https://doi.org/10.1007/s12524-020-01187-4
  15. ↵
    1. Jafarnia-Jahromi, A.,
    2. Broumandan, A.,
    3. Nielsen, J., &
    4. Lachapelle, G.
    (2012). GPS vulnerability to spoofing threats and a review of antispoofing techniques. International Journal of Navigation and Observation, 2012(1) 1–16. https://doi.org/10.1155/2012/127072
  16. ↵
    1. Kedong, W.,
    2. Lei, Y.,
    3. Wei, D., &
    4. Junhong, Z.
    (2006). Research on iterative closest contour point for underwater terrain-aided navigation. Proc. of the Joint IAPR International Workshops, SSPR 2006 and SPR 2006, Hong Kong, China, 252–260. https://doi.org/10.1007/11815921_27
  17. ↵
    1. Kim, Y.
    (2021). Aerial map-based navigation using semantic segmentation and pattern matching. ArXiv. https://doi.org/10.48550/arXiv.2107.00689
  18. ↵
    1. Kim, Y., &
    2. Bang, H.
    (2018). Vision-based navigation for unmanned aircraft using ground feature points and terrain elevation data. Proc. of the Institution of Mechanical Engineers, Part G: Journal of Aerospace Engineering, 232(7), 1334–1346. https://doi.org/10.1177/0954410017690548
  19. ↵
    1. Kim, Y.,
    2. Park, J., &
    3. Bang, H.
    (2018). Terrain-referenced navigation using an interferometric radar altimeter. NAVIGATION, 65(2), 157–167. https://doi.org/10.1002/navi.233
  20. ↵
    1. Leines, M. T., &
    2. Raquet, J. F.
    (2015). Terrain reference navigation using SIFT features in lidar rangebased data. Proc. of the 2015 International Technical Meeting of the Institute of Navigation, Dana Point, CA, 239–250. https://www.ion.org/publications/abstract.cfm?articleID=12622
  21. ↵
    1. Lerner, R.,
    2. Rivlin, E., &
    3. Rotstein, H. P.
    (2006). Pose and motion recovery from feature correspondences and a digital terrain map. IEEE Transactions on Pattern Analysis and Machine Intelligence, 28(9), 1404–1417. https://doi.org/10.1109/TPAMI.2006.192
    CrossRefPubMed
  22. ↵
    1. Li, W., &
    2. Swetits, J. J.
    (2006). The linear L1 estimator and the Huber M-estimator. Society for Industrial and Applies Mathematics Journal on Optimization, 8(2), 457–475. https://doi.org/10.1137/S1052623495293160
  23. ↵
    1. Livshitz, A., &
    2. Idan, M.
    (2020). Preview control approach for laser-range-finder-based terrain following. IEEE Transactions on Aerospace and Electronic Systems, 56(2), 1318–1331. https://doi.org/10.1109/TAES.2019.2933955
  24. ↵
    1. Low, K.-L.
    (2004). Linear least-squares optimization for point-to-plane ICP surface registration (Tech. Rep.). University of North Carolina. https://www.comp.nus.edu.sg/~lowkl/publications/lowk_point-to-plane_icp_techrep.pdf
  25. ↵
    1. Lowe, D. G.
    (2004). Distinctive image features from scale-invariant keypoints. International Journal of Computer Vision, 60(2), 91–110. https://doi.org/10.1023/B:VISI.0000029664.99615.94
    CrossRef
  26. ↵
    1. Rusinkiewicz, S., &
    2. Levoy, M.
    (2001). Efficient variants of the ICP algorithm. Proc. of the Third International Conference on 3-D Digital Imaging and Modeling, 3DIM, Quebec City, QC, Canada, 145–152. https://doi.org/10.1109/IM.2001.924423
  27. ↵
    1. Sala, P.,
    2. Sim, R.,
    3. Shokoufandeh, A., &
    4. Dickinson, S.
    (2006). Landmark selection for vision-based navigation. IEEE Transactions on Robotics, 22(2). https://doi.org/10.1109/TRO.2005.861480
  28. ↵
    1. Scaramuzza, D., &
    2. Fraundorfer, F.
    (2011). Visual odometry [Tutorial]. IEEE Robotics and Automation Magazine, 18(4), 80–92. https://doi.org/10.1109/MRA.2011.943233
    CrossRef
  29. ↵
    1. Schober, P., &
    2. Schwarte, L. A.
    (2018). Correlation coefficients: Appropriate use and interpretation. Anesthesia and Analgesia, 126(5), 1763–1768. https://doi.org/10.1213/ANE.0000000000002864
    CrossRefPubMed
  30. ↵
    1. Sun, D.
    (2023). The reviewed of ray tracing technology optimization. Academic Journal of Science and Technology, 8(3), 54–55. https://doi.org/10.54097/x2d4m989
  31. ↵
    1. Turek, P.,
    2. Grzywiński, S., &
    3. Bużantowicz, W.
    (2021). Selected issues and constraints of image matching in terrain-aided navigation: A comparative study. IntechOpen. https://doi.org/10.5772/intechopen.95039
  32. ↵
    1. Zhang, J.,
    2. Liu, W., &
    3. Wu, Y.
    (2011). Novel technique for vision-based UAV navigation. IEEE Transactions on Aerospace and Electronic Systems, 47(4), 2731–2741. https://doi.org/10.1109/TAES.2011.6034661
    CrossRef
Back to top

In this issue

Print
Download PDF
Article Alerts
Email Article
Citation Tools
A Robust Approach to Vision-Based Terrain-Aided Localization
Dan Navon, Ehud Rivlin,, Hector Rotstein
NAVIGATION: Journal of the Institute of Navigation Mar 2025, 72 (1) navi.683; DOI: 10.33012/navi.683
Twitter logo Facebook logo Mendeley logo
Bookmark this article

Jump to section

Keywords