Kalman Filters for Mapping and Localization

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

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

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 16-735, Howie Choset with slides from and Z. Dodds

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

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

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 16-735, Howie Choset with slides from and Z. Dodds blanking time Polaroid sonar emitter/receivers No lower range limit for paired sonars...

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 16-735, Howie Choset with slides from and Z. Dodds

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

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

Souped-Up Robots

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?

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

The Latest, Coolest... Light field camera (passive) Capture the light going in every direction at every 3D point ieee.org http://excelmathmike.blogspot.com

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?

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

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

Properties of Gaussians XX~NN μμ, σσ 2 YY = aaaa + bb YY~NN(aaaa + bb, aa2 σσ 2 ) XX 1 ~NN μμ 1, σσ 2 1 2 XX 2 ~NN μμ 2, σσ 2 2 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

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)

Kalman Filter Seminal paper published in 1960 Great web page at http://www.cs.unc.edu/~welch/kalman/ 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

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

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

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)

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 16-735, Howie Choset with slides from and Z. Dodds

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 = 0 0 0 1 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)

Fully Observable vs Partially Observable 16-735, Howie Choset

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)

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

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

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

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)

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

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!)

Example Let plug some numbers m = 1 t = 0.5

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 )

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!

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

Artificial Landmarks Lippsett

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 http://ais.informatik.uni-freiburg.de

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 1 + 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0 ttvv tt 1 cos (θθ tt 1 ) ttvv tt 1 sin (θθ tt 1 ) ttωω tt 1 Landmarks don t depend on external input

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

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

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

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!

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

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

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 http://www.pracsyslab.org

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 http://homepages.inf.ed.ac.uk/

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

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

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

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.