Control & communication@liu NUMERICAL METHODS FOR NAVIGATION Introduction to Linköping University Traditional Extended Kalman (EKF) filters or recent particle

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)


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


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


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.


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


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


Aircraft navigation

New (2G) integrated navigation /landing system for JAS:

•Sensor fusion and diagnosis

•Terrain navigation


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


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


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


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!


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


Car positioning II

1. After slight bend, four particle clusters left


Car positioning III


2. Convergence after turn


Car positioning IV


2. Convergence after turn

3. Spread along the road


• 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


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


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!


2D Example

• Simulated flight trajectory on GIS• Snapshots at t=0, 20 and 31 seconds• Red: true Green: estimate

Terrain-aided navigation


Terrain-aided navigation


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!


Ship navigation

• Radar and sea chart input to particle filter• Support or backup to more vulnerable GPS

Documents

Control & communication@liu NUMERICAL METHODS FOR NAVIGATION Introduction to Linköping University Traditional Extended Kalman (EKF) filters or recent particle