NUMERICAL METHODS FOR NAVIGATION

1. 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)

2. Linköping – NorrköpingSweden’s fourth “metropolitan” region Linköping 133 000 inhabitants Norrköping 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

3. Science Parks 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

4. 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.

5. Communication Systems, LiTH Research areas in communication systems: • Sensor fusion • Diagnosis • Adaptive filtering and fault detection www.control.liu.se

6. 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

7. Aircraft navigation • New (2G) integrated navigation /landing system for JAS: • Sensor fusion and diagnosis • Terrain navigation

8. NINS System Block Diagram Basic Sensors Support Sensors INS ADC RALT GPS SPS PPS DGPS DME NINS Processor Data Fusion Kalman filter TERNAV Integrity Monitoring GIS Databases: GIS Server Position and Velocity Corrections - Elevation - Ground Cover - Obstacle - Runway NINS estimated Position and Velocity Position and Velocity from INS Abbreviations & Acronyms 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 INS: Inertial Navigation System ADC: Air Data Computer RALT: Radar Altimeter PPS: Precise Positioning Service

9. Positioning: GIS as a sensor 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

10. 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

11. Motivating example: car positioning • Randomize a large number of positions on the roads, each one with an associated orientation in [0, 2p] • Recursive ad-hoc solution: • 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!

12. Car positioning I • 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)

13. Car positioning II • After slight bend, four particle clusters left

14. Car positioning III • After slight bend, four particle clusters left • Convergence after turn

15. Car positioning IV • After slight bend, four particle clusters left • Convergence after turn • Spread along the road

16. Car positioning V • 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

17. Kalman versus particle filter • Linear Gaussian model Kalman filter optimal filter • Non-linear non-Gaussian model • Linearize model: Extended Kalman filter optimal filter to approximate model • Particle filter approximate numerical solution with arbitrary accuracy for exact model

18. Particle filter algorithm Example: x(t+1)=x(t)+v(t)+w(t), y(t)=h(x(t))+e(t) x(t) • Generic Particle Filter • Generate random states • Compute likelihood • Resampling: • Prediction: x(1) h(x) y(1) 2 3 4 • Cramer-Rao: position error > altitude error * velocity error / sqrt(terrain variation) • The particle filter normally attains the Cramer-Rao bound! • h(x) terrain map y(t)=barometric altitude - height radarv(t) from INS

19. 2D Example Terrain-aided navigation • Simulated flight trajectory on GIS • Snapshots at t=0, 20 and 31 seconds • Red: trueGreen: estimate

20. Terrain-aided navigation

21. 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!

22. Ship navigation • Radar and sea chart input to particle filter • Support or backup to more vulnerable GPS