Date post: | 20-Dec-2015 |
Category: |
Documents |
View: | 223 times |
Download: | 1 times |
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