Air Force Institute of Technology

AFIT Scholar

Faculty Publications

Summer 2018

Improvements for Vision-based Navigation of Small, Fixed-wing

Unmanned Aerial Vehicles
Robert C. Leishman
Air Force Institute of Technology

Jeremy Gray

John F. Raquet
Air Force Institute of Technology

Adam Rutkowski
Air Force Research Laboratory

Follow this and additional works at: https://scholar.afit.edu/facpub

Part of the Controls and Control Theory Commons, and the Signal Processing Commons

Recommended Citation
Leishman, Robert C.; Gray, Jeremy; Raquet, John F.; and Rutkowski, Adam, "Improvements for Vision-
based Navigation of Small, Fixed-wing Unmanned Aerial Vehicles" (2018). Faculty Publications. 675.
https://scholar.afit.edu/facpub/675

This Article is brought to you for free and open access by AFIT Scholar. It has been accepted for inclusion in
Faculty Publications by an authorized administrator of AFIT Scholar. For more information, please contact
[email protected].
 Improvements for Vision-based Navigation of Small, Fixed-wing
Unmanned Aerial Vehicles
Robert C. Leishman1 , Jeremy Gray1 , John Raquet1 , and Adam Rutkowski2

Abstract— Investigating alternative navigation approaches an adequate navigation solution for any sufficient duration;
for use when GPS signals are unavailable is an active area and that something like vision is essential. There are two
of research across the globe. In this paper we focus on the key questions addressed by this research to further the
navigation of small, fixed-wing unmanned aerial vehicles (UAVs)
that employ vision-based approaches combined with other capabilities of a designed system. The first is, can MEMS-
measurements as a replacement for GPS. We demonstrate with grade IMU data, or rather, attitude estimates derived from
flight test data that vehicle attitude information, derived from that data, be used to benefit vision algorithms? And the
cheap, MEMS-based IMUs is sufficient to improve two different second, what additional information is needed to bound the
types of vision processing algorithms. Secondly, we show analyt- drift in a visual inertial odometry (VIO) solution for a fixed
ically and with flight test data that range measurements to one
other vehicle with global pose is sufficient to constrain the global wing UAV? It is known that if there is no loop closure-type
drift of a visual inertial odometry-based navigation solution. data, essentially creating a SLAM algorithm, the drift of a
Further, we demonstrate that such ranging information is not VIO system grows without bounds.
needed at a fast rate; that bounding can occur using data as The research discussed in this article demonstrates and
infrequent as 0.01 Hz. details synergies that can be utilized in the development of
I. I NTRODUCTION small UAVs that can navigate with vision sensors without
Investigating alternative navigation approaches for use the direct availability of GPS information. It is clear that a
when GPS signals are unavailable is an active area of re- small UAV with only the core onboard sensors: MEMS IMU
search across the globe. When multiple sources of precision (accelerometer, gyroscope, and magnetometer) and pressure
navigation and timing (PNT) information are available and (barometric and airspeed) cannot maintain a good navigation
can be fused together, the resulting navigation solution is solution for any sufficient duration. As has been discussed,
more accurate and more robust. There are a variety of vision navigation methods have demonstrated good utility in
areas where alternative navigation solutions are of interest providing an alternative to GPS. The contributions made is
and a variety of phenomenologies upon which alternative this article are, first, that MEMS IMU sensors can be utilized
navigation estimates can be developed [11], [5], [22]. In to improve the navigation results of at least two different
this paper we focus on the navigation of small unmanned types of vision algorithms; second, it is shown that a range
aerial vehicles (UAVs) that employ vision-based approaches to only one other vehicle with known pose is sufficient to
combined with other measurements. constrain the drift of a VO-based navigation solution.
There is a vast array of current and active research on vi- The paper is organized as follows. First relevant back-
sion navigation approaches for small UAVs, for example [9], ground material is discussed, including the GIVINS project,
[19], [15], [2], [16], [4], [20], [7], [10]. Many of these the two types of vision navigation algorithms, details regard-
approaches provide results competitive with or better than ing the autopilots and associated sensors used in the flight
estimates computed using GPS signals, over short (hundreds tests, a discussion of why attitude information can provide
of meters) distances. a benefit to vision algorithms and finally an explanation of
The work described in this paper, however, takes a slightly the ranging concept used in the paper. Section III discusses
different tack. One current research aim for the ANT Center, the filter framework used to combine IMU, barometric,
of which this paper is a part, is to demonstrate GPS-denied VO and ranging data. Section IV derives the analytic non-
or degraded navigation of small, fixed-wing UAV platforms linear observability of the filter framework in Section III.
over longer (tens of kilometers) trajectories. It is clear that A summary of the relevant details regarding the flight test
a small UAV with only the core onboard sensors: MEMS data used in the results is in Section V. Results for attitude
IMU (accelerometer, gyroscope, and magnetometer) and aiding of vision algorithms are found in Section VI. Results
pressure (barometric altitude and airspeed) cannot maintain demonstrating bounding the drift of a VO-based solution
using the range to only one other vehicle are in Section VII.
*This work was supported by AFRL/RWWI under the GIVINS Program, Finally, conclusions are offered in Section VIII.
an international collaboration between AFRL and the Singaporean Ministry
of Defense II. BACKGROUND
1 R.C. Leishman, J. Gray, and J. Raquet are with the Autonomy and Navi-
gation Technology (ANT) Center, Air Force Institute of Technology, Wright- The material in this section provides some essential back-
Patterson AFB, OH 45433 USA first.last at afit.edu ground information to support the development and results
2 A. Rutkowski is with the Air Force Research Laboratory, Muni-
tion Directorate, Eglin AFB, FL, 32542 USA adam.rutkowski at below. Several topics are covered as part of the background:
us.af.mil a major source for the funding of this work, the two types of
vision navigation methods utilized, commercial small-UAV point cloud of features. This enabled trade studies for varying
autopilots and sensors, why attitude aiding for visual navi- sensor parameters to simulate different quality sensors and
gation algorithms is beneficial, and finally details regarding camera look angles. Camera quality was simulated by adding
the ranging scenario and sensors utilized. noise (with a standard deviation of a certain number of
pixels) to the pixel position of each feature.
A. GIVINS Project For a given normalized translation vector t in East-North-
The objective of the GPS, Inertial, Vision Integrated Navi- Up coordinates, as determined from visual odometry, the
gation System (GIVINS) program is to develop an integrated two directions of zero velocity are determined from the
navigation system using a vision sensor, a standard Global full Singular Value Decomposition of t (Png 2014). This
Positioning System (GPS) receiver, and a consumer grade produces a matrix U where the first column is t and the last
Inertial Measurement Unit (IMU). The GIVINS program is a two columns are perpendicular to t. The matrix Σ will be
4 year international project agreement (PA #AF-PA-13-0001) [1 0 0]T and V will be 1. Similarly, a full QR factorization
with the Singapore Ministry of Defence (SG MINDEF) that could be used to compute the perpendicular directions.
began 30 Oct 2013. IMU measurements were simulated by inverting the PVA
The original plan for the GIVINS program was to imple- data from the ASPN data sets, then adding FOGM noise
ment the system on a ground vehicle and test in an urban according to the desired sensor quality to be simulated.
environment. Also, the original plan was for the US and Rohde (2015) was the first to apply the visual zero velocity
SG to develop and use a common set of hardware. However, update technique to real world data.
due to difficulties associated with hardware being commonly Given the success of using this visual and inertial sensor
available in both countries, it was decided to develop separate measurements with GPS pseudoranges, the GPS pseudor-
harware solutions. Also, rather than test in an active urban anges were replaced with actual range measurements to a
environment, it was decided to shift the focus of the project vehicle in a high flyer / low flyer concept.
to aerial applications. SG chose to develop a solution for a
quad rotor, while the US chose to work with a fixed wing B. Absolute Positioning Approach
platform.
Two vision navigation approaches are utilized in demon-
The original technical approach was to use zero velocity
strating that attitude estimates derived from MEMS IMU can
updates for vision-aided inertial navigation. A zero velocity
add value to vision processing algorithms. The first is an
update, as the name suggests, resets the velocity estimate
absolute positioning (AP) algorithm that provides a global
of the navigation system to zero when it is known that the
position and attitude measurement, illustrated in Figure 1,
system is at rest. Zero velocity updates have been effective
by comparing the current camera image and a known map
at reducing the drift of an Inertial Navigation System, par-
of images, e.g. Google Earth. The algorithm used in this
ticularly for land based systems. Although it is much more
research was developed in the ANT Center and is described
difficult to reach zero velocity (i.e. stop) when suspended in
in detail in [17], [22]. We include here a brief summary of
the air (and impossible for a fixed wind aircraft that can’t
the key points of the algorithm, for reference.
hang on the prop), the direction of vehicle motion can be per-
ceived by an onboard visual system. It can then be deduced • The algorithm requires a geo-reference database (known
that there is no component of velocity perpendicular to this map) as an input. This must be prepared beforehand,
direction. In effect, a zero velocity update can be performed and is critical to the functionality of the algorithm,
in two directions. It was expected that this approach will see [22] for extensive details.
have similar performance to existing solutions. Furthermore, • The current image and current estimate of global posi-
if GPS is available, then it should be used. If at any time tion and orientation (pose) of the vehicle are sent to the
there are not enough GPS satellites visible to compute a full algorithm.
GPS position solution (which commonly happens in urban • SIFT features are found on the current image.
environments), then it is still possible to use pseudorange • A database image tile is retrieved from the map based
measurements to the visible satellites to aid the navigation on the global pose estimate. This tile contains georef-
solution. erenced SIFT features.
The first attempt at using vision-based zero velocity up- • A brute-force matcher finds the best matching features
dates was performed by Png (2014). He first developed a between the current image and the image tile.
simulation environment for GPS pseudoranges, IMU mea- • At this point there are two possible approaches, depend-
surements (∆V and ∆θ), and visual features. Rather than ing on the selection of an attitude aided solution or a
developing a full scene generation capability, the approach non-attitude aided solution.
used was to perform feature tracking on images from real – If the attitude aided solution is selected, the attitude
datasets (scenarios 2, 6, and 7 of the ASPN dataset). These information from the autopilot is used directly to
features were then mapped to 3D feature coordinates to find the translation estimate between the image
create a 3D map of feature locations. In this way, a more tile and the current image. This is done using a
realistic point cloud that was derived from real data was 3 Degree of Freedom (DoF) perspective n point
achieved rather than using a pseudo-randomly generated (PNP) algorithm.
 – If a non-attitude aided solution is requested then • The RANSAC algorithm in the OpenCV ”findEssen-
the full pose information is recovered from the 6 tialMat” function robustly computes an essential matrix
DoF PNP algorithm. and identifies valid matches (inliers) that support the
computation.
Camera i+1 At this point, the choice is made on whether to utilize
Camera i •
frame
frame the rotation matrix found as part of the essential ma-
ci ci trix (non-attitude aided) or to use the rotation matrix
Rn , Tn
provided by the autopilot (attitude aided). If the non-
ci+1
Rn
ci+1
, Tn
attitude aided solution is chosen, the algorithm chooses
Global the appropriate rotation matrix extracted from the es-
Origin: sential matrix.
North, • Each of the matched, inlier features are expressed in
East,
Down the navigation frame using the selected rotation matrix.
Inherent in this step is the addition of yaw information
Fig. 1. Global measurement and associated coordinate systems. into the measurement. The source of the yaw informa-
tion is a magnetometer.
• A depth estimate is computed for each feature, using
C. Visual Odometry Approach the height information from the barometer.
The second vision algorithm used is a visual odometry • All the inlier measurements, from each image, are
(VO) algorithm that provides a relative, or incremental, mea- averaged together and then the means are subtracted
surement between the previous camera frame and the current to find the translation vector.
camera frame, see Figure 2 below. Consequently, it is often • The computed/selected rotation and computed trans-
utilized as a velocity update when the position information in lation are returned as the six DoF solution by the
a filter state vector is expressed in a global coordinate frame. algorithm.
This is an important distinction between the VO and the
Absolute Positing Approach described above, as the Absolute D. Autopilots
Positioning approach provides global measurements. This The growth of available low-cost commercial autopilots
VO algorithm is used in both objectives for this research over the past decade has provided many new avenues for per-
outlined above. forming previously difficult or even impossible operations.
These operations are spread across both commercial and
cj+1 cj+1 defense domains. Some examples of these operations include
Rci , Tci
Camera i Camera infrastructure inspection, reconnaissance, crop inspection,
frame i+1 frame and many others. Currently, the cost of these hobby-grade
autopilots range anywhere from $300 to as inexpensive as
$10 with varying capabilities.
In this research, Pixhawk autopilots were used as the flight
Global controller for each fixed wing as well as a sensor data source.
Origin: These autopilots contain redundant accelerometers, gyros,
North, barometers, and a single magnetometer, and utilize an ex-
East, ternal GPS, magnetometer, and airspeed sensor. The sensors
Down contained in the Pixhawk, specifically the accelerometers and
gyros, are of a low consumer grade (cost about $6 for the
Fig. 2. Relative measurement and associated coordinate systems. IMU boards). During the flight tests for constraining the drift
on the VIO solution, two Pixhawk autopilots were utilized.
The VO approach used in this work is described in detail The first was connected to GPS and the data from it is used
in [6]. The algorithm uses detected features, specifically the as truth. The second autopilot was purposely not connected
AKAZE [1] variety, to compute the relative transformation to GPS; the data from this autopilot is used for evaluating
between two images. The following is a quick summary of the filter from a GPS-denied standpoint.
the algorithm to understand the principle steps.
• The current and just-previous image are sent to the
E. Why Attitude Aiding is Helpful
algorithm, along with camera calibration, the body to When using vision data for navigation, there is often a
Navigation rotation matrices for each image and other very high correlation between attitude and position. Consider,
parameters, like the current barometric height. for example, the three pictures shown in Figure 3. The left
• AKAZE features and descriptors are detected on both picture is the original picture, taken approximately 1m from
images. A a brute force descriptor matcher, using the the bookshelf. The center picture was obtained by keeping
Hamming distance, finds the closest matching features the camera in the same position, but rotating it approximately
between the two images. 1.7◦ to the left, and the right picture was obtained by moving
the camera 3cm to the left (but no rotation). It is clear that the
center and the right pictures are very similar, which indicates
that any vision algorithm which would seek to measure both
rotation and translation will have difficulty distinguishing
between the two.
In this kind of situation, we are taking advantage of the
knowledge of attitude obtained from a Pixhawk autopilot that
is not connected to GPS. As a result, the algorithm only
needs to solve for position (or delta-position in the case of
visual odometry), given a known attitude (or delta-attitude),
which should significantly increase the performance of the
position/delta-position estimation algorithm. The results in
Section VI demonstrate that low-commercial-grade autopilot
attitude information can be used to improve vision navigation
solutions.

Original After 1.7° Rotation After 3cm Translation

Fig. 3. Demonstration showing correlation between translation and rotation, Fig. 4. An illustration of the ranging scenario used in the flight test. The
as demonstrated by similarity between center (rotated) and right (translated) upper plane has access to GPS signals, the lower plane does not. It uses
images. autopilot sensors plus visual odometry to navigate, and then it fuses in the
range to the upper aircraft. We use a common Navigation frame for both
vehicles. We assume that GPS is available at the beginning of the lower
planes flight to establish the common frame.

F. Ranging Measurements
The second objective in this study, as outlined above, is III. VO, IMU, P RESSURE AND R ANGE DATA F USION
to investigate the amount of information that is required to
bound the drift of a VIO solution for a UAV. The proposed We first introduce a couple of the coordinate frames
method for bounding the drift is to utilize range data obtained 𝑖Ƹ𝑁
utilized throughout the paper in preparation of the filter
through a ranging radio to another aircraft/UAV that has derivation, illustrated in Figure 5. The first coordinate frame
global pose information, likely through GPS. is the body-fixed frame of the lower UAV, which is fixed to
The concept is that one aircraft, the one receiving GPS, is the center𝑗Ƹof
𝑁 mass of the vehicle with the ı̂b axis out the nose,
able to fly higher; above the range of the jamming, out of the the ̂b axis out the right wing, and k̂b axis out of the belly.
urban canyon, or out of whatever condition that is limiting 𝑘෠𝑁
The second is the navigation frame, which is a local frame
the access to GPS signals. The lower vehicle is navigating with the ı̂N axis aligned with magnetic North, the ̂N axis
using IMU and VO information (VIO approach), without aligned with East, and the k̂N axis pointing toward the center
GPS. The upper vehicle provides, through the same ranging of the Earth [21]. The navigation frame sits at a point on the
radio, its position and range to the lower UAV. The concept Earth’s surface near where the flight occurs and for short,
is illustrated in Figure 4. low altitude UAV flights is assumed to be inertial [3]. We
Actual flight test data is used to demonstrate the concepts utilize the convention described in [3], Chapter 2, including
proposed in the paper in addition to an analytic observability the 3-2-1 Euler angle sequence and the wind triangle.
analysis in Section IV. Range radios were provided by EN- The extended Kalman filter used to demonstrate bounding
SCO Corp. through a cooperative research and development of a VIO solution, which fuses IMU, pressure, VO and range
agreement. measurements, largely follows the development in Chapter 8
The research builds on initial work in [8]. All the results of [3]. We have extended the filter by including states for
in Section VII pertain to estimating the position of the lower altitude and vertical velocity. The states for the filter are
UAV, following the filter framework established in Section III the north position, east position, ground speed, course angle,
immediately below. wind speed in north direction, wind speed in east direction,
 from the baro altimeter, and finally the range measurement.
The measurement models are described briefly below.
𝑖Ƹ𝑏 1) Wind Pseudo-Measurement: The wind pseudo-
measurement update is the same proposed in [3], where
the geometry in the wind triangle is used to equate the
wind, ground and air speeds using the angles χ and ψ. This
measurement is more of a constraint, as the ”measurement”
𝑗Ƹ𝑏 should always equal zero.
𝑘෠𝑏    
0 V cos(ψ) + wn − Vg cos(χ)
hw (x, u) = = a (4)
0 Va sin(ψ) + wn − Vg sin(χ)
𝑖Ƹ𝑁
2) VO Velocity Update: The VO algorithm [6] utilized
for this paper is briefly described above. An important point
𝑗Ƹ𝑁 to recall is the yaw information, from a magnetometer,
has been infused into the VO measurements. Consequently,
𝑘෠𝑁 the measurement provides both a ground speed and course
angle update for the filter. A speed update is used since
Fig. 5. Coordinate frames used in the filtering derivations. Body-fixed the position information derived from the VO algorithm is
frame is in blue, the inertial frame is in orange.
relative to the image before it and does not provide position
information relative to the navigation frame [13]. However,
the VO algorithm is provided the best estimate of the rotation
yaw, altitude, and vertical velocity:
between the current body frame and the navigation frame,
 T
x = pn pe Vg χ wn we ψ pa p˙a . (1) allowing it to express the velocity in the navigation frame.
We can then update both the course angle χ and the ground
The north, east and altitude positions are with respect to speed Vg elements of the state with the output of the VO
the navigation frame. Altitude is simply the negative of the velocity [Vn , Ve , Va ] according to
position in the down direction. The relationship between ψ
tan−1 ( VVne )
" #
and χ is explained using the wind triangle [3]. ψ is the angle
between the ı̂N axis and the ı̂b axis in the horizontal plane. χ zvo (x) = p . (5)
Vn2 + Ve2 + Va2
is the angle between the ı̂b axis and the ground speed vector,
in the horizontal plane. Consequently, the measurement function is trivially,
The state x advances according to the nonlinear model  
χ
ẋ = f (x, u), where the input u is composed of the airspeed, hvo (x) = . (6)
Vg
pitch rate, yaw rate, roll angle, pitch angle:
 T 3) Altitude Update: The altitude update is quite simple,
u = Va q r φ θ ; (2)
the information from the barometer in the autopilot directly
updates the altitude state
f (x, u) =

Vg cos(χ)
 ha (x) = pa . (7)
Vg sin(χ)
4) Ranging Update: The measurement of range between
 
 (V cos(ψ)+w )(−V ψ̇ sin(ψ))+(V sin(ψ)+w )(V ψ̇ cos(ψ)) 
 a n a a e a
the lower vehicle and the upper vehicle includes the actual

 Vg 
g
tan(φ) cos(χ − ψ) range in meters between the two vehicles, as well as the
 
 V g

 (3) GPS coordinates in latitude, longitude, and height of the
 
 0
upper vehicle. We utilize a single GPS measurement of
 
 0 
sin(φ) cos(φ) the lower vehicle, taken at the beginning of the flight, to
 
 q cos(θ) + r cos(θ)

define a common navigation frame of reference for both
 
 ṗa 
0 vehicles, thereafter we only use the lower vehicle GPS
measurements for truth data. We do not address the issue
The inputs u for this work were recorded from the Pixhawk of obtaining a common navigation reference frame between
autopilot that was purposely not connected to a GPS receiver. two vehicles for the case when GPS is never available for
More details regarding the hardware configuration are pro- the lower vehicle; in such an instance, an estimate of the
vided in Section V below. upper vehicle’s position would likely need to be added to
A. Measurement Updates the filter state and a different coordinate system utilized. The
measurement function used is
There are several measurement updates used in the de- p
veloped EKF, they include a wind pseudo-measurement, a hr (x, uu ) = (un − pn )2 + (ue − pe )2 + (ud + pa )2 ,
velocity update from the visual odometry, an altitude update (8)
where uu = [un , ue , ud ]T are the coordinates of the upper A. Analytic Observability
vehicle in the now-common navigation frame. Recall that ud
and pa point in opposite directions. The first step in assembling O in (11) is to compute the
gradient of the nonlinear measurement functions, (4), (6),
(7), (8), with respect to the state x. For reference, h1 and h2
B. Filter Implementation are from (4), h3 and h4 from (6), h5 from (7) and h6 from
The above methods were implemented in the Scorpion (8) respectively. This is found in (12) below.
Estimation Framework, created by the Autonomy and Navi- (12) provides the first 6 rows to (11); note that the
gation Technology Center (ANT) at the Air Force Institute of boldfaced h(x, u) implies that the partials for each hj (x, u)
Technology. The implementation used an Extended Kalman have been stacked to form a matrix. In (12) there is only
Filter and the data was run post-process through the filter. one state, p˙a , that does not receive any information from
All the results pertain to estimating the states of a lower UAV the measurements directly. However, the rank of (12) is only
using the filter described. six, thus we require at least three more linearly independent
equations in O to achieve local observability of the system.
IV. O BSERVABILITY A NALYSIS : VO PLUS R ANGING The second row of (11) requests the partial of L1f hj (x, u).
Using the chain rule, this becomes
The observability rank condition for nonlinear systems
T
outlined in Chapter 7 of [23], which is also summarized quite ∂L1f hj (x) ∂ 2 hj (x)

∂hj (x) ∂f (x)
well in [12], pertains only to the case where the nonlinear = 2
f (x) + (13)
∂x ∂x ∂x ∂x
equations can be expressed linearly in the control:
∂ 2 h (x)
m
X where ∂xj 2 is the Hessian of hj (x). It is useful to look at
ẋ = f (x) + ui (t)g[x(t)] (9) both of the terms in (13) to see what each part contributes
i=0 to the observability.
yj = hj (x), for j = 1 : l (10) The first half of (13) contains second-order derivatives
and is quite sparse. Only measurement functions (4) and (8)
Upon examining the models defined in Section III above, contain non-zero terms. The second order gradients of these
it is apparent that the filter model utilized does not exactly three measurements, multiplied by f (x)T , results in three
fit this constraint. The process to find the observability rank column vectors (14), (15), and (16) respectively.
condition for such a system becomes much more arduous, Equations (14), (15) and (16) are transposed and stacked,
involving many more uses of the chain rule. We have found along with three zero row-vectors for measurement func-
that by neglecting second-order and higher derivatives with tions (6) and
respect to u, whose magnitudes are usually small, the same   (7) to form the matrix representation of
T
∂ 2 h(x)
result for the observability index may be achieved, namely ∂x2 f (x) , which is not shown due to size restrictions.
Note the first two terms in (16) are non-zero. This infor-
 0 
∂Lf hj (x,u) mation, though second-order, enables the observability of
 ∂L1 h∂x the horizontal position [pn , pe ]. No second-order information
 f j (x,u) 

O≈
 ∂x
,

(11) contributes to the observability of ṗn .
..

 . 
 The second half of (13) contains combinations of only
∂Lif hj (x,u) first-order derivatives and is expressed in (17).
∂x (17) below clearly adds observability information to ṗa ,
with in the final column. However, there is not any additional in-
formation added for the horizontal position [pn , pe ], marked
L0f hj (x, u) = hj (x, u) by lack of non-zero terms in the first two columns.
∂Lif h(x, u) Analytically, by combining the two parts of (13) with (12)
Li+1
f hj (x, u) = f (x, u). we have a 12 by 9 matrix that is of rank 9. Meaning, we
∂x
do not need any higher-order Lie derivatives to form (11).
Where the notation Lf hj (x, u) denotes the Lie derivative of The reason that the observability matrix is of full rank with
the function hj (x, u) with respect to the vector field f (x, u), range to only one known landmark is that yaw information is
which results in a scalar; and i ∈ n − 1 and j ∈ l. A provided to the filter through the visual odometry method we
Lie derivative describes the information within the function have chosen to use. Without the yaw information provided,
hj (x, u) that is contained along the vector field f (x, u). Then the observability grammian would be of rank 8 and we
gradients of the Lie derivatives are used to assemble the would need at least one other known landmark to achieve full
observability index. The l = 6 measurement equations from observability. Interesting to note, however, is that if we were
Eqs.4, 6, 7, 8 form the set of hj (x, u) for the system. Note to follow a more conservative estimate of the observability
that assuming partial derivatives with respect to u are zero and ignore any second-order and higher partial derivatives
is actually a more conservative estimate than computing and in (11), we would be left with multiples of (17), as shown
including those terms. in [23], with ∂h(x)
∂x = C and ∂x ∂f
= A. (11) then can be
 − cos (χ) Vg sin (χ) 0 −Va sin (ψ)
 
0 0 1 0 0
 0 0 − sin (χ) −Vg cos (χ) 1 0 Va cos (ψ) 0 0
∂L0f h(x, u) 
 
0 0 0 1 0 0 0 0 0
= 0 0 1 0 0 0 0 0 0 ,
 (12)
∂x 
 
 0 0 0 0 0 0 0 1 0
√−un
pn e −ue
p√
0 0 0 0 0 pa
√+ud 0
σ1 σ1 σ1
where
2 2 2
σ1 = (pe − ue ) + (pn − un ) + (pa + ud ) .

0
 
 0 
g sin(χ) tan(ϕ) cos(χ−ψ)
 
 
 Vg 
 Va sin(χ)ψ̇(we cos(ψ)−wn sin(ψ)) 
2
∂ h1 (x) g cos (χ) tan (ϕ) cos (χ − ψ) + Vg

f (x) = (14)
 
∂x2

 0 


 0 


 −Va cos (ψ) ψ̇ 

 0 
0

 
0

 0 

g cos(χ) tan(ϕ) cos(χ−ψ)

 − Vg


 Va cos(χ)σ2 (we cos(ψ)−wn sin(ψ)) 
2
∂ h2 (x) g sin (χ) tan (ϕ) cos (χ − ψ) − Vg

f (x) = (15)
 
∂x2

 0 


 0 


 −Va sin (ψ) σ2 

 0 
0

V cos(χ) Vg sin(χ)

g ṗa
3/2 (∆n + 2σ1 ) + 3/2 (∆n + ∆e ) + 3/2 (∆n − ∆d )
 2σ1 σ1 σ1 
Vg sin(χ) V cos(χ) ṗa
 (∆e + 2σ1 ) + g 3/2 (∆n + ∆e ) +
3/2 3/2 (∆e − ∆ d ) 
2σ1 σ1 σ1
 
 

 0 

∂ 2 h6 (x)  0 
f (x) = (16)
 
∂x2

 0 


 0 


 0 

 ṗa (2σ − ∆ ) + Vg cos(χ) (∆ − ∆ ) + Vg sin(χ) (∆ − ∆ )
 2σ3/2 1 d 3/2 n d 3/2 e d 
1 σ1 σ1
0
where,
∆n = un − pn
∆e = ue − pe .
∆d = ud + pa .

expressed just as a linear system observability index Following this assumption, (11) would never be full rank and

C
 the system would not be observable, even though it should
 CA  be. The only information available to the filter for horizontal
Ol ≈  .  .
 
(18) position would be a nonlinear combination of pn and pe and
 .. 
CAn−1
 ∂hj (x) ∂f (x)
= (17)
∂x ∂x
Va cos(χ)σ1 σ2
− g sin(χ) tan(ϕ) cos(χ−ψ) Va cos(χ) sin(ψ)ψ̇
− Va cos(χ) cos(ψ)ψ̇ Va cos(χ)ψ̇σ3
 
0 0 Vg 2 Vg −σ4 Vg Vg σ4 + Vg 0 0
g cos(χ) tan(ϕ) cos(χ−ψ)
+ Va sin(χ) ψ̇σ2 Va sin(χ) sin(ψ)ψ̇
− Va cos(ψ) sin(χ)ψ̇ Va sin(χ)ψ̇σ3
 
0
 0 Vg Vg 2
σ5 Vg Vg Vg − σ5 0 0 

g tan(ϕ) cos(χ−ψ)
0 0 − −σ6 0 0 σ6 0 0
 
 Vg 2 ,

0
 0 Va ψ̇σ2
− Vg 2 0 − Va sin(ψ)
Vg
ψ̇ Va cos(ψ)ψ̇
Vg − VaVψ̇σ
g
3
0 0 

 
0 0 0 0 0 0 0 0 1 
−∆n cos(χ) ∆e sin(χ) −Vg ∆e cos(χ) Vg ∆n sin(χ) 2pa
0 0 √ − √ √ + √ 0 0 0 0 √+2ud
σ1 σ1 σ1 σ1 σ1

where
σ2 = we cos (ψ) − wn sin (ψ)
σ3 = wn cos (ψ) + we sin (ψ)
σ4 = g sin (χ − ψ) sin (χ) tan (ϕ)
σ5 = g sin (χ − ψ) cos (χ) tan (ϕ)
g sin (χ − ψ) tan (ϕ)
σ6 =
Vg

it would not be separable. In Section VII we analyze the times of the year, the visual seasonal differences were not
rank and condition of the observability index (11) with the significant, nor did the structures or other permanent visual
flight data for some additional insights in the observability cues change in the vicinity of the flight profile.
question for this problem.
A. 2015 Flight Test: AP
B. Average Normalized Estimation Error Squared
The flight test conducted in October of 2015 [18] utilized
Another metric that proves useful in analyzing estima- a 12 foot Telemaster model aircraft. The vehicle contained
tors is the Average Normalized Estimation Error Squared a single pixhawk running Ardupilot firmware, located at the
(ANEES) [14]. The ANEES metric shows insight into center of gravity (CG). A payload bay approximately one
whether an estimator is credible by demonstrating and quan- foot aft of the CG contained a Prosilica camera and an
tifying how much the results are optimistic or pessimistic. Intel Next Unit of Computing (NUC). The NUC collected
ANEES is defined as sensor data and filter solutions from the Pixhawk along with
1 X
M camera images. An autonomous flight profile was flown at
¯ = (xi − x̂i )0 P−1
i (xi − x̂i ), (19) approximately 250 meters above ground level (AGL) and
M i=1
resembled a large loop overflying all the available visual
where x is the true state, x̂ is the estimated state, and M is features in the area.
the number of timesteps. This flight data was used to compute the results of the AP
If the ANEES of the filter is significantly greater than n vision algorithm, found in Section VI.
(the length of x), the filter is optimistic; meaning that the
B. 2017 Flight Test: VO
covariance represented by the filter is too small. When the
ANEES is significantly under n the filter is pessimistic and The flight test conducted in July 2017 utilized two Sig
the covariance is too large. When an estimator is optimistic, Rascal 110 model airplanes with 2.8 meter wingspans, driven
it is overconfident in the estimate of the covariance bounds, by electric motors. Data was collected onboard each plane
which can lead to filter divergence. and then brought together through post processing. Details
regarding each plane are below.
V. F LIGHT T EST Data from this flight test was used to compute the results
The two data sets analyzed in this research were collected for the VO algorithms, which are used in the results discussed
from flight tests performed by AFIT in 2015 and 2017. For in Sections VI and VII.
each test, a fixed-wing small UAS was flown in either an 1) Upper Plane: The upper plane utilized a Pixhawk 2 as
autonomous or semi-autonomous mode while simultaneously the flight controller and was flown in an autonomous loiter
collecting sensor measurements and navigation solutions with a radius of 100 meters, at 18 meters per second, at an
from an autopilot. Both flights were accomplished at Himsel altitude of about 250 meters AGL. The payload consisted of a
Field at Camp Atterbury, IN, which consists of a concrete small UDOO Quad computer and an ENSCO ranging radio.
parking apron and runway, grass, trees, and various build- The computer logged data from the autopilot at about 4 Hz
ings. Though each flight was accomplished during different and ranging radio at about 20 Hz. The GPS coordinates were
up-converted to 20 Hz using a cubic-spline interpolation for B. VO Results
processing in the filter at maximum ranging rates. After the performance gain was discovered for the AP
2) Lower Plane: The lower plane was flown in a manual vision processing algorithm by simply including attitude
mode due to complications with the communications equip- information from the autopilot, we were eager to try the
ment right before takeoff. It was flown over an area directly technique on other vision navigation approaches. The VO
below the upper plane but within a span of about 500 by algorithm developed in-house was a natural fit. We investi-
500 meters for about twelve minutes, at a speed of about gated the limits of the performance improvement a bit more
22 meters per second, within an altitude band of 75 to 150 with the VO algorithm than the AP algorithm.
meters AGL. Several pertinent facts were learned or confirmed in initial
The payload consisted of two Pixhawk autopilots (one testing that deserve mentioning, which are not discussed in
disconnected from the GPS receiver, the Payload Pixhawk), the results below. We investigated using different numbers
an ENSCO ranging radio, an Allied Vision Prosilica GT1290 of features, different rates of attitude information, different
camera, a five-point ethernet switch, and an Intel NUC RANSAC tuning parameters and different rates of camera
computer. Data was logged onboard the NUC computer at imagery (over the same distance, so wider/narrower gaps).
variable rates, which proved a little challenging during post- We found that the results stayed fairly constant over the
processing: 9Hz monochrome camera images, 20Hz ranging number of features detected, up to a point; the performance
data, 140Hz GPS-denied IMU and altimeter, 5Hz GPS- obviously dropped off rapidly when little-to-no features were
denied airspeed, 4Hz GPS data. detected, as expected. We confirmed in earlier testing that
attitude information must either be time-synchronized with
VI. R ESULTS : ATTITUDE -A IDING VO the imagery or taken at a much higher rate (five-to-ten
times), otherwise the attitude aiding solution is considerably
The following results are partitioned by the vision al- worse than a vision-only solution. In changing the RANSAC
gorithms. The AP algorithm is evaluated using the flight parameters, we noticed a considerable performance change,
test data from 2015 on both the non-attitude aided and so we found parameters that maximized the performance
attitude aided solutions. The VO algorithm is evaluated using without making the solutions brittle and maintained the value
flight test data from July 2017 for both non-attitude aided constant for all our results. The largest performance effect
and attitude aided solutions. Position data provided by the was found by varying the rate of the imagery, results of
autopilots with GPS data are used as truth for evaluating the which are shown below.
results. 1) 8 Hz VO: Figure 8 shows results in a different format
than Figures 6 and 7 above. Here we show the change in
position measurements between successive images from the
A. AP Results truth in blue ”x”’s; the attitude aided VO outputs, in red
Figures 6 and 7 demonstrate the error in the north position ”+”’s; and the vision-only VO solution in black ”o”’s. A
estimates for the AP approach. Figure 6 demonstrates the perfect result would be for the black and red marks to overlay
error for the non-attitude aided solution. Figure 7 provides the blue ”x”’s. Note that these change-in-position results will
the results for the attitude-aided solution. Note the differ- not show drift, as the GPS truth data was converted to relative
ences in scale of the vertical axis between the two figures. position transformations between the image timesteps.
Comparing the two figures provides insight to the error As is shown in Figure 8, the attitude aiding solution does
results when an attitude solution is provided to the AP well at estimating the correct change in position measure-
algorithm from the autopilot. The attitude aided results are ment. Some of the non-attitude aided solutions are also
significantly improved. Not shown are the errors in the East correct, but many of the non-attitude aided solutions report
and Down directions, however they follow a similar pattern. a near-zero result. The reason for the binary behavior, where
A comparison of the overall 3D RMS error values is in the the solution is either close to correct or near zero, is that the
table below, where it is clearly seen that the RMS is reduced vision-only solution seems to have ascribed many of these
by a factor of four by including the attitude information from small position changes to attitude changes, see Section II-E.
the autopilot. This is illustrated by the distinct bias in the change in pitch
angle found by the non-attitude aided solution, compared to
TABLE I the change in attitude information between images provided
C OMPARISON OF THE 3D RMS ERRORS OVER THE WHOLE SET OF DATA by both the Pixhawk Autopilots, in Figure 9.
( TWO FIGHTS ) FOR THE A BSOLUTE P OSITIONING VISUAL NAVIGATION Table II reports the RMS errors for the North, East, and
APPROACH . Down directions between the attitude and non-attitude aided
solutions. The attitude aided solution reduces the RMS in the
Absolute Positioning RMS (m) North and East directions by more than 15 times. The VO
No Attitude Aiding 3D RMS 69.4 solutions in general, whether attitude or non-attitude aided
Attitude Aiding 3D RMS 16.2
did not estimate changes in Down (or height) very well.
2) 2 Hz VO: Figures 10 and 11 are the result of running
the VO at 1/4th the rate. Increasing the change in time
Fig. 6. Error in north position estimates using the Absolute Positioning approach and no attitude aiding, with time. East and Down error plots are similar.
Error points are in black, one standard deviation bounds marked by the red/brown dashed lines, mean at the blue dot-dashed line. The vertical dashed line
marks flight one versus flight two (data was concatenated).

Fig. 7. Error in north position estimates using the Absolute Positioning approach and attitude aiding. East and Down error plots are similar. Note the
difference in scale of the vertical axis as compared to Figure 6. Error points are in black, one standard deviation bounds marked by the red/brown dashed
lines, mean at the blue dot-dashed line. The vertical dashed line marks flight one versus flight two (data was concatenated).

TABLE II
80% decrease in the North and East non-attitude aiding RMS.
RMS OF THE CHANGES IN POSITION FOR ALL THREE DIMENSIONS ,
Also notice the solutions in both images at about 135 seconds
COMPARING THE NON - ATTITUDE AIDED RESULTS TO THE ATTITUDE
until the end, where the attitude bias is gone, and all the
AIDED RESULTS . T HE RMS CHANGES BY MORE THAN A FACTOR OF 15.
solutions are close.
Visual Odometry - 8Hz RMS (m)
TABLE III
No Attitude Aiding North RMS 136.6 RMS OF THE CHANGES IN POSITION FOR ALL THREE DIMENSIONS ,
No Attitude Aiding East RMS 77.8 COMPARING THE NON - ATTITUDE AIDED RESULTS TO THE ATTITUDE

No Attitude Aiding Down RMS 0.25 AIDED RESULTS WITH 2H Z CAMERA UPDATES .

Attitude Aiding North RMS 5.5

Visual Odometry - 2Hz RMS (m)
Attitude Aiding East RMS 5.3
No Attitude Aiding North RMS 17.2
Attitude Aiding Down RMS 0.25
No Attitude Aiding East RMS 15.7
No Attitude Aiding Down RMS 1.2
Attitude Aiding North RMS 25.3
between images increases the signal to noise of the position Attitude Aiding East RMS 24.1
measurement, allowing the filter to more easily differentiate Attitude Aiding Down RMS 1.2
between translation and rotation. This is shown in Figure 10
with the Non-Attitude aided solutions holding tighter to the
attitude-aided and truth curves. This is also shown by the It is observed that the non-attitude aided solution improves
Fig. 8. The change in position in the north direction between images for the truth (GPS) solution, in blue ”x”’s; the attitude aided VO solution, in red
”+”’s; and the VO vision-only solution in black ”o”’s. A perfect result would be for the black and red marks to overlay the blue ”x”’s. The East plot is
similar, changes in down direction were not estimated well by either the attitude aided or non-attitude aided VO algorithms.

Fig. 9. The change in attitude from image to image for the pitch angle from the truth (GPS) Pixhawk, in blue; the Non-GPS Pixhawk, in green, and the
non-attitude aided VO solution, in red ”+”. Note the strong bias of one to two degrees in the red line over time. This bias means that the VO ascribed the
difference between the two images to be a change in attitude, not position.

when the image acquisition rate decreases; found by com- provides the ”visibility” to the process to allow the algorithm
paring the RMS results in Table II with those in Table III. to extract the differences between translational and rotational
Also note that the attitude aided solution decreases in perfor- motion, giving the approach improved performance. This
mance. The non-attitude aided solution improves due to the assumption remains true as long as there is still sufficient
overall signal-to-noise increase with a wider baseline. The overlap to find corresponding features in both images.
noise on the images stays relatively constant regardless of
the image acquisition rate. However, the signal grows with a The reason for the decrease in performance of the attitude
longer baseline. This improved signal-to-noise ratio generally aided solution is due to the quality of the attitude estimates
and the fact that a slower image acquisition rate means that
Fig. 10. The change in position in the north direction with images at 2Hz for truth (GPS), in blue ”x”’s; the attitude aided outputs, in red ”+”’s; and the
vision-only solution in black ”o”’s. The East plot is similar, changes in down direction were not estimated well by either the attitude aided or non-attitude
aided algorithms.

Fig. 11. The change in attitude estimates from image to image for the pitch angle with images at 2Hz from the truth (GPS) Pixhawk, in blue; the
Non-GPS Pixhawk, in green, and the non-attitude aided VO solution, in red ”+”. Note the occasional bias in the red ”+”’s over time. This bias means that
the VO ascribed the difference between the two images to be a change in attitude, not position.

the attitudes must be accurate over a longer duration. This Also, observe the overall attitude estimation performance in
can be seen by comparing Figures 9 and 11. In Figure 9 Figures 12 and 13. There are dramatic differences over the
above, the change in attitude for the 8Hz images is estimated length of the flight. The poor performance of the ”payload”
similarly between the ”truth” Pixhawk and the ”payload” Pixhawk estimates is due to the inability to observe and
Pixhawk autopilots. The comparison between those two estimate the accelerometer biases, as no GPS information
autopilots is worse over the longer duration, in Figure 11. is available. During short interval comparisons, the attitude
Fig. 12. A comparison of the pitch angle estimated from the two autopilots over the flight. Notice that the Payload Pixhawk did not have access to GPS
information, consequently it does not have observability into the accelerometer biases, causing the attitude estimates to drift. In a comparison between this
and Figure 9, the small changes in attitude at 8Hz compare well. However, when the imagery is extracted at 2Hz, the larger change in attitude does not
align as well, noted by the performance decrease in RMS, Table III.

estimates line up, like those in Figure 9. But as the interval update rate for both VO data and Ranging data. Next we
lengthens, the errors in the attitude estimates degrade the examine some approximated ANEES results (19) for some
solution of the VO algorithm. of the filter states, which provides some insight into how
consistent the filter is performing. We do not have truth
C. Altitude Aiding Summary
data for all the filter states and some of this data had to
We have demonstrated that attitude aiding of two different be approximated, e.g. velocity information obtained through
types of vision algorithms can be highly beneficial from a a dirty-derivative of the position data; nevertheless looking at
fixed-wing small UAS platform, under 300 meters altitude. ANEES provides insight into the performance of the filter.
This is possible even when the attitude estimates are derived Some of the state estimates from the many generated are
from consumer-grade MEMs sensors and are only accurate to next examined in detail to get a better perspective on what
within a few degrees. We found that this relationship is fairly the provided overall RMS and ANEES numbers really mean.
constant over the number of features found in the images Finally, we provide numerical analysis of the Observability
(for greater than about 40 to 50 features in each image). The Index (11) throughout the flight.
slower the rate that the imagery is acquired, the tighter the
requirements on the accuracy of the attitude estimates, par- A. RMS Results
ticularly for lightweight, highly dynamic, small UAVs. Also,
it is critical to either have attitude information acquired much The filter was run across a wide range of update rates
faster than the imagery, by about an order of magnitude, or for the VO and Ranging measurements and the overall RMS
have the attitude and the imagery synchronized. If either of values were extracted for each run. The VO update rate was
these conditions is not met, an attitude aided solution will varied from 0 to 9 Hz, in integer steps. The Ranging was
give much worse results than a non-aided solution. varied from 0 to 20 Hz in integer steps and then additionally
at lower rates between 0 and 1Hz: 0.01Hz to 0.09Hz by
VII. R ESULTS : VO C OMBINED W ITH R ANGING 0.01Hz steps and 0.1Hz to 0.9Hz by 0.1Hz steps. Running
This section transitions to demonstrate the ability to bound the filter across this wide swath of measurement update rates
the covariance of VIO estimates using range measurements allows visibility into trends in the data. Insight into how the
to one other vehicle with known global pose. The same filter performance varies with the VO and Ranging update
VO algorithm, with attitude aiding, is utilized throughout rates is apparent.
the research in this section. We begin by reviewing high- We note that the filter was not tuned independently for
level results and then we will descend down into greater each of these iterations and so is not optimal at all points
detail. First we will review the filter performance using the shown. Tuning was completed using the first 300 seconds of
overall root-mean squared (RMS) error in North and East data from the flight on a few selections of VO and Ranging
directions and examine what happens as we change the update rates, predominately at 3 Hz VO and 4 Hz Ranging.
Fig. 13. A comparison of the roll angle estimated from the two autopilots over the flight. Notice that the Payload Pixhawk did not have access to GPS
information, consequently it does not have observability into the accelerometer biases, causing the attitude estimates to drift.

VO Rate and Ranging Rate With RMS Error for North

160 0
Then, the entire batch at all ranging and VO update rates 2
3
was computed using the same tuning parameters. Again, this 140 4
approach is sub-optimal but makes the analysis feasible. The 5
6
120
VO algorithm was not able to compute solutions over the 7
RMS Error(m) in North

8
entire flight for the 1 Hz and 2 Hz settings. The 1 Hz results 100 9
are excluded, as the duration was only about 130 seconds.
The 2 Hz so these results are still shown, but the data reach 80

only 552 seconds into the flight, instead of the full 720. The 60
lines for 0Hz in the figures below signify that VO information
was not used, only internal autopilot sensors and range data. 40

The datapoints at 0Hz Ranging correspond to cases where

20
no range data was utilized, only internal sensors and VO
measurements. 0
0 5 10 15 20
Figure 14 provides a glimpse of the overall RMS error in Ranging Rate (Hz)
the north position of the lower plane. The lines in the figure
correspond to different rates of VO measurements, noted by Fig. 14. The overall RMS error is shown in the North direction for position,
at different VO update rates (different lines), and for different Ranging
the legend. The x-axis refers to the Ranging measurement update rates (along X axis). Notice how the RMS error drops dramatically
update rates. Note how quickly the RMS error drops at with the introduction of range data. By about 3Hz or so, there is minimal
the beginning of the figure; it does not take a high rate of improvement for faster range measurement update rates. Note the differences
between not having VO data (0 line) and having range data (the other lines).
Ranging data to begin to bound the drift in the filter. This is The filter was largely tuned on 3Hz VO data.
shown more clearly in a zoomed-in version of Figure 14 in
Figure 15.
By about 3Hz Ranging, there is minimal improvement by
including Ranging data any faster. A similar story is found adjustment. There is also some difference in performance
in examining the East RMS error in Figure 16 below. among the different VO rates, but not by a large margin;
Figures 14 and 16 also illustrate that there is a significant 3Hz VO data is a better performer over many range rates in
difference in the performance of the filter when VO data is the two figures. Recall that tuning of the filter was completed
removed, shown by the line labeled “0” and the performance using the first 300 seconds of data from the flight on a few
with some VO data, all the remaining plot lines. This selections of VO and Ranging update rates, predominately
difference could be made more drastic by a state of the art 3 Hz VO and 4 Hz Ranging. Then, the entire batch at all
VO algorithm; the algorithm we utilize is a basic comparison ranging and VO update rates was computed using the same
of two images and does not do any advanced processing, tuning parameters. Consequently, the 3Hz line represents a
such as tracking features over multiple frames or bundle bounding of the performance of this VO system and any of
 VO Rate and Ranging Rate With RMS Error for North
0
2 are responsible for the ANEES to be higher than it ought to
55 3
4
be.
5 VO Rate and Ranging Rate With RMS Error for NEES
50
6 0
7 2
RMS Error(m) in North

45 8 9
3
9 4
40 8 5
6
7 7
35
8
6 9
30

ANEES
5
25
4
20
3

0.5 1 1.5 2 2.5 3 3.5 4 2

Ranging Rate (Hz)
1

Fig. 15. Zoomed version of Figure 14, showing the steep descent of RMS
error with range rate. It was found that even Ranging data at 0.01 Hz (once 0 1 2 3 4 5
every 100 seconds) was sufficient to bound the covariance of the filter. Also, Ranging Rate (Hz)
for clarity, the line for 0 Hz VO was removed.
VO Rate and Ranging Rate With RMS Error for East Fig. 17. This figure shows the ANEES for only the 3D position data
160 [pn , pe , pa ], at different VO update rates (lines), and for different Ranging
update rates (X axis). The line for 0Hz VO is not visible in this figure, as it
140 was zoomed to show detail on the other lines. The ANEES for a consistent
filter in this scenario would lie at 3, since there are 3 states considered
120
in this calculation. Only the 2 Hz VO line is below 3, meaning that it is
0 slightly under-confident (covariance is slightly bigger than it actually is).
RMS Error(m) in East

2
100 3
4
80 5 C. Detailed State Estimates
6
7
8
Detailed results of the filter performance for the state
60
9 estimates are discussed in this section. First we examine
40 state error estimates for the case of 5 Hz VO and 3 Hz
Ranging, one of the best cases shown in Figures 14 and 16.
20
Next the filter results at 8 Hz VO and 0.2 Hz Ranging are
0 shown to demonstrate the results with a slow Ranging update
0 5 10 15 20
Ranging Rate (Hz)
and more middle of the road performance. Next a quick
review of the position results at 4 Hz VO and 0.01 Hz
Fig. 16. The overall RMS error in the East direction is shown, at different Ranging are shown, demonstrating that bounding of the
VO update rates (lines), and for different Ranging update rates (along x filter covariance occurs even at that low Ranging rate. The
axis). Notice how the RMS error drops dramatically with the introduction
of range data. By about 3Hz or so, there is minimal improvement for faster results for 0 Hz Ranging and then 0 Hz VO are then shown
range measurement update rates. Note the differences between not having to demonstrate that the filter performs as expected at the
VO data (0 line) and having range data. The filter was largely tuned on 3Hz extremes. Overall, the results will demonstrate a converging
VO data.
but slightly overconfident filter; and most importantly, a
filter covariance that is bounded by the inclusion of Ranging
the other VO update rates could be tuned down to close to information to one other vehicle with known pose.
this performance. 1) 5 Hz VO and 3 Hz Ranging: Figure 18 shows the errors
in the state estimates of the filter over the whole flight, with
B. ANEES VO updates at 5 Hz and Ranging updates at 3 Hz. Also
Figure 17 displays the ANEES over the whole batch of shown are 3σ bounds using the estimated covariance. Note
VO and Ranging update rates. This computation of ANEES that the filter covariances remain bounded over the trajectory
only considers the 3 position states in the filter pn , pe and even though Ranging data is available to only one landmark.
pa and the associated reduced covariance. The ANEES for As discussed in Section IV, the complete observability of
all these lines should therefore be at 3 for all permutations the state is due to the combination of the yaw information
of VO and Ranging updates. On the whole, the filter is provided by the VO algorithm and the range to the one
inconsistent and the estimated covariance is smaller than the known vehicle.
true covariance actually is, with the only exception being 2) 8 Hz VO and 0.2 Hz Ranging: The errors in the states
the 2 Hz VO results, which are slightly under-confident. for a different scenario are presented in Figure 19. Here, VO
However, this inconsistency is not egregious. Detailed state data is received at 8 Hz and Ranging at 0.2 Hz. Similarities
estimates will be shown in the next section, and as will be to the the case in Figure 18 can be seen, including similar
seen, there are only a few periods of the 12 minute flight that locations in time where the filter estimates are not consistent.
 5Hz VO 3Hz Range

p
200 n 200
3 3
3 3
100 100

Error (Deg)
p n Error (m)

0 0

-100 -100

-200 -200
0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700

200 pe 200
3 3
3 3
100 100

Error (Deg)
p e Error (m)

0 0

-100 -100

-200 -200
0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700

Vg
20 p
a
20 3
3 3
10 3

V g Error (m/s)
p a Error (m)

0
0
-20
-10

-20 -40
0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700

20 p a dot 30
wn
3
20
p a dot Error (m/s)

we

(m/s)
10 3
10

e
0

wn & w
0
-10
-10

-20 -20
0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700
Time (s) Time (s)

Fig. 18. This figure displays the error in the states (except wind states - no truth information was available) along with 3σ bounds of the covariances for
the scenario of 5 Hz VO and 3 Hz Ranging. For the most part, the states are well-behaved, meaning they remain bounded by the 3σ bounds, even though
the accuracy is less than desirable. However, there are portions of the trajectory where errors in each state do exceed the bounds; ψ is particularly prone
to overconfidence.

Note the larger scale of the y-axes in Figure 19. The 3σ D. Numeric Observability
bounds are larger and the performance in pe is worse than
An evaluation of the numerical characteristics of the
before. The key point here is that the horizontal position is
observability index can be completed by an inspection of
still bounded, even at the low rate of 0.2 Hz Ranging.
the rank of the matrix (11) at all timesteps within the flight.
3) 0.01 Hz Ranging: Figure 20 demonstrates that the If (11) is not full rank, the system is not observable.
horizontal position is still bounded at the even lower rate Figure 23 shows the rank of (11) evaluated using the filter
of 0.01 Hz Ranging for 4 Hz VO; although the performance states and inputs at about 4 Hz throughout the entire flight
is quite worse than those with higher Ranging rates, as ex- for the case of 5 Hz VO and 2 Hz Ranging. Note that it is full
pected from Figure 14. This fact demonstrates that infrequent rank throughout the flight, as was expected from the analytic
ranging data from a single known location is sufficient to expression. Range to only one known landmark and the yaw
bound the drift of a VIO estimation approach for fixed-wing information from the VO are sufficient to fully constrain the
UAVs that has access to yaw and pressure information. covariance of a VIO plus pressure system onboard a fixed-
wing UAV. We note that the rank of O was also full rank for
4) Range-only Measurements: Figure 21 shows the error every other combination of VO and Ranging update rates,
in the states when VO data is not available, but Ranging with the exception of 0 Hz Ranging.
data is received at 10 Hz. Figure 21demonstrates that the
1) Condition Number: The problem with only analyzing
divergent-convergent cycle behavior exist even when VO data
the rank of (11) is that there is no additional information
is not available. Note the drop in yaw (ψ) performance,
provided, no ability to see how strong the local observability
which demonstrates that the χ update from the VO is
condition is. The conjecture we pose, without proof as yet,
providing the yaw information.
is that if the observability index is poorly conditioned then
5) VO-only Measurements: Figure 22 plots the error in the the flow of information from the measurements to the states
state for 5 Hz VO and 0 Hz Ranging. As expected, the pn will be sensitive to errors and more fragile.
and pe estimates grow without bound since those positions The condition number of a matrix gives an indication
are unobservable without the range. If the VO were not also of the sensitivity of that matrix to changes in input for
providing yaw information through the χ measurement in a computation like Ax = b. If the matrix A is poorly
(6), ψ would also be unobservable. conditioned then small changes (or errors) in the input vector
 8Hz VO 0.2Hz Range

400 p 200
n
3
3 3
200 100 3

Error (Deg)
p n Error (m)

0 0

-200 -100

-400 -200
0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700

pe
200 3
200 3
3 3
100

Error (Deg)
p e Error (m)

0
0
-200
-100

-200 -400
0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700

20 20
p
a
3 0
10

V g Error (m/s)
3
p a Error (m)

-20
0
Vg
-40
3
-10
-60 3

-20 -80
0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700

20 60
p a dot
wn
3
p a dot Error (m/s)

(m/s)
10 3 40 we

e
0 20

wn & w
-10 0

-20 -20
0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700
Time (s) Time (s)

Fig. 19. The error in the states are shown with the exception of wind states, where truth data was not available, along with 3σ bounds of the covariance
for the scenario of 8 Hz VO and 0.2 Hz Ranging. The results are fairly similar to those in Figure 18 above. Note differences in the scales on the y-axes.
The largest difference from the results in Figure 18 is the performance in pe .
4Hz VO 0.01Hz Range

1000 pn at another200time. 3
3 3
500 3 Analytically,
100
(11) was shown to be full rank in Section IV.
Error (Deg)
p n Error (m)

0 However, there 0 are one or two segments of the flight where

-500 state estimates
-100 begin to diverge and then the filter quickly
-1000
re-converges.
-200
This is clearly seen in the detailed state plots
0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700
above as well as the attached video of the 3D plots.
1000 Figure 20024 plots the condition number of (11) over the
whole flight, again for 3 Hz VO update rate and 1 Hz
Error (Deg)
p e Error (m)

500 0
Ranging. The condition is quite poor for some periods of
0 p
e
the flight.-200
3 3

-500
3 Although-400
a little difficult to see from Figure 24, the 3

0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700
minimum condition number for this scenario is 303, the mean
20
Fig. 20. The error in the horizontal position states are shown along pwith is 3.1e3 and 20 the mode is 386. There are several times in the
V
g
a 3
3σ bounds
10
of the covariance for the scenario of 4 Hz VO and 0.01 3 Hz flight when10 the condition number reaches very high (¿6e4) 3
V g Error (m/s)

3
p a Error (m)

Ranging. The position covariance is bounded, even though ranging data is and once were 0 it ascends to over 2.2e5, clearly denoting
only received
0 every 100 seconds.
that the system
-10 is approaching being unobservable from a
-10
numerical -20stability standpoint.
-20 -30
0 100 200 300 400 500 600 700 The condition 0 number
100 of (11)300throughout
200 400 the
500 flights
600 of 700

x can result in large changes (or errors) in the output vector all the different range and update rates are similar in nature,
20 p dot 30
b. The condition number is defined as a ratio of the largest 3
a
denoting that different rates of measurement are not affecting w n
p a dot Error (m/s)

3 w
(m/s)

to the10 smallest singular values of a matrix. Ideally, it should 20

the quality of the observability of the system. To improve the
e

be equal, or close to, one for a well-conditioned matrix.

e

0 10
condition number, and thus the observability of the system,
wn & w

Recall
-10
that the observability index O represents how more measurements
0
are needed. This result is not surprising,
information
-20
0
from
100
the200measurements
300
and the500prediction
400 600
model700
due to the -10 expectations going into the program. Range
0 100 200 300 400 500 600 700
flows to the states. If the rankTimeof(s)(11) is deficient it means to another landmark would provideTime this
(s) extra information,

that there is insufficient information from the measurements changing the condition of O and improving the performance.
and model to distinguish the state at one time with the state However, it is important to realize that range to a single
 0Hz VO 10Hz Range

400 p
n
200
3
3
3
200 3 100

Error (Deg)
p n Error (m)

0 0

-200 -100

-400 -200
0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700

500 pe
3
200 3
3

Error (Deg)
3
p e Error (m)

0 0

-200
-500
0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700

V
20 pa
g
3
3 60 3
10

V g Error (m/s)
3 40
p a Error (m)

20
0
0
-10 -20
-40
-20
0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700

20 p a dot 20
3
p a dot Error (m/s)

(m/s)
10 3 10

e
0 0

wn & w
-10 -10 wn
we
-20 -20
0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700
Time (s) Time (s)

Fig. 21. This figure displays the error in the states, again with exception of the wind states, along with 3σ bounds of the covariance for the scenario of
0 Hz VO and 10 Hz Ranging, in other words, VO data was not used 5Hz forVOthese results. Note differences in the scales on the y-axes.
0Hz Range

pn
2000 200
3
3 3
1000 100 3
Error (Deg)
p n Error (m)

0 0

-1000 -100

-2000 -200
0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700
pe
1500 3 200
3
3
1000
100 3
Error (Deg)
p e Error (m)

500
0
0
-100
-500

-1000 -200
0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700

20 pa 20
3
3
10 0
V g Error (m/s)
p a Error (m)

0 -20 V
g
3
-10 -40 3

-20 -60
0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700

20 p dot 40
a
3 wn
p a dot Error (m/s)

3 we
(m/s)

10
20
e

0
wn & w

0
-10

-20 -20
0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700
Time (s) Time (s)

Fig. 22. This figure displays the error in the states along with 3σ bounds of the covariance for the scenario of 5 Hz VO and 0 Hz Ranging, in other
words, Ranging data was not used for these results. The horizontal position will grow without bound in this case, as those positions are unobservable
without Ranging information.
 Rank of O over Flight
5Hz VO 2Hz Range
15
by about an order of magnitude, or have the attitude and
the imagery synchronized. If either of these conditions is
not met, an attitude aided solution will provide much worse
10 results than a non-aided solution.
The second key conclusion is that range to a single vehicle
Rank

with known global position provides a tremendous benefit

to a vehicle using a relative vision approach, like VO. The
5 range information allows the vehicle to constrain the global
drift and make the vision data begin to match up with the
true trajectory, when pressure and magnetometer data are
available. A key take-away is that even though the global
0
0 100 200 300 400 500 600 700
position is observable in this scenario, the solution is poorly
Time (s) conditioned and can exhibit regions of divergence before the
solution re-converges. It is possible that the solution at those
Fig. 23. The rank of (11) for the case of 5 Hz VO and 2 Hz Ranging points becomes multi-modal, with several positions being
updates. Note that the matrix is full rank throughout the flight, length(x) =
9. The O matrix is also full rank for every other combination of VO and near-equally likely. Perhaps the use of a multi-modal filter,
Ranging updates, with the exception of 0 Hz, or no Ranging information. like a particle filter, would alleviate some these issues. In the
future we plan to investigate the use of a particle filter on
10 4
2 this problem, through the Scorpion framework.
1.8
The third key conclusion is a follow-on to the second.
Ranging data is not needed at high rates to be effective
1.6
at constraining global drift. It was observed that rates of
1.4 0.01 Hz, or once every 100 seconds, was sufficient to bound
1.2 the filter covariance over the 12 minute flight available for
Condition

1
this data analysis. Granted, faster rates than 0.01 Hz do help
with the accuracy of the solution, but the improvement is
0.8
minimal above an update rate of about 3 Hz, for the scenario
0.6 investigated.
0.4
ACKNOWLEDGMENT
0.2

0 We would like to thank Rick Patton and Dave Thacker for

0 100 200 300 400 500 600 700
their help and expertise with the UAVs. We also acknowledge
Time (s)
and are grateful for the previous work completed by Tim
Fig. 24. The condition number of (11) for the case of 5 Hz VO and 2 Hz Machin and Ben Fain.
Ranging updates, note the exponent applied to the y-axis. Even though the
matrix is full rank throughout the flight, the matrix is poorly conditioned D ISCLAIMER
in several periods of the flight. It is not surprising, based on this result
shown here, that there are portions of the trajectory where the filter begins The views expressed in this paper are those of the authors
to diverge and then re-converges.
and do not reflect the official policy or position of the
United States Air Force, Department of Defense, or U.S.
landmark can drastically improve system performance of a Government.
GPS-denied, visually guided UAV by bounding the drift in
R EFERENCES
the planar position states.
[1] P. F. Alcantarilla, A. Bartoli, and A. J. Davison, “KAZE features,” in
VIII. CONCLUSIONS Euoropean Conference on Computer Vision, Berlin, Germany, 2012,
pp. 214–227.
Several key conclusions can be drawn from the results
[2] H. Bai and C. N. Taylor, “Control-enabled Observability in Visual-
demonstrated above. The first is that attitude data derived Inertial Odometry,” in International Conference on Unmanned Aircraft
from cheap, consumer-grade MEMS IMU sensors is suffi- Systems (ICUAS), Miami, FL, 2017.
ciently useful to improve the performance of vision naviga- [3] R. W. Beard and T. W. McLain, Small Unmanned Aircraft. Princeton
University Press, 2012.
tion algorithms. The improvement is useful for both relative [4] M. Bloesch, S. Omari, M. Hutter, and R. Siegwart, “Robust visual iner-
algorithms, such as VO, and global algorithms, such as tial odometry using a direct EKF-based approach,” IEEE International
PNP, for comparing imagery to known maps. We found Conference on Intelligent Robots and Systems, vol. 2015-Decem, pp.
298–304, 2015.
this relationship to be fairly constant over the number of [5] A. J. Canciani and A. Force, “Absolute Positioning Using the Earth s
features found. It was also found that the slower the rate Magnetic Anomaly Field,” Navigation: Journal of The ION, vol. 63,
of imagery, the tighter the requirements on the accuracy of no. 2, pp. 111–126, 2016.
[6] D. J. Carson, J. F. Raquet, and K. J. Kauffman, “Aerial Visual-Inertial
the attitude estimates are. Also, it is critical to either have Odometry Performance Evaluation,” in ION Pacific PNT. Honolulu:
attitude information acquired much faster than the imagery, Institute of Navigation, 2016.
 [7] J. Engel, J. Sturm, and D. Cremers, “Scale-aware navigation of Meeting of the Satellite Division of the Institute of Navigation,
a low-cost quadrocopter with a monocular camera,” Robotics and ION GNSS 2016, vol. 2, pp. 1305–1311, 2016. [Online].
Autonomous Systems, vol. 62, no. 11, pp. 1646–1656, 2014. [Online]. Available: https://www.scopus.com/inward/record.uri?eid=2-s2.0-
Available: http://dx.doi.org/10.1016/j.robot.2014.03.012 85017309692partnerID=40md5=c099ffec3f855b77aac407a33384a995
[8] B. M. Fain, “Small Fixed-wing Aerial Positioning using Inter-vehicle [18] T. Machin, “REAL-TIME IMPLEMENTATION OF VISION-AIDED
Ranging Combined with Visual Odometry,” Ph.D. dissertation, Air MONOCULAR NAVIGATION FOR SMALL FIXED-WING UN-
Force Institute of Technology, 2017. MANNED AERIAL SYSTEMS,” Ph.D. dissertation, Air Force In-
[9] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-Manifold stitute of Technology, 2016.
Preintegration for Real-Time Visual Inertial Odometry,” IEEE Trans- [19] I. Sa, M. Kamel, M. Burri, M. Bloesch, R. Khanna, M. Popovic,
actions on Robotics, vol. 33, no. 1, pp. 1–21, 2017. J. Nieto, and R. Siegwart, “Build your own visual-inertial
[10] C. Forster, M. Pizzoli, and D. Scaramuzza, “Appearance-based Ac- odometry aided cost-effective and open-source autonomous drone,”
tive, Monocular, Dense Reconstruction for Micro Aerial Vehicles,” submitted to IEEE RAM, pp. 1–21, 2017. [Online]. Available:
Robotics: Science and Systems X, no. 200021, 2014. http://arxiv.org/abs/1708.06652
[11] D. A. Grejner-Brzezinska, C. K. Toth, T. Moore, J. F. Raquet, M. M. [20] V. Sazdovski, A. Kitanov, and I. Petrovic, “Implicit observation
Miller, and A. Kealy, “Multisensor Navigation Systems: A Remedy model for vision aided inertial navigation of aerial vehicles
for GNSS Vulnerabilities?” Proceedings of the IEEE, vol. 104, no. 6, using single camera vector observations,” Aerospace Science
pp. 1339–1353, 2016. and Technology, vol. 40, pp. 33–46, 2015. [Online]. Available:
[12] Y. Kawano and T. Ohtsuka, Observability analysis of nonlinear http://dx.doi.org/10.1016/j.ast.2014.09.019
systems using pseudo-linear transformation. IFAC, 2013, vol. 9, no. [21] D. H. Titterton and J. L. Weston, Strapdown Inertial Navigation
PART 1. [Online]. Available: http://dx.doi.org/10.3182/20130904-3- Technology, 2nd ed. The Institution of Engineering and Technology,
FR-2041.00100 2004.
[13] R. C. Leshman, J. Gray, and J. Raquet, “Utilization of UAV Autopilots [22] D. Venable and J. Raquet, “Large scale image aided navigation,” IEEE
in Vision-based Alternative Navigation,” in GNSS+. Portland, OR: Transactions on Aerospace and Electronic Systems, vol. 52, no. 6, pp.
Institute of Navigation, 2017. 2849–2860, 2016.
[23] M. Vidyasagar, Nonlinear Systems Analysis, 2nd ed. Philadelphia,
[14] X. R. Li, Z. Zhao, and V. P. Jilkov, “Practical measures and test for
PA: Society for Industrial and Applied Mathematics, 2002.
credibility of an estimator,” Proc. Workshop on Estimation, Tracking,
and . . . , pp. 481–495, 2001.
[15] Y. Lin, F. Gao, T. Qin, W. Gao, T. Liu, W. Wu, Z. Yang, and
S. Shen, “Autonomous aerial navigation using monocular visual-
inertial fusion,” Journal of Field Robotics, no. May, pp. n/a–n/a,
2017. [Online]. Available: http://dx.doi.org/10.1002/rob.21732
[16] Y. Ling, T. Liu, and S. Shen, “Aggressive quadrotor flight using dense
visual-inertial fusion,” in Proceedings - IEEE International Conference
on Robotics and Automation, vol. 2016-June, 2016.
[17] T. Machin, J. F. Raquet, D. Jacques, and D. Venable, “Real-
time implementation of vision-aided navigation for small fixed-
wing unmanned aerial systems,” 29th International Technical