numerical methods for navigation
DESCRIPTION
NUMERICAL METHODS FOR NAVIGATION. 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. - PowerPoint PPT PresentationTRANSCRIPT
control & communication@liu
NUMERICAL METHODS FOR NAVIGATION
• 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)
control & communication@liu
Linköping133 000 inhabitants
Norrköping124 000 inhabitants
Linköping – NorrköpingSweden’s fourth “metropolitan” region
• >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
control & communication@liu
Science Parks
Mjärdevi Science Park150 companies, 5000 employees,focus: communication, automotive safety, business systems
Berzelius Science Park20 companies,
focus: bioscience
Pro Nova Science Park80 companies, focus: IT
control & communication@liu
Aerospace projects at 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.
control & communication@liu
Communication Systems, LiTH
Research areas in communication systems:• Sensor fusion • Diagnosis• Adaptive filtering and fault detection
C o m m u n ica tio n S ys te m s1 0 em p loye es
A u to m a tic C o n tro l2 0 em p loye es
9 o the r d ivis io ns
D e pt o f E E1 5 0 e m p lo ye es
8 o th er d ep t's
In s titu te o f te ch n o lo gy F a cu lty o f h e a lth sc ie n ces F a cu lty o f A rts a n d S c ien ces
L iU2 5 00 0 s tu d en ts
2 0 0 0 e m p loye es
www.control.liu.se
control & communication@liu
Short CV
•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
control & communication@liu
Aircraft navigation
New (2G) integrated navigation /landing system for JAS:
•Sensor fusion and diagnosis
•Terrain navigation
control & communication@liu
NINS System Block DiagramNINS System Block Diagram
Kalmanfilter
- Elevation- Ground Cover- Obstacle- Runway
Integrity Monitoring
Data Fusion
Position andVelocity Corrections
Position and Velocityfrom INS
NINS estimatedPosition and Velocity
NINS Processor
Abbreviations & Acronyms
INS: Inertial Navigation SystemADC: Air Data ComputerRALT: Radar AltimeterPPS: Precise Positioning Service
GPS: Global Positioning SystemSPS: Standard Positioning ServiceDGPS: Differential GPSTERNAV: Terrain Referenced Navigation
GIS: Geographical Information SystemNINS: New Integrated Navigation SystemDME: Distance Measuring Equipment
GIS Databases: GIS Server
TERNAV
ADC
Basic Sensors Support Sensors
GPSSPSPPS
DGPSRALTINS DME
control & communication@liu
Digital Terrain Elevation Database: 200 000 000 grid points
50 meter between points
2.5 meters uncertaintyGround Cover Database: 14 types of vegetationObstacle Database: All man made obstacles above 40 m
Positioning: GIS as a sensor
GIS animation: ground collision avoidance system
control & communication@liu
Motivating example: car positioning
• 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
control & communication@liu
Motivating example: car positioning
• Recursive ad-hoc solution:
–Randomize a large number of positions on the roads, each one with an associated orientation in [0, 2]
–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!
control & communication@liu
Car positioning I
• First attempt: off-line Matlab evaluation of logged data against logged GPS position
• Initizalization of PF in a known neighborhood
Position estimateTrue position (GPS)
Particles
control & communication@liu
Car positioning II
1. After slight bend, four particle clusters left
control & communication@liu
Car positioning III
1. After slight bend, four particle clusters left
2. Convergence after turn
control & communication@liu
Car positioning IV
1. After slight bend, four particle clusters left
2. Convergence after turn
3. Spread along the road
control & communication@liu
• Particle filter using street map and v(t), from car’s ABS sensors.
• Off-line evaluation against GPS
• Satellite image background• Green - true position• Blue – estimate• Red - particles
Car positioning V
)(t
control & communication@liu
Kalman versus particle filter
• Linear Gaussian model
Kalman filter optimal filter• Non-linear non-Gaussian model
1. Linearize model: Extended Kalman filter optimal filter to approximate model
2. Particle filter approximate numerical solution with arbitrary accuracy for exact model
ttt
ttt
exhy
wxfx
)(
)(1
ttt
ttt
eCxy
wAxx
1
control & communication@liu
Particle filter algorithm
Generic Particle Filter
1. Generate random states
2. Compute likelihood
3. Resampling:
4. Prediction:
)( 0)(
0 xpx i
))(( )()( itte
it xhyp
Nx i
tit
it
1, )()()(
wit
it
it
it pwwxfx
)()()()(1 ,)(
Example: x(t+1)=x(t)+v(t)+w(t),
y(t)=h(x(t))+e(t)
234
x(t)
h(x)
x(1)
y(1)
• h(x) terrain map y(t)=barometric altitude - height radarv(t) from INS
1. Cramer-Rao: position error > altitude error * velocity error / sqrt(terrain variation)
2. The particle filter normally attains the Cramer-Rao bound!
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
Terrain-aided navigation
control & communication@liu
Car positioning VII
• 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!
control & communication@liu
Ship navigation
• Radar and sea chart input to particle filter• Support or backup to more vulnerable GPS