NUMERICAL METHODS FOR NAVIGATION
control & communication@liu
• Introduction to Linköping University
• Traditional Extended Kalman (EKF) filters or recent particle
filters (PF)?
• Illustrative examples when PF is used with geographical
information systems (GIS)
Linköping – Norrköping
Sweden’s fourth “metropolitan” region
control & communication@liu
Linköping
Norrköping
133 000 inhabitants
•
•
•
•
•
•
124 000 inhabitants
>25000 students
>240 full professors
>1,400 research students
>140 doctoral degrees/year
>70 licentiate degrees/year
Highly dependent on external
funding
• 34% of the students from the
region
Science Parks
control & communication@liu
Mjärdevi Science Park
150 companies, 5000 employees,
focus: communication, automotive
safety, business systems
Berzelius Science Park
20 companies,
focus: bioscience
Pro Nova Science Park
80 companies, focus: IT
Aerospace projects at LiU
control & communication@liu
• IDA/ISY: WITAS, the Wallenberg Laboratory for Information
Technology and Autonomous Systems, is engaged in goal-directed
basic research in the area of intelligent autonomous vehicles and
other autonomous systems.
• IKP: The Graduate School for Human-Machine Interaction (HMI)
• ISY/IDA: The competence center ISIS: ISIS is a cooperation
between several research groups at Linköping University, and
several industrial partners. Its mission is to do research around
methods for developing systems for control and supervision.
Communication Systems, LiTH
control & communication@liu
LiU
25000 students
2000 employees
Institute of technology
www.control.liu.se
Communication Systems
10 employees
Faculty of health sciences
Dept of EE
150 employees
8 other dept's
Automatic Control
20 employees
9 other divisions
Research areas in communication systems:
• Sensor fusion
• Diagnosis
• Adaptive filtering and fault detection
Faculty of Arts and Sciences
Short CV
control & communication@liu
•Fredrik Gustafsson, born 1964, MSc 1988, PhD
1992.
•Prof in Communication systems, Dept of Elec
Eng since 1999.
•Author of 120 international papers, 15 patent
applications, 4 books and one Matlab toolbox
•Supervisor of 4 graduated PhD’s, 12 lic degrees
(currently supervising 10 students) and over 100
master theses.
•Owner of Sigmoid AB, co-founder of NIRA
Dynamics AB and Softube AB.
•www.control.isy.liu.se/~fredrik
Aircraft navigation
control & communication@liu
New (2G) integrated navigation
/landing system for JAS:
•Sensor fusion and diagnosis
•Terrain navigation
NINS System Block Diagram
Support &
Sensors
control
communication@liu
Basic Sensors
GPS
INS
ADC
SPS
PPS
DGPS
RALT
DME
NINS Processor
TERNAV
GIS Databases:
- Elevation
- Ground Cover
- Obstacle
- Runway
GIS Server
Data Fusion
Kalman
filter
Integrity
Monitoring
Position and
Velocity
Corrections
NINS estimated
Position and Velocity
Position and Velocity
from INS
Abbreviations & Acronyms
INS: Inertial Navigation System
ADC: Air Data Computer
RALT: Radar Altimeter
PPS: Precise Positioning Service
GPS: Global Positioning System
SPS: Standard Positioning Service
DGPS: Differential GPS
TERNAV: Terrain Referenced Navigation
GIS: Geographical Information System
NINS: New Integrated Navigation System
DME: Distance Measuring Equipment
Positioning: GIS as a sensor
control & communication@liu
GIS animation: ground
collision avoidance system
Digital Terrain Elevation Database: 200 000 000 grid
points
50 meter between
points
2.5 meters
uncertainty
Ground Cover Database: 14 types of vegetation
Obstacle Database: All man made obstacles above 40
m
Motivating example: car positioning
control & communication@liu
• Given: wheel speeds and street map
• Assumption: car is located on a road
(most of the time)
• Intuitive approach using map matching:
–Integration of wheel speeds on one axle
gives a trajectory
–Try all orientations and translations of the
trajectory and compute the fit to map
• Three-dimensional search with
many local minima
Motivating example: car positioning
control & communication@liu
• Recursive ad-hoc solution:
–Randomize a large number of positions
on the roads, each one with an associated
orientation in [0, 2p]
–Translate each of them according to
wheel speeds. Keep only the ones that are
left on a road. Let the other ones explore
‘similar’ paths.
• Next: the particle filter in action!
Car positioning I
control & communication@liu
• First attempt: off-line Matlab
evaluation of logged data against
logged GPS position
• Initizalization of PF in a known
neighborhood
Particles
Position
estimate
True position
(GPS)
Car positioning II
control & communication@liu
1. After slight bend, four particle
clusters left
Car positioning III
control & communication@liu
1. After slight bend, four particle
clusters left
2. Convergence after turn
Car positioning IV
control & communication@liu
1. After slight bend, four particle
clusters left
2. Convergence after turn
3. Spread along the road
Car positioning V
control & communication@liu
• Particle filter using street map
 (t ) from car’s ABS
and v(t),
sensors.
• Off-line evaluation against GPS
• Satellite image background
• Green - true position
• Blue – estimate
• Red - particles
Kalman versus particle filter
control & communication@liu
•
Linear Gaussian model
xt 1  Axt  wt
yt  Cxt  et
•
Kalman filter optimal filter
Non-linear non-Gaussian model
xt 1  f ( xt )  wt
yt  h( xt )  et
1. Linearize model: Extended Kalman filter optimal filter to
approximate model
2. Particle filter approximate numerical solution with arbitrary
accuracy for exact model
Particle filter algorithm
control & communication@liu
Example: x(t+1)=x(t)+v(t)+w(t),
y(t)=h(x(t))+e(t)
x(t)
Generic Particle Filter
1. Generate random states x0(i )  p( x0 )
x(1)
h(x)
2. Compute likelihood
t(i )  pe ( yt  h( xt(i ) ))
1
3. Resampling: x   ,  
N
2
4. Prediction:x(i )  f ( x(i ) )  w(i ) , w(i )  p 3
t 1
t
t
t
w
4
(i )
t
1.
2.
(i )
t
y(1)
(i )
t
Cramer-Rao: position error > altitude error *
velocity error / sqrt(terrain variation)
•
h(x) terrain map
The particle filter normally attains the Cramer-Rao y(t)=barometric altitude - height radar
bound!
v(t) from INS
Terrain-aided navigation
control & communication@liu
2D Example
• Simulated flight trajectory on GIS
• Snapshots at t=0, 20 and 31 seconds
• Red: true Green: estimate
Terrain-aided navigation
control & communication@liu
Car positioning VII
control & communication@liu
•
•
•
•
•
•
Light green: particles
Red – GPS
Blue: estimate (after convergence)
Real-time implementation on
Compac iPAQ
Works without or with GPS
Map database background
• Complete navigator with voice
guidance!
Ship navigation
• Radar and sea chart input to particle filter
• Support or backup to more vulnerable GPS
control & communication@liu