Control & communication@liu NUMERICAL METHODS FOR NAVIGATION Introduction to Linköping...

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