Kalman Filters for Mapping and Localization

Transcription

1 Kalman Filters for Mapping and Localization

2

3

4

5 Sensors If you can t model the world, then sensors are the robot s link to the external world (obsession with depth) Laser Kinect IR rangefinder sonar rangefinder Light-field camera

6 Sensors Robots link to the external world... Sensors, sensors, sensors! and tracking what is sensed: world models Gyroscope (orientatiation) GPS Force/ Torque Inertial measurement unit (gyro + accelerometer) Compass

7 Infrared sensors Noncontact bump sensor (1) sensing is based on light intensity. object-sensing IR looks for changes at this distance diffuse distance-sensing IR (2) sensing is based on angle receved. IR emitter/detector pair , Howie Choset with slides from and Z. Dodds

8 Infrared Calibration The response to white copy paper (a dull, reflective surface) raw values (put into 4 bits) inches 15º increments in the dark , Howie Choset with slides from and Z. Dodds fluorescent light incandescent light

9 Infrared Calibration energy vs. distance for various materials ( the incident angle is 0º, or head-on ) ( with no ambient light ) , Howie Choset with slides from and Z. Dodds

10 Sonar sensing single-transducer sonar timeline 0 a chirp is emitted into the environment 75µs typically when reverberations from the initial chirp have stopped the transducer goes into receiving mode and awaits a signal... limiting range sensing.5s after a short time, the signal will be too weak to be detected , Howie Choset with slides from and Z. Dodds blanking time Polaroid sonar emitter/receivers No lower range limit for paired sonars...

11 Sonar effects (a) Sonar providing an accurate range measurement (b-c) Lateral resolution is not very precise; the closest object in the beam s cone provides the response (d) Specular reflections cause walls to disappear (e) Open corners produce a weak spherical wavefront (f) Closed corners measure to the corner itself because of multiple reflections --> sonar ray tracing , Howie Choset with slides from and Z. Dodds

12 Sonar modeling initial time response accumulated responses blanking time cone width spatial response , Howie Choset with slides from and Z. Dodds

13 Laser Ranging LIDAR/Laser range finder , Howie Choset with slides from and Z. Dodds LIDAR map

14 Souped-Up Robots

15 The 3d time-of-flight camera Recent, Cool... (1) RF-modulated optical radiation field output (2) Reflects off the environment (3) Time-interval input measurements provides data (not time-integrated!) How can we get an image from this information?

16 More recent, Cooler... Structured light: Project a known dot pattern with an IR transmitter (invisible to humans) Infer depth from deformation to that pattern depth from focus: Points far away are blurry depth from stereo: Closer points are shifted

17 The Latest, Coolest... Light field camera (passive) Capture the light going in every direction at every 3D point ieee.org

18 The Problem Mapping: What is the world around me (geometry, landmarks) sense from various positions integrate measurements to produce map assumes perfect knowledge of position Localization: Where am I in the world (position wrt landmarks) sense relate sensor readings to a world model compute location relative to model assumes a perfect world model Together, these are SLAM (Simultaneous Localization and Mapping) How can you localize without a map? How can you map without localization? All localization, mapping or SLAM methods are based on updating a state: What makes a state? Localization? Map? Both? How certain is the state?

19 Representations for Bayesian Robot Localization Discrete approaches ( 95) Topological representation ( 95) uncertainty handling (POMDPs) occas. global localization, recovery Grid-based, metric representation ( 96) global localization, recovery Particle filters ( 99) sample-based representation global localization, recovery AI Kalman filters (late-80s?) Gaussians approximately linear models position tracking Robotics Multi-hypothesis ( 00) multiple Kalman filters global localization, recovery

20 Gaussian (or Normal) Distribution Univariate pp xx ~NN μμ, σσ 2 pp xx = 1 2ππσσ ee 1(xx μμ) 2 2 σσ 2 µ -σ σ Multivariate pp xx ~NN μμ, Σ pp xx = 1 (2ππ) dd/2 Σ 1 ee 2 xx μμ TT Σ 1 (xx μμ) 1/2

21 Properties of Gaussians XX~NN μμ, σσ 2 YY = aaaa + bb YY~NN(aaaa + bb, aa2 σσ 2 ) XX 1 ~NN μμ 1, σσ XX 2 ~NN μμ 2, σσ XX 1 + XX 2 ~NN(μμ 1 + μμ 2, σσ 1 + σσ 2 ) We stay in the Gaussian world as long as we start with Gaussians and perform only linear transformations. Same holds for multivariate Gaussians

22 Dynamical Systems System that changes We will represent a system by its state xx tt at time t We will also require that a system be observable Jeb Corliss Oliver uu tt uu tt+1 uu tt+1 external command input xx tt xx tt+1 xx tt+2 zz tt zz tt+1 zz tt+2 process model (state transition) observation model (measurements of the states)

23 Kalman Filter Seminal paper published in 1960 Great web page at Recursive solution for discrete linear filtering problems A state x R n A measurement z R m Discrete (i.e. for time t = 1, 2, 3, ) Recursive process (i.e. x t = f ( x t-1 ) ) Linear system (i.e x t = A x t-1 ) The system is defined by: 1) linear process model 2) linear measurement model x t = A x t-1 + B u t-1 + w t-1 state control transition Input (optional) Gaussian white noise How a state transitions into another state z t = H x t + v t observation model Gaussian white noise How a state relates to a measurement

24 Kalman Filter 1. Prior estimate xx tt at step t : Use the process model to predict what will be the next state of the robot 2. Posterior estimate xx tt at step t : Use the observation model to correct the prediction by using sensor measurement Compute posterior estimate as a linear combination of the prior estimate and difference between the actual measurement and expected measurement x ˆ = xˆ + K ( z Hxˆ ) t t t Where the Kalman gain KK tt is a blending factor that adds a measurement innovation. t t

25 Kalman Filter Define prior error between true state and prior estimate and the prior covariance as Define posterior error between true state and posterior estimate and it s posterior covariance as The gain K that minimizes the posterior covariance is defined by Note that K t e = x xˆ t Σ t = E( e te t e t If RR 0 then KK tt = HH 1 and xx tt = xx tt + KK tt zz tt HHxx tt = xx If Σ tt 0 then KK tt = 0 and xx tt = xx tt = x t t xˆ Σt = E( ete t T t T t T T = Σ H ( HΣ H + R) t t ) ) 1

26 Kalman Filter Recipe: 1. Start with an initial guess xx 0, Σ 0 2. Compute prior (prediction) xx tt = AAxx tt 1 + BBuu tt 1 Σ tt = AAΣ tt 1 AA TT + QQ Time update (predict) REPEAT 3. Compute posterior (correction) KK tt = Σ tt HH TT HHΣ tt HH TT + RR 1 xx tt = xx tt + KK tt zz tt HHxx tt Σ tt = 1 KK tt HH Σ tt Measurement update (correct)

27 Initial xx 0, Σ 0 Predict xx 1 from xx 0 and uu 0 xx 1 = AAxx 0 + BBuu 0 Σ 1 = AAΣ 0 AA TT + QQ Correction using zz 1 KK 1 = Σ 1 HH TT HHΣ 1 HH TT + RR 1 xx 1 = xx 1 + KK 1 zz 1 HHxx 1 Σ 1 = 1 KK 1 HH Σ 1 Predict xx 2 from xx 1 and uu 1 xx 2 = AAxx 1 + BBuu 1 Σ 2 = AAΣ 1 AA TT + QQ , Howie Choset with slides from and Z. Dodds

28 Observability zz tt = HHxx tt If H does not provide a one-to-one mapping between the state and the measurement, then the system is unobservable zz tt = xx tt In this case H is singular, such that many states can generate the same observation KK tt = Σ tt HH TT HHΣ tt HH TT + RR 1 rank AAAA TT = rank AA rank AA + BB rank AA + rank(bb)

29 Fully Observable vs Partially Observable , Howie Choset

30 Kalman Filter Limitations Assumptions: Linear process model xx tt+1 = AAxx tt + BBuu tt + ww tt Linear observation model zz tt = HHxx tt + vv tt White Gaussian noise NN(0, Σ ) What can we do if system is not linear? Non-linear state dynamics xx tt+1 = ff xx tt, uu tt, ww tt Non-linear observations zz tt = h xx tt, vv tt Linearize it! xx tt+1 xx tt+1 + AA xx tt xx tt + WWww tt zz tt zz tt + HH xx tt xx tt + VVvv tt xx tt+1 = ff xx tt, uu tt, 0 zz tt = h(xx tt, 0)

31 Extended Kalman Filter Where A, H, W and V are Jacobians defined by AA xx tt = ff 1 (xx tt, uu tt, 0) ff 1(xx tt, uu tt, 0) xx 1 xx NN ff MM (xx tt, uu tt, 0) ff MM(xx tt, uu tt, 0) xx 1 xx NN WW xx tt = ff 1 (xx tt, uu tt, 0) ff 1(xx tt, uu tt, 0) ww 1 ww NN ff MM (xx tt, uu tt, 0) ff MM(xx tt, uu tt, 0) ww 1 ww NN HH xx tt = h 1 (xx tt, 0) h 1(xx tt, 0) xx 1 xx NN h MM (xx tt, 0) h MM(xx tt, 0) xx 1 xx NN VV xx tt = h 1 (xx tt, 0) h 1(xx tt, 0) vv 1 vv NN h MM (xx tt, 0) h MM(xx tt, 0) vv 1 vv NN

32 EKF for Range-Bearing Localization State ss tt = xx tt yy tt θθ tt TT 2D position and orientation Input uu tt = vv tt ωω tt TT linear and angular velocity Process model ff ss tt, uu tt, ww tt = xx tt 1 + ( tt)vv tt 1 cos (θθ tt 1 ) yy tt 1 + ( tt)vv tt 1 sin (θθ tt 1 ) θθ tt 1 + ( tt)ωω tt 1 + ww xxtt ww yy ww θθtt y y t l 1 r 1 b 1 x t l N Given a map, the robot sees N landmarks with coordinates ll 1 = xx ll1 yy ll1 TT,, ll NN = xx llnn yy llnn TT The observation model is x zz tt = hh 1 (ss tt, vv 1 ) hh NN (ss tt, vv NN ) hh ii ss tt, vv tt = xx tt xx llii 2 + yytt yy llii 2 tan 1 + yy tt yy llii θθ xx tt xx tt llii vv rr vv bb

33 Linearize Process Model ff ss tt, uu tt, ww tt = xx tt 1 + ( tt)vv tt 1 cos (θθ tt 1 ) yy tt 1 + ( tt)vv tt 1 sin (θθ tt 1 ) θθ tt 1 + ( tt)ωω tt 1 + ww xxtt ww yy ww θθtt AA xx tt = ff 1 (xx tt, uu tt, 0) ff 1(xx tt, uu tt, 0) xx 1 xx NN ff MM (xx tt, uu tt, 0) ff MM(xx tt, uu tt, 0) xx 1 xx NN AA = 1 0 tt vv tt 1 sin (θθ tt 1 ) 0 1 tt vv tt 1 cos (θθ tt 1) 0 0 1

34 Linearize Observation Model zz tt = hh 1 (ss tt, vv 1 ) hh NN (ss tt, vv NN ) hh ii ss tt, vv tt = xx tt xx llii 2 + yytt yy llii 2 tan 1 yy tt yy + llii θθ xx tt xx tt llii vv rr vv bb HH ss tt = hh 1 (ss tt, 0) xx tt hh 1 (ss tt, 0) hh 1 (ss tt, 0) yy tt θθ tt hh NN (ss tt, 0) xx tt yy tt θθ tt hh NN (ss tt, 0) hh NN (ss tt, 0)

35 Extended Kalman Filter Kalman Filter Recipe: Given Extended Kalman Filter Recipe: Given Prediction Prediction Measurement correction Measurement correction

36 Kalman Filter for Dead Reckoning Robot moves along a straight line with state x = [ p, v ] T p: position v: velocity u is the input force applied to the robot Newton s 2 nd law first order finite difference: Integrate velocity Robot has velocity sensor Integrate acceleration zz tt = 0 1 pp tt vv tt The robot position is not measured The measured velocity depends on the robot velocity (duh!)

37 Example Let plug some numbers m = 1 t = 0.5

38 From Localization to Mapping For us, the landmarks have been a known quantity (we have a map with the coordinates of the landmarks), but landmarks are not part of the state Two choices: Make the state the location of the landmarks relative to the robot (I also know exactly where I am ) - No notion of location relative to past history - No fixed reference for landmarks irobot Make the state the robot location now (relative to where we started) plus landmark locations + Landmarks now have fixed location - Knowledge of my location slowly degrades (but this is inevitable )

39 EKF for Range-Bearing Localization State ss tt = xx tt yy tt θθ tt TT 2D position and orientation Input uu tt = vv tt ωω tt TT linear and angular velocity Process model ff ss tt, uu tt, ww tt = xx tt 1 + ( tt)vv tt 1 cos (θθ tt 1 ) yy tt 1 + ( tt)vv tt 1 sin (θθ tt 1 ) θθ tt 1 + ( tt)ωω tt 1 + ww xxtt ww yy ww θθtt y y t l 1 r 1 b 1 x t l N Given a map, the robot sees N landmarks with coordinates ll 1 = xx ll1 yy ll1 TT,, ll NN = xx llnn yy llnn TT The observation model is x zz tt = hh 1 (ss tt, vv 1 ) hh NN (ss tt, vv NN ) hh ii ss tt, vv tt = xx tt xx llii 2 + yytt yy llii 2 tan 1 + yy tt yy llii θθ xx tt xx tt llii vv rr vv bb

40 Data Association From observation model, we have an expected Observation of landmark #1 Observation of landmark #N So if we have N landmarks l 1,, l N and we are given a scan z t, how do associate each landmark to a scan observation? msdn.microsoft.com ll iitt = ll iitt + KK tt zz iitt hh ii xx tt, 0 Observation of landmark #i must be in the i th row!

41 Natural Visual Landmarks Visual landmarks: SIFT, SURF, corners, Se IJRR

42 Artificial Landmarks Lippsett

43 EKF for Range-Bearing Localization State ss tt = xx tt yy tt θθ tt TT 2D position and orientation Input uu tt = vv tt ωω tt TT linear and angular velocity Process model ff ss tt, uu tt, ww tt = xx tt 1 + ( tt)vv tt 1 cos (θθ tt 1 ) yy tt 1 + ( tt)vv tt 1 sin (θθ tt 1 ) θθ tt 1 + ( tt)ωω tt 1 + ww xxtt ww yy ww θθtt y y t l 1 r 1 b 1 x t l N Given a map, the robot sees N landmarks with coordinates ll 1 = xx ll1 yy ll1 TT,, ll NN = xx llnn yy llnn TT The observation model is x zz tt = hh 1 (ss tt, vv 1 ) hh NN (ss tt, vv NN ) hh ii ss tt, vv tt = xx tt xx llii 2 + yytt yy llii 2 tan 1 + yy tt yy llii θθ xx tt xx tt llii vv rr vv bb

44 Kalman Filters and SLAM Localization: state is the location of the robot Mapping: state is the location of 2D landmarks SLAM: state combines both If the state is then we can write a linear observation system note that if we don t have some fixed landmarks, our system is unobservable (we can t fully determine all unknown quantities) Covariance Σ is represented by

45 EKF Range Bearing SLAM Prior State Estimation State ss tt = xx tt yy tt θθ tt ll 1 TT ll NN TT position/orientation of robot and landmarks coordinates Input uu tt = vv tt ωω tt TT forward and angular velocity The process model for localization is xx tt 1 ttvv tt 1 cos (θθ tt 1 ) ss tt = yy tt 1 + ttvv tt 1 sin (θθ tt 1 ) θθ tt 1 ttωω tt 1 This model is augmented for 2N+3 dimensions to accommodate landmarks. This results in the process equation xx tt yy tt θθ tt ll 1tt ll NNtt = xx tt 1 yy tt 1 θθ tt 1 ll 1tt 1 ll NNtt ttvv tt 1 cos (θθ tt 1 ) ttvv tt 1 sin (θθ tt 1 ) ttωω tt 1 Landmarks don t depend on external input

46 EKF Range Bearing SLAM Prior Covariance Update We assume static landmarks. Therefore, the function f(s,u,w) only affects the robot s location and not the landmarks. Σ tt = AA tt Σ tt 1 AA tt TT + WW tt QQWW tt TT Jacobian of the process model The motion of the robot does not affect the coordinates of the landmarks AA = xx xx θθ xx θθ θθ 0 θθ θθ θθ 0 II Jacobian of the robot motion 2Nx2N identity

47 EKF Range Bearing SLAM Kalman Gain Compute the Jacobian H i of each h i and then stack them into one big matrix H. Note that h i only depends on 5 variables: x t, y t, θ t, x li, y li Need to be in the correct columns of H

48 EKF Range Bearing SLAM Measurement Observe N landmarks zz iitt = rr iitt φφ iitt TT Must have data association Which measured landmark corresponds to h i? If s t contains the coordinates of N landmarks in the map, h i predicts the measurement of each landmark Must figure which measured landmark corresponds to h i ss tt = ss tt + KK tt zz tt hh ss tt, 0 Make sure that each landmark observation zz iitt appears in the correct rows of zz tt ll iitt = ll iitt + KK tt zz iitt hh ii ss tt, 0

49 EKF Range Bearing SLAM Posterior Update From K t and H update the posterior state estimate ss tt = ss tt + KK tt zz tt hh ss tt, 0 Σ tt = II KK tt HH tt Σ tt Tada! And we are done!

50 Bearing-Only SLAM Lets assume one landmark for now s = h( s) = tan z = h(s) wikipedia [ x y θ ] T 1 l x y l l x l y y x θ Tully 2008 Often use omni-directional sensor

51 Why Bearing-Only SLAM is Challenging We cannot estimate the landmark location with one measurement We must guess the range and initialize with a large covariance due to the lack of range information The location is very uncertain and difficult to resolve with low parallax measurements The measurement model is very nonlinear, which breaks conventional filtering techniques Tully 2008

52 Bearing-Only SLAM with EKF EKF uses the standard Kalman update The Kalman gain is computed through a linearization about the current estimate The result diverges Very dependent on the initialization guess of landmarks

53 Mono SLAM A visual landmark with a single camera does not provide range Data association is given by tracking or matching visual descriptors/patches Robot Vision, Imperial College

54 Submaps As landmarks are added, the state vector grows For large maps only a few landmarks can be visible at any given time Use submaps to reduce the number of landmarks to manageable numbers

55 Navigation: RMS Titanic Leonard & Eustice EKF-based system 866 images 3494 camera constraints Path length 3.1km 2D / 3.4km 3D Convex hull > 3100m min. data / 39 min. ESDF* *excludes image registration time

56 Search of Flight 370 Inertial Navigation System (INS): A glorified IMU with Kalman filter

57 Basic system modeling ideas Summary Kalman filter as an estimation method from a system model Linearization as a way of attacking a wider variety of problems Mapping localization and mapping into EKF Extensions for managing landmark matching and not-wellconstrained systems.