Report

NUMERICAL METHODS FOR NAVIGATION control & [email protected] • 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 control & [email protected] Linköping Norrköping 133 000 inhabitants • • • • • • 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 Science Parks control & [email protected] 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 Aerospace projects at LiU control & [email protected] • 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 control & [email protected] LiU 25000 students 2000 employees Institute of technology www.control.liu.se Communication Systems 10 employees Faculty of health sciences Dept of EE 150 employees 8 other dept's Automatic Control 20 employees 9 other divisions Research areas in communication systems: • Sensor fusion • Diagnosis • Adaptive filtering and fault detection Faculty of Arts and Sciences Short CV control & [email protected] •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 control & [email protected] New (2G) integrated navigation /landing system for JAS: •Sensor fusion and diagnosis •Terrain navigation NINS System Block Diagram Support & Sensors control [email protected] Basic Sensors GPS INS ADC SPS PPS DGPS RALT DME NINS Processor TERNAV GIS Databases: - Elevation - Ground Cover - Obstacle - Runway GIS Server Data Fusion Kalman filter Integrity Monitoring Position and Velocity Corrections NINS estimated Position and Velocity Position and Velocity from INS Abbreviations & Acronyms INS: Inertial Navigation System ADC: Air Data Computer RALT: Radar Altimeter PPS: Precise Positioning Service 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 Positioning: GIS as a sensor control & [email protected] 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 Motivating example: car positioning control & [email protected] • 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 control & [email protected] • Recursive ad-hoc solution: –Randomize a large number of positions on the roads, each one with an associated orientation in [0, 2p] –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 control & [email protected] • 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) Car positioning II control & [email protected] 1. After slight bend, four particle clusters left Car positioning III control & [email protected] 1. After slight bend, four particle clusters left 2. Convergence after turn Car positioning IV control & [email protected] 1. After slight bend, four particle clusters left 2. Convergence after turn 3. Spread along the road Car positioning V control & [email protected] • Particle filter using street map (t ) from car’s ABS and v(t), sensors. • Off-line evaluation against GPS • Satellite image background • Green - true position • Blue – estimate • Red - particles Kalman versus particle filter control & [email protected] • Linear Gaussian model xt 1 Axt wt yt Cxt et • Kalman filter optimal filter Non-linear non-Gaussian model xt 1 f ( xt ) wt yt h( xt ) et 1. Linearize model: Extended Kalman filter optimal filter to approximate model 2. Particle filter approximate numerical solution with arbitrary accuracy for exact model Particle filter algorithm control & [email protected] Example: x(t+1)=x(t)+v(t)+w(t), y(t)=h(x(t))+e(t) x(t) Generic Particle Filter 1. Generate random states x0(i ) p( x0 ) x(1) h(x) 2. Compute likelihood t(i ) pe ( yt h( xt(i ) )) 1 3. Resampling: x , N 2 4. Prediction:x(i ) f ( x(i ) ) w(i ) , w(i ) p 3 t 1 t t t w 4 (i ) t 1. 2. (i ) t y(1) (i ) t Cramer-Rao: position error > altitude error * velocity error / sqrt(terrain variation) • h(x) terrain map The particle filter normally attains the Cramer-Rao y(t)=barometric altitude - height radar bound! v(t) from INS Terrain-aided navigation control & [email protected] 2D Example • Simulated flight trajectory on GIS • Snapshots at t=0, 20 and 31 seconds • Red: true Green: estimate Terrain-aided navigation control & [email protected] Car positioning VII control & [email protected] • • • • • • 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 control & [email protected]