Open AccessArticle

UAV Path Optimization for Angle-Only Self-Localization and Target Tracking Based on the Bayesian Fisher Information Matrix

Kutluyil Dogancay

^1,*,†

and

Hatem Hmam

^2,†

UniSA STEM, University of South Australia, Mawson Lakes Campus, Mawson Lakes, SA 5095, Australia

Defence Science & Technology Group, Sensors and Effectors Division, Edinburgh, SA 5111, Australia

Author to whom correspondence should be addressed.

^†

These authors contributed equally to this work.

Sensors 2024, 24(10), 3120; https://doi.org/10.3390/s24103120

Submission received: 8 April 2024 / Revised: 6 May 2024 / Accepted: 13 May 2024 / Published: 14 May 2024

(This article belongs to the Special Issue Sensors and Sensor Fusion Technology in Autonomous Vehicles)

Download

Browse Figures

Versions Notes

Abstract

In this paper, new path optimization algorithms are developed for uncrewed aerial vehicle (UAV) self-localization and target tracking, exploiting beacon (landmark) bearings and angle-of-arrival (AOA) measurements from a manoeuvring target. To account for time-varying rotations in the local UAV coordinates with respect to the global Cartesian coordinate system, the unknown orientation angle of the UAV is also estimated jointly with its location from the beacon bearings. This is critically important, as orientation errors can significantly degrade the self-localization performance. The joint self-localization and target tracking problem is formulated as a Kalman filtering problem with an augmented state vector that includes all the unknown parameters and a measurement vector of beacon bearings and target AOA measurements. This formulation encompasses applications where Global Navigation Satellite System (GNSS)-based self-localization is not available or reliable, and only beacons or landmarks can be utilized for UAV self-localization. An optimal UAV path is determined from the optimization of the Bayesian Fisher information matrix by means of A- and D-optimality criteria. The performance of this approach at different measurement noise levels is investigated. A modified closed-form projection algorithm based on a previous work is also proposed to achieve optimal UAV paths. The performance of the developed UAV path optimization algorithms is demonstrated with extensive simulation examples.

Keywords:

self-localization; target tracking; autonomous vehicles; Bayesian Fisher information matrix; A-optimality criterion; D-optimality criterion; Kalman filter

1. Introduction

Target tracking using AOA measurements has been studied extensively. A critical assumption made when collecting AOA measurements is that the sensor location is known either exactly or is subject to some uncertainty or noise. A GNSS receiver is commonly used to determine the sensor location in outdoor environments. In situations where the GNSS is either not available or cannot be relied upon, beacons or landmarks can provide an alternative means for sensor self-localization using, for example, beacon bearings obtained from an imaging system.

Self-localization using beacons or landmarks is an active research area, especially in autonomous robotics. Early work focused on developing low-complexity linear estimators for the position and orientation of a robot from landmark bearings [1]. A two-stage algorithm consisting of a coarse search followed by a closed-form solution was proposed in [2] using bearing measurements from three beacons or landmarks. Improved pseudolinear estimators for self-localization from landmark bearings were developed in [3,4]. In [5], the vision-based self-localization methods used in the RoboCup middle-size league competition were reviewed and their performances were assessed in a robot soccer environment. A self-localization approach for a mobile robot using multiple candidate landmarks and an omni-directional vision system was proposed in [6]. A range-only simultaneous localization and mapping (SLAM) algorithm was developed in [7] using range measurements acquired from a sonar sensor and inter-node distance measurements for networked landmarks. This approach does not use direction-of-arrival information from landmarks, therefore making it well suited for environments where a clear line of sight to landmarks is not available. In [8], hop-count-distance-based self-localization was considered in a wireless network and the effect of landmark placement on the self-localization accuracy was studied. An improved robot self-localization method was proposed in [9] by estimating robot odometric errors and landmark poses independently from the robot pose, thereby allowing the use of dynamic landmarks in addition to fixed landmarks. Distributed sensor self-localization algorithms were developed in [10] for networked beacons where each node is able to broadcast information about its own position to facilitate self-localization for all nodes. In [11], a low-cost self-localization method suitable for indoor robots was developed using wheel-based odometry, bearing measurements from acoustic beacons, and prediction of beacon bearings from self-localization and orientation estimates. In [12], the performance of a beacon-based self-localization system was studied and a neural network approach was proposed for making design decisions in relation to optimal beacon placement and inference of robot self-localization from noisy measurements in a given environment. Vehicle pose estimation (position and orientation) from multi-modal sensor information and a reference map that contains the landmarks within the vehicle’s field of view were considered in [13]. A deep neural network was developed to learn the mapping between the measured landmarks and the reference map.

Sensor path optimization to maximize the accuracy of self-localization is a research topic of significant interest. UAV trajectory optimization for self-localization using 3D landmarks was studied in [14]. A 3D random sample consensus algorithm was developed, incorporating a modified Kalman filter. In [15], an indoor self-localization algorithm for a swarm of robots was proposed using active optical beacons. Optimal UAV trajectories for stationary and mobile beacons were investigated in [16] using the covariance of predicted Kalman filter state estimates [17] and building on the D-optimality criterion [18]. Self-localization methods for UAVs commonly use optical sensors for landmark or beacon angle measurements with respect to the UAV location and orientation. A comprehensive review of visual and optical sensing methods for self-localization is available in [19].

Once the sensor locations are estimated, the target AOA measurements can be processed to produce target tracks. Finding optimal UAV trajectories for AOA target localization and tracking has been researched extensively (see, e.g., [20] and the references therein). In this paper, our objective is to optimize the flight path of a single UAV, equipped with an AOA sensor collecting bearing measurements from beacons and a manoeuvring target, for joint self-localization (UAV location and orientation estimation) and target tracking. The joint estimation problem is solved optimally by modelling the interactions between the UAV location estimates and target track estimates in a recursive Bayesian estimation framework. It is worth mentioning that treating self-localization and target tracking separately as a two-stage estimation problem, where the self-localization errors are not fully accounted for by the target tracker, would yield non-optimal results. Despite its immense potential for practical application, the problem of UAV path optimization for joint self-localization and target tracking has not received much attention in the literature. In [21], a Kalman filtering approach for self-localization and target tracking was proposed under the strong assumption of perfect knowledge of UAV orientation, which is not justified in practice. The optimization criterion adopted in [21] minimizes the trace of the covariance of Kalman filter state estimates, akin to [17,18]. However, the entire state vector (including the velocity estimates) of the Kalman filter, rather than only target location estimates, is used in the optimization, which does not prioritize target tracking and increases the computational complexity as a result of increased matrix dimensions.

In this paper, new UAV path optimization algorithms are developed for the joint estimation problem of UAV self-localization and target tracking. The estimation problem is first cast into a Kalman filtering framework in the 2D plane using an augmented state vector which includes the target and UAV kinematic parameters (location and velocity vectors) and the UAV orientation angle. This formulation then allows the recursive Bayesian Fisher information matrix (BFIM) [22] to be adopted as a performance bound, as well as the basis for determining optimal UAV paths in a principled manner. Scalar measures of optimization are obtained from A- and D-optimality criteria which minimize the trace of the inverse of the recursive BFIM and maximize the determinant of the recursive BFIM, respectively [23,24,25]. The main contributions of the paper are as follows:

An augmented Kalman filter formulation for joint angle-only self-localization (including orientation estimation) and target tracking, which provides an optimal and convenient solution to deal with self-localization and orientation uncertainties, and their impact on target tracking performance.
A- and D-optimality criteria for UAV path optimization using an approximate BFIM and focusing on target location estimates in the augmented Kalman filter state vector for accurate optimization results in the face of self-localization uncertainties.
Analysis of measurement noise effects on optimal UAV paths generated by A- and D-optimality criteria, exposing some shortcomings of BFIM-based optimization.
Three UAV path optimization algorithms tailored for joint self-localization and target tracking based on the A- and D-optimality criteria, and the closed-form projection algorithm in [20].

The paper is organized as follows. Section 2 describes the joint UAV self-localization and target tracking problem. The A- and D-optimality criteria are also introduced. In Section 3, the augmented state space model for Kalman filtering is formulated, leading to an extended Kalman filter estimator. Section 4 develops the UAV path optimization algorithms based on the A- and D-optimality criteria and an approximation of the recursive BFIM. The closed-form projection algorithm is also described. In Section 5, extensive simulation studies are presented to demonstrate the efficacy of the developed UAV path optimization algorithms in target localization and tracking. The concluding remarks are made in Section 6.

2. Problem Definition

The UAV self-localization and target tracking problem considered in this paper is depicted in Figure 1. The UAV uses the bearings from N beacons at a priori known locations

b_{i} = {[b_{i 1}, b_{i 2}]}^{T}

i = 1, \dots, N

, in the global Cartesian coordinate system to estimate its own location

s_{k} = {[s_{1 k}, s_{2 k}]}^{T}

at time

k = 0, 1, 2, \dots

and the misalignment between its local coordinate system

x^{'} y^{'}

and the global coordinate system

x y

, given by the orientation angle

ϕ_{k}

. For an unambiguous solution, at least three beacons are required (

N \geq 3

). Furthermore, the beacons and the UAV must not be collinear or cocircular to avoid unobservable geometries with no unique solution [3]. The orientation angle represents platform vibrations, navigation errors, etc., and is usually time-varying. At time k, the beacon bearing angles are given by

θ_{i k} = {tan}^{- 1} (\frac{b_{i 2} - s_{2 k}}{b_{i 1} - s_{1 k}}) - ϕ_{k}, i = 1, \dots, N

(1)

where

{tan}^{- 1} (\cdot)

is the four-quadrant arctangent function.

The UAV collects angle-of-arrival (AOA) measurements from a manoeuvring target located at

p_{k} = [x_{k}, y_{k}]

in the global Cartesian coordinate system. The AOA in the absence of measurement noise is

θ_{k} = {tan}^{- 1} (\frac{y_{k} - s_{k 2}}{x_{k} - s_{k 1}}) - ϕ_{k} .

(2)

Note that in both (1) and (2), the angles observed at the UAV are adjusted account for the orientation angle

ϕ_{k}

A nearly constant velocity motion model [26] is assumed for the manoeuvring target. Accordingly, the dynamical equation for target state transitions is

x_{t, k + 1} = F_{t} x_{t, k} + n_{t, k}, k = 0, 1, \dots

(3)

where

x_{t, k} = {[x_{k}, {\dot{x}}_{k}, y_{k}, {\dot{y}}_{k}]}^{T}

is the target state vector which contains the target location

p_{k}

and its velocity

[{\dot{x}}_{k}, {\dot{y}}_{k}]

in global coordinates at time k. The state transition matrix is given by

F_{t} = [\begin{matrix} A & 0 \\ 0 & A \end{matrix}], A = [\begin{matrix} 1 & T \\ 0 & 1 \end{matrix}]

(4)

where T denotes the time interval in seconds between discrete-time instants k. In (3),

n_{t, k}

is the process noise which accounts for unknown target manoeuvres and is a zero-mean white Gaussian random vector with covariance

Q_{t} = [\begin{matrix} q_{t x} B & 0 \\ 0 & q_{t y} B \end{matrix}], B = [\begin{matrix} T^{4} / 4 & T^{3} / 2 \\ T^{3} / 2 & T^{2} \end{matrix}]

(5)

where

q_{t x}

and

q_{t y}

are determined from the maximum target acceleration [26].

The main problem addressed in this paper is to determine an optimal UAV trajectory or flight path in the sense of optimizing a well-defined performance measure for target tracking. As the UAV needs to estimate its own location from beacon bearings while engaged in target tracking, the performance measure in question must invariably take into account the UAV self-localization performance. Poor self-localization will degrade target tracking by adding to the uncertainty in target AOA measurements. Thus, the coupling between self-localization and target tracking should be uncovered, which is achieved by an augmented Kalman filter.

To derive UAV path optimization algorithms, we adopt the A- and D-optimality criteria for experimental design [23,24,25]. The self-localization and target tracking problem is first cast into a Bayesian estimation framework using the extended Kalman filter (EKF). However, it should be stressed that the results derived in the paper are not restricted to the EKF and can be extended to other variants of the Kalman filter in a straightforward way. The optimal performance bound for the Kalman filter is given by the recursive Bayesian Fisher information matrix (BFIM) [22], which can be approximated to simplify its computation using readily available Kalman filter estimates.

The A-optimality criterion aims to minimize the trace of the inverse BFIM. As the inverse BFIM is the the Bayesian Cramer–Rao lower bound (BCRLB) (also known as the posterior Cramer–Rao lower bound), which is the theoretical lower bound on the covariance of any unbiased Bayesian estimator, the A-optimality criterion is equivalent to minimizing the bound on the mean-squared error (MSE). For a two-dimensional estimation problem, the A-optimality criterion minimizes the sum of squared major and minor axes of the error ellipse corresponding to the BCRLB.

The objective of the D-optimality criterion is to maximize the determinant of the BFIM, which is the same as minimizing the determinant of the BCRLB, given the inverse matrix relationship between the BFIM and BCRLB. Thus, the D-optimality criterion minimizes the product of squared major and minor axes of the error ellipse for the BCRLB, which is proportional to the area of the error ellipse in a two-dimensional estimation problem.

Both optimality criteria aim to reduce the size of the error ellipse for the BCRLB by minimizing either the sum or product of its squared semi-axes. They produce identical optimal geometries for some sensor modalities and specific scenarios, and completely different optimal geometries for others. For tracking problems where the optimal sensor path can only achieve incremental optimality, the two criteria have been observed to generate different optimal paths for a moving sensor (see, e.g., [20]).

3. Augmented State-Space Model for Self-Localization and Tracking

For joint UAV self-localization and target tracking using a Kalman filter, we consider an augmented state-space model, whereby the augmented state vector contains all the unknown parameters to be estimated

x_{k} = {[x_{k}, {\dot{x}}_{k}, y_{k}, {\dot{y}}_{k}, s_{1 k}, {\dot{s}}_{1 k}, s_{2 k}, {\dot{s}}_{2 k}, ϕ_{k}]}^{T} .

(6)

The UAV state vector

{[s_{1 k}, {\dot{s}}_{1 k}, s_{2 k}, {\dot{s}}_{2 k}]}^{T}

, which includes the UAV location

s_{k}

and velocity

{[{\dot{s}}_{1 k}, {\dot{s}}_{2 k}]}^{T}

, is part of the augmented state vector for self-localization purposes.

The dynamical equation for augmented state transitions is

x_{k + 1} = \underset{F}{\underset{︸}{[\begin{matrix} F_{t} & 0 & 0 \\ 0 & F_{s} & 0 \\ 0 & 0 & λ \end{matrix}]}} x_{k} + n_{k}

(7)

where the state transition matrices for the target and UAV are identical (

F_{t} = F_{s}

), assuming a time-synchronized AOA and beacon bearing measurements, and the augmented process noise is

n_{k} = [\begin{matrix} n_{t, k} \\ n_{s, k} \\ n_{ϕ, k} \end{matrix}] .

(8)

The individual process noise components for the target, UAV and orientation angle are

n_{t, k} \sim N (0, Q_{t})

[see (3) and (5)],

n_{s, k} \sim N (0, Q_{s})

and

n_{ϕ, k} \sim N (0, σ_{ϕ}^{2})

, respectively, with

Q_{s} = [\begin{matrix} q_{s x} B & 0 \\ 0 & q_{s y} B \end{matrix}]

(9)

where

q_{s x}

and

q_{s y}

are the acceleration parameters for the UAV. Thus, the process noise for the augmented state space

n_{k}

in (8) is a zero-mean white Gaussian random vector with covariance

Q = [\begin{matrix} Q_{t} & 0 & 0 \\ 0 & Q_{s} & 0 \\ 0 & 0 & σ_{ϕ}^{2} \end{matrix}] .

(10)

In (7), the orientation angle is modelled as a first-order autoregressive process

ϕ_{k + 1} = λ ϕ_{k} + n_{ϕ, k}

(11)

where

0 < λ < 1

, so the variance of

ϕ_{k}

remains bounded at

σ_{ϕ}^{2} / (1 - λ^{2})

The nonlinear measurement equation for the augmented state-space model is made up of target AOA and beacon bearing measurements collected by the UAV:

z_{k} = [\begin{matrix} {\hat{θ}}_{k} \\ {\hat{θ}}_{1 k} \\ {\hat{θ}}_{2 k} \\ ⋮ \\ {\hat{θ}}_{N k} \end{matrix}] = \underset{h (x_{k})}{\underset{︸}{[\begin{matrix} h (x_{k}) \\ h_{1} (x_{k}) \\ h_{2} (x_{k}) \\ ⋮ \\ h_{N} (x_{k}) \end{matrix}]}} + w_{k}

(12)

where

\begin{matrix} h (x_{k}) = {tan}^{- 1} (\frac{y_{k} - s_{2 k}}{x_{k} - s_{1 k}}) - ϕ_{k} \\ h_{i} (x_{k}) = {tan}^{- 1} (\frac{b_{i 2} - s_{2 k}}{b_{i 1} - s_{1 k}}) - ϕ_{k}, i = 1, \dots, N \end{matrix}

(13)

and

w_{k} \sim N (0, R)

is the zero-mean white Gaussian measurement noise with a diagonal covariance matrix

R

of size

(N + 1) \times (N + 1)

. The diagonal entries of

R

contain the noise variances for the target AOA and beacon bearing measurements:

R = [\begin{matrix} σ_{θ}^{2} & 0 \\ σ_{θ_{1}}^{2} \\ ⋱ \\ 0 & σ_{θ_{N}}^{2} \end{matrix}] .

(14)

Equations (7) and (12) define the augmented state-space model for joint self-localization and target tracking. The measurement Equation (12) can be linearized by approximating the nonlinear functions in (13) by means of a truncated Taylor series expansion:

\begin{matrix} h (x_{k}) \approx h_{k} x_{k} \\ h_{i} (x_{k}) \approx h_{i k} x_{k} \end{matrix}

(15)

where

\begin{matrix} h_{k} & = \frac{\partial h (x_{k})}{\partial x_{k}} \end{matrix}

(16)

\begin{matrix} = [- \frac{y_{k} - s_{2 k}}{∥ p_{k} - s_{k} ∥^{2}}, 0, \frac{x_{k} - s_{1 k}}{∥ p_{k} - s_{k} ∥^{2}}, 0, \frac{y_{k} - s_{2 k}}{∥ p_{k} - s_{k} ∥^{2}}, 0, - \frac{x_{k} - s_{1 k}}{∥ p_{k} - s_{k} ∥^{2}}, 0, - 1] \end{matrix}

(17)

\begin{matrix} = [- \frac{sin (θ_{k} + ϕ_{k})}{∥ p_{k} - s_{k} ∥}, 0, \frac{cos (θ_{k} + ϕ_{k})}{∥ p_{k} - s_{k} ∥}, 0, \frac{sin (θ_{k} + ϕ_{k})}{∥ p_{k} - s_{k} ∥}, 0, - \frac{cos (θ_{k} + ϕ_{k})}{∥ p_{k} - s_{k} ∥}, 0, - 1] \end{matrix}

(18)

and

\begin{matrix} h_{i k} & = \frac{\partial h_{i} (x_{k})}{\partial x_{k}} \end{matrix}

(19)

\begin{matrix} = [0, 0, 0, 0, \frac{b_{i 2} - s_{2 k}}{∥ b_{i} - s_{k} ∥^{2}}, 0, - \frac{b_{i 1} - s_{1 k}}{∥ b_{i} - s_{k} ∥^{2}}, 0, - 1] \end{matrix}

(20)

\begin{matrix} = [0, 0, 0, 0, \frac{sin (θ_{i k} + ϕ_{k})}{∥ b_{i} - s_{k} ∥}, 0, - \frac{cos (θ_{i k} + ϕ_{k})}{∥ b_{i} - s_{k} ∥}, 0, - 1] . \end{matrix}

(21)

Substituting (15) into (12) results in a linear approximation for the measurement equation:

z_{k} = H_{k} x_{k} + w_{k}, H_{k} = [\begin{matrix} h_{k} \\ h_{1 k} \\ h_{2 k} \\ ⋮ \\ h_{N k} \end{matrix}]

(22)

which is used by the EKF to linearize the state space model [27].

The computational steps of the EKF for state estimation are summarized below:

State Prediction:

$x_{k | k - 1} = F x_{k - 1 | k - 1} mean of prior$

(23)

$P_{k | k - 1} = F P_{k - 1 | k - 1} F^{T} + Q covariance of prior$

(24)
State Update:

${\tilde{z}}_{k} = z_{k} - h (x_{k | k - 1}) innovation$

(25)

$G_{k} = P_{k | k - 1} H_{k | k - 1}^{T} {(H_{k | k - 1} P_{k | k - 1} H_{k | k - 1}^{T} + R)}^{- 1} Kalman gain$

(26)

$x_{k | k} = x_{k | k - 1} + G_{k} {\tilde{z}}_{k} state estimate$

(27)

$P_{k | k} = (I - G_{k} H_{k | k - 1}) P_{k | k - 1} covariance of state estimate$

(28)

where $x_{k | k - 1}$ is the state prediction at time k given all the measurements up to time $k - 1$ , and $x_{k | k}$ is the filtered state estimate at time k given all the measurements up to time k. $P_{k | k - 1}$ and $P_{k | k}$ are the error covariance matrices for state prediction $x_{k | k - 1}$ and filtered state estimate $x_{k | k}$ , respectively, and $H_{k | k - 1}$ is the Jacobian matrix $H_{k}$ in (22) evaluated at $x_{k} = x_{k | k - 1}$ :

$H_{k | k - 1} = [\begin{matrix} h_{k | k - 1} \\ h_{1, k | k - 1} \\ h_{2, k | k - 1} \\ ⋮ \\ h_{N, k | k - 1} \end{matrix}] = {H_{k}|}_{x_{k} = x_{k | k - 1}} .$

(29)

The EKF recursively computes the Bayesian estimate for the state vector $x_{k | k}$ and its covariance $P_{k | k}$ using the Gaussian prior for the state available from the previous recursion, $x_{k | k - 1}$ and $P_{k | k - 1}$ , and the measurements collected at the current recursion, $z_{k}$ .

The EKF is initialized by the mean and covariance of the state at

k = 0

\begin{matrix} x_{0 | - 1} = E {x_{0}} \end{matrix}

(30)

\begin{matrix} P_{0 | - 1} = Cov {x_{0}} . \end{matrix}

(31)

Furthermore, the EKF requires prior knowledge of the process and measurement noise covariances,

Q

and

R

, respectively.

4. UAV Path Optimization

4.1. Estimation Bound for the EKF

To develop UAV path optimization algorithms based on the A- and D-optimality criteria, we require a performance bound on the covariance of the filtered state estimates computed by the EKF. The BCRLB provides this bound. For the EKF, the BFIM, which is the inverse of the BCRLB, is recursively computed using [22]

Φ_{k} = {(F Φ_{k - 1} F^{T} + Q)}^{- 1} + E_{x_{k}} {H_{k}^{T} R^{- 1} H_{k}}

(32)

where

H_{k}

is the Jacobian matrix in (22) calculated at the true target state

x_{k}

. Equation (32) is the sum of prior information

{(F Φ_{k - 1} F^{T} + Q)}^{- 1}

available from the previous recursion and contributions from measurements at the current recursion

E_{x_{k}} {H_{k}^{T} R^{- 1} H_{k}}

. Under mild conditions, (32) can be approximated as

\begin{matrix} Φ_{k} & \approx P_{k | k}^{- 1} \\ \approx P_{k | k - 1}^{- 1} + H_{k | k - 1}^{T} R^{- 1} H_{k | k - 1} \\ \approx P_{k | k - 1}^{- 1} + \underset{I_{k}}{\underset{︸}{\frac{1}{σ_{θ}^{2}} h_{k | k - 1}^{T} h_{k | k - 1}}} + \underset{J_{k}}{\underset{︸}{\sum_{i = 1}^{N} \frac{1}{σ_{θ_{i}}^{2}} h_{i, k | k - 1}^{T} h_{i, k | k - 1}}} \end{matrix}

(33)

where only the readily available EKF estimates are used, thereby avoiding computationally expensive Monte Carlo simulations to compute the expectation over

x_{k}

on the right-hand side of (32). Referring to (21), we note that in (33), the contribution of beacon bearing measurements is a sparse block matrix

\begin{array}{l} \begin{matrix} 4 & 5 \end{matrix} \\ J_{k} = [\begin{matrix} 0 & 0 \\ 0 & * \end{matrix}] \begin{matrix} 4 \\ 5 \end{matrix} \end{array}

(34)

where only the

5 \times 5

lower diagonal submatrix has nonzero entries.

Let

Σ_{k} = Φ_{k}^{- 1}

denote the recursive BCRLB which gives the lower bound on the covariance of

x_{k | k}

for any unbiased estimate. Writing

Φ_{k}

and

Σ_{k}

in a block matrix form:

\begin{array}{l} \begin{matrix} 4 & 5 \end{matrix} \\ Φ_{k} = [\begin{matrix} A_{k} & B_{k} \\ C_{k} & D_{k} \end{matrix}] \begin{matrix} 4 \\ 5 \end{matrix} \end{array} \begin{array}{l} \begin{matrix} 4 & 5 \end{matrix} \\ Σ_{k} = Φ_{k}^{- 1} = [\begin{matrix} Σ_{11, k} & Σ_{12, k} \\ Σ_{21, k} & Σ_{22, k} \end{matrix}] \begin{matrix} 4 \\ 5 \end{matrix} \end{array}

(35)

we have

Σ_{11, k} = A_{k}^{- 1} + A_{k}^{- 1} B_{k} {(D_{k} - C_{k} A_{k}^{- 1} B_{k})}^{- 1} C_{k} A_{k}^{- 1}

(36)

which is the recursive BCRLB for target tracking. The second term on the right-hand side of (36) represents the contribution of self-localization to the target tracking performance, arising from the coupling between self-localization and target tracking, as evidenced by nonzero submatrices

B_{k}

and

C_{k}

Φ_{k}

. As our ultimate objective is to optimize the target tracking performance by means of appropriate UAV manoeuvres, the

4 \times 4

submatrix of the BCRLB,

Σ_{11, k}

, will be the focus of attention. This is different from previous work, where the entire

Σ_{k}

was used [16,21].

In (33), the contributions of the target AOA and beacon bearing measurements (

I_{k}

and

J_{k}

, respectively) to the recursive BFIM are influenced by several factors, the most obvious of which are the angle noise variances

σ_{θ}^{2}

and

σ_{θ_{i}}^{2}

, and the distances of the UAV from the target and beacons (i.e.,

∥ p_{k} - s_{k} ∥

and

∥ b_{i} - s_{k} ∥

). For a moment, consider the contribution of

I_{k}

individually in terms of the target AOA noise variance and distance of the UAV from the target. Referring to (18) and (33), it is clear that the product

σ_{θ}^{2} ∥ p_{k} - s_{k} ∥

determines the extent to which the target range

∥ p_{k} - s_{k} ∥

contributes to the recursive BFIM. The smaller

σ_{θ}^{2} ∥ p_{k} - s_{k} ∥

, the larger the contribution of

I_{k}

to the recursive BFIM will be. Specifically, if

σ_{θ}^{2}

is very small, decreasing

∥ p_{k} - s_{k} ∥

will not impact the recursive BFIM significantly. This means that the distance reduction between the UAV and the target will not be prioritized, instead favouring an almost circular UAV trajectory around the target. Conversely, for a large

σ_{θ}^{2}

, reducing the distance between the UAV and the target will have a significant impact on the recursive BFIM, thereby guiding the UAV closer to the target. This observation exposes a potential pitfall for A- and D-optimality-based UAV path optimization algorithms built on the recursive BFIM when the angle measurements are very accurate, for example, thanks to a high-precision imaging camera onboard the UAV.

The allowable maximum target distance from the beacons is an important design parameter, which determines the size of the coverage area to achieve a minimum target tracking performance that is acceptable. As the UAV becomes close to the target, the target tracking performance improves if

I_{k}

is considered alone. However, if the close proximity between the UAV and target translates into a significant increase in the UAV distance from the beacons, the UAV self-localization performance will degrade as the contribution of

J_{k}

Φ_{k}

, which is captured in the submatrix of

Φ_{k}

D_{k}

, diminishes with an increased

∥ b_{i} - s_{k} ∥

[see (21) and (33)]. In (35), the submatrices

A_{k}

B_{k}

and

C_{k} = B_{k}^{T}

are not affected by changes in

∥ b_{i} - s_{k} ∥

, and only

D_{k}

depends directly on

∥ b_{i} - s_{k} ∥

. Rewriting (36) as

Σ_{11, k} = {(A_{k} - B_{k} D_{k}^{- 1} B_{k}^{T})}^{- 1}

(37)

it can be shown that

A_{k} ≻ B_{k} D_{k}^{- 1} B_{k}^{T}

(i.e.,

A_{k} - B_{k} D_{k}^{- 1} B_{k}^{T}

is positive definite) for a positive-definite

Φ_{k}

, and a decrease in the eigenvalues of

D_{k}

resulting from a significant increase in

∥ b_{i} - s_{k} ∥

will lead to an increase in both the trace and determinant of the covariance matrix

Σ_{11, k}

. This in turn implies a poor tracking performance. In such a situation, the UAV path optimization will attempt to maximize the sum of contributions from

I_{k}

and

J_{k}

by balancing the UAV distances from the target and beacons rather than guiding the UAV close to the target, which will likely result in a UAV trajectory somewhere in between the target and the beacon cluster. In general, the farther away the target is from the beacons, the worse the tracking performance will be. The acceptable minimum tracking performance, such as the minimum mean-square error for target location estimates, dictates how far the target can be from the beacons.

4.2. UAV Path Optimization Algorithm Based on the A-Optimality Criterion

The objective of the A-optimality criterion is to minimize the trace of the recursive BCRLB for target location estimates over UAV waypoints:

s_{k | k - 1}^{*} = \underset{s_{k | k - 1} \in S_{k}}{arg min} J_{A, k} (s_{k | k - 1})

(38)

where

s_{k | k - 1}^{*}

is the next optimal waypoint for the UAV,

J_{A, k} (s_{k | k - 1})

is the trace of the BCRLB for target location estimates, and

S_{k}

is the set of permissible waypoints for the UAV at time k, compliant with the maximum turnrate, UAV speed and the time interval for angle measurements T. Defining the BCRLB for target location estimates as

Σ_{p, k} = [\begin{matrix} σ_{11, k} & σ_{13, k} \\ σ_{31, k} & σ_{33, k} \end{matrix}]

(39)

which is related to

Σ_{11, k}

via

Σ_{11, k} = {[\begin{matrix} σ_{11, k} & * & σ_{13, k} & * \\ * & * & * \\ σ_{31, k} & * & σ_{33, k} & * \\ * & * & * \end{matrix}]}_{4 \times 4}

(40)

we have

J_{A, k} (s_{k | k - 1}) = tr Σ_{p, k} = σ_{11, k} + σ_{33, k} .

(41)

As illustrated in Figure 2,

S_{k}

contains equally spaced discrete points on an arc that is centred at the current heading of the UAV subtending an angle of

2 ϑ_{max}

, where

ϑ_{max}

is the maximum turnrate for the UAV. The members of

S_{k} = {s_{1 k}, s_{2 k}, \dots, s_{M k}}

must satisfy

s_{i k} - s_{k - 1 | k - 1} = s ν (ϑ_{k - 1}) and | Δ ϑ_{k - 1} | < ϑ_{max}, i = 1, 2, \dots, M

(42)

where s is the distance between successive waypoints (the radius of the maximum turnrate arc that contains the members of

S_{k}

ν (ϑ_{k - 1})

is the unit vector for the heading at time

k - 1

ν (ϑ_{k - 1}) = [\begin{matrix} cos ϑ_{k - 1} \\ sin ϑ_{k - 1} \end{matrix}]

(43)

and

Δ ϑ_{k - 1} = ϑ_{k - 1} - ϑ_{k - 2}

is the change in heading angle at time

k - 1

The optimal heading towards the next waypoint is calculated relative to the estimated UAV location

s_{k - 1 | k - 1}

and orientation angle

ϕ_{k - 1 | k - 1}

computed by the EKF. For simulation purposes, it is necessary to determine the actual UAV motion in global coordinates derived from path optimization and EKF estimates. This is achieved by mapping the computed waypoints to the global Cartesian coordinate system using

\begin{matrix} s_{k - 1 | k - 1} ⟶ s_{k - 1} \\ s_{k | k - 1}^{*} = s_{k - 1 | k - 1} + s ν (ϑ_{k - 1}^{*}) ⟶ s_{k} = s_{k - 1} + s ν (ϑ_{k - 1}^{*} + \underset{\begin{matrix} orientation estimation \\ error \end{matrix}}{\underset{︸}{ϕ_{k - 1} - ϕ_{k - 1 | k - 1}}}) \end{matrix}

(44)

where

ϑ_{k - 1}^{*}

is the optimal UAV heading at time

k - 1

obtained from (38).

The computational steps of the A-optimality criterion-based path optimization algorithm are summarized below:

1.: Given s and $ϑ_{max}$ , determine the equally spaced M members of $S_{k}$ according to (42) and Figure 2.
2.: Re-calculate $H_{k | k - 1}$ and $Φ_{k}$ in (33) for each candidate waypoint $s_{k | k - 1} \in S_{k}$ .
3.: Re-calculate $Σ_{11, k}$ for each $Φ_{k}$ in Step 2 using (35) and (36).
4.: Calculate $J_{A, k} (s_{k | k - 1})$ for each $Σ_{11, k}$ in Step 3, and find $s_{k | k - 1}$ for which $J_{A, k} (s_{k | k - 1})$ is minimized.

4.3. UAV Path Optimization Algorithm Based on the D-Optimality Criterion

The D-optimality criterion maximizes the determinant of the recursive BFIM for target location estimates over UAV waypoints. In terms of the recursive BCRLB, this is equivalent to

s_{k | k - 1}^{*} = min_{s_{k | k - 1} \in S_{k}} J_{D, k} (s_{k | k - 1})

(45)

where

J_{D, k} (s_{k})

is the determinant of

Σ_{p, k}

in (39):

J_{D, k} (s_{k}) = det (Σ_{p, k}) = σ_{11, k} σ_{33, k} - σ_{31, k} σ_{13, k} .

(46)

The set of permissible waypoints

S_{k}

is defined in (42). The optimal waypoint

s_{k | k - 1}^{*}

found in (45) is mapped to the global coordinates according to (44).

The computational steps of the D-optimality criterion-based path optimization algorithm are

1.: Given s and $ϑ_{max}$ , determine the equally spaced M members of $S_{k}$ according to (42) and Figure 2.
2.: Re-calculate $H_{k | k - 1}$ and $Φ_{k}$ in (33) for each candidate waypoint $s_{k | k - 1} \in S_{k}$ .
3.: Re-calculate $Σ_{11, k}$ for each $Φ_{k}$ in Step 2 using (35) and (36).
4.: Calculate $J_{D, k} (s_{k | k - 1})$ for each $Σ_{11, k}$ in Step 3, and find $s_{k | k - 1}$ for which $J_{D, k} (s_{k | k - 1})$ is minimized.

4.4. Modified Projection Algorithm

A major criticism of the A- and D-optimality-based UAV path optimization algorithms presented in the previous two subsections is that they require a numerical search over M candidate waypoints in the set

S_{k}

at every time instant k. This can prove computationally expensive or even prohibitive in practice. It is possible to develop an alternative closed-form approach, dispensing with an exhaustive numerical search by exploiting the knowledge of what would constitute an optimal geometry at a given time k if the UAV could be moved anywhere with no constraints on the distance between successive waypoints s. In our previous work, we showed that for AOA localization, the optimal sensor location is along the line extension of the minor axis of the Gaussian prior covariance error ellipse in the 2D plane [20]. Using the EKF estimates, this means that an optimal next waypoint for the UAV is ideally on a line overlapping the minor axis of the predicted state estimate covariance corresponding to the target location. However, this is not guaranteed to be achievable as the UAV motion is restricted by speed and turnrate constraints. Therefore, the proposed approach is to move the UAV to a waypoint where it is closer to the line extension of the minor axis. This idea was exploited in [20] to devise an optimization method called the projection algorithm. In this subsection, we show how to modify it so that it can be applied to the UAV path optimization problem for self-localization and target tracking.

To begin with, define the covariance matrix for the predicted target location estimate (covariance of the prior):

P_{loc, k | k - 1} = [\begin{matrix} p_{k | k - 1} (1, 1) & p_{k | k - 1} (1, 3) \\ p_{k | k - 1} (3, 1) & p_{k | k - 1} (3, 3) \end{matrix}]

(47)

where

P_{k | k - 1} = {p_{k | k - 1} (i, j)}

is the covariance of state prediction

x_{k | k - 1}

which includes all the state variables. The state prediction for the target location gives the mean of the prior:

x_{loc, k | k - 1} = [\begin{matrix} x_{k | k - 1} \\ y_{k | k - 1} \end{matrix}]

(48)

where

x_{k | k - 1} = {[x_{k | k - 1}, {\dot{x}}_{k | k - 1}, y_{k | k - 1}, {\dot{y}}_{k | k - 1}, \dots]}^{T} .

(49)

Given the Gaussian prior

N (x_{loc, k | k - 1}, P_{loc, k | k - 1})

and the UAV location estimate

s_{k - 1 | k - 1}

, available from the previous EKF recursion at time

k - 1

, the projection algorithm guides the UAV towards the line extension of the minor axis of the Gaussian prior at time k, subject to the maximum turnrate constraint. Figure 3 illustrates the operation of the projection algorithm. As the prior for the target location estimate is extracted from EKF state prediction, the coupling between self-localization and target tracking still affects the optimal UAV path decisions as the prior is updated recursively.

The projection algorithm proceeds as follows:

1.: Given $s_{k - 1 | k - 1}$ , $x_{loc, k | k - 1}$ , $P_{loc, k | k - 1}$ , $ϑ_{max}$ , $ϑ_{k - 2}$ and s, calculate the minor axis of the error ellipse of $P_{loc, k | k - 1}$ from the eigenvector of $P_{loc, k | k - 1}$ associated with its smallest eigenvalue.
2.: Calculate the predicted target range $d_{k | k - 1} = ∥ x_{loc, k | k - 1} - s_{k - 1 | k - 1} ∥$ .
3.: Find $ψ_{k - 1}$ , the intersection of the line extension of the minor axis with the circle of radius $d_{k | k - 1}$ centred at $x_{loc, k | k - 1}$ , closest to $s_{k - 1 | k - 1}$ .
4.: Calculate the waypoint vector $Δ s_{k - 1} = s (ψ_{k - 1} - s_{k - 1 | k - 1}) / ∥ ψ_{k - 1} - s_{k - 1 | k - 1} ∥$ and the optimal heading angle $ϑ_{k - 1}^{*} = ∠ Δ s_{k - 1}$
5.: Find the optimal heading change $Δ ϑ_{k - 1}^{*} = ϑ_{k - 1}^{*} - ϑ_{k - 2}$ .
6.: If $| Δ ϑ_{k - 1}^{*} | < ϑ_{max}$ , then the next optimal waypoint is $s_{k | k - 1}^{*} = s_{k - 1 | k - 1} + Δ s_{k - 1}$ . Otherwise, restrict the heading angle for $Δ s_{k - 1}$ to the maximum turnrate: $ϑ_{k - 1}^{*} = ϑ_{k - 2} + sign (Δ ϑ_{k - 1}^{*}) ϑ_{max}$ .
7.: Find the actual UAV location $s_{k}$ using (44).

The UAV path optimization algorithms derived in this section all have the same computational architecture, which is depicted in Figure 4. The algorithms only differ in the way the optimal heading angle

ϑ_{k - 1}^{*}

k = 1, 2, \dots

, is computed. The A- and D-optimality algorithms use a numerical search over permissible sets of candidate waypoints given by (38) and (45), respectively, and the projection algorithm incorporates a closed-form geometric solution (see Steps 4–7 above) to find the optimal heading.

5. Simulation Studies

This section presents numerical simulation examples to demonstrate the performance of the UAV path optimization algorithms for self-localization and target tracking developed in Section 4. In the simulations, two scenarios for a stationary (non-moving) target and a manoeuvring target are considered. A comparison with alternative approaches is also presented towards the end of the section.

For the stationary target, the unknown target location is a Gaussian random vector

p_{0} \sim N ([\begin{matrix} - 10 \\ - 5 \end{matrix}], [\begin{matrix} 9.2500 & - 9.0933 \\ - 9.0933 & 19.7500 \end{matrix}]) km .

(50)

The process noise is

n_{t, k} = 0

with

q_{t x} = q_{t y} = 0

in (3) and (5), and the initial target velocity is

{[{\dot{x}}_{0}, {\dot{y}}_{0}]}^{T} = 0

in (3), as the target is not moving.

The manoeuvring target’s initial location is also given by (50). The target acceleration parameters are

q_{t x} = q_{t y} = 10^{- 10}

km²/s⁴. The initial target velocity is

{[{\dot{x}}_{0}, {\dot{y}}_{0}]}^{T} = {[2.5 \times 10^{- 3}, 2.5 \times 10^{- 3}]}^{T}

km/s, which corresponds to

12.7279

km/h.

The Gaussian prior for the initial UAV location is

s_{0} \sim N ([\begin{matrix} - 36.8116 \\ - 27.4976 \end{matrix}], [\begin{matrix} 10.3015 & - 1.7101 \\ - 1.7101 & 19.6985 \end{matrix}]) km .

(51)

The distance between the mean initial target and UAV locations is

∥ E {p_{0}} - E {s_{0}} ∥ = 35

km. The UAV moves with a constant speed of

0.025

km/s (90 km/h) and a maximum turnrate of

ϑ_{max} = 3^{\circ}

/s. Its initial heading in the global coordinate system is

ϑ_{0} = 0^{\circ}

. The orientation angle for the UAV,

ϕ_{k}

, in (11) has the parameters

λ = 0.8

ϕ_{0} = 10^{\circ}

and

σ_{ϕ} = 2^{\circ}

. To facilitate UAV self-localization, four beacons are used (

N = 4

) located at

b_{1} = {[- 45, - 45]}^{T}

km,

b_{2} = {[45, - 45]}^{T}

km,

b_{3} = {[- 45, 45]}^{T}

km and

b_{4} = {[45, 45]}^{T}

km.

The EKF is initialized by

x_{0 | - 1} = [\begin{matrix} x_{0 | - 1} \\ {\dot{x}}_{0 | - 1} \\ y_{0 | - 1} \\ {\dot{y}}_{0 | - 1} \\ s_{1, 0 | - 1} \\ {\dot{s}}_{1, 0 | - 1} \\ s_{2, 0 | - 1} \\ {\dot{s}}_{2, 0 | - 1} \\ ϕ_{0 | - 1} \end{matrix}] = [\begin{matrix} - 10 \\ 0 \\ - 5 \\ 0 \\ - 36.8116 \\ 0 \\ - 27.4976 \\ 0 \\ 10^{\circ} \end{matrix}], P_{0 | - 1} = [\begin{matrix} P_{t, 0 | - 1} & 0 \\ P_{s, 0 | - 1} \\ 0 & σ_{ϕ}^{2} \end{matrix}]

(52)

where

{[x_{0 | - 1}, y_{0 | - 1}]}^{T} = E {p_{0}}

{[s_{1, 0 | - 1}, s_{2, 0 | - 1}]}^{T} = E {s_{0}}

ϕ_{0 | - 1} = ϕ_{0}

, and the diagonal block submatrices of

P_{0 | - 1}

are

P_{t, 0 | - 1} = [\begin{matrix} 9.2500 & 0 & - 9.0933 & 0 \\ 0 & 0 & 0 & 0 \\ - 9.0933 & 0 & 19.7500 & 0 \\ 0 & 0 & 0 & 0 \end{matrix}], P_{s, 0 | - 1} = [\begin{matrix} 10.3015 & 0 & - 1.7101 & 0 \\ 0 & 0 & 0 & 0 \\ - 1.7101 & 0 & 19.6985 & 0 \\ 0 & 0 & 0 & 0 \end{matrix}]

(53)

which are obtained from the Gaussian priors for the initial target and UAV locations in (50) and (51).

For UAV self-localization, the UAV acceleration parameters in (9) are set equal to

q_{s x} = q_{s y} = 10^{- 6}

km²/s⁴. The time interval between angle measurements is

T = 10

s. The standard deviations for target AOA and beacon bearing measurement noise are assumed to be identical with

σ = σ_{θ} = σ_{θ_{1}} = \dots = σ_{θ_{4}}

, and are chosen from the set

{0 . 1^{\circ}, 1^{\circ}, 2^{\circ}}

, covering high- to low-precision angle measurements. The diagonal measurement covariance matrix

R

is constructed from the target AOA and beacon bearing noise variances according to (14).

Figure 5, Figure 6 and Figure 7 show the optimal UAV paths for a stationary target in the global coordinates computed by the UAV path optimization algorithms based on the A- and D-optimality criteria and the projection algorithm at three different noise levels

σ \in {0 . 1^{\circ}, 1^{\circ}, 2^{\circ}}

. The A- and D-optimality-based UAV path optimization algorithms use a waypoint search set

S_{k}

with

M = 10

members. The simulations run for 800 EKF recursions for a single realization of the random processes for target location, orientation angle and angle measurement noise. The initial UAV location is indicated with “

t_{0}

”. The 2-

σ

error ellipses for the target location on initialization and the final EKF estimate are shown by black ellipses. The 2-

σ

error ellipses for intermediate EKF estimates of the target location computed at every 50 recursions are shown by the gray ellipses. We observe that the A-optimality algorithm tends to circle around the target in favour of approaching it, which becomes even more pronounced at small noise (see Figure 5a). This observation is in agreement with an earlier remark made in Section 4.1 in relation to the relative insensitivity of the recursive BFIM to target distance reduction at small noise levels. The D-optimality algorithm circles around the target before approaching the target. The projection algorithm approaches the target much faster than the other two algorithms, clearly prioritizing target range reduction. This makes it the algorithm of choice, particularly in low-noise scenarios.

The optimal UAV paths for target tracking in the global coordinates computed by the A- and D-optimality UAV path optimization algorithms and the projection algorithm are depicted in Figure 8, Figure 9 and Figure 10 for noise levels

σ \in {0 . 1^{\circ}, 1^{\circ}, 2^{\circ}}

. The initial UAV and target locations are indicated with “

t_{0}

”. We make similar observations to the stationary target case in the previous simulations that the A-optimality algorithm tends to circle around the target more than the other two algorithms. The projection algorithm exhibits a direct approach towards the target followed by close manoeuvres around the target dictated by the maximum turnrate.

Figure 11 and Figure 12 show the root mean-square error (RMSE) for EKF target location estimates achieved by the UAV path optimization algorithms for a stationary and manoeuvring target, respectively. The RMSE values were calculated using 400 Monte Carlo simulation runs at the same measurement noise levels as before. In addition to the algorithms developed in this paper, two modifications of the A- and D-optimality-based algorithms are also simulated to illustrate the effects of using the full Kalman filter covariance matrix rather than restricting the optimization problem to the target location covariance. Full Kalman filter covariances were used in [16,21]. In Figure 11 and Figure 12, the modified A-optimality algorithm that minimizes the trace of the filtered state estimate covariance for all state variables (UAV and target kinematic parameters, as well as UAV orientation) is labelled “

min tr P_{k | k}

”, and the modified D-optimality algorithm that maximizes the determinant of the inverse covariance matrix for all state variables is labelled “

max | P_{k | k}^{- 1} |

”.

Overall, the projection algorithm achieves the best target tracking performance. At small noise levels, corresponding to high-precision angle measurements, all BFIM-based algorithms perform badly. The modified D-optimality algorithm is observed to have by far the worst performance. This can be explained by noting that the determinant of the full BFIM, which is approximated by

P_{k | k}^{- 1}

in (33), is maximized if

∥ b_{i} - s_{k} ∥ = 0

for any

i \in {1, \dots, N}

[see (19)–(21)]. This simply means that the UAV will be attracted towards a nearby beacon and become stuck there, leading to an extremely poor tracking geometry, as evident from the RMSE curves for “

max | P_{k | k}^{- 1} |

” in Figure 11 and Figure 12. Therefore, care should be exercised in particular when using the D-optimality criterion by focusing on the relevant entries of

P_{k | k}

rather than the whole

P_{k | k}

, as carried out in Section 4.3, to avoid undesirable consequences.

The average RMSE values for the path optimization algorithms for stationary and manoeuvring target cases are listed in Table 1 and Table 2, respectively. The average RMSE is computed by finding the mean RMSE over a time interval during which the RMSE of the projection algorithm has approximately levelled off. Evidently, the projection algorithm achieves the best RMSE performance with the fastest convergence to the minimum RMSE in almost all the simulations.

It is expected that an increased number of beacons will improve not only the UAV self-localization performance, but also the target tracking performance by increasing the eigenvalues of

D_{k}

[see (37)]. Table 3 and Table 4 show the average RMSE values after increasing the number of beacons to six (

N = 6

) by introducing two more beacons at

b_{5} = {[22.5, - 22.5]}^{T}

km and

b_{6} = {[- 22.5, 22.5]}^{T}

km. A comparison with Table 1 and Table 2 confirms that the performance of the projection algorithm and, to a lesser degree, the D-optimality algorithm improves with an increase in the number of beacons. The computational complexity of the EKF is on the order of the cube of the state vector size [28], which is

O (729)

for the augmented state-space model. Therefore, increasing the number of beacons will not add to the computational complexity significantly.

In general, how well the individual path optimization algorithms perform will depend on the relative beacon–UAV–target geometry, as well as the measurement noise. As is clear from Figure 11 and Figure 12, the achievable RMSE performance in the steady state is proportional to angle noise variances. It is conceivable that as the target moves away from the beacons, the target tracking performance will deteriorate as a result of optimal UAV location being far from the beacons and/or the target, hampering the tracking performance.

6. Conclusions

In this paper, we have developed new UAV path optimization algorithms for joint angle-only self-localization and target tracking in the absence of orientation angle information. The recursive BFIM was utilized to derive A- and D-optimality criterion-based algorithms. The potential shortcomings of BFIM-based optimization were considered and demonstrated in simulation studies. The modified projection algorithm, which aims to move the UAV closer to an optimal geometry, was shown to perform well. The developed algorithms are based on a sound foundation in recursive Bayesian estimation theory and make full use of prior information and measurements in relation to target tracking.

UAV self-localization can be further improved by considering a priori knowledge of UAV dynamics, such as the constant velocity and maximum turnrate, optimal beacon placement and dynamic beacon selection as the UAV moves. The self-localization performance will degrade as the UAV moves away from the beacons, producing poor target tracking results. In addition, undesirable self-localization geometries such as near cocircularity of the UAV and beacons will hamper the tracking performance significantly. These situations can be avoided by optimal dynamic beacon selection. An improved integration of self-localization from beacon bearings with an inertial navigation system (INS) onboard the UAV [29] will also enhance the self-localization performance.

Author Contributions

Conceptualization, K.D. and H.H.; Investigation, K.D. and H.H.; Methodology, K.D. and H.H.; Validation, K.D. and H.H.; Writing—original draft, K.D.; Writing—review and editing, H.H. All authors have read and agreed to the published version of the manuscript.

Funding

This research received no external funding.

Institutional Review Board Statement

Not applicable.

Informed Consent Statement

Not applicable.

Data Availability Statement

Data are contained within the article.

Conflicts of Interest

The author declares no conflicts of interest.

References

Shimshoni, I. On mobile robot localization from landmark bearings. IEEE Trans. Robot. Autom. 2002, 18, 971–976. [Google Scholar] [CrossRef]
Hmam, H. Mobile Platform Self-Localization. In Proceedings of the 2007 Information, Decision and Control, Adelaide, SA, Australia, 12–14 February 2007; pp. 242–247. [Google Scholar] [CrossRef]
Dogancay, K. Self-localization from landmark bearings using pseudolinear estimation techniques. IEEE Trans. Aerosp. Electron. Syst. 2014, 50, 2361–2368. [Google Scholar] [CrossRef]
Shao, H.J.; Zhang, X.P.; Wang, Z. Efficient Closed-Form Algorithms for AOA Based Self-Localization of Sensor Nodes Using Auxiliary Variables. IEEE Trans. Signal Process. 2014, 62, 2580–2594. [Google Scholar] [CrossRef]
Adorni, G.; Cagnoni, S.; Enderle, S.; Kraetzschmar, G.K.; Mordonini, M.; Plagge, M.; Ritter, M.; Sablatnög, S.; Zell, A. Vision-based localization for mobile robots. Robot. Auton. Syst. 2001, 36, 103–119. [Google Scholar] [CrossRef]
Kang, H.D.; Jo, K.H. Self-localization of autonomous mobile robot from the multiple candidates of landmarks. In Proceedings of the Optomechatronic Systems III, Stuttgart, Germany, 12–14 November 2002; Yoshizawa, T., Ed.; International Society for Optics and Photonics, SPIE: Bellingham, WA, USA, 2002; Volume 4902, pp. 428–435. [Google Scholar] [CrossRef]
Djugash, J.; Singh, S.; Kantor, G.; Zhang, W. Range-only SLAM for robots operating cooperatively with sensor networks. In Proceedings of the 2006 IEEE International Conference on Robotics and Automation, ICRA 2006, Orlando, FL, USA, 15–19 May 2006; pp. 2078–2084. [Google Scholar] [CrossRef]
Benbadis, F.; Obraczka, K.; Cortes, J.; Brandwajn, A. Exploring Landmark Placement Strategies for Self-Localization in Wireless Sensor Networks. In Proceedings of the 2007 IEEE 18th International Symposium on Personal, Indoor and Mobile Radio Communications, Athens, Greece, 3–7 September 2007; pp. 1–5. [Google Scholar] [CrossRef]
Guerrero, P.; Ruiz-del Solar, J. Improving Robot Self-localization Using Landmarks’ Poses Tracking and Odometry Error Estimation. In Proceedings of the RoboCup 2007: Robot Soccer World Cup XI, Atlanta, GA, USA, 9–10 July 2007; Visser, U., Ribeiro, F., Ohashi, T., Dellaert, F., Eds.; Springer: Berlin/Heidelberg, Germany, 2008; pp. 148–158. [Google Scholar]
Vemula, M.; Bugallo, M.F.; Djurić, P.M. Sensor self-localization with beacon position uncertainty. Signal Process. 2009, 89, 1144–1154. [Google Scholar] [CrossRef]
Ogiso, S.; Kawagishi, T.; Mizutani, K.; Wakatsuki, N.; Zempo, K. Self-localization method for mobile robot using acoustic beacons. Robomech J. 2015, 2, 12. [Google Scholar] [CrossRef]
Schaff, C.; Yunis, D.; Chakrabarti, A.; Walter, M.R. Jointly optimizing placement and inference for beacon-based localization. In Proceedings of the 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, 24–28 September 2017; pp. 6609–6616. [Google Scholar] [CrossRef]
Engel, N.; Hoermann, S.; Horn, M.; Belagiannis, V.; Dietmayer, K. DeepLocalization: Landmark-based Self-Localization with Deep Neural Networks. arXiv 2019, arXiv:1904.09007. [Google Scholar]
Karpenko, S.; Konovalenko, I.; Miller, A.; Miller, B.; Nikolaev, D. UAV Control on the Basis of 3D Landmark Bearing-Only Observations. Sensors 2015, 15, 29802–29820. [Google Scholar] [CrossRef] [PubMed]
Duan, M.; Lei, X.; Duan, Z.; Zheng, Z. A Minimalist Self-Localization Approach for Swarm Robots Based on Active Beacon in Indoor Environments. Sensors 2023, 23, 4926. [Google Scholar] [CrossRef] [PubMed]
McGuire, J.L.; Law, Y.W.; Dogancay, K.; Ho, S.Y.; Chahl, J. Optimal Maneuvering for Autonomous Vehicle Self-Localization. Entropy 2022, 24, 1169. [Google Scholar] [CrossRef] [PubMed]
Dogancay, K. Single- and multi-platform constrained sensor path optimization for angle-of-arrival target tracking. In Proceedings of the 2010 18th European Signal Processing Conference, Aalborg, Denmark, 23–27 August 2010; pp. 835–839. [Google Scholar]
Dogancay, K. UAV Path Planning for Passive Emitter Localization. IEEE Trans. Aerosp. Electron. Syst. 2012, 48, 1150–1166. [Google Scholar] [CrossRef]
Couturier, A.; Akhloufi, M.A. A review on absolute visual localization for UAV. Robot. Auton. Syst. 2021, 135, 103666. [Google Scholar] [CrossRef]
Dogancay, K. Optimal Geometries for AOA Localization in the Bayesian Sense. Sensors 2022, 22, 9802. [Google Scholar] [CrossRef] [PubMed]
Zhu, B.; Xu, S.; Rice, F.; Dogancay, K. Angle-Of-Arrival Target Tracking Using A Mobile UAV In External Signal-Denied Environment. In Proceedings of the ICASSP 2023—2023 IEEE International Conference on Acoustics, Speech and Signal Processing (ICASSP), Rhodes Island, Greece, 4–10 June 2023; pp. 1–5. [Google Scholar] [CrossRef]
Van Trees, H.L.; Bell, K.L.; Tian, Z. Detection, Estimation, and Modulation Theory, Part I: Detection, Estimation, and Filtering Theory, 2nd ed.; Wiley: Hoboken, NJ, USA, 2013. [Google Scholar]
Fedorov, V.V. Theory of Optimal Experiments; Academic Press: New York, NY, USA, 1972. [Google Scholar]
Goodwin, G.C.; Payne, R.L. Dynamic System Identification: Experiment Design and Data Analysis; Academic Press: New York, NY, USA, 1977. [Google Scholar]
Uciński, D. Optimal Measurement Methods for Distributed Parameter System Identification; CRC Press: Boca Radon, FL, USA, 2005. [Google Scholar]
Bar-Shalom, Y.; Blair, W.D. (Eds.) Multitarget-Multisensor Tracking: Applications and Advances; Artech House: Boston, MA, USA, 2000; Volume III. [Google Scholar]
Anderson, B.D.O.; Moore, J.B. Optimal Filtering; Prentice Hall: Hoboken, NJ, USA, 1979. [Google Scholar]
Daum, F.E. Extended Kalman Filters. In Encyclopedia of Systems and Control; Baillieul, J., Samad, T., Eds.; Springer International Publishing: Cham, Switzerland, 2021; pp. 751–753. [Google Scholar] [CrossRef]
Guner, D.R.L. Inertial Navigation System Improvement Using Ground Station Data. Ph.D. Thesis, Middle East Technical University, Ankara, Turkey, 2012. [Google Scholar]

Figure 1. Geometry for UAV self-localization and target tracking using beacon bearings and the target AOA. The UAV location

s_{k}

, its orientation angle

ϕ_{k}

and target location

p_{k}

are unknown and are to be estimated jointly from noisy beacon bearing and target AOA measurements.

Figure 1. Geometry for UAV self-localization and target tracking using beacon bearings and the target AOA. The UAV location

s_{k}

, its orientation angle

ϕ_{k}

and target location

p_{k}

are unknown and are to be estimated jointly from noisy beacon bearing and target AOA measurements.

Figure 2. Illustration of the set of permissible waypoints

S_{k}

and maximum turnrate

ϑ_{max}

Figure 2. Illustration of the set of permissible waypoints

S_{k}

and maximum turnrate

ϑ_{max}

Figure 3. Modified projection algorithm for UAV path optimization.

Figure 4. Computational architecture for UAV path optimization and target tracking. The A-optimality, D-optimality and projection algorithms differ in the way

ϑ_{k - 1}^{*}

k = 1, 2, \dots

, is computed.

Figure 4. Computational architecture for UAV path optimization and target tracking. The A-optimality, D-optimality and projection algorithms differ in the way

ϑ_{k - 1}^{*}

k = 1, 2, \dots

, is computed.

Figure 5. Optimal UAV paths for a stationary target at

σ = 0 . 1^{\circ}

computed by the (a) A-optimality algorithm, (b) D-optimality algorithm and (c) projection algorithm. (d) A close-up of the projection algorithm. The initial UAV location is marked with “

t_{0}

”. Black dots and lines indicate the 2-

σ

error ellipses for initial and final EKF target location estimates. Gray dots and lines show the 2-

σ

error ellipses for intermediate EKF estimates.

Figure 5. Optimal UAV paths for a stationary target at

σ = 0 . 1^{\circ}

computed by the (a) A-optimality algorithm, (b) D-optimality algorithm and (c) projection algorithm. (d) A close-up of the projection algorithm. The initial UAV location is marked with “

t_{0}

”. Black dots and lines indicate the 2-

σ

error ellipses for initial and final EKF target location estimates. Gray dots and lines show the 2-

σ

error ellipses for intermediate EKF estimates.

Figure 6. Optimal UAV paths for a stationary target at

σ = 1^{\circ}

computed by the (a) A-optimality algorithm, (b) D-optimality algorithm and (c) projection algorithm. (d) A close-up of the projection algorithm. The initial UAV location is marked with “

t_{0}

”. Black dots and lines indicate the 2-

σ

error ellipses for initial and final EKF target location estimates. Gray dots and lines show the 2-

σ

error ellipses for intermediate EKF estimates.

Figure 6. Optimal UAV paths for a stationary target at

σ = 1^{\circ}

computed by the (a) A-optimality algorithm, (b) D-optimality algorithm and (c) projection algorithm. (d) A close-up of the projection algorithm. The initial UAV location is marked with “

t_{0}

”. Black dots and lines indicate the 2-

σ

error ellipses for initial and final EKF target location estimates. Gray dots and lines show the 2-

σ

error ellipses for intermediate EKF estimates.

Figure 7. Optimal UAV paths for a stationary target at

σ = 2^{\circ}

computed by the (a) A-optimality algorithm, (b) D-optimality algorithm and (c) projection algorithm. (d) A close-up of the projection algorithm. The initial UAV location is marked with “

t_{0}

”. Black dots and lines indicate the 2-

σ

error ellipses for initial and final EKF target location estimates. Gray dots and lines show the 2-

σ

error ellipses for intermediate EKF estimates.

Figure 7. Optimal UAV paths for a stationary target at

σ = 2^{\circ}

computed by the (a) A-optimality algorithm, (b) D-optimality algorithm and (c) projection algorithm. (d) A close-up of the projection algorithm. The initial UAV location is marked with “

t_{0}

”. Black dots and lines indicate the 2-

σ

error ellipses for initial and final EKF target location estimates. Gray dots and lines show the 2-

σ

error ellipses for intermediate EKF estimates.

Figure 8. Optimal UAV paths for target tracking (

σ = 0 . 1^{\circ}

) computed by the (a) A-optimality algorithm, (b) D-optimality algorithm and (c) projection algorithm. (d) A close-up of the projection algorithm. The initial UAV and target locations are marked with “

t_{0}

”. Black dots and lines indicate the 2-

σ

error ellipses for initial and final EKF target location estimates. Gray dots and lines show the 2-

σ

error ellipses for intermediate EKF estimates.

Figure 8. Optimal UAV paths for target tracking (

σ = 0 . 1^{\circ}

t_{0}

”. Black dots and lines indicate the 2-

σ

error ellipses for initial and final EKF target location estimates. Gray dots and lines show the 2-

σ

error ellipses for intermediate EKF estimates.

Figure 9. Optimal UAV paths for target tracking (

σ = 1^{\circ}

t_{0}

”. Black dots and lines indicate the 2-

σ

error ellipses for initial and final EKF target location estimates. Gray dots and lines show the 2-

σ

error ellipses for intermediate EKF estimates.

Figure 9. Optimal UAV paths for target tracking (

σ = 1^{\circ}

t_{0}

”. Black dots and lines indicate the 2-

σ

error ellipses for initial and final EKF target location estimates. Gray dots and lines show the 2-

σ

error ellipses for intermediate EKF estimates.

Figure 10. Optimal UAV paths for target tracking (

σ = 2^{\circ}

t_{0}

”. Black dots and lines indicate the 2-

σ

error ellipses for initial and final EKF target location estimates. Gray dots and lines show the 2-

σ

error ellipses for intermediate EKF estimates.

Figure 10. Optimal UAV paths for target tracking (

σ = 2^{\circ}

t_{0}

”. Black dots and lines indicate the 2-

σ

error ellipses for initial and final EKF target location estimates. Gray dots and lines show the 2-

σ

error ellipses for intermediate EKF estimates.

Figure 11. RMSE for EKF target location estimates for a stationary target at (a)

σ = 0 . 1^{\circ}

, (b)

σ = 1^{\circ}

, and (c)

σ = 2^{\circ}

Figure 11. RMSE for EKF target location estimates for a stationary target at (a)

σ = 0 . 1^{\circ}

, (b)

σ = 1^{\circ}

, and (c)

σ = 2^{\circ}

Figure 12. RMSEfor EKF target location estimates for a manoeuvring target at (a)

σ = 0 . 1^{\circ}

, (b)

σ = 1^{\circ}

, and (c)

σ = 2^{\circ}

Figure 12. RMSEfor EKF target location estimates for a manoeuvring target at (a)

σ = 0 . 1^{\circ}

, (b)

σ = 1^{\circ}

, and (c)

σ = 2^{\circ}

Table 1. Average RMSE for a stationary target (

N = 4

Table 1. Average RMSE for a stationary target (

N = 4

Algorithm	$σ = 0 . 1^{\circ}$	$σ = 1^{\circ}$	$σ = 2^{\circ}$
A-optimality	2.61	1.19	1.04
D-optimality	1.26	0.49	0.68
Projection	0.15	0.35	0.50
Modified A-optimality ( $min tr P_{k \| k}$ )	2.22	0.41	0.43
Modified D-optimality ( $max \| P_{k \| k}^{- 1} \|$ )	988.6	33.3	39.8

Table 2. Average RMSE for a manoeuvring target (

N = 4

Table 2. Average RMSE for a manoeuvring target (

N = 4

Algorithm	$σ = 0 . 1^{\circ}$	$σ = 1^{\circ}$	$σ = 2^{\circ}$
A-optimality	2.42	1.81	1.63
D-optimality	1.56	0.89	0.99
Projection	0.16	0.44	0.59
Modified A-optimality ( $min tr P_{k \| k}$ )	2.49	0.65	0.79
Modified D-optimality ( $max \| P_{k \| k}^{- 1} \|$ )	967	31.1	37.3

Table 3. Average RMSE for a stationary target (

N = 6

Table 3. Average RMSE for a stationary target (

N = 6

Algorithm	$σ = 0 . 1^{\circ}$	$σ = 1^{\circ}$	$σ = 2^{\circ}$
A-optimality	2.26	1.25	1.08
D-optimality	1.06	0.38	0.48
Projection	0.05	0.23	0.37

Table 4. Average RMSE for a manoeuvring target (

N = 6

Table 4. Average RMSE for a manoeuvring target (

N = 6

Algorithm	$σ = 0 . 1^{\circ}$	$σ = 1^{\circ}$	$σ = 2^{\circ}$
A-optimality	2.45	1.84	1.82
D-optimality	1.30	0.93	0.91
Projection	0.09	0.30	0.51

Disclaimer/Publisher’s Note: The statements, opinions and data contained in all publications are solely those of the individual author(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to people or property resulting from any ideas, methods, instructions or products referred to in the content.

© 2024 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https://creativecommons.org/licenses/by/4.0/).

Share and Cite

MDPI and ACS Style

Dogancay, K.; Hmam, H. UAV Path Optimization for Angle-Only Self-Localization and Target Tracking Based on the Bayesian Fisher Information Matrix. Sensors 2024, 24, 3120. https://doi.org/10.3390/s24103120

AMA Style

Dogancay K, Hmam H. UAV Path Optimization for Angle-Only Self-Localization and Target Tracking Based on the Bayesian Fisher Information Matrix. Sensors. 2024; 24(10):3120. https://doi.org/10.3390/s24103120

Chicago/Turabian Style

Dogancay, Kutluyil, and Hatem Hmam. 2024. "UAV Path Optimization for Angle-Only Self-Localization and Target Tracking Based on the Bayesian Fisher Information Matrix" Sensors 24, no. 10: 3120. https://doi.org/10.3390/s24103120

Note that from the first issue of 2016, this journal uses article numbers instead of page numbers. See further details here.

Article Menu

UAV Path Optimization for Angle-Only Self-Localization and Target Tracking Based on the Bayesian Fisher Information Matrix

Abstract

1. Introduction

2. Problem Definition

3. Augmented State-Space Model for Self-Localization and Tracking

4. UAV Path Optimization

4.1. Estimation Bound for the EKF

4.2. UAV Path Optimization Algorithm Based on the A-Optimality Criterion

4.3. UAV Path Optimization Algorithm Based on the D-Optimality Criterion

4.4. Modified Projection Algorithm

5. Simulation Studies

6. Conclusions

Author Contributions

Funding

Institutional Review Board Statement

Informed Consent Statement

Data Availability Statement

Conflicts of Interest

References

Share and Cite

Article Metrics

Article Access Statistics

Further Information

Guidelines

MDPI Initiatives

Follow MDPI