Handheld Ground-Penetrating Radar Antenna Position Estimation Using Factor Graphs

Abstract

Accurate localization of handheld ground-penetrating radar (HH-GPR) systems is critical for high-quality subsurface imaging and precise geospatial mapping of detected buried objects. In our previous works, we demonstrated that a UWB positioning system with an extended Kalman filter (EKF) employing a proprietary pendulum (PND) dynamics model yielded highly accurate results. Building on that foundation, we present a factor-graph-based estimation algorithm to further enhance the accuracy of HH-GPR antenna trajectory estimation. The system was modeled under realistic conditions, and both the EKF and various factor-graph algorithms were implemented. Comparative evaluation indicates that the factor-graph approach achieves an improvement in localization accuracy from over 30 to almost 50 percent compared to the EKF PND. The sparse matrix representation inherent in the factor graph enabled an efficient iterative solution of the underlying linearized system. This enhanced positioning accuracy is expected to facilitate the generation of clearer, more distinct underground images, thereby supporting the potential for more reliable identification and classification of buried objects and infrastructure.

1. Introduction

Modern radar-based sensing systems play a crucial role in subsurface exploration, enabling the detection of buried objects and the characterization of underground structures. Such specialized radar systems are commonly called ground-penetrating radars (GPRs). A variety of GPR solutions are available, catering to a wide range of applications in both civilian and military domains [1,2,3,4,5,6]. While GPR systems are frequently deployed on manned or unmanned mobile ground platforms, in challenging environments—such as indoor settings, debris-strewn areas, or densely vegetated terrain—compact and portable handheld ground-penetrating radars (HH-GPRs) are often utilized [7,8,9].

This paper addresses an important problem encountered when using HH-GPR, namely, accurately determining the antenna’s position during scanning. Precise positioning is crucial for correctly mapping the acquired radargrams to the antenna’s successive locations along the non-rectilinear scanning path at the time of signal transmission. Such accurate localization is essential for ensuring reliable geospatial mapping of the data and for enabling effective synthesis of images of the buried objects. However, estimating the trajectory of a handheld GPR antenna is challenging due to factors such as operator-induced motion, terrain irregularities, and external disturbances affecting sensor measurements [10,11].

Traditional localization methods, such as the Kalman filter (KF) [12,13], rely on the assumption of a Markov process, where each estimated state is dependent only on its immediate predecessor. The KF employs recursive updates to estimate the system’s state based on sequential measurements, making it computationally efficient [12,13,14]. However, its accuracy is often limited by the inability to fully incorporate complex motion constraints and measurement uncertainties within each iteration. An alternative approach to solving the localization problem is the use of factor graphs, in which nodes represent the estimated positions of the radar system at different time steps, and edges encode constraints between these positions. Factor graphs offer a batch optimization framework for trajectory estimation by jointly considering all available measurements and state transitions, as opposed to the sequential updates used in filtering techniques. This smoothing-based approach allows for improved handling of nonlinear motion models and sparse or noisy observations.

Originally introduced by Kschischang et al. [15] for function factorization in probabilistic graphical models, factor graphs were additionally employed in sum-product algorithms for efficient marginalization. In the field of robotics and navigation, factor graphs have been widely adopted for solving Simultaneous Localization and Mapping (SLAM) problems, offering a flexible and robust framework for trajectory estimation [16,17]. Further advancements have expanded their applicability to nonlinear constraints and real-time navigation systems [16,18]. Factor graphs have found broad application across numerous fields, serving as a robust framework for localization and trajectory estimation tasks [19,20,21]. Their ability to integrate all available sensor measurements in a batch optimization setting makes them particularly well-suited for environments with complex motion dynamics and sparse observations. Despite their proven advantages, the application of factor graphs for tracking handheld GPR devices during terrain scans remains a relatively underexplored area, motivating this study.

In our previous research [10,11], a system that processes distance measurements between stationary and mobile ultrawideband (UWB) radio transceivers using various extended Kalman filters (EKFs) was detailed. Building on that foundation, we now present a factor-graph-based estimation algorithm designed to further enhance the accuracy of HH-GPR antenna trajectory estimation. This article aims to investigate the use of factor graphs as an alternative to traditional filtering techniques for estimating the position and trajectory of a handheld GPR system during terrain scanning. The main contributions of this paper are as follows:

• A comprehensive development and detailed presentation of a factor-graph-based algorithm for HH-GPR antenna trajectory estimation, including analysis of its computational complexity and memory usage, with a comparison to EKF. The proposed algorithm is used to process distance measurements in a UWB positioning system and uses two variants of motion models and two variants of observation models.
• Presentation of new results of simulative tests for the UWB positioning system with the factor-graph-based algorithm and their comparison with the results obtained with the use of the EKF.

The reminder of this paper is organized as follows. A description of the UWB positioning system and the formulation of the localization problem within the factor graph framework are given in Section 2. The methodology and the results of simulative testing of the factor graph algorithm as well as its comparison against the EKF are presented in Section 3. Finally, Section 4 contains concluding remarks.

2. Positioning System and Estimation Algorithm

2.1. Positioning System for Handheld Ground-Penetrating Radar

In the practical application of a handheld ground-penetrating radar, two operational approaches can be considered. The first approach involves conducting the scanning process over a predefined area—for example, when searching a hazardous region with clearly established boundaries. In such cases, the area may be divided into strips of appropriate width, which are then scanned sequentially as shown in Figure 1. An alternative scenario involves performing ad hoc scanning in dangerous, hard-to-access terrain along the operator’s irregular path, such as when determining a safe route through a hazardous zone, as explained in Figure 2. Both figures also include components of the positioning system, which will be described in detail later in the paper.

For these defined scenarios, we have developed a positioning system that utilizes distance measurements between stationary and mobile ultrawideband (UWB) radio transceivers. The system, shown in Figure 3 and described in detail in [10,11], is presented here briefly for completeness. It comprises four UWB base stations of known locations (labeled ${\mathrm{M}}_1 ÷ {\mathrm{M}}_4$), strategically positioned in a safe area outside the designated scanning region, and two mobile UWB units: ${\mathrm{M}}_{\mathrm{A}}$, mounted on the GPR antenna, and ${\mathrm{M}}_{\mathrm{S}}$, attached to the shoulder of the sapper.

The measurement system operates by simultaneously determining the distances between each base station and the mobile units ($d_{\mathrm{A}1} ÷ d_{\mathrm{A}4}$ and $d_{\mathrm{S}1} ÷{ d}_{\mathrm{S}4}$). In our previous works [10,11], to process these measurements and estimate the HH-GPR antenna’s position along with other motion parameters, we applied various custom estimation algorithms based on the Kalman filter. Several dynamic models—including a constant velocity (CV) model and a proprietary pendulum model (PND)—were employed. Although these methods significantly reduced positioning errors compared to a UWB system without filtration, achieving centimeter-level accuracy, the factor-graph-based algorithm discussed in this paper appears to be a promising step forward for even better estimation results.

2.2. Application of Factor Graph in Positioning of Handheld Ground-Penetrating Radar

A factor graph is a type of bipartite graph that provides a structured way to model problems involving two distinct types of nodes: hidden variables and factors. Variable nodes correspond to the key unknowns that need to be estimated, such as the state trajectory of an object, while factor nodes impose constraints (such as dynamic constraints), or incorporate measurements that relate to these variables.

In this framework, connections exist only between variable nodes and factor nodes, indicating dependencies between them. An edge linking a variable node to a factor node signifies that the variable’s value influences the factor’s computation or plays a role in determining its outcome. By structuring problems in this manner, a complex global function can be broken down into a collection of simpler local functions, making analysis and computation more efficient.

The factor graph describing the analyzed problem of estimating the HH-GPR position using UWB modules consists of:

• Variable nodes representing the GPR antenna’s state at different time steps (${\mathbf{x}}_1$…${\mathbf{x}}_M$), which include its position and velocity with accordance to a given motion model.
• Factor nodes, which either represent likelihood terms derived from range measurements (${\mathbf{z}}_1$…${\mathbf{z}}_N$, shown in red in Figure 4 and Figure 5) or transition model functions ($\mathit{f}$, shown in blue in Figure 4 and Figure 5) that encode dynamic behavior of the system.

The conducted research considered and compared factor graphs constructed for two variants of motion models and two observation models. The motion models included the Constant Velocity (CV) model [10,12]:

$$\left[\begin{array}{c}{\dot{x}}_A\\ {\dot{v}}_x\\ {\dot{y}}_A\\ {\dot{v}}_y\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{x}_A\\ v_x\\ y_A\\ v_y\end{array}\right]+\left[\begin{array}{cc}0& 0\\ 1& 0\\ 0& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{u}_{v_x}\\ u_{v_y}\end{array}\right],$$

(1)

and a proprietary non-linear model based on the pendulum equation (PND) [10,11]:

$$\left[\begin{array}{c}{\dot{x}}_A\\ {\dot{y}}_A\\ {\dot{x}}_S\\ {\dot{y}}_S\\ \dot{\theta }\\ \dot{\omega }\\ \dot{a}\end{array}\right]=\left[\begin{array}{c}\omega \left(y_A-y_S\right)\\ -\omega \left(x_A-x_S\right)\\ 0\\ 0\\ \omega \\ -{\displaystyle \frac{a}{l}}\mathit{sin}\theta \\ 0\end{array}\right]+\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 1& 0& 0\\ 0& 1& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{u}_{x_S}\\ u_{y_S}\\ u_a\end{array}\right],$$

(2)

where:

$x_A,y_A$—coordinates of the ${\mathrm{M}}_A$ module position,

$v_x,v_y$—orthogonal velocities of the ${\mathrm{M}}_A$ module position,

$x_S,y_S$—coordinates of the ${\mathrm{M}}_S$ module position,

$u_{x_S},u_{y_S}$—Gaussian white noises modeling random ${\mathrm{M}}_S$ module motions,

$l$—horizontal distance between ${\mathrm{M}}_S$ and ${\mathrm{M}}_A$,

$\theta$—angle between the horizontal projection of the HH-GPR antenna handle and the central axis of the scanning section,

$\omega$—angular velocity of the ${\mathrm{M}}_A$ module motion,

$a$—acceleration forcing the ${\mathrm{M}}_A$ module motion, and

$u_a$—Gaussian white noise representing random changes of $a$.

Both observation models used eight distance measurements between each base station and the mobile units ($d_{\mathrm{A}1}÷d_{\mathrm{A}4}$ and $d_{\mathrm{S}1}÷d_{\mathrm{S}4}$). They differed in whether a pseudo-measurement for the arm length $l$ was included—a parameter that remains nearly constant and depends on both the HH-GPR handle length and the sapper’s arm height $h$. The version without this pseudo-measurement is presented in Equation (3).

$$\left[\begin{array}{c}{d}_{A1}\left(k\right)\\ d_{A2}\left(k\right)\\ d_{A3}\left(k\right)\\ d_{A4}\left(k\right)\\ d_{S1}\left(k\right)\\ d_{S2}\left(k\right)\\ d_{S3}\left(k\right)\\ d_{S4}\left(k\right)\end{array}\right]=\left[\begin{array}{c}\sqrt{{\left(x_A\left(k\right)-x_1\right)}^2+{\left(y_A\left(k\right)-y_1\right)}^2}\\ \sqrt{{\left(x_A\left(k\right)-x_2\right)}^2+{\left(y_A\left(k\right)-y_2\right)}^2}\\ \sqrt{{\left(x_A\left(k\right)-x_3\right)}^2+{\left(y_A\left(k\right)-y_3\right)}^2}\\ \sqrt{\begin{array}{c}{\left(x_A\left(k\right)-x_4\right)}^2+{\left(y_A\left(k\right)-y_4\right)}^2\end{array}}\\ \sqrt{{\left(x_S\left(k\right)-x_1\right)}^2+{\left(y_S\left(k\right)-y_1\right)}^2+h^2}\\ \sqrt{{\left(x_S\left(k\right)-x_2\right)}^2+{\left(y_S\left(k\right)-y_2\right)}^2+h^2}\\ \sqrt{{\left(x_S\left(k\right)-x_3\right)}^2+{\left(y_S\left(k\right)-y_3\right)}^2{+h}^2}\\ \sqrt{\begin{array}{c}{\left(x_S\left(k\right)-x_4\right)}^2+{\left(y_S\left(k\right)-y_4\right)}^2+h^2\end{array}}\end{array}\right]+\left[\begin{array}{c}{v}_{A1}\left(k\right)\\ v_{A2}\left(k\right)\\ v_{A3}\left(k\right)\\ v_{A4}\left(k\right)\\ v_{S1}\left(k\right)\\ v_{S2}\left(k\right)\\ v_{S3}\left(k\right)\\ v_{S4}\left(k\right)\end{array}\right],$$

(3)

where:

$d_{Aj}$—distance between a $j$-th beacon and the ${\mathrm{M}}_A$ module,

$d_{Sj}$—distance between a $j$-th beacon and the ${\mathrm{M}}_S$ module,

$x_j,y_j$—coordinates of a $j$-th beacon position,

$x_A,y_A$—coordinates of the ${\mathrm{M}}_A$ module position,

$x_S,y_S$—coordinates of the ${\mathrm{M}}_S$ module position,

$h$—sapper’s arm height, and

$v_{Aj},v_{Sj}$—distance measuring errors for ${\mathrm{M}}_A$ and ${\mathrm{M}}_S$ modules.

Figure 4 illustrates a factor graph used to estimate the trajectory of a handheld ground-penetrating radar antenna, based on the observation model presented in Equation (3). Each factor represents a distinct element of the trajectory, such as state transitions (motion model) or alignment with UWB observations (measurement model).

When a pseudo-measurement accounting for the length $l$ is included, the observation model presented in Equation (3) is augmented by appending the following equation, which represents a system constraint:

$$l=\sqrt{\begin{array}{c}{\left(x_S\left(k\right)-x_A\left(k\right)\right)}^2+{\left(y_S\left(k\right)-y_A\left(k\right)\right)}^2\end{array}}+v_{SA}\left(k\right),$$

(4)

where $v_{SA}$ represents the pseudo-measurement error.

The factor graph for the system model incorporating Equation (4) is illustrated in Figure 5.

2.3. Factor-Graph-Based Estimation Algorithm Details

A factor graph represents the joint probability distribution of a set of variables by expressing it as a product of multiple factors. This formulation allows the problem to be reinterpreted as an optimization task, where the objective is to minimize the sum of residuals associated with all factors.

For trajectory estimation, this optimization problem can be written as:

$$\begin{array}{r}\widehat{\mathit{\chi }}=\arg \underset{\mathit{\chi }}{\min }{\displaystyle {\sum }_{i=1}^M} {\left(\mathit{f}\left({\mathbf{x}}_{i-1}\right)-{\mathbf{x}}_i\right)}^{\mathrm{T}}{\mathbf{Q}}^{-1}\left(\mathit{f}\left({\mathbf{x}}_{i-1}\right)-{\mathbf{x}}_i\right)+\\ +{\displaystyle {\sum }_{k=1}^N} {\left(\mathit{h}\left({\mathbf{x}}_k\right)-{\mathbf{z}}_k\right)}^{\mathrm{T}}{\mathbf{R}}^{-1}\left(\mathit{h}\left({\mathbf{x}}_k\right)-{\mathbf{z}}_k\right),\end{array}$$

(5)

where $\mathit{\chi }$ represents the set of possible states, $M$ is the number of dynamic constraints along trajectory, $\mathit{f}$ denotes the transition vector function which describes system dynamics, ${\mathbf{x}}_i$ is the state vector at $i$-th step, $\mathbf{Q}$ is the process noise covariance matrix, $N$ is the number of measurements, $\mathit{h}$ represents the measurement function, ${\mathbf{z}}_k$ is the $k$-th measurement and $\mathbf{R}$ denotes the covariance matrix for measurement noise. Equation (5) can be simplified using the ${‖·‖}_{\mathsf{\Sigma }}^2$ notation for a squared Mahalanobis distance, given a covariance matrix $\mathsf{\Sigma }$:

$$\widehat{\mathit{\chi }}=\arg \underset{\mathit{\chi }}{\min }{\displaystyle {\sum }_{i=1}^M} {‖\mathit{f}\left({\mathbf{x}}_{i-1}\right)-{\mathbf{x}}_i‖}_{\mathbf{Q}}^2+{\displaystyle {\sum }_{k=1}^N} {‖\mathit{h}\left({\mathbf{x}}_k\right)-{\mathbf{z}}_k‖}_{\mathbf{R}}^2.$$

(6)

Solving this optimization problem involves determining the state configuration that minimizes the negative log-likelihood via a weighted least-squares approach. This is achieved through error minimization using iterative local linearization with either Gauss–Newton or Levenberg–Marquardt methods. Terms in the least-squares objective function (6) associated with factors defined by $\mathit{f}\left(\mathbf{x}\right)$ and $\mathit{h}\left(\mathbf{x}\right)$ are approximated using a Taylor expansion around the current estimate $\widehat{\mathbf{x}}$. To this end, the following relationships can be applied:

$$\begin{array}{c}{\left(\mathit{f}\left({\mathbf{x}}_{i-1}+{∆\mathbf{x}}_{i-1}\right)-\left({\mathbf{x}}_i+{∆\mathbf{x}}_i\right)\right)}^{\mathrm{T}}{\mathbf{Q}}^{-1}\left(\mathit{f}\left({\mathbf{x}}_{i-1}+{∆\mathbf{x}}_{i-1}\right)-\left({\mathbf{x}}_i+{∆\mathbf{x}}_i\right)\right)=\\ ={\left(\mathit{f}\left({\mathbf{x}}_{i-1}\right)-{\mathbf{x}}_i+\left[\begin{array}{cc}{\mathbf{F}}_{i-1}& -\mathbf{I}\end{array}\right]\left[\begin{array}{c}{∆\mathbf{x}}_{i-1}\\ {∆\mathbf{x}}_i\end{array}\right]\right)}^{\mathrm{T}}{\mathbf{Q}}^{-1}\left(\mathit{f}\left({\mathbf{x}}_{i-1}\right)-{\mathbf{x}}_i+\left[\begin{array}{cc}{\mathbf{F}}_{i-1}& -\mathbf{I}\end{array}\right]\left[\begin{array}{c}{∆\mathbf{x}}_{i-1}\\ {∆\mathbf{x}}_i\end{array}\right]\right)=\\ {=‖\mathit{f}\left({\mathbf{x}}_{i-1}\right)-{\mathbf{x}}_i+\left[\begin{array}{cc}{\mathbf{F}}_{i-1}& -\mathbf{I}\end{array}\right]\left[\begin{array}{c}{∆\mathbf{x}}_{i-1}\\ {∆\mathbf{x}}_i\end{array}\right]‖}_{\mathbf{Q}}^2,\end{array}$$

(7)

$$\begin{array}{c}{\left(\mathit{h}\left({\mathbf{x}}_k+∆{\mathbf{x}}_k\right)-{\mathbf{z}}_k\right)}^{\mathrm{T}}{\mathbf{R}}^{-1}\left(\mathit{h}\left({\mathbf{x}}_k+∆{\mathbf{x}}_k\right)-{\mathbf{z}}_k\right)=\\ ={\left(\mathit{h}\left({\mathbf{x}}_k\right)-{\mathbf{z}}_k+{\mathbf{H}}_k∆{\mathbf{x}}_k\right)}^{\mathrm{T}}{\mathbf{R}}^{-1}\left(\mathit{h}\left({\mathbf{x}}_k\right)-{\mathbf{z}}_k+{\mathbf{H}}_k∆{\mathbf{x}}_k\right)=\\ ={‖\mathit{h}\left({\mathbf{x}}_k\right)-{\mathbf{z}}_k+{\mathbf{H}}_k∆{\mathbf{x}}_k‖}_{\mathbf{R}}^2,\end{array}$$

(8)

where ${\mathbf{F}}_{i-1}$ is a Jacobian of $\mathit{f}\left({\mathbf{x}}_{i-1}\right)$ and ${\mathbf{H}}_k$ is a Jacobian of $\mathit{h}\left({\mathbf{x}}_k\right)$. To simplify the notation of further calculations, we replace the functions $\mathit{f}\left({\mathbf{x}}_{i-1}+{∆\mathbf{x}}_{i-1}\right)-\left({\mathbf{x}}_i+{∆\mathbf{x}}_i\right)$ and $\mathit{h}\left({\mathbf{x}}_k+∆{\mathbf{x}}_k\right)-{\mathbf{z}}_k$ with a unified error function $\mathit{e}\left(\mathbf{x}\right)$ with its covariance matrix Σ. Accounting for the $M$ dynamic constraints and $N$ measurement factors, Equation (6) becomes:

$$\widehat{\mathit{\chi }}=\arg \underset{\mathit{\chi }}{\min }{\displaystyle {\sum }_{j=1}^{M+N}} {‖{\mathit{e}}_j\left(\mathbf{x}\right)‖}_{\mathsf{\Sigma }}^2,$$

(9)

This can be rewritten as searching for the optimal step:

$$\mathsf{\Delta }\widehat{\mathit{\chi }}=\arg \underset{\mathsf{\Delta }\mathit{\chi }}{\min }{\displaystyle {\sum }_{j=1}^{M+N}} {‖{\mathit{e}}_j(\mathbf{x}+\mathsf{\Delta }\mathbf{x})‖}_{\mathsf{\Sigma }}^2,$$

(10)

Which can then provide state estimates that are incrementally refined through iterative updates:

$${\widehat{\mathit{\chi }}}_j={\widehat{\mathit{\chi }}}_{j-1}+\mathsf{\Delta }{\widehat{\mathit{\chi }}}_j.$$

(11)

One approach to solving this optimization problem is to apply QR decomposition [18]. To this end, the appropriate transformations of Equation (10) must be performed based on putting the Mahalanobis distance in terms of the Euclidean distance with ${\mathsf{\Sigma }}^{1/2}$ as the matrix square root of $\mathsf{\Sigma }$:

$${‖{\mathit{e}}_j\left(\mathbf{x}\right)‖}_{\mathsf{\Sigma }}^2={‖{\mathsf{\Sigma }}^{-\mathrm{T}/2}{\mathit{e}}_j\left(\mathbf{x}\right)‖}^2,$$

(12)

Leading to restating the optimal step search as:

$$\mathsf{\Delta }\widehat{\mathit{\chi }}=\arg \underset{\mathsf{\Delta }\mathit{\chi }}{\min }{\sum }_{j=1}^{M+N} {‖{{\mathsf{\Sigma }}_j}^{-\mathrm{T}/2}{\mathit{e}}_j\left(\mathbf{x}\right)+{{\mathsf{\Sigma }}_j}^{-\mathrm{T}/2}{\mathbf{J}}_j\mathsf{\Delta }\mathbf{x}‖}^2,$$

(13)

where ${\mathbf{J}}_j$ is the Jacobian of the $j$-th error function.

By defining two new structures, a sparse matrix $\mathbf{A}$:

$$\mathbf{A}=\left[\begin{array}{ccc}{{\mathsf{\Sigma }}_1}^{-\mathrm{T}/2}{\mathbf{J}}_1& \dots & \dots \\ ⋮& {{\mathsf{\Sigma }}_j}^{-\mathrm{T}/2}{\mathbf{J}}_j& ⋮\\ \dots & \dots & {{\mathsf{\Sigma }}_{M+N}}^{-\mathrm{T}/2}{\mathbf{J}}_{M+N}\end{array}\right],$$

(14)

and a stacked vector $\mathbf{b}$:

$$\mathbf{b}=\left[\begin{array}{c}{{\mathsf{\Sigma }}_1}^{-\mathrm{T}/2}{\mathit{e}}_1\left(\mathbf{x}\right)\\ \begin{array}{c}⋮\\ {{\mathsf{\Sigma }}_j}^{-\mathrm{T}/2}{\mathit{e}}_j\left(\mathbf{x}\right)\\ ⋮\end{array}\\ {{\mathsf{\Sigma }}_{M+N}}^{-\mathrm{T}/2}{\mathit{e}}_{M+N}\left(\mathbf{x}\right)\end{array}\right],$$

(15)

and using $\mathbf{A}$ and $\mathbf{b}$, the optimization criterion can be formulated as a typical least-squares problem:

$$\mathsf{\Delta }\widehat{\mathit{\chi }}=\arg \underset{\mathsf{\Delta }\mathit{\chi }}{\min } {‖\mathbf{A}\mathsf{\Delta }\mathit{\chi }+\mathbf{b}‖}^2.$$

(16)

The above derivation interprets the factor graph problem as a sparse matrix representation, reformulating the estimation process as the iterative solution of a system of linear equations. Here, $\mathbf{A}$ is the sparse information matrix of the system [18], $\mathsf{\Delta }\widehat{\mathit{\chi }}$ represents the update to the state variables, and $\mathbf{b}$ is a residual vector containing stacked residuals from all constraints and measurements. Since factor graphs typically exhibit sparse interactions—with each variable influenced by only a limited number of factors—the matrix $\mathbf{A}$ is structured so that each hidden variable corresponds to a block of columns, and each factor corresponds to a block of rows, with nonzero entries appearing only where a variable and a factor are directly connected. To demonstrate the sparsity of the $\mathbf{A}$ matrix in the factor-graph algorithm, an example from one simulation is shown in Figure 6.

The above figure illustrates the matrix structure, where each block corresponds to a different type of constraint. Such sparsity, resulting from the limited interactions among state variables, enables efficient numerical solutions. Blue elements represent dynamic constraints linking consecutive states, while red elements denote measurement factors. Understanding this structure is essential, as it allows the use of numerical solvers that exploit sparsity to improve computational efficiency.

The solution for the optimization problem stated in Equation (16) can be found by transforming the matrix $\mathbf{A}$ using QR decomposition:

$$\mathbf{A}={\mathbf{Q}}_{\mathrm{q}\mathrm{r}}\left[\begin{array}{c}{\mathbf{R}}_{\mathrm{q}\mathrm{r}}\\ 0\end{array}\right],$$

(17)

where ${\mathbf{Q}}_{\mathrm{q}\mathrm{r}}$ is a rotation matrix and ${\mathbf{R}}_{\mathrm{q}\mathrm{r}}$ is an upper-triangular matrix which is ${\mathbf{A}}^{\mathrm{T}}\mathbf{A}={{\mathbf{R}}_{\mathrm{q}\mathrm{r}}}^{\mathrm{T}}{\mathbf{R}}_{\mathrm{q}\mathrm{r}}$. In the context of sparse factor graph optimization, the QR decomposition is performed using implicit transformations (e.g., Givens rotations or Householder reflections) [19], and the explicit form of the rotation matrix ${\mathbf{Q}}_{\mathrm{q}\mathrm{r}}$ is typically not constructed at all as it is not necessary in further calculations. Instead, the focus in these algorithms is placed on the resulting upper-triangular structure ${\mathbf{R}}_{\mathrm{q}\mathrm{r}}$. In our experiments, for the QR decomposition we rely on MATLAB^® version 2024b implementation of SuiteSparseQR [22], which is designed to handle sparse matrices effectively.

Using Equation (17) the following derivation is performed:

$${‖\mathbf{A}\mathsf{\Delta }\mathit{\chi }+\mathbf{b}‖}^2={‖{\mathbf{Q}}_{\mathrm{q}\mathrm{r}}\left[\begin{array}{c}{\mathbf{R}}_{\mathrm{q}\mathrm{r}}\\ 0\end{array}\right]\mathsf{\Delta }\mathit{\chi }+\mathbf{b}‖}^2.$$

(18)

The ${\mathbf{Q}}_{\mathrm{q}\mathrm{r}}$ matrix and consequently ${{\mathbf{Q}}_{\mathrm{q}\mathrm{r}}}^{\mathrm{T}}$ are orthogonal matrices, so ${{\mathbf{Q}}_{\mathrm{q}\mathrm{r}}}^{\mathrm{T}}$ can be applied to the right-hand side of Equation (18). As $\mathrm{d}\mathrm{e}\mathrm{t} {{\mathbf{Q}}_{\mathrm{q}\mathrm{r}}}^{\mathrm{T}}=1$, it preserves the Euclidean norm:

$${‖{\mathbf{Q}}_{\mathrm{q}\mathrm{r}}\left[\begin{array}{c}{\mathbf{R}}_{\mathrm{q}\mathrm{r}}\\ 0\end{array}\right]\mathsf{\Delta }\mathit{\chi }+\mathbf{b}‖}^2={‖{{{\mathbf{Q}}_{\mathrm{q}\mathrm{r}}}^{\mathrm{T}}\mathbf{Q}}_{\mathrm{q}\mathrm{r}}\left[\begin{array}{c}{\mathbf{R}}_{\mathrm{q}\mathrm{r}}\\ 0\end{array}\right]\mathsf{\Delta }\mathit{\chi }+{{\mathbf{Q}}_{\mathrm{q}\mathrm{r}}}^{\mathrm{T}}\mathbf{b}‖}^2,$$

(19)

and then, as ${{{\mathbf{Q}}_{\mathrm{q}\mathrm{r}}}^{\mathrm{T}}\mathbf{Q}}_{\mathrm{q}\mathrm{r}}=\mathbf{I}$, the right-hand side of Equation (19) can be further simplified as follows:

$${‖{{{\mathbf{Q}}_{\mathrm{q}\mathrm{r}}}^{\mathrm{T}}\mathbf{Q}}_{\mathrm{q}\mathrm{r}}\left[\begin{array}{c}{\mathbf{R}}_{\mathrm{q}\mathrm{r}}\\ 0\end{array}\right]\mathsf{\Delta }\mathit{\chi }+{{\mathbf{Q}}_{\mathrm{q}\mathrm{r}}}^{\mathrm{T}}\mathbf{b}‖}^2={‖\left[\begin{array}{c}{\mathbf{R}}_{\mathrm{q}\mathrm{r}}\\ 0\end{array}\right]\mathsf{\Delta }\mathit{\chi }+{{\mathbf{Q}}_{\mathrm{q}\mathrm{r}}}^{\mathrm{T}}\mathbf{b}‖}^2.$$

(20)

By introducing the partition ${{\mathbf{Q}}_{\mathrm{q}\mathrm{r}}}^{\mathrm{T}}\mathbf{b}=\left[\begin{array}{c}\mathbf{d}\\ \mathbf{c}\end{array}\right]$, where the vector $\mathbf{d}$ $\in {\mathbb{R}}^{M{n}_x}$ corresponds to the part matching the size of $\mathsf{\Delta }\mathit{\chi }$ and $\mathbf{c}\in {\mathbb{R}}^{N{n}_z}$ is the remaining part, the norm can be split accordingly, leading to the final simplification:

$${‖\left[\begin{array}{c}{\mathbf{R}}_{\mathrm{q}\mathrm{r}}\\ 0\end{array}\right]\mathsf{\Delta }\mathit{\chi }+{{\mathbf{Q}}_{\mathrm{q}\mathrm{r}}}^{\mathrm{T}}\mathbf{b}‖}^2={‖\left[\begin{array}{c}{\mathbf{R}}_{\mathrm{q}\mathrm{r}}\\ 0\end{array}\right]\mathsf{\Delta }\mathit{\chi }+\left[\begin{array}{c}\mathbf{d}\\ \mathbf{c}\end{array}\right]‖}^2={‖{\mathbf{R}}_{\mathrm{q}\mathrm{r}}\mathsf{\Delta }\mathit{\chi }+\mathbf{d}‖}^2+{‖\mathbf{c}‖}^2.$$

(21)

Since ${‖\mathbf{c}‖}^2$ is a constant, it does not depend on $\mathsf{\Delta }\mathit{\chi }$ and the minimization problems reduces to solving:

$${\mathbf{R}}_{\mathrm{q}\mathrm{r}}\mathsf{\Delta }\mathit{\chi }=-\mathbf{d},$$

(22)

which is an upper-triangular system that can be solved efficiently using back-substitution [18]. Once a solution $\mathsf{\Delta }\widehat{\mathit{\chi }}$ is obtained, the state vector $\widehat{\mathit{\chi }}$ is updated according to the iterative procedure from Equation (11). The algorithm based on QR factorization is more numerically stable than solving the normal equations ${\mathbf{A}}^{\mathrm{T}}\mathbf{A}\mathsf{\Delta }\widehat{\mathit{\chi }}={\mathbf{A}}^{\mathrm{T}}\mathbf{b}$ [18].

2.4. Computational Complexity and Memory Usage

From a computational standpoint, the extended Kalman filter and factor graph-based optimization represent fundamentally different approaches to state estimation. EKF is a recursive filter that proceeds over $M$ prediction steps and $N$ correction steps (which often can be equal). The prediction step typically involves a matrix multiplication and covariance propagation with a computational cost of $O\left(n_x^2\right)$, where $n_x$ is the length of the state vector. The update step requires inverting the dense innovation covariance matrix which is the most computationally intensive operation with an asymptotic complexity of $O\left(n_z^{2.8}\right)$, where $n_z$ is the length of the measurement vector [16]. Thus, the total computational cost of EKF can be approximated as $O\left({Mn}_x^2+{Nn}_z^{2.8}\right)$. In terms of memory, EKF requires maintaining and updating a state vector with $n_x$ elements and a dense covariance matrix of size $n_x\times n_x$.

In contrast, factor graph-based methods formulate the estimation problem as a global non-linear least squares optimization, typically solved over $L$ iterations using iterative linearization techniques. In each iteration, a sparse linear system $\mathbf{A}\mathsf{\Delta }\mathit{\chi }+\mathbf{b}$ is constructed at a cost of $O(M{n}_x+N{n}_z)$. The system is than solved for $\mathsf{\Delta }\widehat{\mathit{\chi }}$ using the sparse QR factorization which is $O\left(\left(M{n}_x+N{n}_z\right)log\left(M{n}_x\right)\right)$ and this leads to the following total asymptotic complexity of the factor graph method $O\left(L\left(M{n}_x+N{n}_z\right)log\left(M{n}_x\right)\right)$ [23]. The batch factor graph approach requires significantly more memory than EKF—due to the need to store the matrix $\mathbf{A}$ and vector $\mathbf{b}$ which have sizes $\left(M{n}_x+N{n}_z\right)\times M{n}_x$ and $\left(M{n}_x+N{n}_z\right)\times 1$ respectively. It is worth mentioning that by applying fixed-lag smoothing or sliding window techniques, both computational complexity and memory usage can be bounded, thereby enabling the practical deployment of factor graph-based optimization on embedded platforms [18].

In summary, EKF offers low computational complexity and memory footprint per iteration, making it suitable for real-time, resource-constrained applications. However, factor graph optimization provides higher accuracy and scalability, particularly in large-scale or high-precision systems, albeit at the cost of increased computational and memory requirements.

3. Results

The described UWB positioning system dedicated for HH-GPR was modelled in MATLAB^® version 2024b. The simulation of its operation and the demonstration of its performance using various estimation algorithms required several steps. First, assumptions regarding the layout of the positioning system’s components were adopted. The assumed locations of the base stations and the sapper were as follows: ${\mathrm{M}}_1\left(\mathrm{0,0}\right)$, ${\mathrm{M}}_2\left(\mathrm{100,0}\right)$, ${\mathrm{M}}_3\left(-\mathrm{50,30}\right)$, ${\mathrm{M}}_4\left(\mathrm{150,30}\right)$, and ${\mathrm{M}}_{\mathrm{S}}\left(\mathrm{80,50}\right)$, and they are presented in Figure 7.

Then, the GPR antenna trajectory was generated, assuming similar motion parameters as described in our previous work [11]. Similarly to [11], the measurements between pairs of mobile UWB modules ${\mathrm{M}}_S$ and ${\mathrm{M}}_A$ and stationary modules ${\mathrm{M}}_1{÷\mathrm{M}}_4$ were generated assuming that their errors are normally distributed with zero mean and a standard deviation of $2 \mathrm{c}\mathrm{m}$. These measurements were further processed by various filters.

All the considered estimation algorithms were implemented. They included the extended Kalman filter with the PND dynamics model, which proved to be the most accurate version of the EKF in our previous research [11], and four versions of factor-graph algorithms. The factor-graph algorithms differed in the dynamics model used (CV or PND) and in whether a pseudo-measurement for the arm length was included. The parameters assumed in the dynamics and observation models underlying both the EKF and factor-graph algorithms were chosen, as in [11], to tune the algorithms and minimize their estimation errors.

Based on these assumptions, multiple simulation runs were performed and the estimated position of the HH-GPR antenna was acquired. Based on these results, the average RMS errors along the entire trajectory were calculated. Example results of trajectory estimation using a factor graph with the CV motion model and with pseudo-measurements are shown in Figure 8. This figure illustrates how the estimated trajectory evolves from its initial guess, through successive approximations, to the final smooth result.

Figure 9 compares the final estimated trajectory from the factor-graph algorithm with the trajectory estimated using the EKF with the PND model. This version of the EKF proved to be superior to other extended Kalman filters in our previous research [10,11]. Although the factor-graph algorithm shows even better accuracy in this single-run demonstration, this result is provided solely for illustration purposes.

A more comprehensive comparison of the positioning accuracy of the various considered algorithms is provided in

Table 1. Comparison of RMS positioning errors for tested estimation algorithms.

Estimation Method	RMSE [m]	Improvement [%]
EKF PND	0.0149	-
Factor graph (CV)	0.0097	34.9
Factor graph (CV + pseudo-measurement)	0.0091	39.0
Factor graph (PND)	0.0080	46.3
Factor graph (PND + pseudo-measurement)	0.0076	49.0

. These results are based on 300 simulation runs with different realizations of process noise and measurement errors.

As can be seen, all the factor-graph algorithms are more accurate than the best-performing EKF based on the pendulum model. The improvement in accuracy ranges from over 30 to almost 50 percent, which paves the way for enhanced performance of the positioning system. This improved accuracy in practical GPR applications implies that the synthesized underground images may be less blurred and more distinct, thereby facilitating the identification and classification of detected objects and infrastructure.

4. Conclusions

The simulation results demonstrate that factor graph-based estimation provides higher accuracy in determining the position of the handheld ground-penetrating radar (HH-GPR) antenna compared to the extended Kalman filter. As previously mentioned, EKF operates as a Markov process, which can introduce estimation inconsistencies over time, whereas factor graphs leverage the entire trajectory, resulting in a smoother and more precise solution.

Among the evaluated factor graph models, the one incorporating the dedicated PND dynamics outperformed the model based on the simpler CV assumption. This indicates that for HH-GPR motion, a more complex dynamic model can provide better estimation accuracy, likely due to its ability to capture the specific characteristics of the sapper’s movement. Nevertheless, the CV model remains attractive due to its simplicity and lower computational complexity—it avoids the need to compute Jacobians at every iteration and for every factor, making it well-suited for applications with limited processing power. Furthermore, the inclusion of a pseudo-measurement—specifically, the known length of the sapper’s arm—further enhanced the performance, highlighting the value of integrating prior structural constraints into the estimation framework.

An additional advantage of the factor graph approach is its flexibility in incorporating modifications. The framework allows for easy integration of alternative motion models or additional pseudo-measurements without fundamentally changing the estimation structure. This adaptability makes factor graphs a robust tool for refining and optimizing estimation accuracy in various operational conditions.

While these findings provide valuable insights into antenna position estimation for HH-GPR, future work will focus on validating the proposed methods in real-world conditions. Planned experiments will involve high-precision reference systems such as Vicon to obtain ground-truth trajectories and assess the practical performance of the factor graph-based approach. These real-world evaluations will help refine the estimation models and further confirm the advantages of factor graph optimization over traditional filtering methods.