Introduction to Simultaneous Locomotion and Mapping

1
Introduction to Simultaneous Locomotion and
Mapping
2
Overview

• Definition of SLAM
• Why it's a hard problem
• Introduction to Kalman Filters
• Applying Kalman Filters to Solve SLAM Problems
• Visualizations of Solutions
• A Nod to Other Approaches

3
Definition Localization and Mapping

• Simultaneous Localization and Mapping (SLAM) asks
• Where am I in the world reference frame?
• What are the obstacles and features of the world?
• These problems are the Localization and Mapping
  problems
• Localization given a map, observations, and
  actions, determine the location of the robot
• Mapping given a trajectory and observations,
  determine a map of the environment

4
(No Transcript)
5
Odometry

• The method of determining location solely from
  the internal sensors of the robot no map!
• Integrate actions to get new state.

Single-Disk Shaft Encoder A perforated
disk is mounted on the shaft and placed between
the emitterdetector pair. As the shaft rotates,
the holes in the disk chop the light beam.
6
Uncertainty from Noise

• However, our calculated trajectory is inaccurate
  due to noise factors
• Due to calibration, vibration, slippage,
  geometric differences,
• The uncertainty grows with time.

7
Localization With Landmarks

• We can use observations to reduce the uncertainty
  in our pose estimations.

8
Mapping

• Complement problem to localization We know
• our position, xk,
• our history of positions, Xk-1, and
• noisy observations of the environment, Zk, and
• we want to make a map of the environment, M.

9
What is a map?

• Set of features with relationships.
• Features
• Occupancy grid
• Lines
• Anything identifiable by the robot
• Relations
• Rigid-body transformation
• Topological

10
SLAM Combines Both
Unknown Map
Neither the map nor the location is known.
11
Formal Problem Statement

• What isP(xk, mZ0k, U0k, x0)?

• Problem Quantities
• xk location of vehicle at time k
• uk a control vector applied at k-1 to drive
  the vehicle from xk-1 to xk
• mi location of ith landmark
• zk observation of a landmark taken at time k
• X0k history of states x1,x2, x3, ...,
  xk
• U0k history of control inputs u1, u2,
  u3, ..., uk
• m set of all landmarks
• Z0k history of all observations z1, z2,
  ..., zk

mi

• P(xk, mZ0k, U0k, x0)?

mi
12
The Holy Grail

• With SLAM, robots gain a major step toward
  autonomy. They can be placed at an unknown
  location, with unknown surroundings, and they can
  work out what's around them and where they are.

13
Difficulties

• All our knowledge is probabilistic
• We have a chicken and egg problem
• If we knew the map, we could calculate our
  location
• If we knew our location, we could calculate the
  map

14
Kalman Filter Overview

• The Kalman Filter (KF) is a recursive algorithm
  to estimate the state of a process based on
• A model of how the process changes,
• A model of what observations we expect, and
• Discrete, noisy observations of the process.
• Predictions of the state of the processare made
  based on a model.
• Observations are made and our estimate is
  updated given thenew observation.

15
Foundations of Kalman Filter

• The purpose of the KF is to calculate the
  distribution of the state at time k, given noisy
  observations.
• However, filtering continuous distributions
  generates state distributions whose
  representation grows without bound.
• An exception to this rule is the Gaussian
  distribution, which only needs a mean and
  variance to define the distribution.

16
Linear Gaussian Distributions

• Under the continuous operations needed for KFs
• Conditioning
• Bayes' ruleIf our prior distribution is a
  Gaussian, and our transition models are linear
  Gaussian, then our calculated distribution is
  also Gaussian.

17
Conditioning

• We can find P(Y) Sum of P(Y,z) over all z.
• P(y,z) 0.4, P(y, not z) 0.2, . P(y) 0.6
• And P(Y,z) P(Yz)P(z) from the product rule.
• So P(Y) Sum of P(Yz)P(z)
• For continuous distributions, this becomes P(Y)
  Integral of P(Yz)P(z) over all zwhich is
  where we get ..but if our P(x) is only true
  given e0k, so is our result..

18
Bayes' Rule

• We know that P(a and b) P(ab)P(b) P(b and a)
  P(ba)P(a)?
• Therefore, P(ba) P(ab)P(b)/P(a)?
• With additional evidence, this becomes P(ba, e)
  P(ab,e)P(be)/P(ae)?
• Notice that this is a probability distribution,
  and all values will sum to one P(ae) just acts
  as a normalizer.
• This yields

19
KF Recursion

• We want , and we have
  .
• Note that by using conditioning, we can
  calculateas long as we know
  , which is referred to as the motion model.
• Also, by the generalized Bayes' rule, we
  haveassuming we know , the
  sensor model.

20
Process Motion Model

• The model of how the process changes is referred
  to as the motion model. A new state of the
  process is assumed to be a linear function of the
  previous state and the control input with
  Gaussian noise, p(w) N(0,Q), added.xk
  Axk - 1 Buk 1 wk 1where A and B
  are matrices capturing our process model.

21
Process Sensor Model

• The sensor model is also required to be a linear
  Gaussianzk Hxk vkwhere H is a
  matrix that relates the state to the observation
  and p(v) N(0,R).

22
Covariance

• The multi-dimensional analog of
  varianceexpected squared difference between
  elements and the mean. It quantifies the
  uncertainty of our estimate.

23
Kalman Filter Algorithm

• The Kalman Filter operates on Gaussian
  distributions, and so only needs to calculate a
  mean, x, and a covariance, C.
• Prediction stepxk1k Axk BukCk1k
  ACkAT Q cov(Axk,Axk) Q
• Updatexk1k1 xk1k K(zk1
  Hxk1k)Ck1 (I KH)CK1k

24
K, Kalman Gain Factor

• K is a measure of how to weight the prediction
  and the observation
• A high value of K results in the measurement
  being trusted
• A low value of K results in the prediction being
  trusted.
• K is based on the covariances K
  Pk1kHT(HPk1kHT R)-1
• Limit of K as R -gt 0 H-1
• Limit of K as Pk1k -gt 0 0

25
Initial State

• With the previous formulas, we can calculate an
  update. What's our initial state?
• We need x0, C0, R, Q.
• X0 is our initial guess at the process state
• C0 is how certain we our of our guess
• R is the noise in the sensor model (measured)?
• Q is the noise in the process model (estimated)?

26
Quick Example

• Estimate a slightly variable 1D signal by noisy
  observationxk1 xk wzk1 xk v
• Therefore, A 1, B 0, H 1.
• Let's assume Q 0.00001 and R 0.01 and x0
  0 and C0 1Q and R often need to be tuned
  for performance

27
Result
28
Probabalistic SLAM

• Is SLAM a Kalman Filter problem?
• We want , and we can
  assume we know the previous state,
  .
• And with conditioning ...gets us the predicted
  next state.
• And the generalized Bayes' rule integrates
  zk.
• So we need and
  .

29
Kalman Formulation for SLAM

• looks exactly like the
  motion model from the Kalman Filter.
• And if we incorporate the landmark locations into
  our system state, xk, then is
  the same as the sensor model.
• We assume landmarks don't move, sopk1 pk.
• Our observations are of landmarks,zk
  Hpp-Hrrk wwhere Hpp-Hrrk Hxk and r is
  the robot pose.

30
SLAM Example

• Suppose we have a robot moving in 1D that moves a
  certain velocity every timestep. Also assume the
  velocity has noise added,
• Also assume the landmarks don't move,
• This provides our motion model.

31
SLAM Example

• Further, the observations we expect are
• Thus we have A and H, respectively below, x0,
  R, Q, and C0 must be given, and B is 0.

32
Adding Landmarks

• As more landmarks are discovered, they are added
  to the state.
• All the problem matrices are updated and their
  dimension is increased.

33
Why this Works

• Kalman Filters have been proven to solve the SLAM
  problem by Dissanyake, et. al.
• This is due to the fact that the correlation
  between landmarks monotonically increases
• Thus, relative positions of the landmarks become
  well known, and thus a map of the landmarks
  relative to eachother becomes well known.
• Thus, the solution converges.

34
Limitations of Kalman Filter

• Unimodal distribution cannot account for
  choice.
• Computation is O(n2) where n is the size of the
  state vector.
• Landmark association and loop closure.
• Requires a linear process model for the system.
• This final weakness is addressed by the Extended
  Kalman Filter (EKM).

35
Extended Kalman Filter

• The assumption of linear motion models and sensor
  models is a serious limitation.
• What if we let either of these models be
  non-linear?xk1 f(xk, uk, wk)zk
  h(xk, vk)where f and h can be non-linear
  and v and w are zero mean Gaussian noise.

36
EKM Mean Calculation

• Like the Kalman Filter, the EKM works by
  calculating a new distribution mean and
  covariance given the models and observations.
• The means are propagated by the models, without
  the noise,xk 1 f(xk, uk, 0)zk
  h(xk, 0)

37
EKM Covariances

• The covariance estimates are based on a
  linearization of the models using Jacobians.Ck
  1 FkCkFk WkQkWkwhere C is
  the covariance matrix, Fk is the Jacobian of f
  with respect to the state variables and Wk is
  the Jacobian of f with respect to the noise
  parameters, and Q is the covariance of the motion
  model's noise.

38
EKM Gain

• Finally, we need a new form for KKk
  CkJkT(JkPkJkT VkRkVkT)-1where
  C is our covariance, J is the Jacobian of h, the
  sensor model, with respect to the state
  variables, V is the Jacobian of the sensor model
  with respect to its noise, and R is the
  covariance of the noise in the sensor model.

39
EKM Example

• A car model requires a non-linear motion
  model dx v cos(theta) dy
  v sin(theta) dtheta (v tan(phi)) / L

40
Example Motion Model

• Fixed steering angle,fixed velocity

41
Example Observation Model

• Let's assume our observations are taken from an
  overhead camera.zk1 zk vkwhere
  vk is noise.

42
Prediction Step

• Prediction of the mean is calculated from motion
  model. xk 1k f(xk, uk, 0)?
• Jacobians needed to calculate the new covariance.

43
Update Step

• Predicted measurement given by the sensor
  model zk1k h(xk, 0)?
• Final estimate is xk1 xk1k K(zk
  zk1k
• This requires the gain, Kk
  CkJkT(JkPkJkT VkRkVkT)-1

44
Update Step cont.

• The Gain requires two JacobiansKk
  CkJkT(JkPkJkT VkRkVkT)-1
  
• All that's left is the final covariance,
  Ck1 Ck1 (I - KkJk)Ck1k

45
Result

• With blue as the true trajectory, red as the
  observation, and green as the estimate.

46
EKM Issues

• Landmark association and loop closure.
• Unimodal distribution cannot account for
  choice.
• Computation is O(n2) where n is the size of the
  state vector.

47
Landmark Association

• How can the robot tell if it's looking at
  landmark mi or mj?
• What if your landmarks are just laser ranges?

48
Constructing lines from Scans
49
Scan Matching

• The robot scans, moves, and scans again.
• Short term motion noise results in features
  having to be recognized.

50
Iterated Closest Point

• Inputs two point clouds
• Output refined transformation.
• Method
• 1. Associate points by the nearest neighbor
  criteria.
• 2. Estimate the parameters using a mean square
  cost function.
• 3. Transform the points using the estimated
  parameters.
• 4. Iterate (re-associate the points and so on).

51
Loops

• ICP can handle small offsets, though it has high
  computational cost and is subject to local
  minima.
• What about large offsets?

52
EKM Issues

• Landmark association and loop closure.
• Unimodal distribution cannot account for
  choice.
• Computation is O(n2) where n is the size of the
  state vector.

53
Computation Cost

• A common approach to attempt to cut down the
  computation cost, which is O(n2) where n is the
  size of the state space (and thus landmarks), is
  to only consider a local map.
• These need to be stitchedtogether to achieve
  global navigation.

54
Particle Filter Another Approach

• The idea behind a particle filter is to use a
  number of samples to approximate the
  distributions.
• Advantage you don't need to assume linear
  Gaussian processes models, and could be faster
• Idea one generate particles uniformly with
  values 0, 1 throw out any particle at x if
  value(x) gt P(x)?
• Idea two generate particles from a proposal
  distribution and assign each a weight.

55
Rao-Blackwellization

• For SLAM problems, the state space is too large
  for particle filtering.
• However, if our joint state P(x1, m1) can be
  partitioned with the product rule
  P(m1x1)P(x1), and if P(m1x1) can be
  calculated analytically, only P(x1) needs to be
  sampled.
• This is called Rao-Blackwellization, and allows
  particle filtering for SLAM problems.

56
FastSLAM

• FastSLAM is a particle filter approach to SLAM
  using Rao-Blackwellization.
• Note that the joint SLAM problem can be
  factoredP(X0k, mZ0k,U0k,x0)
  P(mX0k,Z0k)P(X0kZ0k,U0k,x0)
  When conditioned on the trajectory, m is
  independent of U

57
FastSLAM

• Our joint distribution at time k is
  wki,X0ki,P(mX0ki, Z0k)nwhere X
  is a sample trajectory, w is the calculated
  weight, and P(mX,Z) is our analytically
  calculated distribution of the landmarks.

58
FastSLAM Recursive Calculations

• For recursive step, assume prior
  estimates wk-1i,X0k-1i,P(mX0k-1i,
  Z0k-1)n
• 1) A new pose is sampled from a proposal
  distribution. This could simply be the motion
  model. xki P(xkxk-1i, uk)?
• 2) Assign each sample a weight
• 3) Use EKM on each particle to solve for maps.