Page 1
TRACKING GROUND TARGETS WITH MEASUREMENTS OBTAINED FROM A SINGLE
MONOCULAR CAMERA MOUNTED ON AN UNMANNED AERIAL VEHICLE
by
DUSTIN DENEAULT
B.S., Kansas State University, 2006
A THESIS
submitted in partial fulfillment of the requirements for the degree
MASTER OF SCIENCE
Department of Mechanical and Nuclear Engineering
College of Engineering
KANSAS STATE UNIVERSITY
Manhattan, Kansas
2007
Approved by:
Major Professor
Dr. Dale Schinstock
Page 2
Abstract
The core objective of this research is to develop an estimator capable of tracking the states of
ground targets with observation measurements obtained from a single monocular camera
mounted on a small unmanned aerial vehicle (UAV). Typical sensors on a small UAV include
an inertial measurement unit (IMU) with three axes accelerometer and rate gyro sensors and a
global positioning system (GPS) receiver which gives position and velocity estimates of the
UAV. Camera images are combined with these measurements in state estimate filters to track
ground features of opportunity and a target. The images are processed by a keypoint detection
and matching algorithm that returns pixel coordinates for the features. Kinematic state equations
are derived that reflect the relationships between the available input and output measurements
and the states of the UAV, features, and target. These equations are used in the development of
coupled state estimators for the dynamic state of the UAV, for estimation of feature positions,
and for estimation of target position and velocity.
The estimator developed is tested in MATLAB/SIMULINK, where GPS and IMU data are
generated from the simulated states of a nonlinear model of a Navion aircraft. Images are also
simulated based upon a fabricated environment consisting of features and a moving ground
target. Target observability limitations are overcome by constraining the target vehicle to follow
ground terrain, defined by local features, and subsequent modification of the target’s observation
model. An unscented Kalman filter (UKF) provides the simultaneous localization and mapping
solution for the estimation of aircraft states and feature locations. Another filter, a loosely
coupled Kalman filter for the target states, receives 3D measurements of target position with
estimated covariance obtained by an unscented transformation (UT). The UT uses the mean and
covariance from the camera measurements and from the UKF estimated aircraft states and
feature locations to determine the estimated target mean and covariance. Simulation results
confirm that the new loosely coupled filters are capable of estimating target states. Experimental
data, collected from a research UAV, explores the effectiveness of the terrain estimation
techniques required for target tracking.
Page 3
iii
Table of Contents
List of Figures................................................................................................................................vi
List of Tables.................................................................................................................................ix
Acknowledgements......................................................................................................................... x
Dedication......................................................................................................................................xi
Acronyms......................................................................................................................................xii
Nomenclature...............................................................................................................................xiii
Chapter 1: Introduction................................................................................................................... 1
1.1 Previous Work ................................................................................................................ 1
1.2 Research Objectives........................................................................................................ 4
1.3 Estimator Design............................................................................................................. 4
1.4 Overview......................................................................................................................... 5
Chapter 2: Hardware Setup............................................................................................................. 6
2.1 Unmanned Aerial Vehicle Airframe............................................................................... 6
2.2 Piccolo II Autopilot ........................................................................................................ 7
2.3 Marlin Digital Camera.................................................................................................... 8
2.4 PC104 Stack.................................................................................................................... 9
2.5 Target Tracking Hardware Schematic.......................................................................... 10
Chapter 3: Model Development.................................................................................................... 12
3.1 Assumptions.................................................................................................................. 12
3.2 State Equations ............................................................................................................. 13
3.2.1 Aircraft Nonlinear State Equations........................................................................... 13
3.2.1.1 Aircraft Dynamic Equations............................................................................. 14
3.2.1.2 Aircraft Observation Equations........................................................................ 16
3.2.2 Feature Nonlinear State Equations............................................................................ 16
3.2.2.1 Feature Dynamic Equations.............................................................................. 17
3.2.2.2 Feature Observation Equations......................................................................... 17
3.2.3 Target Nonlinear State Equations............................................................................. 19
3.2.3.1 Target Dynamic Equations ............................................................................... 20
Page 4
iv
3.2.3.2 Target Observation Equations........................................................................... 20
Chapter 4: Observer Research ...................................................................................................... 22
4.1 Kalman Filter................................................................................................................ 24
4.1.1 Prediction.................................................................................................................. 25
4.1.2 Correction ................................................................................................................. 26
4.2 Extended Kalman Filter................................................................................................ 27
4.2.1 Modifications to Kalman Filter................................................................................. 28
4.3 Unscented Kalman Filter.............................................................................................. 29
4.3.1 Unscented Transformation........................................................................................ 29
4.3.1.1 Mean and Covariance Estimation Example...................................................... 32
4.3.2 Unscented Kalman Filter Algorithm......................................................................... 36
Chapter 5: Observability Issues.................................................................................................... 40
5.1 Feature Observability.................................................................................................... 40
5.2 Target Observability..................................................................................................... 42
Chapter 6: Novel Solution ............................................................................................................ 44
6.1 Loosely Coupled Estimation Solution.......................................................................... 46
6.2 Mean and Covariance Extraction and Inclusion........................................................... 48
Chapter 7: Simulation................................................................................................................... 49
7.1 Environment and Setup................................................................................................. 49
7.2 Feature Updating........................................................................................................... 53
7.3 Results........................................................................................................................... 54
Chapter 8: Feature Initialization................................................................................................... 58
8.1 Single Triangulation ..................................................................................................... 59
8.1.1 Analysis..................................................................................................................... 61
8.2 Ground Plane Projection............................................................................................... 64
8.2.1 Analysis..................................................................................................................... 66
8.3 Depth Conditioning....................................................................................................... 70
8.3.1 Analysis..................................................................................................................... 74
8.4 Initialization Technique Conclusions ........................................................................... 78
Chapter 9: Experimental Results .................................................................................................. 82
9.1 Feature Extractor........................................................................................................... 83
Page 5
v
9.1.1 Scale Invariant Feature Transform............................................................................ 83
9.1.2 Extraction Algorithm................................................................................................ 86
9.1.2.1 High Level Block Diagram............................................................................... 87
9.2 Feature Manager........................................................................................................... 93
9.2.1 Status Description..................................................................................................... 93
9.2.2 Updating Mean Estimate and Covariance Matrix..................................................... 94
9.3 Results........................................................................................................................... 94
Chapter 10: Conclusion and Recommendations......................................................................... 113
References................................................................................................................................... 115
Appendix A – Quaternion Background Information.................................................................. 117
Appendix B – Kalman Filter Algorithm..................................................................................... 119
Appendix C – Extended Kalman Filter Algorithm..................................................................... 120
Page 6
vi
List of Figures
Figure 21: ECat UAV.................................................................................................................... 6
Figure 22: Piccolo II Autopilot Hardware..................................................................................... 7
Figure 23: Marlin Digital Camera................................................................................................. 8
Figure 24: PC104 Stack................................................................................................................. 9
Figure 25: Hardware Component Wiring Schematic.................................................................. 11
Figure 31: Feature Observation Development Assuming Pinhole Camera Model..................... 18
Figure 41: Discrete Time Linear System and Observer Relationships ....................................... 23
Figure 42: (a) The PDF of Two Random Variables x1 and x2, and (b) the Corresponding
Probability Ellipses and Sigma Points.................................................................................. 31
Figure 43: Simple Nonlinear Example........................................................................................ 33
Figure 44: Mean and Covariance Transformation Using (a) EKF Linearization Approximation,
(b) UT Symmetric Sigma Points, and (c) Monte Carlo Sampling........................................ 35
Figure 51: The Triangulation of a Feature as Viewed from Two Different Poses...................... 41
Figure 52: The Lack of Triangulation of a Moving Target as Viewed from Two Different Poses
............................................................................................................................................... 42
Figure 61: The Relationships for the Redefined Target Observation Model.............................. 44
Figure 62: The Relationships between the SLAM UKF, UT, and the Target KF....................... 47
Figure 71: Target Tracking Estimator Simulink Diagram........................................................... 50
Figure 72: Simulation Environment (a) Top View and (b) Side View ....................................... 52
Figure 73: Feature One Position Error Magnitude...................................................................... 55
Figure 74: Aircraft Orientation Error Magnitude........................................................................ 55
Figure 75: Target Position Error Magnitude............................................................................... 56
Figure 76: Target Velocity Error Magnitude............................................................................... 57
Figure 81: Single Triangulation................................................................................................... 60
Figure 82: LC Single Triangulation Feature Initial Position Error Magnitude Plot.................... 62
Figure 83: LC Single Triangulation Feature Final Position Error Magnitude Plot..................... 63
Figure 84: LC Single Triangulation Initial Feature Covariance Analysis................................... 64
Page 7
vii
Figure 85: Ground Plane Projection............................................................................................ 65
Figure 86: SLAM Ground Plane Projection Feature Initial Position Error Magnitude Plot....... 67
Figure 87: SLAM Ground Plane Projection Feature Final Position Error Magnitude Plot ........ 68
Figure 88: LC Ground Plane Projection Feature Initial Position Error Magnitude Plot............. 68
Figure 89: LC Ground Plane Projection Feature Final Error Magnitude Plot............................. 69
Figure 810: LC Ground Plane Projection Initial Feature Covariance Analysis.......................... 70
Figure 811: Depth Conditioning.................................................................................................. 71
Figure 812: Particle Location and Histogram for a Feature after (a) One View, (b) Three Views,
(c) Five Views, and (d) Seven Views ................................................................................... 73
Figure 813: SLAM Depth Conditioning Feature Initial Position Error Magnitude Plot............. 75
Figure 814: SLAM Depth Conditioning Feature Final Position Error Magnitude Plot.............. 76
Figure 815: LC Depth Conditioning Feature Initial Position Error Magnitude Plot................... 76
Figure 816: LC Depth Conditioning Feature Final Position Error Magnitude Plot.................... 77
Figure 817: LC Depth Conditioning Initial Feature Covariance Analysis.................................. 78
Figure 818: SLAM and LC Ground Plane and Depth Conditioning Initial Feature Position Error
Magnitude as a Function of Feature Density........................................................................ 79
Figure 819: SLAM and LC Ground Plane and Depth Conditioning Final Feature Position Error
Magnitude as a Function of Feature Density........................................................................ 80
Figure 820: LC GPS/INS UKF and Feature UKF State Filters................................................... 81
Figure 91: SIFT Features............................................................................................................. 85
Figure 92: Feature Matches......................................................................................................... 86
Figure 93: Feature Extractor Block Diagram.............................................................................. 87
Figure 94: Three Image Example................................................................................................ 88
Figure 95: Match between Keypoint in Old and Previous Images.............................................. 89
Figure 96: Match between Keypoint in Previous and Current Images........................................ 90
Figure 97: Match between Keypoint in Old and Current Images................................................ 90
Figure 98: Match Relationships between a Keypoint in All Three Images................................. 91
Figure 99: Rare Keypoint Matching Error .................................................................................. 91
Figure 910: 3D GPS and Position Estimates............................................................................... 95
Figure 911: 2D GPS and Position Estimates Overhead View..................................................... 96
Figure 912: Roll Angle Estimates ............................................................................................... 96
Page 8
viii
Figure 913: Pitch Angle Estimates.............................................................................................. 97
Figure 914: Yaw Angle Estimates............................................................................................... 97
Figure 915: Georeferenced Estimator Initial North and East Feature Locations....................... 99
Figure 916: Georeferenced Final North and East Feature Locations....................................... 100
Figure 917: Aerial Image of Feature 17 .................................................................................... 101
Figure 918: Aerial Image of Features 11, 53, and 66................................................................ 102
Figure 919: Aerial Image of Features 13 and 14....................................................................... 103
Figure 920: Georeferenced Initial 3D Feature Locations......................................................... 105
Figure 921: Georeferenced Initial East and Altitude Feature Locations.................................. 106
Figure 922: Georeferenced Final 3D Feature Locations.......................................................... 107
Figure 923: Georeferenced Final East and Altitude Feature Locations................................... 108
Figure 924: 3D LIDAR Data Points and Final Feature Positions ............................................. 110
Figure 925: LIDAR Data Points and Final East and Altitude Feature Locations..................... 111
Figure 926: Average Altitude Error as a Function of the Number of Correction Updates ....... 112
Page 9
ix
List of Tables
Table 41: Mean and Covariance Matrix for Random Variables ................................................. 31
Table 42: Initial Mean Vector and Covariance Matrix................................................................ 33
Table 43: Transformed Mean Vector and Covariance Matrix .................................................... 36
Table 71: Noise Statistics............................................................................................................ 51
Table 91: Feature Manager Required Variables and Definitions................................................ 87
Table 92: Status Flag Values and Descriptions........................................................................... 93
Page 10
x
Acknowledgements
I would like to personally thank my Major Professor, Dr. Dale Schinstock, for providing me with
the opportunity to work in the AVS Lab at KSU. I have found the overall experience to be
challenging, exciting, and very rewarding. I would also like to acknowledge Dr. Chris Lewis for
his continual help with AVS Lab affairs, including personal research assistance and conference
paper collaboration. Also, many thanks go to Dr. Warren White for the mentoring, both as a
teacher and employer, he has provided me from the time I started school at KSU.
Finally, I need to thank Craig Buckley for his dedicated and persistent programming of the
PC104 Stack and integration of the digital camera. His work helped collect the experimental
data shown in this paper.
Page 11
xi
Dedication
I dedicate this paper to all of those who have ever given me a chance, including teachers,
coaches, friends, and family members.
I also want to personally dedicate this paper to Jennifer, who has and will continue to make me a
better person.
Page 12
xii
Acronyms
ARF – Almost Ready to Fly KF – Kalman Filter
AVS – Autonomous Vehicle Systems KSU – Kansas State University
BEC – Battery Elimination Circuit LC – Loosely Coupled
CAN – Controller Area Network LIDAR – Light Detection and Ranging
CCT – Cloud Cap Technology, Inc. NED – North East Down
DGPS – Differentially Corrected Global
Positioning System
DOF – Degree of Freedom
EKF – Extended Kalman Filter
FOV – Field of View
GPIO – General Purpose Input Output
GPS – Global Positioning System
GPS/INS – Global Positioning System aided
Inertial Navigation Solution
IDE – Integrated Drive Electronics
IMU – Inertial Measurement Unit
INS – Inertial Navigation Solution
OI – Operator Interface
PDF – Probability Density Function
PWM – Pulse Width Modulation
RAM – Random Access Memory
RC – Remote Control
RV – Random Variables
UAV – Unmanned Aerial Vehicle
UKF – Unscented Kalman Filter
USB – Universal Serial Bus
UT – Unscented Transformation
2D – Two Dimensional
3D – Three Dimensional
Page 13
Nomenclature
Filter Variables
xˆv  State Estimate
a
xˆv  Augmented State Estimate
zˆv  Output Estimate
χ  Sigma Points
a
χ  Augmented Sigma Points
ζ  Output Sigma Points
v  Weight of Sigma Points
u~ v  Corrupted Input Measurement
z~ v  Corrupted Output Measurement
wv  White Input Noise
vv  White Output Noise
W
x
N  Size of State Vector
xx
P  State Covariance
a
P  Augmented State Covariance
zz
P  Innovation Covariance
xz
P  Cross Covariance
Q  Input Noise Covariance
R  Output Noise Covariance
K  Kalman Gain
Φ  State Transition Model
Γ  Control Input Model
Model Variables
a pv
 Inertial Frame Aircraft Position
}{i
}{ia p&v
qv  Quaternion
 Inertial Frame Aircraft Velocity
}{if pv
fiv
 Inertial Frame Feature Position
}{b
 Body Frame Feature Image Space
Pixel Coordinates
}{itpv
 Inertial Frame Target Position
}{itp&v
 Inertial Frame Target Velocity
}{btiv
 Body Frame Target Image Space Pixel
Coordinates
}{ib
R
 Body Frame to Inertial Frame
Rotation Matrix
}{bi
R
 Inertial Frame to Body Frame
Rotation Matrix
xiii
Page 14
Subscripts and Superscripts
}{) (
⋅
b
 Body Frame Coordinate System
}{) (
⋅
i
 Inertial Frame Coordinate System
m ) (⋅
 Measurement
)(⋅  Mean
) ˆ (⋅  State Estimate
~
(⋅  White Noise Corrupted
()
1  1 −−
⋅
kk
 Corrected or Final Value from Previous Time Step
()
1 −
⋅
kk
 Predicted Value for Current Time Step
()kk
⋅
 Corrected or Final Value for Current Time Step
)
xiv
Page 15
Chapter 1: Introduction
This thesis details the development of an estimation technique which tracks the position and
velocity of ground targets with measurements obtained from a single monocular camera mounted
on an unmanned aerial vehicle (UAV). The tracking is limited to ground targets as this
constraint is necessary to deal with observability limitations associated with single vision target
tracking approaches. The well known simultaneous localization and mapping (SLAM)
estimation technique is also applied in estimating the states of the UAV and local ground
features defining the environment since it is required as an intermediate step for the solution of
this complex problem. SLAM refers to the class of optimal state estimation methods in which
both vehicle states and the environment about the vehicle are simultaneously estimated within a
probabilistic framework. Specifically, knowledge of the relationships between the UAV and the
ground features, observed as distinctive gradient changes in images, aid in the development of a
solution for the aircraft states and a digital terrain map of the UAV’s environment.
1.1 Previous Work
Many disciplines have investigated solutions to the target tracking problem for various
applications including tracking military convoys, air traffic monitoring, and surveillance
systems. Much of this work utilizes range and bearing sensors such as radar or laser scanners.
For example, both [1] and [7] modified aircraft tracking algorithms to incorporate radar
measurements taken from stationary positions. While these algorithms are readily extended to
tracking ground vehicles, the targets must be located in the vicinity of the measurement device.
For many situations, this may be simply impractical depending upon the nature of the targets
(e.g. friend or enemy). Ref. [14] provides an alternative to fixed sensor locations by assuming
moving target indicator reports, or target position measurements, are available from an aircraft
sensor. Even though the author limits this approach by only considering ground vehicles moving
in a 2D plane, the use of aircraft in tracking ground targets provides several advantages over
fixed radar stations. Many aerial vehicles have a tremendous range of operation, are not limited
by ground topography, and are often flown autonomously. In recent years, the use of UAVs for
missions which require great precision or are too dangerous for manned flight has increased due
1
Page 16
to their autonomous capabilities. However, many smaller framed UAVs are generally not
equipped with either radar or laser range finders due to weight and power limitations. For such
power and weight limited systems, cameras are the sensor of choice.
UAV mounted vision systems are appealing for several reasons and have been applied in a
variety of applications including geophysical surveying, remote sensing, ecological research, and
autonomous navigation. In general, cameras are lightweight, inexpensive, and provide
informative data that can be utilized in many different ways. While it is possible to obtain range
measurement from stereoscopic cameras, this is not a viable option on UAVs because the
stereoscopic effects are limited by the large distance the camera is located from the terrain.
Therefore, monocular cameras are predominately used aboard aircraft. The use of cameras in
tracking targets is also an emerging UAV research field. In target tracking applications, cameras
have an advantage over several ranging sensors, which are typically active and allow the target to
know when it is being observed.
However, several implications arise from the inherit projection of 3D space onto 2D image
space. The inability to resolve scale for scene reconstruction from images alone is well known
within machine vision literature. Recovery of scale requires other sensors such as the global
positioning system (GPS) that provide absolute position or control points with known locations
within the scene. Even with GPS sensors, this loss of scale continues to pose observability issues
when tracking moving targets with video. Essentially, the component of the target’s velocity
along a line between it and the camera is ambiguous or unobservable. Therefore, other
constraints or assumptions are necessary before target tracking from monocular vision is
possible. One possible solution constrains the target motion to a 2D ground plane. After taking
video images of the local terrain and targets from a UAV, the authors of [19] reconstruct the
camera poses based upon projective homography matrices developed from the camera images
and a georeferenced satellite image. The pose information for each picture provides a means to
determine a latitude and longitude position measurement of the target. While novel in some
aspects, this method is limited to flat ground planes, and when a georeferenced image is
unavailable, the estimator performance degrades rapidly. Other works appear content on using
multiple coordinated UAVs for tracking ground targets. Much of the work outlined in [13] is
2
Page 17
based upon the realization that inexpensive UAVs are often instrumented with low cost sensors
due to the high loss rate associated with autonomous flight and hostile target tracking.
Therefore, a necessity for success involves the coordinated flight and collaboration of sensor data
obtained from multiple UAVs. This includes the use of several UAVs outfitted with monocular
cameras. Multiple camera poses obtained at the same instance provide triangulation of the
target’s position in 3D space, which negates the camera scaling loss generated by any one
camera.
This thesis researches another solution to the target tracking problem. Like [19], the target is
constrained to follow the ground terrain and is captured in images obtained from a UAV
mounted camera. However, most terrain undulates in 3D space and terrain models are seldom
available to the accuracy required. Therefore, this thesis borrows some techniques of SLAM to
estimate the 3D terrain near the moving target. Unlike moving targets, stationary features are
observable using vision from a moving platform. If the ground features can be accurately
mapped, then terrain information, combined with camera states and image measurements,
provides an alternative method to determine the 3D target position measurement vector.
The SLAM problem has received substantial attention in recent years. A wide array of sensors
and configurations of sensors have been studied including binocular vision, laser scanners, radar,
and ultrasonic sensors. Currently, SLAM techniques are applied to monocular vision systems in
[2], [3], and [12]. In [2], the authors assume that an air vehicle maintains a constant altitude and
that all the features are located in a flat plane. With these assumptions, they can reconstruct the
2D location of the aircraft and a 2D map of the features. In another work involving a UAV with
monocular vision, [3], the researchers utilize artificially placed features with a known size. In
this case, the range to the fiducials is estimated directly from the image, allowing a 3D
reconstruction. Unfortunately, both [2] and [3] have synthetically modified the problem to
overcome the image scaling difficulties. In order to find 3D monocular SLAM implementations,
more literature searches were required. The research conducted by Andrew Davison et al.
explores MonoSLAM techniques as they apply to a system defined by a single camera with no
other external sensors [12]. Since their camera does not receive absolute position from a GPS
sensor, the estimation filter is initialized with a priori knowledge of a few feature locations.
3
Page 18
Provided with this initial scale, the filter is able to systematically generate the 3D location of the
camera as it varies with time and a map of features locations in 3D space.
1.2 Research Objectives
This thesis seeks to accomplish two major tasks. The first is to develop a filter that will estimate
the navigation states of a UAV and the 3D terrain feature locations defining its environment.
The model developed must accurately incorporate typical UAV inertial measurement unit (IMU)
input data and GPS and camera image output data in a recursive state estimator. This research
differs from [2] and [3] by not limiting the environment to a 2D plane and considers features
with unknown size. Since GPS is available, scale will not have to defined as in [12].
The next objective is to develop a novel technique which incorporates information obtained from
the aircraft and terrain estimator for use in tracking a ground vehicle. This requires redefinition
of the observation model to cope with the unobservability issues present with 3D target tracking
from a single camera. The solution must also adhere to statistical theory present within filter
derivations.
1.3 Estimator Design
The estimator design consists of two loosely coupled (LC) filters: one for SLAM and one for the
target. In filter terminology, LC filters obtain input and/or output measurements that are
estimates from other filters. Many GPS aided inertial navigation solution (GPS/INS) filters,
commonly used for aircraft navigation, are loosely coupled. In these filters, the output GPS
position and velocity measurements are actually estimates determined from satellite constellation
and pseudo range measurements. In contrast, tightly coupled aircraft navigation filters use the
actual satellite information as direct output measurements. In regards to the target tracking
estimator setup, LC refers only to separation of aircraft and feature states from target states,
although the SLAM filter is truly loosely coupled with respect the GPS position and velocity
measurements.
In the first filter, GPS is used to overcome the scale issue and obtain a 3D SLAM estimate of
both the UAV’s navigation states and the terrain feature locations. The results also apply when
4
Page 19
the aircraft and feature states are estimated in separate LC estimators, which may be necessary
for certain applications. Terrain features that were observed, but have gone out of view, are
discarded in order to maintain reasonable computational loads. Thus the map is only maintained
in the vicinity of the target. An unscented Kalman filter generates the estimated states and the
associated state error covariance matrix for the system. A Feature Manager handles updating the
state vector and covariance matrices as required by the addition and deletion of feature states.
The moving target’s states are housed within another state estimator that incorporates the
estimates from the first filter. An unscented transformation (UT) is employed to propagate the
means and uncertainties of the SLAM states and camera measurements into a 3D measurement
vector for the target with an estimated covariance. The 3D target position measurement allows
both the position and velocity of the target to be estimated where the accuracy of this tracking
method is directly correlated with the accuracy of the SLAM estimator.
1.4 Overview
A brief overview of this thesis is given here. Chapter 2 explains the experimental setup used for
data collection. This includes describing the airframe and the various electronic hardware
components, including sensors, used by a research UAV named the ECat. Chapter 3 details the
state equation development for the experimental setup. Chapter 4 reviews background
information and important assumptions regarding the Kalman filter family of estimators. A brief
heuristic explanation of the observability issues encountered in monocular vision approaches
follows in Chapter 5. Chapter 6 details the proposed solution, which utilizes the UT and loosely
coupled estimators. This is followed with simulation results in Chapter 7 that illustrate the
viability of the theoretical development. Chapter 8 outlines the development and analysis of
three different feature initialization techniques. Experimental terrain mapping results are given
in Chapter 9 followed by the conclusions and recommendations of Chapter 10.
5
Page 20
Chapter 2: Hardware Setup
The experimental data presented in this thesis is collected using the ECat UAV from the
Autonomous Vehicle Systems (AVS) Lab at KSU. The research focuses on target tracking using
lightweight, inexpensive instruments on a UAV. The hardware on the ECat is typical of what
might be expected on such a system. It is outfitted with a digital camera, data collection
computer, and an autopilot with the typical set of sensors that are included in inexpensive
GPS/INS and air data systems. However, the camera in the ECat is not gimbaled as might be in
UAV systems targeted in this research.
2.1 Unmanned Aerial Vehicle Airframe
A SIG Kadet Senior ARF hobbyist kit is used as the basic ECat UAV airframe. The high wing
design and approximate 2m wingspan provide stability and adequate lift. Thrust is generated by
a Hacker C50 brushless motor with a 16x10” CAM carbon composite folding propeller. Hitec
HS81 servos actuate the elevator, rudder, aileron, and flap control surfaces. The ECat airframe
is shown in Fig 21.
Figure 21: ECat UAV
6
Page 21
A few modifications to the basic airframe are necessary for hardware accommodation and
mission specific requirements. A larger lightweight carbon composite payload bay was designed
and inserted in place of the original balsa wood compartment, which relied on several cross
members for structural support. Another noticeable change includes the addition of skid style
landing gear, allowing grass prairie landings. Finally, a balsa wood autopilot and camera mount
resides in the fore compartment between the firewall and the payload bay bulkhead. This mount
is designed to keep the camera and IMU axes orthogonal as is assumed in model development.
2.2 Piccolo II Autopilot
The Piccolo II autopilot, purchased from Cloud Cap Technology, Inc. (CCT), provides the
functionality required for the ECat UAV navigation and control applications. With a mass of
233 grams and a 12.2cm x 6.1cm x 3.8cm size, the autopilot avionics unit, pictured in Fig. 22,
readily mounts within most UAV airframes.
IMU
GPS MODULE
Figure 22: Piccolo II Autopilot Hardware
A Motorola MPC555 processor, executing at 40 MHz, provides computation and communication
with five RS232 payload ports, up to ten PWM servo channels, two CAN ports, six GPIO pins,
and four analog input pins. These peripheral devices are interfaced via a 44 pin D sub connector
and a high density 25 pin microdot connector. The daughter cards are comprised of several
daughter boards including a MHX910 Datalink Radio chipset, a Motorola M12 GPS module, an
IMU, and dual ported mpxv50045 dynamic pressure and mpx4115a static pressure sensors. The
radio link allows the streaming of data to and from a ground station unit that provides a
7
Page 22
networking interface between multiple avionics units and CCT’s operator interface (OI)
software. The OI, which executes on a personal computer, displays telemetry updates and also
enables the dynamic changing of commands, gains, and flight plans. The IMU, precalibrated by
CCT, delivers three axis gyro and accelerometer readings to the processor over a serial port. The
gyros measure angular rates up to 5.2 radians per second, and the accelerometers record up to
10G accelerations. The Motorola M12 GPS unit generates an estimate of the position and
velocity of the Synergy Systems AR05 antenna. The Piccolo also supports DGPS corrections
received from the ground station. The pitot and static ports retrieve air data information vital for
true air speed estimation.
CCT also distributes free aircraft simulator and Flight Gear graphical display software as part of
their development package. This allows hardware intheloop and software intheloop
laboratory testing necessary for the extensive Piccolo II source code modifications required for
this research.
2.3 Marlin Digital Camera
High rate images are captured using a Marlin model MF201C digital camera from Allied Vision
Technologies pictured in Fig. 23. With a mass of less than 120 grams and a 5.8cm x 4.4 cm x
2.9 cm dimension (without the lens), this camera provides a practical solution for UAV vision
systems. The camera is triggered externally through a HiRose plug connector input pin. The
images, up to two Mega pixel resolution, are accessible through a 400 Mb/s Firewire A bus. The
lens is a Kowa with an 8mm nominal focal length, which generates an approximate 30° x 39°
field of view (FOV).
Figure 23: Marlin Digital Camera
8
Page 23
2.4 PC104 Stack
A PC104 stack from Kontron collects and stores images and external sensor data for post
processing. This 700 MHz Pentium 3 data collection computer was chosen for its small form
factor, minimal weight, processing capabilities, and data storage devices. A basic PC104 stack
contains a motherboard, flash to IDE converter board, and power supply. Two additional boards
were purchased from Advanced Digital Logic and installed on the stack to provide a
communication interface to avionics’ processor and the digital camera. These include a
controller area network (CAN) board and a IEEE 1394 Firewire A board, which both utilize the
PC104+ Bus. An additional 2G of removable flash memory is accessible through the USB port.
The complete PC104 stack is shown in Fig. 24.
Figure 24: PC104 Stack
A Gentoo distribution of the Linux 2.6.22 kernel and operating system provides a versatile
programmable interface to the PC104 stack hardware components. Threaded software services
the 6.2 Peak Linux CAN driver and the Firewire Linux kernel driver interrupts. The Piccolo
avionics’ data are collected, parsed, and stored in a text file located on the external memory drive
along with the compressed images from the digital camera. The Marlin camera software library
is also stored on the PC104 stack and allows camera mode changes and programmable
adjustments.
9
Page 24
2.5 Target Tracking Hardware Schematic
Fig. 25 illustrates the relationships between all of the ECat’s hardware components. An 8000
mah LiPo Thunder Power battery provides a nominal 18.5V to the system. Both 12V and 5V
BECs regulate voltage input into the Piccolo avionics unit. The Hacker motor controller, PC104
Stack, and voltage divider also have power supplied directly from the battery. The high
resistance voltage divider scales the battery voltage from 018.5V to 05V for analog input
battery voltage monitoring. The 900 MHz antenna, GPS antenna, and pitot/static tube mate
directly into their respective ports and connectors located on the front panel of the avionics’ case.
The autopilot supplies PWM signals to actuate the rudder, elevator, aileron, flaps, and motor
controller. A CAN 2.0 B bus is used to ferry relevant sensor data from the Piccolo to the PC104
Stack at 20 Hz (autopilot control law rate). CAN is used for communication instead of serial
communication for its higher bandwidth capability (up to 1 Mega baud) and its cyclic
redundancy checking. The autopilot is also responsible for triggering the camera when new GPS
data is available. This is accomplished at a rate of approximately 4Hz with one of the discrete
output pins. The PC104 Stack powers the camera and retrieves the triggered images through the
Firewire A cable.
10
Page 25
Piccolo
Avionics
44Pin D Sub
Connector
25Pin Microdot
Connector
PC104 Stack
Marlin
Camera
Input Trigger
Battery
GPS
Firewire A
CAN 2.0 B
Antenna
900 MHz
Air Data
Pitot
Static
BEC
8000 mah LiPo
Voltage
Divider
12 V
18.5 V
Firewire A
BEC
5 V
Actuators
Motor
−
−
1
2
23
8
Pin
Pin
Pin
Pin
26
11
40
−
−
Motor Controller
Flaps
Aileron
Elevator
Rudder
PWM
−−30
−15
−44
Pins
Pins
Pins
−
−
−
21
8
13
24
1
Pin
Pin
Pin
Pin
Pin
−
−
−
12
22
Pin
Pin
Hi
Lo
Gnd
−
−
−
Gnd
OI /
05 V
018.5 V
Figure 25: Hardware Component Wiring Schematic
11
Page 26
Chapter 3: Model Development
An integral step in simulation and estimator development includes defining a set of realistic
assumptions that accurately describe the actual hardware. In this thesis, the setup consists of
modeling the relatively small ECat UAV flying at low velocity and altitude over varying terrain.
Sensor data are available from an IMU, a GPS module, and a monocular digital camera.
3.1 Assumptions
The following list details important assumptions used for model development.
• The aircraft’s initial position, or base station, defines the location of the inertial North
East Down (NED) frame, . This assumption is valid for highly maneuverable low
velocity aircraft flying over a small area.
}{i
• The gravity vector will be constant during the flight. Its orientation will be aligned with
the “down” axis of .
}{i
• The three angular rate sensors and three accelerometers within the IMU are located at the
center of mass of the aircraft and are aligned with the axes of the body fixed reference
frame, . The GPS antenna is near the center of mass.
}{b
• The camera is located at the center of mass pointing down along the z axis of the body
frame. Therefore, the camera frame is coincident with the body frame.
• The intrinsic parameters of the camera are known including the focal length, image
center, skew coefficient, and radial and tangential distortions.
• A scale invariant distinctive feature based extraction algorithm exists for locating features
within discrete images and for specifying the corresponding image coordinate locations
from overlapping images.
• Targets are distinguishable from stationary features in images. However, no other
properties about the target are known.
• The targets are limited to ground targets that must remain in contact with local ground
terrain.
12
Page 27
3.2 State Equations
As is typical for GPS aided inertial navigation solutions, kinematic equations rather than kinetic
equations are used. This allows for estimation of the aircraft and target states without knowledge
of the intrinsic parameters of the aircraft and target. Therefore, the resulting state estimation
technique is less dependent on the specific application. Kinematic relationships generally tend to
decrease the number of states necessary for estimation while providing a quantifiable means for
determining output and process noise.
For the development, three sets of kinematic equations are required: one for the UAV, one for
the features, and one for the target. The general form for these nonlinear state equations is given
by
),
v
,
v
(
v
wuxfx
vvv
v
&v=
v=
(3.1)
where xv is state vector, is the input vector,
the zero mean white output noise vector, and zv is the output vector. In the following discussion,
),(
vxh
wv is the zero mean white input noise vector, vvis
z
(3.2)
uv
(3.1) will be referred to as the dynamic model as it defines the relationship between the
derivative of the state vector and the nonlinear vector function, ) (⋅
fv
, of the states, inputs, and
process noise. Equation (3.2) will be referred to as the observation or output model where the
v
nonlinear output vector function, , defines the combination of states and output noise that
form the output vector.
) (⋅
h
3.2.1 Aircraft Nonlinear State Equations
The definitions for the aircraft state vector, input vector, and output vector are shown below.
The “a” subscript denotes aircraft states.
⎥
⎦
⎥
⎥
⎤
⎢
⎣
⎢
⎢
⎡
≡
q
p
p
&v
x
ia
ia
a
v
v
v
}{
}{
(3.3)
⎥⎥
⎦
⎤
⎢⎢
⎣
⎡
≡
}{
}{
b
b
a
a
ωv
u
v
v
(3.4)
13
Page 28
⎥⎥
⎦
⎤
⎢⎢
⎣
⎡
≡
}{
}{
ia
ia
a
p
p
&v
z
v
r
(3.5)
The state vector, , consists of the UAV’s inertial position vector,
a xv
}{ia pv
, inertial velocity
vector,
}{ia p&v
, and the quaternion vector, qv, where
[]T
qqqqq
3210
≡
v
. (3.6)
The input vector, , contains the body axes accelerations,
a
uv
}{b
av
, and angular rates,
}{b
ωv
. The
output, , contains the inertial position vector,
a zv
}{ia pv
, and inertial velocity vector,
}{ia p&v
.
Quaternions are used to represent the orientation between the inertial and body fixed frames
instead of their Euler angle counterparts. This avoids the singularity condition apparent with
Euler angle attitude representations. A detailed description of the quaternion vector elements,
quaternion to Euler angle conversions, and the quaternion inertial to body frame rotation matrix
is located in Appendix A.
3.2.1.1 Aircraft Dynamic Equations
The aircraft dynamic equations are given by
a x&v, the time derivative of the state vector. The
time derivative of position within the inertial frame,
}{ia p&v
, is simply the inertial velocity vector.
}{}{)
i
(
iaa
pp
dt
d
&v
v
=
(3.7)
The time rate of change of the velocity vector,
)(ia p& &v
, represents the true inertial kinematic
accelerations. However, the accelerometers located on the body frame will measure the true
av
, corrupted with components of zero mean white noise, kinematic accelerations,
}{b
}{ba
wv
, and
the body frame representation of the constant magnitude gravity vector, g . Hence, the
acceleration measurement is given as
[]
T
ibbabbm
gwaa
00
}{}{}{}{
R
−+=
vvv
(3.8)
where is the body frame to inertial frame rotation matrix. The sign of the zero mean noise
term is arbitrary. In (3.8) and subsequent developments, the noise terms are added to the true
}{ib
R
14
Page 29
states strictly for notational consistency. The true inertial accelerations, , are obtained by
solving (3.8) for the true body frame accelerations and rotating this vector into the inertial frame.
v
& &v
=
(
bm ibia
wap
}{}{}{
−=
R
}{i
av
}{}{}{}{
b ib
) [
}
+
iia
aap
v
R
=
(3.9)
]
T
ba
g
00
{
vv
& &v
(3.10)
The other IMU input contains the angular velocity vector measurements from the rate gyros,
ωv
. In accordance with the accelerometers, this measurement contains the true angular
}{bm
velocities and zero mean white noise,
}{bm
v
wv
.
}{}{}{
bbbm
wω
ωω
v
v
+=
(3.11)
The relationship between the time derivatives of the quaternion states, whose elements are
functions of the rotation matrix, and the gyro measurements is known as the “strapdown”
equations [6]
) )((
2
1
}{b
qq
ωv
&v
Ω
=
(3.12)
where
. (3.13)
⎥
⎦
⎥
⎥
⎥
⎤
⎢
⎣
⎢
⎢
⎢
⎡
−
−
−
−−−
=
012
103
230
321
)(
qqq
qqq
qqq
qqq
q
Ω
Solving (3.11) for the true angular velocity vector and substituting into (3.12) results in
))((
2
1
}{}{
bbm
wqq
ω
ω
v
v
&v
−=
Ω
. (3.14)
The concatenation of equations (3.7), (3.10), and (3.14) results in the nonlinear aircraft dynamic
equation vector.
() [
+
]
(
ω
)
⎥
⎦
⎥
⎥
⎥
⎥
⎤
⎢
⎣
⎢
⎢
⎢
⎢
⎡
−
−=
⎥
⎦
⎥
⎥
⎤
⎢
⎣
⎢
⎢
⎡
=
}{}{
}{}{}{
}{
}{
}{
)(
2
1
00
v
bbm
T
babmib
ia
ia
q
ia
a
wq
gwa
p
p
p
& &v
x
ω
v
vv
&v
&v
&v
&v
Ω
R
(3.15)
15
Page 30
3.2.1.2 Aircraft Observation Equations
The output measurements given by the GPS includes estimates of both inertial position,
}{i am
pv
,
and inertial velocity,
}{i am
p&v
. Both of these measurement vectors contain the true inertial position
and velocity and their corresponding noise components,
}{ipa
v
ν
νv
and
}{ipa&
v
ν
.
⎥
⎦
⎤
⎢
⎣
⎡
ν
−
⎥
⎦
⎤
⎢
⎣
⎡
=
⎥
⎦
⎤
⎢
⎣
⎡
=
}{
}{
}{
}{
}{
}{
ip
&
ip
iam
iam
ia
ia
a
a
a
p
p
&v
p
p
&v
z
v
r
v
v
(3.16)
3.2.2 Feature Nonlinear State Equations
The following definitions list the state vector, input vector, and output vector associated with the
features. The “f” subscript denotes feature states.
⎥
⎦
⎥
⎥
⎤
⎢
⎣
⎢
⎢
⎡
≡
f
N
f
v
i
if
M
f
p
p
x
}{
1
}{
v
v
v
(3.17)
⎥
⎦
⎥
⎥
⎤
⎢
⎣
v
⎢
⎢
⎡
≡
0
0
vM
v
f u
(3.18)
⎥
⎦
⎥
⎥
⎤
⎢
⎣
⎢
⎢
⎡
≡
f
N
{
bf
bf
f
i
i
vM
z
}
1
}{
v
(3.19)
The feature state vector, , in (3.17) contains the feature vector locations, , relative to the
NED inertial frame. The number of estimated features may vary with time as new features come
into observation and other features go out of observation. The superscripts are used to identify
the various features where denotes the total number of features comprising the state vector at
any given time. The variable size input vector,
fxv
}{i
f pv
f
N
f uv, in (3.18) is all zero vectors as the features are
fiv
stationary. The output vector, , contains the image space vector,
fzv
}{b
, for each feature. Each
consists of two elements which define the measured x and y components of a single image
space feature representation relative to .
}{bfiv
}{b
16
Page 31
3.2.2.1 Feature Dynamic Equations
Features are assumed stationary, which implies the time derivative of the feature state vector,
fx&v
&v
=
ff
px
, is zero.
0
}{
v
&v
=
i
(3.20)
3.2.2.2 Feature Observation Equations
A monocular camera installed along the body axes of the aircraft captures images at periodic
intervals. Unfortunately these images do not provide a depth measurement to the feature located
in 3D space, but instead capture its image space coordinates. Therefore, the feature observation
equations need to determine the relationships between the aircraft position and orientation, the
intrinsic camera parameters, and the feature’s position that result in its image coordinate
representation. Fig. 31 depicts the relationships between the inertial frame, the camera frame,
and a feature viewed through a pinhole camera model.
17
Page 32
}{b
}{i
}{bxi
}{byi
lf
}{b
cv
}{ia pv
}{if pv
Figure 31: Feature Observation Development Assuming Pinhole Camera Model
The vector, , from the camera frame to the feature, is given by the following equation
}{b
cv
)(
}{}{}{}{
iaifbib
ppc
vvv
−= R
(3.21)
where the rotation matrix, , transforms the inertial frame representations of the feature and
aircraft into the body frame. The image space feature coordinates are realized when the z
cv
component of is equal to the focal length, , of the camera. Therefore, the actual images
coordinates and of a feature are computed as follows.
}{bi
R
}{b
lf
}{bxi
}{byi
[]
}{
}{
}{
}{
}{
010
001
*
100
b
b
l
by
bx
bf
c
c
f
i
i
i
v
v
v
⎥⎦
⎤
⎢⎣
⎡
=
⎥
⎦
⎤
⎢
⎣
⎡
=
(3.22)
18
Page 33
The measured image space coordinate vectors,
}{bfm
iv
, consist of the true image coordinate vector,
, plus a noise vector,
}{bfiv
}{bif
νv
, due to camera calibration and feature extraction errors.
Therefore, the true measured image space coordinates are given by
⎥
⎦
⎥
⎥
⎤
⎢
⎣
⎢
ν
⎢
⎡
ν
−
⎥
⎦
⎥
⎥
⎤
⎢
⎣
⎢
⎢
⎡
=
⎥
⎦
⎥
⎥
⎤
⎢
⎣
⎢
⎢
⎡
=
f
f
f
ff
N
bi
bi
N
fmb
b fm
N
{
bf
bf
f
i
i
vM
i
i
vM
z
}{
1
}{
}{
1
}{
}
1
}{
v
M
v
vv
r
. (3.23)
An implication of the feature observation model exists when the z component of becomes
negative. This negative value switches the signs of the pixel coordinates. The xy plane of the
body axes defines this “singularity” region. In fact, if the z component of approaches zero,
the pixel coordinates and become infinite. In reality, this is never a concern as the field
of view of the camera is less than 180 degrees. However, in filter implementations, this is
possible as several of the estimated states, especially orientation, used in (3.21) and (3.22)
contain a high degree of uncertainty. In practice, this issue is addressed by eliminating image
updates during periods of poor orientation estimates, which usually occur during filter
initialization.
}{b
cv
}{b
cv
}{bxi
}{byi
3.2.3 Target Nonlinear State Equations
Like the aircraft, the target state equations consist of kinematic relationships. However, unlike
the aircraft model, no direct acceleration measurements are available from the target. One
commonly used motion model assumes the velocity of the target is a random walk with
acceleration modeled using zero mean Gaussian white noise. For this technique, a larger
standard deviation in acceleration noise corresponds to a target that is more maneuverable. This
results in greater uncertainty in the target states predicted by the model, thereby weighting the
observation more heavily than the prediction in the state estimation process. In this thesis, the
target velocity is modeled as a random walk with unchanging noise statistics and dynamic
model. For highly maneuverable targets, the Interacting Multiple Model (IMM) estimator can
provide improved accuracy at the expense of increased computational complexity. The
interested reader should consult [1], [7], [8], and [14]. The target tracking concepts developed in
19
Page 34
this thesis pertain equally to both the IMM target estimator and the basic single model estimator
used for the remainder of this development.
The following equations represent the target state vector, input vector, and output vector. The
“t” subscript denotes target states.
⎥⎥
⎦
⎤
⎢⎢
⎣
⎡
≡
}{
}{
it
it
t
p
p
&v
x
v
v
(3.24)
}{itt
w
v
u
vv≡
(3.25)
}{btt
iz
r≡
(3.26)
In (3.24), the target state vector, , contains the target position vector,
txv
}{itpv
, and velocity vector,
, relative to the inertial frame. Since the velocity is modeled using a random walk, the input
vector for the target model, , is a noise term,
}{itp&v
tuv
}{it wv
. The target output vector, , consists of
the camera sensor vector, , which contains the two image space coordinates of the target.
tzr
}{btiv
3.2.3.1 Target Dynamic Equations
The time derivative of the target state vector,
tx&v, is a concatenation of the velocity and
acceleration terms.
}{}{)
i
(
itt
pp
dt
d
&v
v
=
(3.27)
}{}{
itit
wp
v
& &v
=
(3.28)
Therefore, is written compactly as
tx&v
⎥⎥
⎦
⎤
⎢⎢
⎣
⎡
=
⎥⎥
⎦
⎤
⎢⎢
⎣
⎡
=
}{
}{
}{
}{
it
it
it
it
t
w
p
v
p
p
& &v
x
&v
&v
&v
. (3.29)
3.2.3.2 Target Observation Equations
The target observation model is very similar to the feature observation model since the camera
image provides the only output measurement for the target. The image space coordinate vector,
tiv
}{i
vv
= R
, is given by (3.22) when in (3.21) is replaced with
}{bf pv
}{itpv
.
)(
}{}{}{}{
iaitbib
ppc
v
−
(3.30)
20
Page 35
[]
}{
}{
}{
}{
}{
010
001
*
100
b
b
l
by
bx
bt
c
c
f
i
i
i
v
v
v
⎥⎦
⎤
⎢⎣
⎡
=
⎥
⎦
⎤
⎢
⎣
⎡
=
(3.31)
The target measurement pixel measurement vector,
}{btm iv
, consists of the true image space vector,
, positively perturbed by a noise vector,
}{btiv
}{bitvv
, for similar reasons as described for the feature
observation equations.
}{}{}{
bibtm
i
btt
tviz
v
vv
v
−==
(3.32)
21
Page 36
Chapter 4: Observer Research
The standard procedure for estimating the states of a system involves implementing an observer.
Observers utilize dynamic/observation models, input, and error feedback during state estimation.
The robustness of the observer is highly correlated with how accurately the true system is
modeled. In theory, complete knowledge of a linear system model allows for any desired pole
placement for the observer. This is possible since the observer is implemented in software,
which negates any physical limitations. In actuality, unmodeled dynamics and noisy input and
output data limit the achievable bandwidth of the state estimator. The state equations developed
in the previous sections are no exception. The input and output sensors are all assumed to
include zero mean, white noise components. The Kalman filter family of estimators considers
the noise statistics, current estimate uncertainty, and state equations during state estimation.
Background information regarding the Kalman filter (KF), extended Kalman filter (EKF), and
unscented Kalman filter (UKF) is presented in this chapter.
A typical discrete time observer setup and its relationships with a linear time invariant system are
shown in Fig. 41. The continuous time linear state equation model for this system is given by
&v
F +=
v
=
where xv is the state vector, u is the input vector, and
uxx
vv
G
v
(4.1)
xz
H
(4.2)
v
zv is the output vector. This system is
described in discrete time by
11
−−+=
kkk
uxx
vvv
ΓΦ
v
(4.3)
kk
xz
v
H
=
. (4.4)
Φ and are often referred to as the state transition model and control input model,
respectively. Their actual values may vary slightly depending upon the discretization method
used and the integration timestep.
Γ
22
Page 37
H
System
Observer
1
−
k uv
Γ
kxv
kzv
Φ
1
−
kxv
1
−
k
wv
1
~
−
k uv
kvv
kz~ v
kzˆv
k
K
1 1
− k
ˆ
−
kxv
1
ˆ
−
kkxv
kkx
ˆv
Γˆ
Φˆ
Hˆ
Figure 41: Discrete Time Linear System and Observer Relationships
The observer block contains additional complexities. The observer receives input,
1
~
−
k uv
wv
, and
output,
kz~ v, measurements that are corrupted with their respective noise components,
1
−
k
and
. The major difference between the observer and the system models is the correction step.
The error between the observer output,
kvv
kzˆv, and the actual system measured output,
kz~ v, is
multiplied by a feedback gain and used to correct the predicted state estimate, .
1
ˆ
−
kkxv
From another perspective, the observer illustrated in Fig. 41 is estimating the system defined by
(4.1) and (4.2), which can be rewritten as
111
~
u
−−−
−+=
kkkk
wxx
vvvv
ΓΓΦ
(4.5)
kkk
vxz
vv
v
~
+= H
(4.6)
where
23
Page 38
111
~
u
−−−
v
~
+=
kkk
wu
vvv
(4.7)
kkk
vzz
v
v
+=
. (4.8)
wv
These equations take the standard form for the discrete time KF when the process noise,
1
−
k
,
originates from the input source. Various forms of the KF equations exist, including those that
take into consideration plant modeling and/or plant disturbance errors. While only input noise
disturbances are considered here due to the structure of the model equations previously
developed, the concepts are readily extendable to the other cases.
4.1 Kalman Filter
The KF can be interpreted as a recursive minimum least squares estimator, which optimally
determines the states of a linear system with input, output, and/or process noise. It is optimal in
the sense that it minimizes the trace of the estimated state error covariance matrix. Therefore,
the foundation of the filter relies upon accurate modeling of the propagation of the mean and
covariance matrix of random variables (RV) through linear transformations. This is
accomplished by recalling the linear transformation properties for RVs. Suppose that a vector,
zv
v
=
{} ⋅
E
{ }
zE
=
v
=
zv
(
zE
−=
P
(
xE
HP
−=
, is related to a random vector,
xv, through the linear transformation,
v
H
.
H
xz
(4.9)
Taking the expected value, , of (4.9) results in an expression for the mean.
{}
xE
H
v
H
v
v
(4.10)
xz
(4.11)
The covariance matrix, , for is found by its definition.
zz
P
)(
)(
)
)
{
v
}
T
zz
zzz
vvvv
−
{}
T
T
zz
xxx
H
vvv
−
(4.12)
This is simply
Txxzz
HHPP
=
(4.13)
where the covariance matrix is defined by
xx
P
()()
{}
T
xxxxE
vvvv
−−
. These principles are used in
the KF to determine the mean and covariance propagation of RVs through linear dynamic and
observation models. The Kalman gain, K , is then determined which minimizes the trace of the
estimated state error covariance matrix. If the RVs have Gaussian probability density functions
24
Page 39
(PDF), then they are fully described by their corresponding means and covariance matrices, and
the KF provides the optimal solution. A weaker condition occurs when the input and
measurement noises are white and zero mean, but not necessarily Gaussian. In this case, the KF
provides the optimal linear solution [15]. The filter accomplishes these tasks in two distinct
phases, prediction and correction.
4.1.1 Prediction
During the prediction phase, both the current estimated state mean and error covariance matrix
are updated. The new state estimate
1
ˆ
−
kkxv
is given by
11 1
−
1
~
u
ˆ
x
ˆ
x
−−−
+=
kkkkk
vvv
ΓΦ
. (4.14)
Essentially, (4.14) represents the time integration of the dynamic equation with known initial
~
−
. The next step involves calculation of the updated state conditions and corrupted input,
1
k uv
error covariance matrix. This new covariance matrix , , is given by its definition.
)
k
x
xx
k
 −
k
1
P
( )(
{}
T
kkkkk
xx
k

k
x
ˆ
xx
ˆ
E
vvvv
−−=
−−−
111
P
(4.15)
Substituting in the dynamic model, (4.3), for
kxvand
1
ˆ
−
kkxv
) (
Φ
gives
()
()
()
()
()
{}
T
kkkkkkkk
xx
k

k
wxx
ˆ
wxx
ˆ
E
1111111
−−−−−−−
+−+−=
vvvvvv
ΓΓΦP
(4.16)
where
111
~
u
−−−
−=
kkk
uw
vvv
. (4.17)
Since is uncorrelated with the other terms in (4.16),
(
kkkk
xE
1  11
−−−
−=
ΦP
can be written as
{
k
wE
1
−
Γ
1
−
k
wv
xx
k
 −
}
+
k
1
P
)( ) (
Φ
)()
{
()( ) ((
Γ
))
}
T
k
T
kkkk
xx
wxx
ˆ
x
111 1
−
1
ˆ
v
−−−−
−
v
vvvv
. (4.18)
Realizing the linear properties of covariance, (4.18) is simply
(4.19)
T
k
Txx
k
−
k
xx
k

k
ΓΓQΦΦPP
11  11
−−−
+=
where
()()
{}
T
kkkkkk
xx
k
−
k
xx
ˆ
xx
ˆ
E
11
}
 1
−
11
{
 1
−
1 1
−−−−−
−−=
vvvv
P
(4.20)
( )()
T
kkk
wwE
111
−−−=
vv
Q
. (4.21)
25
Page 40
This series of prediction steps occurs at the rate of new input data. Note that the development
applies to both linear time invariant systems and linear time varying systems, although only time
invariant systems are shown here for notation simplicity.
4.1.2 Correction
When an output measurement is available, the estimated state is updated by
v
)
ˆ
x
~
z
(
ˆ
x
ˆ
x
11
−−
−+=
kkkkkkkk
v
v
v
HK
(4.22)
where
1
ˆ
x
~
z
−
−
kkk
v
v
H
(4.23)
is the measurement residual or error. The measurement vector,
kz~ v, also contains zero mean
white noise, .
kvv
kkk
vxz
vv
v
~
+= H
(4.24)
This correction results in a new posterior estimate covariance matrix, , which is defined by
xx
k
k
P
()()
{}
T
kkkkkk
xx
k

k
xx
ˆ
xx
ˆ
E
vvvv
−−=

P
. (4.25)
Substituting (4.22) and (4.24) into (4.25), expanding the terms, and simplifying the expression
using covariance properties results in an equation for
xx
k
k
P
()()
T
kkk
T
k
xx
k

kk
xx
k

k
KRKHKIPHKIP
+−−=
−1
(4.26)
that is a function of I (Identity Matrix), , , , and
}
.
, where
k
K
H
{
xx
k
 −
k
1
P
k
R
( )( )
k
v
T
kk
vE
r
v
=
R
(4.27)
Up to this point, the Kalman gain determination has not been addressed. However, the
covariance, or uncertainty, of the updated state estimate is a function of this gain. Therefore, this
gain is chosen to minimize the expected error, {
E
}
kkk
xx
ˆ
vv
−

, which is the trace of . Setting
the partial of the trace of with respect to the Kalman gain equal to zero results in an
expression that is solved for the optimal gain.
xx
k
k
P
xx
k
k
P
(4.28)
1
11
)(
−
−−
+=
k
Txx
kk
Txx
kkk
RHHPHPK
The posterior error covariance matrix update is performed by using this gain.
(4.29)
xx
k

kk
xx
k

k
xx
k

k
11
−−−=
HPKPP
26
Page 41
A compact algorithm for the KF is located in Appendix B.
Several intuitive properties result from the Kalman gain and its calculation given in (4.28). The
gain is entirely determined by the current state error covariance matrix, the output matrix, and
the measurement noise statistics, where the current uncertainty is found with the dynamic model
and input noise statistics. In theoretical applications where noise is not considered, the poles of
the observer are placed to provide a very quick transient response to disturbances and incorrect
initial state estimates. For systems that have noisy input and output measurements, choosing a
feedback gain in this manner results in a suboptimal estimate that may even diverge. However,
the optimal Kalman gain determines the pole location based upon the current uncertainty. This
uncertainty essentially defines the feedback gain, and thus the bandwidth, of the system. An
interesting characteristic also arises when the measurement vector is noise free and the output
matrix is square. In this case, the gain becomes
. (4.30)
1
−
= HKk
The use of this gain in (4.22) forces the new current state estimate to be updated in one
correction to the actual state, as observed through the observation model, and sets the posterior
covariance matrix equal to the zero matrix. In other words, an uncorrupted measurement vector
update mitigates any doubt about the uncertainty of the state estimates when the observation
matrix is square and nonsingular. Another interesting characteristic happens when the current
state error covariance matrix tends toward the zero matrix. When this happens, the current state
mean estimates are very likely to be near the actual states. Equation (4.28) then determines a
feedback gain that is also near zero since the innovation covariance, , is pre
multiplied by a small magnitude matrix. A null feedback gain emphasizes the fact that the
system mean is near the true mean and any noisy measurement should therefore have very little
or no effect on the current estimates.
k
Txx
k

k
RHHP
+
−1
4.2 Extended Kalman Filter
The KF provides an optimal state estimate for linear systems. However, most practical systems,
including the developments in this thesis, are defined by nonlinear state equations of the form
given in (3.1) and (3.2). The problem of estimating the predicted mean and covariance from
nonlinear functions has been addressed by many filtering techniques including the EKF. The
27
Page 42
following equation shows the first several elements of a Taylor series expansion of a nonlinear
v
function about an operating point defined by the mean of a random variable,
(
(
v
vv
⎠⎝
If the second and higher order terms of the expansion are considered negligible, then zv is
) (⋅
h
xv.
( )
x
( )
∂
x
)
)()
( )
x
2
(
∂
)
() L
v
x
v
x
v
x
v
v
h
v
x
v
x
v
x
v
x
v
x
v
h
v
v
h
v
z
v
x
v
x
+−
⎟
⎠
⎟
⎞
⎜
⎝
⎜
⎛
∂
−+−
⎟
⎟
⎞
⎜
⎜
⎛
∂
+=
==
T
xx
2
2
1
(4.31)
approximated by
( )
x
( )
x
∂
()
(
xx
xh
hz
xx
vv
v
v
v
v
v
v
vv
−
⎟
⎠
⎟
⎞
⎜
⎝
⎜
⎛
∂
+≈
=
). (4.32)
With this assumption, the new mean is predicted using the nonlinear function, and the new
covariance is found by taking expected value of the outer product of (4.32)
( )
x
)(
z
hz
{
v
v
v=
(
z
(4.33)
)
}
T
zz
zzE
vvvv
−−=
P
(4.34)
( )
x
∂
()
( )
x
∂
()
T
xx
xx
xx
zz
xhxh
⎟
⎠
⎟
⎞
⎜
⎝
⎜
⎛
∂
⎟
⎠
⎟
⎞
⎜
⎝
⎜
⎛
∂
=
==
vvvv
v
v
v
v
v
v
PP
(4.35)
where
( )()
{}
T
xx
xxxxE
vvvv
−−=
P
. (4.36)
4.2.1 Modifications to Kalman Filter
The KF equations require modifications to employ the mean and covariance estimation
techniques of the EKF. At each time step the following Jacobian matrices must be evaluated.
v
F
),
ˆ
x
(
1, 1
−
 1
−−
==
∂
∂
≡
kkk
uux
k
x
f
v
vvvv
(4.37)
),
ˆ
x
(
1, 1
−
 1
−−
==
v
∂
∂
≡
kkk
uux
k
w
f
v
vvvv
v
G
(4.38)
)
ˆ
v
(
1 −
k
=
∂
∂
≡
k xx
k
x
h
v
v
H
(4.39)
These Jacobian matrices replace their counterparts in equations (4.14) through (4.29) with two
exceptions. The mean prediction based upon the dynamic model is often accomplished with a
28
Page 43
higher order integration technique, such as a Runga Kutta algorithm, which utilizes the nonlinear
dynamic model given by (3.1). Also, the
1
ˆ
−
kkxv
H
term in (4.22), (4.23), and (4.24) is replaced
with the nonlinear function given by (3.2). A compact algorithm for the EKF is located in
Appendix C.
The EKF method of estimating the covariance is only an approximation. If the mean and
covariance do not accurately capture the posterior statistics, the EKF solution is not optimal.
However, when implemented at a high frequency with slightly nonlinear equations, this filter
provides very good results.
4.3 Unscented Kalman Filter
While the EKF is effective for many applications, several limitations hinder its performance. As
mentioned, the nonlinear function should exhibit nearly linear characteristics about the current
operating point. If this approximation is not accurate, the state estimates may degrade or even
diverge [15]. The covariance approximation technique also requires calculation of the Jacobian
matrices. Often these derivatives do not exist about discontinuous operating points or their
resulting values are ill conditioned. Even if the Jacobian matrix exist for highly nonlinear
functions, its calculation can be very tedious and error prone, and the resulting matrix elements
are often very complex and increase computational load.
The drawbacks of the EKF approximations have led to the development of a higher fidelity and
derivative free mean and covariance estimation technique for extremely nonlinear functions.
Central to the unscented Kalman filter, the unscented transformation addresses these deficiencies
by using a sampling technique that improves covariance and mean estimates.
4.3.1 Unscented Transformation
The primary concept of the UT involves selecting a set of deterministically chosen weighted
“sigma points” (vectors), which have a known mean, xv, and covariance, . These points are
propagated through the nonlinear model, resulting in transformed points. The statistical data
represented by the transformed points approximates the true PDF. Ref. [4] has shown that the
xx
P
29
Page 44
UT accurately predicts the mean and covariance to the second order, and has similar
computational cost as the EKF.
Various sigma point sets exist. The set used for the UT in this research is known as the basic
symmetric set. The symmetric sigma points, , and their associated weights are generated as
follows where is the dimension of the random vector.
χ
x
N
x
Np
, , 1K
(
+
=∀
(4.40)
)
][
][
p
)
xx
xp
Nx
Pχ
=v
(4.41)
(
][
][
p
xx
xNp
Nx
x
Pχ
−=
+
v
(4.42)
x
Npp
N
WW
x
2
1
][][
==
+
vv
(4.43)
The bracketed subscript in equations (4.41) through (4.43) represents the row or column of a
matrix or the element of a vector which corresponds to the given sigma point. The term
(
][ p
x
N P
denotes the “pth” row or column of the matrix square root. Generally, this is
)
xx
calculated with a numerically stable algorithm such as the Cholesky decomposition. If the
decomposition returns the matrix square root, , in the form , then the columns of
are used. If it returns the matrix square root, C, in the form , then the rows are used.
These sigma points have the desired mean, which is calculated by
C
T
CCP =
C
CCP
T
=
∑
=
p
=
x
N
pp
W
1
x
2
][][χ
v
v
, (4.44)
and the desired covariance, which is calculated by
( )()
T
p
N
∑
=
p
pp
xx
xxW
1
x
vv
v
−−=
][
2
][][
χχP
. (4.45)
Intuitively, symmetric sigma points represent vectors in dimensional space. These vectors
are perturbed about the mean estimate by an amount that properly models the mean and
covariance. These concepts are illustrated in Fig. 42 for a two element random vector with
mean values and covariance matrix given in Table 41. In (a), the PDF for two random variables
and are plotted with the probability along the axis. Three contour lines and sigma
x
N
1x
2 x
z
30
Page 45
points are plotted in (b). The innermost contour line denotes the value in which 68.2% of the
variables should lie assuming a Gaussian distribution. The middle and outer contour lines
correspond to 95.5% and 99.7% confidence intervals, respectively. The sigma points for this
plot are generated using (4.40) through (4.43). The actual points represent the end of vectors,
which originate from the mean values.
Table 41: Mean and Covariance Matrix for Random Variables
Variables Mean Covariance Matrix
⎥⎦
⎤
⎢⎣
⎡
=
2
1
x
x
xv
⎥⎦
⎤
⎢⎣
⎡
=
0
0
xv
⎥⎦
⎤
⎢⎣
5 .
⎡
=
1
5 .5
xx
P
(a)
(b)
10
5
0
5
10
4
2
0
2
4
0
0.01
0.02
0.03
0.04
0.05
0.06
0.07
0.08
x1
x2
Probability
505
3
2
1
0
1
2
3
x1
x2
1 Sigma
2 Sigma
3 Sigma
Sigma Points
Figure 42: (a) The PDF of Two Random Variables x1 and x2, and (b) the Corresponding
Probability Ellipses and Sigma Points
31
Page 46
After the sigma points are generated, they are propagated through a nonlinear function, ) (⋅
h
v
,
which yields the transformed points, .
ζ
()
∑
=
p
=
x
N
pp
h
2
1
][][
χζ
v
(4.46)
The mean is estimated with
∑
=
p
=
x
N
pp
W
1
z
2
][][ζ
v
v
, (4.47)
and transformed covariance is found by
()()
T
p
N
∑
=
p
pp
zz
zzW
1
x
vv
v
−−=
][
2
][][
ζζP
. (4.48)
While this mean and covariance estimation technique is conceptually simple, its effectiveness for
capturing the posterior statistics of nonlinear functions is superior to the first order
approximation used in the EKF in many situations. Also, the simple symmetric set presented in
detail here only represents one set of sigma points. If higher order moment information is known
about the error distribution of a RV, other advanced sigma points sets exist, which capture these
higher moments. For a thorough description of other advanced sigma points see [4].
4.3.1.1 Mean and Covariance Estimation Example
A simple nonlinear mean and covariance estimation example is presented here to demonstrate
the accuracy gains of the UT over the traditional linear approximations utilized in the EKF. The
example, outlined in Fig. 43, consists of the conversion from polar to Cartesian coordinates.
The current mean values for two Gaussian random variables Θ and r and their associated
covariance matrix are assumed known. Mean and covariance estimates for x and are desired.
This problem is often encountered in state estimation as many sensors, such as radar and laser
rangefinders, measure a range with respect to a relative bearing. The nonlinearities become
increasingly apparent as the variance on the angle increases, and thus, this problem is often
studied in nonlinear estimation literature [4].
y
32
Page 47
y
Θ
r
x
Figure 43: Simple Nonlinear Example
Table 42 describes the initial mean vector values and initial covariance matrix used for this
demonstration where
. (4.49)
⎥⎦
⎤
⎢⎣
⎡Θ
r
=
xv
Note that Θ is given in radians while the units for r , x, and are the same but arbitrary.
y
Table 42: Initial Mean Vector and Covariance Matrix
Mean Covariance Matrix
⎥⎦
⎤
⎢⎣
⎡
=
5
7854.
xv
⎥⎦
⎤
⎢⎣
⎡
1 .
=
05.0
0
xx
P
The desired output vector consists of x and
y
(4.50)
⎥⎦
⎤
⎢⎣
⎡
=
y
x
zv
and is calculated with the following nonlinear output model.
()
( )
( )⎥⎦
Θ
⎤
⎢⎣
⎡
Θ
=Θ
sin
cos
,
r
r
rh
v
(4.51)
Figure 44 displays the results of the EKF linearization, UT symmetric sigma points, and Monte
Carlo mean and covariance approximations. The Monte Carlo method provides a benchmark
33
Page 48
comparison for the other two estimation techniques. As in Fig. 42 (b), the ellipses in the first
row of plots represent the true 68.2%, 95.5%, and 99.7% confidence intervals because Θ and r
are Gaussian. The sigma points are denoted with solid black circles as shown in the middle
column plots, (b), while the plus signs in the rightmost column of plots, (c), denote Monte Carlo
points. The circle near the middle of the ellipse contours marks the mean values. The first row
of plots represents the initial mean and covariance of the random variables as given in Table 42.
Both the symmetric sigma points and Monte Carlo generated points are displayed on their
corresponding graphs to emphasize how they model the initial Gaussian statistics. The last row
of plots represents the mean and covariance of the random variables after undergoing the
nonlinear transformation where the symmetric sigma points and Monte Carlo points are plotted
again to provide visual understanding. The true distributions are no longer Gaussian after
transformation, and the confidence interval ellipses in those corresponding figures are used only
to represent crude approximations of probability.
34
Page 49
(a)
(b)
(c)
1012
4
4.5
5
5.5
6
theta
r
202468
2
0
2
4
6
8
x
y
1012
4
4.5
5
5.5
6
theta
r
202468
2
0
2
4
6
8
x
y
1012
4
4.5
5
5.5
6
theta
r
202468
2
0
2
4
6
8
x
y
Figure 44: Mean and Covariance Transformation Using (a) EKF Linearization
Approximation, (b) UT Symmetric Sigma Points, and (c) Monte Carlo Sampling
The actual numerical values for the means and covariance matrices are located in Table 43. The
EKF linearization uses (4.33) and (4.35) to calculate these values where
(
∂
x
( )
x
v
)
( )
Θ
( )
Θ
⎥⎦
⎤
⎢⎣
⎡
ΘΘ−
=
∂
=
) sin(cos
cos)sin(
r
r
h
xx
vv
v
v
. (4.52)
The UT uses (4.40) through (4.43) to generate a symmetric set of sigma points where 2
=
x
N
.
The points are transformed via (4.51), upon which the mean and covariance are determined by
(4.47) and (4.48). The Monte Carlo (MC) points are generated with MATLAB’s
() ⋅
randn
function. These points are randomly generated with a zero mean and unit normal distribution. In
order to get the desired covariance and mean, the generated points are scaled by the matrix
square root, i.e. the Cholesky decomposition, of and then shifted by the mean value.
xx
P
35
Page 50
x samplenumsizevectorrandnspointMC
v
+∗=
)_,(
C
(4.53)
where C is the matrix square root of in the form
xx
P
(4.54)
Txx
CCP
=
After transformation through the output model, the posterior statistics are determined using
() ⋅
mean
() ⋅
cov
MATLAB’s and functions.
Table 43: Transformed Mean Vector and Covariance Matrix
Estimation Technique Mean Covariance
EKF Linearization
Approximation
⎥⎦
⎤
⎢⎣
. 3
⎡
. 3
=
5355
5355
zv
⎥⎦
⎤
⎢⎣
⎡
−
−
. 1
=
27502250. 1
2250. 12750. 1
zz
P
UT Symmetric Sigma Points
⎥⎦
⎤
⎢⎣
. 3
⎡
. 3
=
3617
3617
zv
⎥⎦
⎤
⎢⎣
⎡
−
−
. 1
=
22411136. 1
1136. 1 2241. 1
zz
P
Monte Carlo 1000 Points
⎥⎦
⎤
⎢⎣
. 3
⎡
. 3
=
3773
3589
zv
⎥⎦
⎤
⎢⎣
⎡
−
−
. 1
=
17670242. 1
0242. 12184. 1
zz
P
For this particular example, the UT clearly has a distinct advantage over the EKF linearization
technique for both mean and covariance estimation. While both the UT and the EKF
linearization overestimate covariance matrix values, the UT generates results closer to the
sampled estimate. The same is true for the mean values. Even though the UT provides a better
estimate, it is still an approximation technique as is evident during comparison with Monte Carlo
results. However, the tradeoff between accuracy and computational load makes this method
appealing for practical applications such as realtime state estimation.
4.3.2 Unscented Kalman Filter Algorithm
Julier and Uhlmann modified the basic Kalman Filter algorithm to incorporate the UT’s ability to
improve mean and covariance estimates for nonlinear systems [4]. A detailed version of this
algorithm, which uses symmetric sigma points, is shown in this section for state equations given
in the form of (3.1) and (3.2). This basic filter is also used as the core estimator for this research
due to the nonlinear nature of the state equations presented in Chapter 3, especially the feature
observation model.
36