uav waypoint opportunistic navigation in gnss-denied
TRANSCRIPT
1
UAV Waypoint Opportunistic Navigation in
GNSS-Denied EnvironmentsYanhao Yang Student Member, IEEE, Joe Khalife, Member, IEEE, Joshua Morales, Member, IEEE,
and Zaher M. Kassas, Senior Member, IEEE
Abstract—Navigation of an unmanned aerial vehicle (UAV) toreach a desired waypoint with provable guarantees in globalnavigation satellite system (GNSS)-denied environments is con-sidered. The UAV is assumed to have an unknown initial state(position, velocity, and time) and the environment is assumedto possess multiple terrestrial signals of opportunity (SOPs)transmitters with unknown states (position and time) and oneanchor SOP whose states are known. The UAV makes pseu-dorange measurements to all SOPs to estimate its own statessimultaneously with the states of the unknown SOPs. The way-point navigation problem is formulated as a greedy (i.e., one-steplook-ahead) multi-objective motion planning (MOMP) strategy,which guarantees that the UAV gets to within a user-specifieddistance of the waypoint with a user-specified confidence. TheMOMP strategy balances two objectives: (i) navigating to thewaypoint and (ii) reducing UAV’s position estimate uncertainty.It is demonstrated that in such an environment, formulating thewaypoint navigation problem in a so-called “naive” fashion byheading directly to the waypoint would result in failing to reachthe waypoint. This is due to poor estimability of the environment.In contrast, the MOMP strategy guarantees (in a probabilisticsense) reaching the waypoint. Monte Carlo simulation results arepresented showing that the MOMP strategy achieves the desiredobjective with 95% success rate compared to a 36% success ratewith the naive approach. Experimental results are presented fora UAV navigating to a waypoint in a cellular SOP environment,where the MOMP strategy successfully reaches the waypoint,while the naive strategy fails to do so.
Index Terms—Waypoint navigation, motion planning, un-manned aerial vehicles, GNSS-denied, signals of opportunity.
I. INTRODUCTION
Unmanned aerial vehicles (UAVs) are increasingly being
used in a wide range of civilian and military applications, in
which it is too costly or dangerous to send human-operated
vehicles. These applications include: search and rescue, fire
fighting, traffic monitoring, agriculture, delivery, and surveil-
lance. The majority of missions in these applications require
the UAV to fly to specified waypoints efficiently and reliably
in dynamic and uncertain environments. Reaching these way-
points reliably with no human-in-the-loop requires the UAV to
continuously maintain its position in space and time within the
environment using an accurate and robust navigation system.
This work was supported in part by the National Science Foundation (NSF)under Grants 1929571 and 1929965, in part by the Office of Naval Research(ONR) under Grant and N00014-19-1-2613, and in part by the University ofCalifornia, Irvine Multidisciplinary Engineering Research Initiative program.
Authors’ addresses: Y. Yang is with the Mechanical Engineering Depart-ment at Carnegie Mellon University, Pittsburgh, PA 15213. J. Morales iswith the Electrical Engineering and Computer Science Department and J.Khalife and Z. Kassas are with the Mechanical and Aerospace Department,University of California, Irvine, CA 92697 USA, E-mail: ([email protected]).(Corresponding author: Zaher M. Kassas.)
Today’s UAV navigation systems essentially rely on global
navigation satellite system (GNSS) signals. However, it is
well-known that GNSS signals can become unreliable in en-
vironments where UAVs conduct missions: GNSS signals get
highly attenuated in deep urban canyons and under canopies
[1] and are susceptible to jamming and malicious spoofing [2].
To alleviate these shortcomings during GNSS outages, UAV
navigation systems typically supplement GNSS receivers with
additional sensors (e.g., inertial measurement units (IMUs) [3],
cameras [4], and lasers [5]). However, after prolonged GNSS
unavailability periods, the UAV’s position estimation error
and estimation uncertainty could accumulate to unacceptable
levels, compromising the safety and success of the UAV’s
mission. While the accumulation rate of UAV positioning error
could be reduced by incorporating additional sensors into the
UAV’s sensor-suite, this could violate cost, size, weight, and
power (CSWaP) constraints.
A more elegant and CSWaP-efficient approach is to detect
and map existing features in the unknown environment and to
prescribe the UAV’s trajectory to minimize the UAV’s states
uncertainty, which is estimated using measurements drawn
from the mapped features [6]. Fortunately, there is plenitude
of features in locales of interest, which a UAV may exploit
in the absence of GNSS signals in order to autonomously
navigate to the waypoint (e.g., trees, light poles, buildings,
etc.). This is typically achieved via the well-studied problem
of simultaneous localization and mapping (SLAM) [7]–[9] in
the robotics literature.
Another class of features, which could be exploited in
GNSS-denied environments is signals of opportunity (SOPs)
[10]–[12]. SOPs are ambient radio signals, which are not
intended as navigation sources, e.g., cellular signals [13]–[17],
digital television signals [18], [19], AM/FM radio signals [20],
[21], and low Earth orbit satellite (LEO) signals [22]–[25].
SOPs have been exploited to produce navigation solutions in
a standalone fashion or as an aiding source for an INS in the
absence of GNSS signals [26]. Meter-level accurate navigation
has been demonstrated on ground vehicles with terrestrial
cellular and television SOPs [27]–[29], while sub-meter-level
accurate navigation has been demonstrated on UAVs [30].
The problem of navigating with unknown SOPs is termed
radio SLAM [26] or variations thereof [31]. While the radio
SLAM problem shares similarities with the robotics SLAM
problem, radio SLAM possesses specific complexities due to
the dynamic and stochastic nature of the spatiotemporal state
space. In particular, unlike the static features comprising the
robotics SLAM environment, SOPs are equipped with non-
Preprint of article accepted for publication in IEEE Transactions on Aerospace and Electronic Systems
ideal clocks, whose error (bias and drift) is dynamic and
stochastic. What is more, the number of SOPs could be
limited compared to the plenitude of static features in the
robotics SLAM environment, and the initial uncertainty around
the SOP states could be large. This makes the radio SLAM
environment poorly estimable, and the coupling between the
control objective (motion planning towards the waypoint) and
sensing/estimation objective (motion planning to gather the
“best” information from SOPs) evermore important. Even for
the simple objective of achieving situational awareness in these
environments (i.e., estimating the UAV’s states simultaneously
with estimating the SOPs’ states) without requiring the UAV
to reach a desired waypoint, it was shown that moving in a
random or an open-loop fashion would cause the UAV to get
lost (exhibited by filter divergence) [32].
Initial work on greedy (i.e., one-step look-ahead) motion
planning for optimal information gathering in radio SLAM en-
vironments was conducted in [32], and receding horizon (i.e.,
multi-step look-ahead) trajectory optimization was studied in
[33]. While the proposed approaches were shown to maintain
localization accuracy and map quality by prescribing optimal
trajectories, they did not take into consideration a desired
waypoint as part of the UAV’s mission. Waypoint navigation
brings a second objective that could contradict with the
objective of maintaining a good estimate of the UAV’s states.
In contrast to previous work, this paper considers the problem
of UAV greedy motion planning in a radio SLAM environment
to reach a desired waypoint with provable guarantees.
The contributions of this paper are as follow. First, a multi-
objective motion planning (MOMP) strategy is formulated.
Three cost functions that reflect the two objectives; namely,
(i) mission completion and (ii) uncertainty reduction; are
derived using probabilistic and information theoretic metrics.
It is shown that under certain conditions, these cost functions
are equivalent. Second, the cost function is modified by
introducing weights for the terms pertaining to each objective,
and a method to adaptively set the weights online is proposed.
This adaptation is shown to be essential for successful mission
completion. Third, the paper identifies a stopping criterion
that guarantees that the UAV is within a desired distance to
the waypoint with a desired confidence, declaring the mission
as complete. Fourth, Monte Carlo simulations are presented
showing that the MOMP strategy achieves the desired ob-
jective with 95% success rate compared to a 36% success
rate with a naive approach (i.e., one which prescribes the
shortest trajectory to the waypoint without considering the
UAV’s position uncertainty). Fifth, experimental results are
presented for a UAV navigating to a waypoint in a cellular SOP
environment, where the MOMP strategy successfully reaches
the waypoint, while the naive strategy fails to do so.
The remainder of this paper is organized as follows. Section
II summarizes relevant work in the literature. Section III
formulates the MOMP problem and describes the dynamics
model of the UAV and SOPs, the measurement model, and the
estimator model. Section IV develops the proposed MOMP
strategy. Section V presents simulation results comparing
naive and MOMP strategies. Section VI presents experimental
results of a UAV exploiting signals from unknown cellular
SOP transmitters to navigate to a waypoint using the MOMP
strategy. Section VII gives concluding remarks.
II. RELATED WORK
Waypoint navigation has been widely studied over the last
decade with the development of autonomous aerial vehicles
[34] and mobile robots [35]. To achieve better performance
in waypoint navigation, GPS-based control systems were
adopted, whether in a standalone fashion [36] or in a differ-
ential fashion with an inertial navigation system (INS) [37].
Other approaches relied on fusing measurements from an
omnidirectional camera and a laser rangefinder in a Kalman
filter for outdoor waypoint navigation [38]. The waypoint
navigation problem has been formulated as an optimal control
problem [39], which could be solved by linear quadratic
regulator (LQR) [40], genetic algorithm [41], or a combination
of local and global planning [42]. When the optimization
problem becomes multi-objective, particle swarm optimization
[43] or Pareto-optimality [44] has been adopted. Although
the aforementioned methods achieved accurate waypoint nav-
igation, they did not take into account the effect of motion
on the estimation performance. The effect of having large
estimation error in waypoint navigation may have detrimental
consequences.
Motion planning approaches that optimize the path length
towards a waypoint, while taking into account sensor uncer-
tainties have been investigated in recent literature. However,
sensors can provide either local or global information of
the vehicle’s position. In the case of the former, the motion
planning algorithm accounts for the sensor uncertainty instead
of optimizing for sensor uncertainty [45], [46]. In the case of
the latter, path planning is affected by the predicted quality
of the navigation solution which changes throughout the envi-
ronment. Ray-tracing approaches for path planning were used
to improve channel characterization and enhance positioning
performance for autonomous ground vehicles [47]–[50] or
UAVs [51]–[53], navigating with GNSS and/or SOPs. In
[54], a method to predict the state uncertainty of a UAV
in the presence of uncertain GNSS positioning biases using
stochastic reachability analysis was proposed. A framework
that guarantees that the estimation error remains less than
the safety threshold for ground vehicles while navigating
towards a waypoint was presented in [55]. While effective,
these approaches rely on known maps to pre-calculate the cost
function and perform optimization.
In robotics, sample-based approaches using the belief space
of the robot’s states are common for motion planning in-
volving uncertainty from localization and motion. One of the
sample-based methods for waypoint reaching that considers
state uncertainties is the belief roadmap [56], which builds a
probabilistic roadmap in a robot’s state space. It propagates
beliefs over the roadmap using an extended Kalman filter
(EKF) and plans a path of minimum goal-state uncertainty.
A modification of the sampling method was proposed by [57]
in which minimizing the maximum value of its uncertainty
metric encountered during the traversal of a path to make it
more efficient, while intermittent sensing was considered in
2
Preprint of article accepted for publication in IEEE Transactions on Aerospace and Electronic Systems
[58]. This approach was extended by taking visual-inertial
sensing and laser scanners into consideration in [59] and
[60], respectively. A rollout-policy-based algorithm enabling
online replanning in an efficient manner in belief space to
achieve simultaneous localization and planning in dynamic
environments with the presence of large disturbances was
proposed in [61]. However, for these sample-based approaches,
the sampling method and numerical burden significantly affect
efficiency and performance, as they require the calculation
and evaluation of all the potential paths between the start
point and the destination in each round. Several approaches
in the literature formulated the planning as an optimization
problem, constrained by uncertainty. For example, the sampled
paths were constrained by the estimated error covariance
in [62]. Similarly, [63] applied this constraint in particle
swarm optimization to find the optimal paths satisfying the
covariance bound. Localizability was also adopted as a con-
straint to determine possible regions and the path length was
optimized within these regions for a an autonomous vehicle
[64]. Although this paradigm is formulated as a constrained
optimization problem, the existence of an optimal solution is
not always guaranteed, as the constraint may contradict the
primary goal. In contrast to previous work, this paper considers
opportunistic waypoint navigation in a GNSS-denied via radio
SLAM. To achieve this objective, the paper formulates a
computationally efficient MOMP strategy, which guarantees
(in a probabilistic sense) reaching the waypoint.
III. PROBLEM FORMULATION AND MODEL DESCRIPTION
This section formulates the MOMP problem, describes the
control system model of the UAV, presents the dynamics and
measurement model of the UAV and SOPs, and discusses the
EKF model for estimating the UAV’s and SOPs’ states.
A. Problem Formulation
This paper extends previous work on motion planning by
developing a MOMP strategy for a UAV navigating with
unknown SOPs, which balances two objectives: (i) navigating
to a waypoint and (ii) minimizing UAV’s position uncer-
tainty. The following problem is considered. A UAV has been
dropped into a GNSS-denied environment with minimal a
priori knowledge of its own states. As shown in Fig. 1, the
environment consists of multiple unknown SOPs, from which
the UAV draws and fuses pseudoranges in order to estimate
its own states and the states of the unknown SOPs through
an EKF. The UAV is tasked with navigating to a waypoint
position and is required to arrive at the waypoint within a
specified distance with a specified confidence. The estimation
uncertainty is heavily dependent on the relative geometry
between the UAV and the SOPs. Therefore, the trajectory taken
by the UAV to arrive at the waypoint affects the uncertainty
in its position estimate along the way. When the uncertainty
is very large, the EKF risks diverging, which in turn causes
the UAV to move away from the waypoint, as illustrated in
Fig. 1. To avoid such behavior, a more complex trajectory
is designed to maintain a small uncertainty along the UAV
trajectory, which guarantees to reach the waypoint. This is also
illustrated in Fig. 1. Section IV develops the MOMP strategy.
GNSS Waypoint
SOP 2
SOP 1
SOP 3
UAV
Estimated trajectoryTr
uetrajectory
True tr
ajectory
Estimatedtrajectory
Naive motion planning
MOMP
Fig. 1. A UAV is deployed into an uncertain position in a GNSS-deniedenvironment and is tasked with navigating to a waypoint. A naive strategy(shown in purple) would have the UAV move directly to the waypoint position,risking large position estimation errors, which would cause missing thewaypoint. A MOMP strategy (shown in blue) would have the UAV prescribe amore complex trajectory, which balances two objectives to maintain a reliableposition estimate: (i) moving to the waypoint and (ii) minimizing the UAVposition uncertainty. The MOMP strategy guarantees (in a probabilistic sense)reaching the waypoint.
B. UAV Control System Model
This paper considers motion planning for quadrotor UAVs.
Typical quadrotor controllers require full state feedback, which
consists of the UAV’s three-dimensional (3-D) pose (3-D
position and orientation) and its first time derivative (3-D
linear and angular velocities). UAVs are typically equipped
with a suite of sensors to perform pose estimation, mainly a
GNSS receiver, IMU, barometric pressure sensor or altimeter,
magnetometer, etc. It is worth noting that UAVs rely mainly
on GNSS to produce a position estimate in a global frame,
e.g., Earth-centered Earth-fixed (ECEF) frame. In GNSS-
denied environments, if equipped with the right receivers,
the UAV may draw pseudorange measurements from ambient
SOPs to produce a position estimate in the global frame.
As mentioned in the Introduction, the uncertainty associated
with the SOP-based position estimate heavily depends on the
relative geometry between the UAV and the SOPs. Therefore,
in order to reduce this uncertainty, a motion planning algorithm
is developed to reach the waypoint with a desired confidence.
In addition to state feedback, a desired trajectory is required
to perform UAV pose control. Zero dynamics stabilization
methods were proposed to synthesize full state desired tra-
jectories from desired position and heading trajectories only
[65]. Treating the UAV controller as a black box, the proposed
framework aims at (i) replacing the GNSS receiver with an
EKF estimating the UAV position using SOP pseudoranges
and (ii) perform motion planning to determine the desired
position and heading trajectories that will be passed as an
input to the UAV’s control system. A high-level block diagram
of a typical UAV control system and the proposed system are
shown in Fig. 2. Since the proposed framework uses the UAV’s
control system as a black box, the EKF estimating the UAV’s
state from SOP pseudoranges is formulated independently of
the UAV’s on-board control system. The UAV dynamics model
assumed by the EKF is discussed next.
C. UAV Dynamics Model
For simplicity, this paper assumes a planar environment. Ex-
tensions to three-dimensions (3-D) are straightforward. Such
extension would yield poor estimability of the UAV’s vertical
3
Preprint of article accepted for publication in IEEE Transactions on Aerospace and Electronic Systems
Controller UAV
UAV’s
poseIMU
SOPpseudoranges
Altimeter
Magnetometer
GNSSreceiver
−
+
−
+Zero
dynamics
stabilizer
MotionDesired
waypoint
Desired
trajectory
Desired position
and heading Desired pose
Inner control loop
Innercontrol loop UAV
(a)
(b)
planningalgorithm
EKF
on-board
estimationsystem
Fig. 2. High-level block diagram of the UAV control system. (a) The innercontrol loop consists of a standard UAV control loop, where the desiredtracking position and heading angle trajectories are defined by the user. Azero dynamics stabilizer block synthesizes the tracking trajectories for theremaining states. (b) In the case of GNSS unavailability (note the absenceof the GNSS receiver), the UAV can rely on SOP measurements to maintaina position estimate in a global frame. Moreover, motion planning could beperformed to efficiently navigate to a desired waypoint. The additional blockspertaining to the proposed motion planning framework for navigating withSOPs are shown in blue. It is important to note that the proposed approach isin a fashion where (i) no modifications are needed for the inner control loopand (ii) the proposed motion planning algorithm automatically computes thedesired trajectories that are input to the inner control loop.
position due to poor geometric diversity of the SOPs’ heights.
In such case, other sensors (e.g., altimeter) could be readily
used.
The state vector of the UAV xr is defined as
xr ,[
xT
pv, xT
clk,r
]T
, xpv ,
[
rT
r , rT
r
]T
, xclk,r ,
[
δtr, δtr
]T
,
where rr ∈ R2 and rr ∈ R
2 are the UAV’s two-dimensional
(2-D) position and velocity, respectively; and δtr ∈ R and
δtr ∈ R are the UAV-mounted receiver’s clock bias and drift,
respectively. The UAV’s position and velocity states xpv will
be assumed to evolve according to a 2-D planar motion model
[66]. Taking the omni-directional flying ability of the multi-
rotor into consideration, the UAV’s nominal continuous-time
control input vector ur, is given by
urc , [ac, θc]T,
where ac ∈ R+ and θc ∈ [−π, π) are the continuous-
time absolute acceleration and corresponding heading angle,
respectively. Note that urc is not the actual control input
applied to the UAV, but is the control input computed by
the motion planning algorithm from which the desired 2-D
position and heading can be computed and input to the UAV
controller, as shown in Fig. 2(b). The desired altitude is set
to be constant. Due to actuation errors and process noise, the
actual continuous-time control input vector will be
urc = urc + urc ,[
ac, θc]T
,
where urc ,
[
ac, θc
]T
, and ac and θc are uncorrelated, zero-
mean, white random processes with power spectra qa and
qθ, respectively. The continuous-time state-space model of the
UAV’s position and velocity is formulated as
xpv(t) = Apvxpv(t) +Bpvgr[urc(t)], (1)
Apv ,
[
02×2 I2×2
02×2 02×2
]
, Bpv ,
[
02×2
I2×2
]
gr[urc(t)] ,[
ac(t) cos[θc(t)], ac(t) sin[θc(t)]]T
,
where 02×2 ∈ R2×2 is a matrix of zeros and I2×2 ∈ R
2×2 is
the identity matrix. Since the actual control input is unknown,
the function gr[ur(t)] in (1) is linearized around the nominal
input urc(t), yielding
gr[urc(t)] ≈ gr[urc(t)] + Dr(t)urc(t), (2)
Dr(t) ,
[
cos[θc(t)] −ac(t) sin[θc(t)]sin[θc(t)] ac(t) cos[θc(t)]
]
,
where Dr(t) is the Jacobian matrix of gr[urc(t)] with respect
to urc(t), evaluated at urc(t). Combining (1) and (2) and
discretizing at a sampling interval T assuming zero-order hold
of the control input (i.e., {urc(t) = urc(kT ) , ur(k), kT ≤t < (k+1)T }), the discrete-time UAV’s position and velocity
states can be modeled as
xpv(k + 1) = Fpvxpv(k) + Γpvgr [ur(k)] +wpv(k),
k = 0, 1, 2, . . . (3)
Fpv,
[
I2×2 T I2×2
02×2 I2×2
]
, Γpv,
[
T 2
2 I2×2
T I2×2
]
, ur(k),
[
a(k)θ(k)
]
,
where a(k) , ac(kT ) and θ(k) , θc(kT ), and wpv(k) is the
discretized process noise vector, which is a zero-mean white
sequence with covariance Qpv(k) given by
Qpv(k) =
[
T 3
3 Qc(k)T 2
2 Qc(k)T 2
2 Qc(k) TQc(k)
]
, (4)
Qc(k) , Da,θ(k)Qa,θDT
a,θ(k), Qa,θ = diag [qa, qθ] , (5)
Da,θ(k) , Da,θ(kT ) =
[
cos[θ(k)] −a(k) sin[θ(k)]sin[θ(k)] a(k) cos[θ(k)]
]
. (6)
Furthermore, as in all practical systems, the UAV’s accelera-
tion and velocity are constrained according to{
a(k) ≤ ar,max,
‖rr(k)‖2 ≤ vr,max,(7)
which, for e , [1 0]T
, can also be expressed as{
(
eTur(k))2 ≤ a2r,max,
∥
∥gr [ur(k)] +1Trr(k)
∥
∥
2
2≤(
1Tvr,max
)2.
(8)
The UAV-mounted receiver’s clock state xclk,r is modeled
according to the standard double integrator model driven by
process noise, whose discrete-time dynamics are given by
xclk,r(k + 1) = Fclkxclk,r(k) +wclk,r(k), k = 0, 1, 2, . . . ,(9)
4
Preprint of article accepted for publication in IEEE Transactions on Aerospace and Electronic Systems
where wclk,r is a zero-mean white noise sequence with co-
variance Qclk,r given by
Fclk=
[
1 T
0 1
]
, Qclk,r =
[
SwδtrT + Swδtr
T 3
3 Swδtr
T 2
2
Swδtr
T 2
2 SwδtrT
]
,
where Swδtrand Swδtr
are the power spectra of the continuous-
time white process noise driving the evolution of the clock bias
and drift, which are approximated with the frequency random
walk coefficient h−2,r and the white frequency coefficient h0,r,
leading to Swδtr≈ h0,r
2 and Swδtr≈ 2π2h−2,r.
Combining (3) and (9) yields the discrete-time dynamics of
the UAV’s state vector xr, which is given by
xr(k+1) = Frxr(k)+Γrgr[ur(k)]+wr(k), k = 0, 1, 2, . . . ,(10)
Fr , diag [Fpv, Fr] , Γr ,[
ΓT
pv, 02×2
]T
,
and wr ,
[
wT
pv,wT
clk,r
]T
is the overall process noise covari-
ance, which is a zero-mean white sequence with covariance
Qr = diag[
Qpv, Qclk,r
]
.
D. SOP Dynamics Model
The environment consists of one fully known anchor SOP,
denoted with a subscript a, and m unknown SOPs. To sat-
isfy the observability condition established in [33], [67], the
knowledge of the state vector of the anchor SOP, denoted xsa ,
is assumed. Note that the UAV’s control inputs ur are with
respect to a global coordinate frame in which the SOPs are
expressed. This requires the UAV to have a priori knowledge
about its orientation with respect to this global coordinate
frame through some sensor (e.g., a magnetometer). Note,
however, that the UAV has no a priori knowledge about its
initial position, velocity, or clock errors (bias and drift). The
jth SOP is modeled as a stationary transmitter with state vector
xsj ,
[
rTsj , x
Tclk,sj
]T
, xclk,sj ,
[
δtsj , δtsj
]T
,
where rsj ∈ R2 is the SOP’s planar position and δtsj ∈ R
and δtsj ∈ R are the SOP’s clock bias and drift, respectively.
The SOP’s states are assumed to evolve according to
xsj (k + 1) = Fsxsj (k) +Gswclk,sj (k), (11)
Fs , diag[
I2×2, Fclk
]
, Gs , [02×2 I2×2]T,
where wclk,sj ∈ R2 is the process noise vector driving the jth
SOP’s clock states and is modeled as a zero-mean white noise
sequence with covariance Qclk,sj , which has the same form
as Qclk,r, except that Swδtrand Swδtr
are replaced with SOP
clock-specific spectra, Swδts,jand Swδts,j
, respectively.
E. Observation Model
The UAV-mounted receiver makes pseudorange measure-
ments on the j-th SOPs, which after mild approximations
discussed in [68], can be modeled according to
zj(k) = h[
xr(k), xsj (k)]
+ vj(k) (12)
= ‖rr(k)− rsj (k)‖2 + c · [δtr(k)− δtsj (k)] + vj(k),
where c is the speed of light and vj is the measurement
noise, which is modeled as a discrete-time zero-mean white
Gaussian sequence with variance σ2j . The measurement noise
of all SOPs is assumed to be uncorrelated. Subsequently,
the covariance matrix R of the measurement noise vector
v , [va, v1, . . . , vm]T
is given by R , diag[
σ2a, σ
21 , . . . , σ
2m
]
.
F. EKF Model
The anchor SOP state xsa is assumed to be known at all
time. Hence, the EKF estimates the state vector defined by
x(k) ,[
xT
r (k),xT
s1(k), . . . ,xT
sm(k)]T
using the set of measurements given by {z(l)}kl=0, where
z(l) , [za(l), z1(l), . . . , zm(l)]T
. From (10)–(12), the overall
system equations are given by
x(k + 1) = Fx(k) + Γgr [ur(k)] +Gw(k), (13)
z(k) = h [x(k)] + v(k), (14)
F , diag [Fr,Fs,Fs, . . . ,Fs] , Γ ,[
ΓT
r 0T
4m×2
]T
,
G , diag [I6×6,Gs,Gs, . . . ,Gs] ,
h [x(k)] , [h [xr(k), xsa(k)] , h [xr(k), xs1(k)] , . . . ,
h [xr(k), xsm(k)]]T,
and w(k), defined as
w(k) ,[
wT
r (k),wT
clk,s1(k), . . . ,wT
clk,sm(k)]T
,
is a zero-mean white Gaussian vector with covariance
Q , diag[
Qr,Qclk,s1, . . . ,Qclk,sm
]
.
A standard EKF is implemented based on (13)–(14) to produce
an estimate x(k|i) , E
[
x(k)∣
∣
∣{z(l)}il=0
]
, for i ≤ k, and an
associated error covariance Σ(k|i).
IV. MULTI-OBJECTIVE MOTION PLANNING STRATEGY
This section defines the motion planning problem and
formulates it as a multi-objective optimization problem.
A. Multi-Objective Motion Planning Problem Definition
Consider a UAV that has been deployed into a GNSS-denied
environment with no a priori knowledge of its own states xr.
The environment consists of one anchor SOP whose states
xsa are fully known for all time and m SOPs whose states{
xsj
}m
j=1are unknown. In order to navigate, the UAV fuses
pseudoranges drawn to the SOPs (12) through an EKF to
estimate its own states while simultaneously estimating the
states of the unknown SOPs, as discussed in Section III. The
UAV is tasked with navigating to within a distance d of a
waypoint position rt with probability (1−α), given its EKF-
produced UAV position estimate, denoted rr(k|k), and the
associated block of the estimation error covariance, denoted
Σrr(k|k). Formally, this task is achieved when the following
is satisfied
Pr[
‖rr(k)− rt‖22 ≥ d2 | rr(k|k), Σrr(k|k)
]
≤ α. (15)
5
Preprint of article accepted for publication in IEEE Transactions on Aerospace and Electronic Systems
To navigate to the waypoint and satisfy (15), the UAV
selects a sequence of acceleration inputs ur(k), k = 0, 1, . . ..A naive strategy is to select these inputs so that the UAV
navigates directly toward the waypoint as fast as possible
without violating velocity and acceleration constraints 7. How-
ever, flying in a straight line in the GNSS-denied environment
considered herein, could cause the UAV to miss the waypoint
or cause filter divergence altogether. This is due to poor
estimability of the UAV’s and environment’s states.
In this paper, a more sophisticated MOMP strategy is devel-
oped, where the UAV prescribes its trajectory by balancing two
objectives: (i) navigating towards the waypoint and (ii) making
strategic maneuvers, which do not necessarily move the UAV
towards the waypoint, in order to reduce the UAV’s position
uncertainty. These objectives will be achieved by formulating a
multi-objective optimization problem, which is discussed next.
B. Cost Function Derivation
To formulate the MOMP strategy as an optimization prob-
lem, three quantities must be defined: (i) a cost function, (ii) an
optimization variable, and (iii) constraints on the optimization
variable. The optimization variable is defined as the input
vector ur of the UAV’s acceleration and heading (cf. 3), and
the constraints are readily obtained from the UAV’s maximum
speed and acceleration ratings (cf. 7). In what follows, three
cost functions reflecting the multiple objectives are defined via
two approaches: (i) probabilistic and (ii) information theoretic.
1) Probabilistic Approach: One possible cost function that
reflects the desired objective to reach the waypoint can be
readily derived from the probability inequality in (15). After
dropping the time argument for compactness of notation,
applying Markov’s inequality to (15) yields
Pr[
‖rr − rt‖22 ≥ d2 | rr,Σrr
]
≤E
[
‖rr − rt‖22]
d2
=tr(Σrr
) + ‖rr − rt‖22d2
,
(16)
where tr (·) denotes the trace of a matrix. Note that the
above inequality assumes that the EKF is unbiased, i.e.,
E [rr − rr] = 0. In this case, to satisfy the mission in (15),
one needs to minimize the cost function
JMarkov , tr(Σrr) + ‖rr − rt‖22 (17)
in the hopes that the right-hand side of (16) becomes less than
the desired probability α. The cost function JMarkov explicitly
shows the two objectives the UAV is trying to balance: (i)
mission completion, captured by the term ‖rr − rt‖22, which
is minimized when the UAV’s position estimate is at the target,
and (ii) uncertainty reduction, captured by the term tr(Σrr),
which when minimized, ensures that the UAV’s true position
is as close as possible to the UAV’s position estimate when
the estimate is at the target.
2) Information Theoretic Approaches: Another way to de-
rive a cost function for the MOMP strategy is using infor-
mation theoretic measures. The EKF produces the estimate
rr, which, ignoring the effect of nonlinearities and assuming
Gaussian noise, is the conditional mean of rr with the asso-
ciated estimation error covariance Σrr. Let Nr denote the pdf
of rr conditioned on all the measurements, which is given by
Nr : rr ∼ N (rr,Σrr) , (18)
where N (µ,Σ) denotes the multivariate Gaussian pdf with
mean µ and covariance matrix Σ. Another way to define the
objectives is through a target probability density Nt defined
as
Nt : rr ∼ N(
rt, ǫ2I)
, (19)
where ǫ is a small positive number. Equation (19) is saying
that it is desired that the UAV’s position to be centered at the
target with a small uncertainty. A natural approach to achieve
this is to minimize the distance between the current and target
pdfs. Such distance metrics are commonly used in information
theory, such as the Kullback-Leibler (KL) divergence DKL or
the Wasserstein metric Wp. For Gaussian distributions such as
Nr and Nt, the KL divergence can be expressed as
DKL (Nr‖Nt) =1
2ǫ2
[
tr (Σrr) + ‖rr − rt‖22
]
− 1
2[ln det (Σrr
) + 2− 4 ln ǫ] , (20)
and the Wasserstein metric becomes
W2 (Nr,Nt) =[
tr (Σrr) + ‖rr − rt‖22
]
+ 2ǫ[
ǫ− tr(
Σ1
2
rr
)]
. (21)
Next, define the cost functions
JKL , 2ǫ2DKL (Nr‖Nt)
= JMarkov − ǫ2 [ln det (Σrr) + 2− 4 ln ǫ] , (22)
and
JW , W2 (Nr,Nt)
= JMarkov + 2ǫ[
ǫ− tr(
Σ1
2
rr
)]
. (23)
Minimizing JKL is equivalent to minimizing DKL (Nr‖Nt) for
any value of ǫ > 0, since ǫ is independent of the optimization
variable ur. Both JKL and JW are explicitly expressed as
JMarkov with the addition of constants and terms penalizing
the difference between the current and target pdfs. However, it
is shown next that for small enough ǫ, all three cost functions
are equal up to a small difference δ.
3) Equivalence of Probabilistic and Information Theoretic
Approaches: It is assumed the mission time is upper-bounded
by κ time-steps. Next, let Ppv(k) denote the covariance matrix
of xpv(k), which can be expressed as
Ppv(k) =
[
Prr(k) Prrrr
(k)PT
rrrr(k) Prr
(k)
]
.
where Prr(k) and Prr
(k) are the covariance matrices of rr(k)and rr(k), respectively, and Prrrr
(k) is the cross-covariance.
Moreover, using (5), (6), (7), and the property A � tr (A) Ifor any matrix A � 0, Qc(k) may be upper-bounded as
Qc(k) �(
qa + a2r,maxqθ)
I2×2, (24)
6
Preprint of article accepted for publication in IEEE Transactions on Aerospace and Electronic Systems
where the right-hand side of the inequality is independent of
time. It follows from (4) and (24) that
Qpv(k) �(
qa + a2r,maxqθ)
[
T 3
3 I2×2T 2
2 I2×2T 2
2 I2×2 T I2×2
]
, Qpv. (25)
The following lemma establishes an upper-bound on Prr(k)
for k = 0, 1, . . . , κ.
Lemma IV.1. Consider the system in (3) with Ppv(0) ,
diag[
Prr(0),Prr(0)
]
≻ 0, then, for k = 0, 1, . . . , κ, the
following holds
Prr(k) � Prr
(k), (26)
Prr(k) , P
rr(0)+k2T 2Prr(0)+
k3T 3
3
(
qa + a2r,maxqθ)
I2×2.
(27)
Proof. From (3), the time-evolution of Ppv is obtained to be
Ppv(k)=FkpvPpv(0)
(
Fkpv
)T
+
k−1∑
j=0
Fk−j−1pv Qpv(j)
(
Fk−j−1pv
)T
.
(28)
By expanding (28), using (25), using the following properties
k−1∑
j=0
j =k(k − 1)
2,
k−1∑
j=0
j2 =k(k − 1)(2k − 1)
6,
and looking at the upper diagonal block of the left- and right-
hand sides of (28), (26) is deduced.
Now the following theorem establishes equivalence between
the three cost functions JMarkov, JKL, and JW.
Theorem IV.1. Consider the system (13)–(14), with initial
UAV position and velocity prior covariance Ppv(0), and
assume there exists a positive scalar p> 0 such that
Σrr≥ p I2×2. (29)
Then, over a finite number of time-steps κ, for any δ > 0,
there exists an ǫ⋆ > 0 such that for 0 < ǫ < ǫ⋆, the following
holds
|JKL − JMarkov| < δ, |JW − JMarkov| < δ. (30)
Proof. The EKF is estimating the posterior estimation error
covariance, denoted by Σrr(k|k). Given the properties of
Kalman filters, the following inequality holds
Σrr(k|k) � Prr
(k). (31)
Moreover, from (27), one can see that Prr(k) � Prr
(k + 1),which implies
Prr(k) � Prr
(κ). (32)
Let ζ(κ) , tr[
Prr(κ)]
. Using the property “A � tr (A) I,”for a matrix A � 0, and dropping the time argument k for
compactness of notation, the following can be deduced
Σrr� ζ(κ)I2×2. (33)
The following inequalities can be derived from (33)
|ln detΣrr| ≤ ϕ, (34)
tr(
Σ1
2
rr
)
≤ 2√
ζ(κ), (35)
where ϕ , max {2 |ln ζ(κ)| , 2 |ln p|}. Subsequently, combin-
ing (22) and (34), using the property of the absolute value, the
following is deduced
|JKL − JMarkov| ≤ δ1(ǫ), (36)
where δ1(ǫ) , ǫ2 (ϕ+ 2 + 4 |ln ǫ|). Similarly, combining (23)
and (35), another inequality is obtained
|JW − JMarkov| ≤ δ2(ǫ), (37)
where δ2(ǫ) , 2ǫ(
ǫ+ 2√
ζ(κ))
. One can straightforwardly
see that δ1(ǫ) and δ2(ǫ) are strictly increasing functions of
ǫ > 0. Consequently, for some δ > 0, one can calculate
ǫ1 = δ−11 (δ), ǫ2 = δ−1
2 (δ),
and define ǫ⋆ , min {ǫ1, ǫ2}. Therefore, for any 0 < ǫ < ǫ⋆,
one can guarantee (30).
In order to illustrate Theorem IV.1, Monte Carlo simula-
tions were performed to demonstrate the established bounds
(36) and (37). A total of 20 Monte Carlo realizations were
generated using the simulation settings tabulated in Table I
and ǫ ≡ 1 × 10−4 and p ≡ 1 × 10−4. The covariance
matrix used to calculate JKL, JW, and JMarkov were generated
based on conditions (26) and (29). At each time-step, the cost
function difference was calculated and is plotted in Fig. 3.
The covariance matrix had a constant lower bound according
to Theorem IV.1 and a time-varying upper bound according to
Lemma IV.1. It can be seen that the derived bounds correctly
limit the difference between JKL and JMarkov and between
JW and JMarkov.
0 2 4 6 8 10 12 14 16 18 20
Time (s)
(a)
4
4.5
5
5.5
6
Co
st
fun
ctio
n d
iffe
ren
ce 10-7
0 2 4 6 8 10 12 14 16 18 20
Time (s)
(b)
0
0.05
0.1
Co
st
fun
ctio
n d
iffe
ren
ce
Fig. 3. Cost functions equivalence based on the Markov inequality, KLdivergence, and Wasserstein metric over time. Illustrated are the differencebetween (a) the KL divergence and Markov cost functions and (b) Wassersteinand Markov cost functions calculated from the randomly sampled covariancematrix and the established bounds.
7
Preprint of article accepted for publication in IEEE Transactions on Aerospace and Electronic Systems
C. MOMP Optimization Problem
Theorem IV.1 implies that for a small enough target co-
variance uncertainty, the three cost functions JMarkov, JKL,
and JW are practically equivalent. Consequently, the MOMP
strategy will adopt the cost function J (ur) , JMarkov,
constrained to the UAV dynamics, namely
minimizeur
J (ur) (38)
Subject to: (8), (13), (14).
The optimization problem (38) does not specify when the
mission is completed. A stopping criterion is defined in
Subsection IV-D.
It is desired that the UAV be able to solve the optimiza-
tion online on its on-board processor. In order to meet this
requirement, a greedy approach is adopted, i.e., the optimal
control input is computed one step at a time. However, the
optimizer could get stuck at a local optimum in which one of
the two terms in the cost function dominates. It was observed
in simulations that the distance term usually dominates at the
beginning, causing the UAV to fly directly toward the way-
point. Once close to the waypoint, the trace of the estimation
error covariance starts to dominate, causing the UAV to get
stuck flying around the waypoint to minimize its position
estimate’s uncertainty but to no avail. To avoid such behaviors,
the cost function J (ur) is reformulated to become a weighted
sum given by
J(ur) , wJ1[ur(k)] + (1 − w)J2[ur(k)], (39)
where J1[ur(k)] , ‖rr(k + 1|k)− rt‖22, J2[ur(k)] ,
tr [Σrr(k + 1|k + 1)], and the weight w is changed adaptively.
More specifically, J1 is the Euclidean distance between the
UAV’s position estimate and the waypoint, which corresponds
to the objective of navigating towards the waypoint. Further-
more, J2 is the A-optimality criterion applied to the UAV’s
position estimation covariance [32], which corresponds to the
objective of minimizing the UAV’s position uncertainty. Note
that Σrr(k + 1|k + 1) can be computed at time-step k since
it is independent of the measurements at k + 1. The MOMP
problem is then cast as a greedy motion planning problem to
find the optimal input u∗
r (k), that minimizes J subject to the
dynamics and vr,max and ar,max, formally expressed as
minimizeur(k)
J(ur) = wJ1[ur(k)] + (1 − w)J2[ur(k)] (40)
Subject to: (8), (13), (14),
where the weight is defined as
w , 1(Σrr , d, α),
where 1(Σrr , d, α) is an indicator function used to switch
between each objective and is discussed in Subsection IV-E.
In the next two subsections, a test is developed as a stopping
criterion for task (15), and the indicator function 1(Σrr , d, α)is specified.
Note that in practice, a vehicle has constraints on its
acceleration and velocity which will also constrain the turning
radius of the vehicle. Therefore, as the vehicle approaches
the waypoint, it should reduce its velocity in order to make
“tighter” turns to successfully reach the waypoint. To this end,
an adaptive velocity constraint v′r,max is proposed, which is
defined as
v′r,max = min
{
√
‖rr − rt‖2 ar,max, vr,max
}
, (41)
which is based on the centripetal acceleration. Adjusting the
maximum velocity according to equation (41) will decrease
the velocity of the UAV, to ensure that the UAV does not
get stuck traversing an infinite loop around the waypoint.
Additionally, (41) is effectively a proportional controller, with
proportionality constant√ar,max, which has a realistic effect
of slowing the UAV down as it approaches the waypoint.
Since the cost function is generally non-convex, it was
solved by an exhaustive search-type algorithm, in order to
avoid converging to a local optimum. To this end, the feasible
set of maneuvers at time-step tj was gridded to nj possible
maneuvers. The complexity of evaluating the cost function at
a particular input is O (1) since it is independent of the value
of the input. Consequently, the computational complexity at
a time-step tj will be O (nj). Of course, advanced numerical
optimization solvers can be invoked, which would impact the
corresponding computational complexity.
D. Stopping Criterion
Since the UAV does not have access to its true position rr,
a test to determine if task (15) is satisfied must be formulated
in terms of values the UAV has access to. Specifically, a test is
formulated using the UAV’s EKF-produced position estimation
error covariance Σrr .
Theorem IV.2. Consider an environment consisting of a UAV
that has been deployed into a GNSS-denied environment with
no a priori knowledge of its own states, one anchor SOP
whose states are fully known, m unknown SOPs, and a desired
waypoint position rt. Given the distance tolerance from the
waypoint d, and the specified confidence probability 1 − α.
Then, if
1− F
(
d2
λmax; n,
n∑
i=1
b2i
)
≤ α, (42)
holds, then task (15) is satisfied; where F(
d2
λmax
; n,∑n
i=1 b2i
)
is the cumulative density function (CDF) of a non-central
χ2-distributed random variable with n degrees-of-freedom
and non-centrality parameter∑n
i=1 b2i , evaluated at d2
λmax
,
λmax , maxi
λi(Σrr) is the maximum eigenvalue of Σrr , n is
the dimension of the UAV position states, and b = [b1 . . . bn]⊺
is given by
Σrr= UΛU⊺
b , Λ−1
2U(rr − rt).
Proof. The proof will show that (42) is sufficient to satisfy
(15). The Euclidean distance term ‖rr − rt‖22 in (15) can be
8
Preprint of article accepted for publication in IEEE Transactions on Aerospace and Electronic Systems
expressed as
‖rr − rt‖22 = (rr + rr − rt)⊺(rr + rr − rt)
= (rr + rr − rt)⊺U⊺Λ−
1
2ΛΛ−1
2U(rr + rr − rt)
= (y + b)⊺Λ(y + b)
=
n∑
i=1
λi(yi + bi)2,
(43)
where λi is the ith eigenvalue of Σrr , y , Λ−1
2Urr =[y1 . . . yn]
⊺is the estimation error aligned to the coordinates
defined by the eigenvectors and eigenvalues of the estimation
error covariance matrix; therefore, y ∼ N (0n×1, I2×2).Equation (43) may be upper bounded by replacing all λi with
λmax, which gives
‖rr − rt‖22 =
n∑
i=1
λi(yi + bi)2 ≤ λmax
n∑
i=1
(yi + bi)2. (44)
Therefore, an upper bound for the probability in (15) may be
established by replacing the Euclidean distance term in (15)
by the right-hand side of (44), which gives
Pr(
‖rr − rt‖22 ≥ d2)
≤ Pr
(
λmax
n∑
i=1
(yi + bi)2 ≥ d2
)
.
(45)
Note that λmax
∑n
i=1(yi + bi)2 is non-central χ2-distributed
with n degrees-of-freedom and non-centrality parameter∑n
i=1 b2i , yielding the probability
Pr
(
λmax
n∑
i=1
(yi + bi)2 ≥ d2
)
= 1−F
(
d2
λmax; n,
n∑
i=1
b2i
)
.
(46)
Substituting the right-hand side of (46) into (45) yields
Pr(
‖rr − rt‖22 ≥ d2)
≤ 1− F
(
d2
λmax; n,
n∑
i=1
b2i
)
. (47)
If (42) holds, substituting it into (47) yields (15).
E. Indicator Function Selection
The MOMP strategy needs a mechanism to balance the
objective functions J1 and J2. One way to balance these
objectives is to use a function of the uncertainty of the UAV’s
position estimate as an indicator. If the uncertainty becomes
too large, the UAV should focus on decreasing the uncertainty
until it becomes small enough so that satisfying task (15)
is feasible. Then, the UAV may switch back to navigating
towards the waypoint. To make this happen, the indicator
function 1(Σrr , d, α) is set as a necessary condition for (42)
to be satisfied. This condition is established in the following
lemma.
Lemma IV.2. A necessary condition for criterion (42) to be
satisfied is
ηαλmax − d2 ≤ 0, (48)
where ηα , F−1(1−α;n, 0) is the value of the inverse CDF of
a χ2-distributed random variable with n degrees-of-freedom,
evaluated at 1−α, and n is the dimension of the UAV position
states.
Proof. The proof will proceed by contradiction. Assume that
(48) is not satisfied, i.e., ηαλmax − d2 > 0. Then, from the
definition of ηα, the following holds
d2
λmax< F−1(1− α;n, 0). (49)
Since χ2 CDFs are monotonically increasing, evaluating a n
degrees-of-freedom χ2 CDF at both sides of (49) and moving
some terms yields
1− F
(
d2
λmax; n, 0
)
> α. (50)
Note that the CDF of a n degrees-of-freedom noncentral χ2-
distribution decreases as the non-central parameter increases
[69], i.e.,
F
(
d2
λmax; n, c1
)
≥ F
(
d2
λmax; n, c2
)
, 0 ≤ c1 < c2, (51)
where c1 and c2 are the non-central parameters. By setting
c1 = 0 and c2 =∑n
i=1 b2i ≥ 0, where bi are defined according
to Theorem IV.2, then (51) implies that
1− F
(
d2
λmax; n, 0
)
≤ 1− F
(
d2
λmax; n,
n∑
i=1
b2i
)
. (52)
Combining (50) and (52) yields
1− F
(
d2
λmax; n,
n∑
i=1
b2i
)
> α,
which is in contradiction with (42).
According to Lemma IV.2, the indicator function
1(Σrr , d, α) is selected to be
1(Σrr , d, α) ,
{
1, if ηαλmax − d2 ≤ 0,
0, else. (53)
The MOMP strategy is summarized in Fig. 4.
V. SIMULATION RESULTS
This section presents simulation results of a UAV tasked
with reaching a waypoint in a GNSS-denied environment, but
pseudorange measurements made to unknown SOPs. Three
greedy motion planning strategies to prescribe the UAV’s
trajectory are compared: (i) naive approach, where the UAV
moves directly to the waypoint without considering the quality
of its position estimate or knowledge of the environment, (ii)
MOMP (38), which balances two objectives: 1) minimizing
the distance to the waypoint and 2) minimizing UAV position
uncertainty, and (iii) adaptive MOMP (40), which is similar to
(ii) with the addition of weights to the objectives so that neither
one dominates. A Monte Carlo study is conducted using 500
runs to compare each strategy by evaluating the average time to
reach the waypoint, final root mean squared error (FRMSE) of
the UAV’s estimated position, final root mean squared distance
(FRMSD) towards the waypoint at the end of the mission, and
success rate of reaching the waypoint. If the UAV did not reach
the waypoint within 200 seconds, the mission was stopped and
recorded as failure.
9
Preprint of article accepted for publication in IEEE Transactions on Aerospace and Electronic Systems
ΣOpNav :
xr(k + 1) = Frxr(k) + Γrgr [ur(k)] +wr(k)
xsj(k + 1) = Fsxsj(k) +wsj(k)
za(k) = h [xr(k),xsa(k)] + va(k),
zj(k) = h[
xr(k),xsj(k)]
+ vj(k), j = 1, . . . ,m
UAV and Radio SLAM Environment
Estimator: EKF
u⋆
r(k)=
argminur(k)
J (ur(k), rt, d,α)
Subject to: ΣOpNav(
eTur(k)
)2≤a2r,max
∥
∥ur(k) +1Trr(k)
∥
∥
2
2≤(
1Tv′r,max
)2
Multi-objective motion planning
ur(k) z(k)
rr(k|k),Σrr(k|k)
Stop
Is (42)satisifed?
No Yes
Fig. 4. UAV multi-objective motion planning loop.
A. Simulation Settings
Consider an environment comprising four SOPs and a speci-
fied waypoint. The UAV is deployed into an uncertain position,
has minimal knowledge of the SOPs in the environment,
and is tasked with reaching the known waypoint position
rt. The UAV’s uncertain deployment position is captured by
initializing the EKF with a state estimate, denoted xr(0|− 1),with a large estimation error covariance Σr(0| − 1). One
anchor SOP is available, and the remaining three SOPs are
unknown, i.e., their initial state estimates xsj (0| − 1) have
initial estimation error covariances Σsj (0| − 1) ≻ 04×4,
j = 1, 2, 3.
The Monte Carlo analysis was conducted over 500 runs.
For each run, the EKF was initialized with different initial
estimates and used different realizations of process and mea-
surement noise. The EKF and the simulation settings are
tabulated in Table I. For each Monte Carlo, run the naive,
MOMP, and adaptive MOMP strategies were employed to
prescribe the UAV’s trajectory according to the closed-loop
procedure illustrated in Fig. 4. For each run using the naive
strategy, the mission was declared complete by the UAV by
checking if ‖rr − rt‖ ≤ 5 m, using d from Table I. For each
run using the MOMP strategies, the mission was declared
complete according to the stopping criterion described in 4. It
is important to note that the non-adaptive MOMP strategy did
not declare mission success for any of the runs.
B. Naive and MOMP Strategies Comparison Results
The true and EKF-estimated UAV trajectories, the corre-
sponding final 95th-percentile estimation uncertainty ellipse,
the circle around the waypoint with distance d, and the true
and estimated SOP positions for an example run is illustrated
in Fig. 5, Fig. 6, and Fig. 7 for the naive, MOMP, and
adaptive MOMP strategies, respectively. Table II tabulates the
average time-duration to reach the waypoint, the UAV’s posi-
tion FRMSE, the UAV’s FRMSD, and the mission completion
success rate for the 500 Monte Carlo runs for each strategy. If
the UAV did not reach the waypoint within 200 seconds, the
mission was stopped and recorded as failure. The time duration
to reach the waypoint is computed by recording the elapsed
time from mission start to the time the UAV declares the
TABLE ISIMULATION SETTINGS
Parameter Value
rt [400, 200]T
xr(0) [0, 0, 0, 0, 100, 10]T
xsa [100, 250, 10, 0.1]T
xs1[200,−50, 20, 0.2]T
xs2[300, 300, 30, 0.3]T
xs3[−50, 150, 40, 0.4]T
xr(0 | −1) ∼ N [xr(0),Σr(0 | −1)]xsj (0 | −1) ∼ N [xsj ,Σsj (0 | −1)], j = 1, 2, 3Σr(0 | −1) (5 × 103) · diag[1, 1, 10−2, 10−2, 1, 10−1]Σsj (0 | −1) (103) · diag[1, 1, 1, 10−1], j = 1, 2, 3{h0,r, h−2,r} {2 × 10−19, 2× 10−20}
{h0,sa , h−2,sa} {8× 10−20, 4× 10−23},
{h0,sj , h−2,sj } {8× 10−20, 4× 10−23}, j = 1, 2, 3
{qa, qθ} {0.1 (m/s2)2, 0.004 (rad)2}R diag[400, 500, 600, 700] m2
{vr,max, ar,max} {20 m/s, 5 m/s2}T 0.1 s
1− α 95%d 25 m
mission complete. The UAV’s position FRMSE is computed
by averaging the final position estimation error squared over
all runs, and the UAV’s FRMSD is computed by averaging the
square of the final distance to the waypoint over all the runs.
The success rate is computed by dividing the number of runs
the UAV’s true position was within a distance of d from the
waypoint by the total number of runs.
TABLE IIMOTION PLANNING PERFORMANCE
Strategy Time [s] FRMSE [m] FRMSD [m] Success [%]
Naive 27.50 77.04 77.19 36.00MOMP 200.00 210.38 214.15 32.40
AdaptiveMOMP 91.31 18.95 19.32 95.20
The following conclusions can be drawn from these re-
sults. First, the non-adaptive MOMP strategy performed the
worst in all metrics. The mean time to completion is nearly
200 seconds, the entire simulation run. The distance term
dominates initially causing the UAV to go straight to the
target. Once close to the target, the trace term becomes more
dominant, causing the UAV to fly around in order to reduce
its position uncertainty before heading back to the target.
However, because of the poor estimability around the target,
the UAV cannot bring its position uncertainty down in the
allotted time. In fact, it was observed that the covariance
matrix in the EKF tends to grow as the UAV is flying around
due to the poor SOP geometry around the target. This causes
the UAV to not complete the mission, and explains the large
FRMSE and FRMSD values. The FRMSD and the FRMSE of
the proposed adaptive MOMP strategy was significantly lower
than the naive and non-adaptive MOMP approaches, even if
the timeout runs are removed. Second, the naive approach
declared an average mission complete time that was about 60
seconds sooner than the adaptive MOMP strategy. However,
only about 30% of these missions were actually successful.
Note that in some runs that used the naive strategy, the
10
Preprint of article accepted for publication in IEEE Transactions on Aerospace and Electronic Systems
Fig. 5. Results for the naive motion planning strategy. Illustrated are thetrue and estimated UAV trajectories, true and estimated SOP positions,the waypoint position, the corresponding uncertainty ellipse, and the targetperimeter (specified distance d to the waypoint). It can be seen that whilethe UAV actually fails to reach the target perimeter (ground truth trajectory)despite its estimate indicating otherwise.
Fig. 6. Results for the MOMP strategy. Illustrated are the true and estimatedUAV trajectories, true and estimated SOP positions, the waypoint position, thecorresponding uncertainty ellipse, and the target perimeter. The zoomed box inthe bottom right illustrates the scenario mentioned in Subsection IV-C, wherethe distance term in the cost function dominates at the beginning, causingthe UAV to fly directly toward the waypoint. Once close to the waypoint, thetrace of the estimation error covariance starts to dominate, causing the UAVto get stuck flying around the waypoint to minimize its position estimate’suncertainty but to no avail.
filter diverged and the UAV never reached the waypoint,
but the filter reported mission completed. This is due to
the naive approach failing to consider the uncertainty in the
UAV’s position estimate, causing the UAV to declare mission
complete when the actual position is unacceptably far from
the waypoint. This can be seen in Fig. 5, where the UAV
position estimate had reached the waypoint, whereas the true
UAV position was further than a distance d. In contrast,
the UAV spent more time in the adaptive MOMP strategy
performing maneuvers around the SOPs before moving to
the waypoint, as is illustrated in Fig. 5. The zoomed box
of Fig. 7 shows how the MOMP strategy ensures that the
uncertainty ellipse corresponding to 1−α is contained within
the specified distance d from waypoint before the mission is
declared complete. Moreover, this paper does not assume prior
knowledge of the UAV’s position, velocity, or clock error (bias
and drift). Therefore, the initial errors may be very large. This
is accounted for by selecting a large initial uncertainty. As the
UAV starts moving, its state becomes observable, allowing
the EKF to correct some of the large initial errors. As a
result, the estimate rapidly converges to the true trajectory’s
neighborhood, as apparent from Figs. 5–7.
Fig. 8 shows, on one hand, how the indicator function
condition (48) indicates adaptive MOMP switching costs to
balance information gathering and reaching the waypoint.
On the other hand, without the indicator function, the naive
and non-adaptive MOMP never meet condition (48), which
is necessary for (42), leading to poor performance. These
findings demonstrate the tradeoff between mission duration
and mission success and stress the importance of considering
the UAV’s position estimation uncertainty when prescribing
the UAV’s trajectory.
Fig. 7. Results for the adaptive MOMP strategy. Illustrated are the true andestimated UAV trajectories, true and estimated SOP positions, the waypointposition, the corresponding uncertainty ellipse, and the target perimeter. Thezoomed box in the bottom right illustrates that the final position uncertaintyof the UAV is contained within the target perimeter (i.e., within specifieddistance d to the waypoint).
VI. EXPERIMENTAL RESULTS
This section presents a field experiment demonstrating a
UAV navigating to a waypoint using pseudoranges measure-
ments from cellular SOPs. The naive and adaptive MOMP
strategies are employed and the resulting performance of each
strategy is compared.
11
Preprint of article accepted for publication in IEEE Transactions on Aerospace and Electronic Systems
0 20 40 60 80 100 120 140 160 180 200
Time (s)
-20
-15
-10
-5
0
5
10In
dic
ato
r fu
nctio
n c
on
ditio
n
(m
2)
Naive
MOMP
Adaptive MOMP
Indicator function threshold
Fig. 8. Indicator function condition (48) for the three strategies: Naive,MOMP, and adaptive MOMP. Illustrated are the values of the indicatorfunction condition over time and the dotted line is the threshold of theswitching cost function.
A. Scenario Description
In order to conduct a safe experiment, a post-processing ap-
proach was adopted. To this end, an environment was first built
in simulation based on the real field experimental environment.
Two simulations were run in this created environment, which
corresponded to: (i) naive and (ii) adaptive MOMP. For each
strategy, the UAV was initialized in the same location and
was tasked with reaching the same waypoint position. One
cellular transmitter was allocated as an anchor SOP and two
SOPs were set to be unknown. Next, the produced trajectories
for each motion planning strategy were flown manually by a
piloted UAV. The SOPs had comparable height, at which the
UAV was flown. The piloted UAV true trajectories will serve
as the ground truth prescribed trajectories for each strategy. To
perform data association, an offline technique was performed,
in which the profiles of the pseudoranges were compared with
the distance profiles to each SOP. Finally, the pseudoranges
were fused through an EKF for each motion planning strategy
and the resulting mission performance was compared in terms
of mission duration, UAV position RMSE, and final distance
to the waypoint position.
B. Experimental Setup
An Autel X-Star Premium UAV was equipped with an Ettus
universal software radio peripheral (USRP)-E312R to sample
cellular CDMA signals. The USRP was tuned to a carrier
frequency of 882.75 MHz, which is commonly used by the
cellular provided Verizon Wireless. Signals from three cellular
SOPs were acquired and tracked via the Multichannel Adap-
tive Transceiver Information eXtractor (MATRIX) software-
defined received (SDR) [70], producing pseudoranges to all
SOPs for the entire duration of the flight. A ground truth
trajectory for the UAV was parsed from the UAV’s onboard
navigation system log file, which records position from its
integrated GNSS-aided inertial navigation system (INS). The
anchor SOP’s clock was solved for off-line by subtracting the
true distance from the SOP’s pseudoranges. The experiment
was conducted in Colton, California, USA. The experimental
EKF settings are tabulated in Table III, and the experimental
setup is shown in Fig. 9.
C. Results
The prescribed and estimated UAV trajectories for the naive
and adaptive MOMP strategies are illustrated in Fig. 10.
USRP E312
GPS antenna
Cellular antenna
Autel X-Starpremium UAV
Fig. 9. Experimental setup.
TABLE IIIEXPERIMENT SETTINGS
Parameter Value
rt [−21.27,−334.5]T
xr(0) [−1.6617,−26.98, 0, 0, 0, 0]T
xsa [−3762,−2496, 0,−0.8302]T
xs1[765.4, 4927, 0,−0.2814]T
xs2[380.7, 1611.8, 0,−1.0416]T
xr(0 | −1) ∼ N [xr(0),Σr(0 | −1)]xsj (0 | −1) ∼ N [xsj ,Σsj (0 | −1)], j = 1, 2Σr(0 | −1) (104 · diag[1, 1, 10−2, 10−2, 10−4, 10−6])Σsj (0 | −1) (103 · diag[1, 1, 10−3, 10−5]), j = 1, 2{h0,r, h−2,r} {2 × 10−19, 2× 10−20}{h0,sj , h−2,sj } {8× 10−20, 4× 10−23}, j = 1, 2
{qa, qθ} {0.1 (m/s2)2, 0.004 (rad)2}R diag[400, 400, 400] m2
{vr,max, ar,max} {12 m/s, 3 m/s2}T 0.1 s
1− α 95%d 50 m
The resulting mission performance is tabulated in Table IV.
From this table, it can be concluded that using the adaptive
MOMP strategy to prescribe the UAV trajectory to navigate
to a waypoint resulted in a smaller RMSE and final distance
to the waypoint compared to using the naive strategy. Note
that on one hand, although the naive strategy prescribed the
shortest path and declared the mission successful, the UAV’s
position estimate had large errors, causing the UAV to be
unacceptably far from the waypoint, which in turn translates
to mission failure. The large errors are attributed to poor prior
knowledge of the SOPs in the environment. On the other
hand, the adaptive MOMP strategy prescribed a trajectory
that performed maneuvers around the environment, which
reduced the uncertainty of the SOPs in the environment. The
reduction of the uncertainty of the SOP’s states simultaneously
reduces the uncertainty of the UAV’s position states through
correlation. Eventually, when the UAV’s position estimation
error and error covariance are small enough, the UAV switches
its objective to navigating towards the waypoint, which is
successfully achieved since the filter errors are small.
VII. CONCLUSION
This paper developed a MOMP strategy for a UAV navigat-
ing to a specified waypoint in a GNSS-denied environment.
12
Preprint of article accepted for publication in IEEE Transactions on Aerospace and Electronic Systems
Prescribed naive trajectoryEstimated naive trajectoryPrescribed adaptive MOMP trajectoryEstimated adaptive MOMP trajectory
AnchorSOP
Startingpoint
Waypoint
UnknownSOPs
Fig. 10. Experimental environment and UAV trajectories: prescribed (cyanand blue) and estimated (pink and red) for the naive strategy and the adaptiveMOMP strategy. Map data: Google Earth.
TABLE IVSOLUTIONS PERFORMANCE
Trajectory Time [s] 2-D RMSE [m] 2-D Final Distance [m]
Naive 57.00 40.23 77.44AdaptiveMOMP 268.9 23.26 12.246
The UAV was required to reach the waypoint within a specified
distance with a specified probability. The UAV only had
access to pseudoranges from unknown SOPs, which were
used to simultaneously estimate the UAV’s states along with
the unknown SOPs’ states while the UAV navigated to the
specified waypoint. The MOMP cost functions were derived
in three different approaches: using the Markov inequality, the
KL-divergence, and the Wasserstein metric. It was shown that
all cost functions are equivalent under certain conditions, and
the resulting cost function balanced two objectives: (i) navigate
to the waypoint and (ii) reduce UAV position estimation uncer-
tainty. Adaptive weights were introduced to the MOMP cost
function to avoid the algorithm getting stuck at local minima.
An indicator function was selected, which switches between
the two objectives using a function of the UAV’s position
estimation error covariance. A simple test was derived using
the UAV’s position estimation error covariance to determine
if the UAV had reached the waypoint within the specified
distance and a specified confidence probability. Monte Carlo
simulations and experimental results demonstrated that the
proposed MOMP strategy significantly reduces the UAV’s po-
sition RMSE and the final distance to the waypoint compared
to a naive approach, in which the UAV moves directly to the
waypoint. The adaptive MOMP strategy also outperformed the
naive strategy in terms of mission success rate.
The MOMP strategy studied in this paper requires an indica-
tor function to switch between the two components in the cost
function, when using greedy motion planning. Future work
can extend this framework to adaptive weighting of the two
components of the cost function or adopt a receding horizon
trajectory optimization strategy to consider both components
at the same time. In addition, the cost function of MOMP
is generally non-convex, which upon employing numerical
optimization solvers, could result in convergence to local
optima, in addition to requiring involved computations which
could be infeasible for real-time implementations. Applying
a convex information gathering cost (e.g., innovation-based
metrics [32]) to MOMP could be the subject of future work.
REFERENCES
[1] T. Ren and M. Petovello, “A stand-alone approach for high-sensitivityGNSS receivers in signal-challenged environment,” IEEE Transactions
on Aerospace and Electronic Systems, vol. 53, no. 5, pp. 2438–2448,2017.
[2] R. Ioannides, T. Pany, and G. Gibbons, “Known vulnerabilities of globalnavigation satellite systems, status, and potential mitigation techniques,”Proceedings of the IEEE, vol. 104, no. 6, pp. 1174–1194, February 2016.
[3] J. Gross, Y. Gu, and M. Rhudy, “Robust uav relative navigation withDGPS, INS, and peer-to-peer radio ranging,” IEEE Transactions on
Aerospace and Electronic Systems, vol. 12, no. 3, pp. 935–944, July2015.
[4] M. Li and A. Mourikis, “High-precision, consistent EKF-based visual-inertial odometry,” International Journal of Robotics Research, vol. 32,no. 6, pp. 690–711, May 2013.
[5] A. Soloviev, “Tight coupling of GPS, INS, and laser for urban naviga-tion,” IEEE Transactions on Aerospace and Electronic Systems, vol. 46,no. 4, pp. 1731–1746, October 2010.
[6] H. Feder, J. Leonard, and C. Smith, “Adaptive mobile robot navigationand mapping,” International Journal of Robotics Research, vol. 18, no. 7,pp. 650–668, July 1999.
[7] H. Durrant-Whyte and T. Bailey, “Simultaneous localization and map-ping: part I,” IEEE Robotics & Automation Magazine, vol. 13, no. 2,pp. 99–110, June 2006.
[8] T. Bailey and H. Durrant-Whyte, “Simultaneous localization and map-ping: part II,” IEEE Robotics & Automation Magazine, vol. 13, no. 3,pp. 108–117, September 2006.
[9] C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira,I. Reid, and J. Leonard, “Past, present, and future of simultaneouslocalization and mapping: Toward the robust-perception age,” IEEE
Transactions on robotics, vol. 32, no. 6, pp. 1309–1332, 2016.
[10] J. Raquet and R. Martin, “Non-GNSS radio frequency navigation,” inProceedings of IEEE International Conference on Acoustics, Speech and
Signal Processing, March 2008, pp. 5308–5311.
[11] L. Merry, R. Faragher, and S. Schedin, “Comparison of opportunisticsignals for localisation,” in Proceedings of IFAC Symposium on Intelli-
gent Autonomous Vehicles, September 2010, pp. 109–114.
[12] Z. Kassas, J. Khalife, A. Abdallah, and C. Lee, “I am not afraidof the jammer: navigating with signals of opportunity in GPS-deniedenvironments,” in Proceedings of ION GNSS Conference, 2020, pp.1566–1585.
[13] C. Gentner, T. Jost, W. Wang, S. Zhang, A. Dammann, and U. Fiebig,“Multipath assisted positioning with simultaneous localization and map-ping,” IEEE Transactions on Wireless Communications, vol. 15, no. 9,pp. 6104–6117, September 2016.
[14] K. Shamaei, J. Khalife, and Z. Kassas, “Exploiting LTE signals fornavigation: Theory to implementation,” IEEE Transactions on Wireless
Communications, vol. 17, no. 4, pp. 2173–2189, April 2018.
[15] J. del Peral-Rosado, J. Lopez-Salcedo, F. Zanier, and G. Seco-Granados,“Position accuracy of joint time-delay and channel estimators in LTEnetworks,” IEEE Access, vol. 6, no. 25185–25199, p. April, 2018.
[16] J. Khalife and Z. Kassas, “Opportunistic UAV navigation with carrierphase measurements from asynchronous cellular signals,” IEEE Trans-actions on Aerospace and Electronic Systems, vol. 56, no. 4, pp. 3285–3301, August 2020.
[17] Z. Kassas, A. Abdallah, and M. Orabi, “Carpe signum: seize the signal– opportunistic navigation with 5G,” Inside GNSS Magazine, vol. 16,no. 1, pp. 52–57, 2021.
13
Preprint of article accepted for publication in IEEE Transactions on Aerospace and Electronic Systems
[18] C. Yang, T. Nguyen, and E. Blasch, “Mobile positioning via fusion ofmixed signals of opportunity,” IEEE Aerospace and Electronic SystemsMagazine, vol. 29, no. 4, pp. 34–46, April 2014.
[19] L. Chen, O. Julien, P. Thevenon, D. Serant, A. Pena, and H. Kuusniemi,“TOA estimation for positioning with DVB-T signals in outdoor statictests,” IEEE Transactions on Broadcasting, vol. 61, no. 4, pp. 625–638,2015.
[20] S. Fang, J. Chen, H. Huang, and T. Lin, “Is FM a RF-based positioningsolution in a metropolitan-scale environment? A probabilistic approachwith radio measurements analysis,” IEEE Transactions on Broadcasting,vol. 55, no. 3, pp. 577–588, September 2009.
[21] V. Moghtadaiee and A. Dempster, “Indoor location fingerprinting usingFM radio signals,” IEEE Transactions on Broadcasting, vol. 60, no. 2,pp. 336–346, June 2014.
[22] R. Landry, A. Nguyen, H. Rasaee, A. Amrhar, X. Fang, and H. Ben-zerrouk, “Iridium Next LEO satellites as an alternative PNT in GNSSdenied environments–part 1,” Inside GNSS Magazine, vol. 14, no. 3, pp.56–64., May 2019.
[23] Z. Kassas, J. Morales, and J. Khalife, “New-age satellite-based naviga-tion – STAN: simultaneous tracking and navigation with LEO satellitesignals,” Inside GNSS Magazine, vol. 14, no. 4, pp. 56–65, 2019.
[24] F. Farhangian and R. Landry, “Multi-constellation software-definedreceiver for Doppler positioning with LEO satellites,” Sensors, vol. 20,no. 20, pp. 5866–5883, October 2020.
[25] M. Orabi, J. Khalife, and Z. Kassas, “Opportunistic navigation withDoppler measurements from Iridium Next and Orbcomm LEO satel-lites,” in Proceedings of IEEE Aerospace Conference, March 2021, pp.1–9.
[26] J. Morales and Z. Kassas, “Tightly-coupled inertial navigation systemwith signals of opportunity aiding,” IEEE Transactions on Aerospace
and Electronic Systems, vol. 57, no. 3, pp. 1930–1948, 2021.
[27] M. Driusso, C. Marshall, M. Sabathy, F. Knutti, H. Mathis, andF. Babich, “Vehicular position tracking using LTE signals,” IEEE Trans-
actions on Vehicular Technology, vol. 66, no. 4, pp. 3376–3391, April2017.
[28] K. Shamaei and Z. Kassas, “LTE receiver design and multipath analysisfor navigation in urban environments,” NAVIGATION, Journal of the
Institute of Navigation, vol. 65, no. 4, pp. 655–675, December 2018.
[29] C. Yang and A. Soloviev, “Mobile positioning with signals of opportu-nity in urban and urban canyon environments,” in IEEE/ION Position,
Location, and Navigation Symposium, April 2020, pp. 1043–1059.
[30] J. Khalife and Z. Kassas, “Precise UAV navigation with cellular carrierphase measurements,” in Proceedings of IEEE/ION Position, Location,
and Navigation Symposium, April 2018, pp. 978–989.
[31] C. Yang and A. Soloviev, “Simultaneous localization and mappingof emitting radio sources-SLAMERS,” in Proceedings of ION GNSSConference, September 2015, pp. 2343–2354.
[32] Z. Kassas, A. Arapostathis, and T. Humphreys, “Greedy motion planningfor simultaneous signal landscape mapping and receiver localization,”IEEE Journal of Selected Topics in Signal Processing, vol. 9, no. 2, pp.247–258, March 2015.
[33] Z. Kassas and T. Humphreys, “Receding horizon trajectory optimiza-tion in opportunistic navigation environments,” IEEE Transactions on
Aerospace and Electronic Systems, vol. 51, no. 2, pp. 866–877, April2015.
[34] G. Qian, D. Pebrianti, Y. Chun, Y. Hao, and L. Bayuaji, “Waypointnavigation of quad-rotor MAV,” in Proceedings of IEEE International
Conference on System Engineering and Technology, 2017, pp. 38–42.
[35] Y. Saidi, M. Tadjine, and A. Nemra, “Robust waypoints navigation usingfuzzy type 2 controller,” in Proceedings of International Conference on
Advanced Electrical Engineering, 2019, pp. 1–6.
[36] T. Puls, M. Kemper, R. Kuke, and A. Hein, “GPS-based position controland waypoint navigation system for quadrocopters,” in Proceedings ofIEEE/RSJ International Conference on Intelligent Robots and Systems,2009, pp. 3374–3379.
[37] M. Imamura, R. Tomitaka, Y. Miyazaki, K. Kobayashi, and K. Watan-abe, “Outdoor waypoint navigation for an intelligent wheelchair usingdifferential GPS and INS,” in Proceedings of SICE Annual Conference,vol. 3, 2004, pp. 2193–2196.
[38] R. Tomitaka, K. Kobayashi, and K. Watanabe, “Development of intelli-gent vehicle for waypoints navigation,” in Proceedings of SICE AnnualConference, vol. 1, 2003, pp. 1094–1097.
[39] S. He, H. Shin, A. Tsourdos, and C. Lee, “Energy-optimal waypoint-following guidance considering autopilot dynamics,” IEEE Transactions
on Aerospace and Electronic Systems, vol. 56, no. 4, pp. 2701–2717,2020.
[40] I. Whang and T. Hwang, “Horizontal waypoint guidance design usingoptimal control,” IEEE Transactions on Aerospace and Electronic Sys-tems, vol. 38, no. 3, pp. 1116–1120, 2002.
[41] T. Davies and A. Jnifene, “Multiple waypoint path planning for a mobilerobot using genetic algorithms,” in Proceedings of IEEE International
Conference on Computational Intelligence for Measurement Systems andApplications, 2006, pp. 21–26.
[42] J. Petereit, T. Emter, and C. Frey, “Safe mobile robot motion planningfor waypoint sequences in a dynamic environment,” in Proceedinngsof IEEE International Conference on Industrial Technology, 2013, pp.181–186.
[43] Z. Yan, J. Li, Y. Wu, and G. Zhang, “A real-rime path planning algorithmfor AUV in unknown underwater environment based on combining PSOand waypoint guidance,” Sensors, vol. 19, no. 1, pp. 20–33, 2019.
[44] M. Ataei and A. Yousefi-Koma, “Three-dimensional optimal path plan-ning for waypoint guidance of an autonomous underwater vehicle,”Robotics and Autonomous Systems, vol. 67, pp. 23–32, 2015.
[45] C. Hubmann, J. Schulz, M. Becker, D. Althoff, and C. Stiller, “Au-tomated driving in uncertain environments: Planning with interactionand uncertain maneuver prediction,” IEEE Transactions on IntelligentVehicles, vol. 3, no. 1, pp. 5–17, March 2018.
[46] K. Berntorp, T. Hoang, and S. Di Cairano, “Motion planning ofautonomous road vehicles by particle filtering,” IEEE Transactions on
Intelligent Vehicles, vol. 4, no. 2, pp. 197–210, 2019.[47] H. Karimi and D. Asavasuthirakul, “A novel optimal routing for naviga-
tion systems-services based on global navigation satellite system qualityof service,” Journal of Intelligent Transportation Systems, vol. 18, no. 3,pp. 286–298, May 2014.
[48] A. Nowak, “Dynamic GNSS mission planning using DTM for precisenavigation of autonomous vehicles,” Journal of Navigation, pp. 483–504, May 2017.
[49] S. Ragothaman, M. Maaref, and Z. Kassas, “Autonomous groundvehicle path planning in urban environments using GNSS and cellularsignals reliability maps: Models and algorithms,” IEEE Transactions onAerospace and Electronic Systems, vol. 57, no. 2, pp. 1562–1580, 2021.
[50] S. Ragothaman, M. Maaref, and Z. Kassas, “Autonomous ground vehiclepath planning in urban environments using GNSS and cellular signals re-liability maps: Simulation and experimental results,” IEEE Transactionson Aerospace and Electronic Systems, 2021, accepted.
[51] J. Isaacs, C. Magee, A. Subbaraman, F. Quitin, K. Fregene, A. Teel,U. Madhow, and J. Hespanha, “GPS-optimal micro air vehicle naviga-tion in degraded environments,” in Proceedings of American Control
Conference, June 2014, pp. 1864–1871.[52] S. Ragothaman, M. Maaref, and Z. Kassas, “Multipath-optimal UAV
trajectory planning for urban UAV navigation with cellular signals,”in Proceedings of IEEE Vehicular Technology Conference, September2019, pp. 1–6.
[53] G. Zhang and L. Hsu, “A new path planning algorithm using a GNSSlocalization error map for UAVs in an urban area,” Journal of Intelligentand Robotic Systems, vol. 94, pp. 219–235, 2019.
[54] A. Shetty and G. Gao, “Trajectory planning under stochastic andbounded sensing uncertainties using reachability analysis,” in Proceed-ings of ION GNSS Conference, 2020, pp. 1637–1648.
[55] M. Maaref and Z. Kassas, “Optimal GPS integrity-constrained pathplanning for ground vehicles,” in Proceedings of IEEE/ION Position,
Location, and Navigation Symposium, April 2020, pp. 655–660.[56] S. Prentice and N. Roy, “The belief roadmap: Efficient planning in linear
POMDPs by factoring the covariance,” Robotics Research, vol. 66, pp.293–305, 2011.
[57] B. Englot, T. Shan, S. Bopardikar, and A. Speranzon, “Sampling-basedmin-max uncertainty path planning,” in Proceedings of the Conference
on Decision and Control, December 2016, pp. 6863–6870.[58] S. Bopardikar, B. Englot, A. Speranzon, and J. Berg, “Robust belief
space planning under intermittent sensing via a maximum eigenvalue-based bound,” The International Journal of Robotics Research, vol. 28,pp. 1609–1626, July 2016.
[59] Y. Elisha and V. Indelman, “Active online visual-inertial navigationand sensor calibration via belief space planning and factor graphbased incremental smoothing,” in Proceedings IEEE/RSJ International
Conference on Intelligent Robots and Systems, September 2017, pp.2616–2622.
[60] R. Schirmer, P. Biber, and C. Stachniss, “Efficient path planning in beliefspace for safe navigation,” in Proceedings of IEEE/RSJ International
Conference on Intelligent Robots and Systems, September 2017, pp.2857–2863.
[61] A. Agha-Mohammadi, S. Agarwal, S. Kim, S. Chakravorty, and N. Am-ato, “SLAP: Simultaneous localization and planning under uncertainty
14
Preprint of article accepted for publication in IEEE Transactions on Aerospace and Electronic Systems
via dynamic replanning in belief space,” IEEE Transactions on Robotics,vol. 34, no. 5, pp. 1195–1214, October 2018.
[62] S. Bopardikar, B. Englot, and A. Speranzon, “Multiobjective path plan-ning: Localization constraints and collision probability,” IEEE Transac-
tions on Robotics, vol. 31, no. 3, pp. 562–577, June 2015.[63] S. Singh and P. Sujit, “Landmarks based path planning for UAVs in
GPS-denied areas,” IFAC-PapersOnLine, vol. 49, no. 1, pp. 396–400,2016.
[64] B. Irani, J. Wang, and W. Chen, “A localizability constraint-basedpath planning method for autonomous vehicles,” IEEE Transactions on
Intelligent Transportation Systems, vol. 20, no. 7, pp. 2593–2604, July2018.
[65] A. Das, K. Subbarao, and F. Lewis, “Dynamic inversion with zero-dynamics stabilisation for quadrotor control,” IET Control Theory and
Applications, vol. 3, no. 3, pp. 303–314, 2008.[66] X. Li and V. Jilkov, “Survey of maneuvering target tracking. Part I:
Dynamic models,” IEEE Transactions on Aerospace and ElectronicSystems, vol. 39, no. 4, pp. 1333–1364, 2003.
[67] J. Morales and Z. Kassas, “Stochastic observability and uncertaintycharacterization in simultaneous receiver and transmitter localization,”IEEE Transactions on Aerospace and Electronic Systems, vol. 55, no. 2,pp. 1021–1031, April 2019.
[68] Z. Kassas and T. Humphreys, “Observability analysis of collaborativeopportunistic navigation with pseudorange measurements,” IEEE Trans-actions on Intelligent Transportation Systems, vol. 15, no. 1, pp. 260–273, February 2014.
[69] Y. Sun, A. Baricz, and S. Zhou, “On the monotonicity, log-concavity,and tight bounds of the generalized Marcum and Nuttall Q-Functions,”IEEE Transactions on Information Theory, vol. 56, no. 3, pp. 1166–1186, March 2010.
[70] J. Khalife, K. Shamaei, and Z. Kassas, “Navigation with cellular CDMAsignals – part I: Signal modeling and software-defined receiver design,”IEEE Transactions on Signal Processing, vol. 66, no. 8, pp. 2191–2203,April 2018.
Yanhao Yang is pursuing the M.S. degree with theDepartment of Mechanical Engineering, CarnegieMellon University, Pittsburgh, PA, USA. He receivedthe B.E. degree in Mechanical Engineering fromSouth China University of Technology, Guangzhou,China. He was an exchange student at the Uni-versity of California, Irvine, and a member of theAutonomous Systems Perception, Intelligence, andNavigation (ASPIN) Laboratory. His research inter-ests include motion planning and unmanned aerialvehicles.
Joe Khalife (S’15-M’20) is a postdoctoral fellow atthe University of California, Irvine and member ofthe Autonomous Systems Perception, Intelligence,and Navigation (ASPIN) Laboratory. He received aB.E. in Electrical Engineering, an M.S. in ComputerEngineering from the Lebanese American University(LAU) and a Ph.D. in Electrical Engineering andComputer Science from the University of California,Irvine. From 2012 to 2015, he was a researchassistant at LAU, and has been a member of theASPIN Laboratory since 2015. He is a recipient of
the 2016 IEEE/ION Position, Location, and Navigation Symposium (PLANS)Best Student Paper Award and the 2018 IEEE Walter Fried Award. Hisresearch interests include opportunistic navigation, autonomous vehicles, andsoftware-defined radio.
Joshua J. Morales (S’11) Joshua J. Morales isa Principal with StarNav LLC. Previously, he wasa member of the Autonomous Systems Perception,Intelligence, and Navigation (ASPIN) Laboratory atthe University of California, Irvine. He received aB.S. in electrical engineering from the University ofCalifornia, Riverside and a Ph.D. in electrical en-gineering and computer science from University ofCalifornia, Irvine. He is a recipient of 2016 Honor-able Mention from the National Science Foundation(NSF), and 2018 National Center for Sustainable
Transportation (NCST) Outstanding Graduate Student of the Year award. Hisresearch interests include estimation theory, navigation systems, autonomousvehicles, and cyber-physical systems.
Zaher (Zak) M. Kassas (S’98-M’08-SM’11) is anassociate professor at the University of California,Irvine and director of the Autonomous SystemsPerception, Intelligence, and Navigation (ASPIN)Laboratory. He is also director of the U.S. Depart-ment of Transportation Center: CARMEN (Centerfor Automated Vehicle Research with MultimodalAssurEd Navigation), focusing on navigation re-siliency and security of highly automated transporta-tion systems. He received a B.E. in Electrical Engi-neering from the Lebanese American University, an
M.S. in Electrical and Computer Engineering from The Ohio State University,and an M.S.E. in Aerospace Engineering and a Ph.D. in Electrical andComputer Engineering from The University of Texas at Austin. He is arecipient of the 2018 National Science Foundation (NSF) Faculty Early CareerDevelopment Program (CAREER) award, 2019 Office of Naval Research(ONR) Young Investigator Program (YIP) award, 2018 IEEE Walter FriedAward, 2018 Institute of Navigation (ION) Samuel Burka Award, and 2019ION Col. Thomas Thurlow Award. He is a Senior Editor of the IEEETransactions on Intelligent Vehicles and an Associate Editor of the IEEETransactions on Aerospace and Electronic Systems and the IEEE Transactionson Intelligent Transportation Systems. His research interests include cyber-physical systems, estimation theory, navigation systems, autonomous vehicles,and intelligent transportation systems.
15