Uncertainty Driven Motion Plan

1
Uncertainty Driven Motion Plan

• moving for the sake of sensing

James Solberg, Kevin Lynch, Mitra Hartmann, and
Malcolm MacIver
2
Uncertainty Driven Motion Plan

• Talk Roadmap

• I. State Estimation gt Kalman Filter
• What is meant by state estimation
• Why is it important?
• Example Combining stochastic measurements
• Example fusion of measurements and model
• The Kalman filter
• Extensions of the Kalman filter
• Applications of the Kalman filter in robotics

• II. Moving to minimize uncertainty (time
  permitting)
• Introduction to problem
• Example tactile object recognition
• Example 3-DOF planar robot for localization

3
Estimation

• What is meant by estimation?

Estimator
Data noise Data noise Data noise
Estimation
z
estimate
H
y
Stochastic process

• Examples
• Voltage from sensor y Hz
• output of low-pass filter
• - distribution N(µ, s2). E y µ

4
Estimation

• So, what is the big deal about estimation?

• For fully observable systems with perfect
  sensors, states are exactly known.
• But, real systems are stochastic.
• No model is perfect (plant, sensors, etc.)
• Dynamical systems act on inputs PLUS
  disturbances. (unmodeled disturbances)
• Sensors do not provide perfect (noise) and
  complete (unobservable state)

disturbances
new state
desired state
plant
error
control signal

Controller
-
measured state
sensor
noise
5
Estimation

• An example of combining independent stochastic
  measurements

• Lost out at sea estimate coordinates using the
  stars.
• The measurement z1 is an estimate of the state
  x
• The estimation is described as a distribution
  with a mean and variance the Conditional
  Probability Density Function

probability
Conditional probability density function
6
Estimation

• Combining independent measurements

• We then use GPS to estimate our position
• We have two measurements of the same state
• How do we combine them?

How can we get this distribution?
combined
GPS
GPS
star sighting
star sighting
7
Estimation

• Mathematically combining the two estimates

The best way to combine the measurements is to
take a weighted average of the two measurements
(look for this equation in todays HRH meeting)
New estimate
weighting
weighting
star measurement
GPS measurement
New (overall) variance
Like adding resistors in parallel
8
Estimation

• Non-static estimation with system model

prior
prediction from model
Disturbance on u
previous estimate
dx
Nominal velocity
weight
prior
prior
posterior
measurement
no measurement
9
The Kalman Filter

• an optimal recursive data processing algorithm

• OPTIMAL
• Linear dynamics
• Measurements linear w/r to state
• Errors in sensors and dynamics must be zero-mean
  (un-bias) white Gaussian

• RECURSIVE
• Does not require all previous data
• Incoming measurements modify current estimate

DATA PROCESSING ALGORITHM The Kalman filter is
essentially a technique of estimation given a
system model and concurrent measurements (not a
function of frequency)
10
The Discrete Kalman Filter
(PDFs)
11
The Discrete Kalman Filter
Define
error vector
The Error Covariance Matrix
So, P is now our measure of uncertainty of the
states
The Kalman filter minimizes P
measurement sample
Measurement Update Correct
Model Forecast Predict
12
The Discrete Kalman Filter

• First part model forecast prediction

State transition
Control signal
State prediction
prior estimate
Process noise covariance
Error covariance prediction
Prediction is based only the model of the system
dynamics.
13
The Discrete Kalman Filter

• The Kalman gain, K Do I trust my model or
  measurements?

variance of the predicted states
--------------------------------------------------
---------- variance of the predicted measured
states
measurement sensitivity matrix
measurement noise covariance
14
The Discrete Kalman Filter

• Second part measurement update correction

actual measurement
prior state prediction
state correction
Kalman gain
predicted measurement
posterior estimate
update error covariance matrix (posterior)
recall sensor model
(posterior prior)
15
Extensions of the Kalman filter

• What do you need?

• linear model of system dynamics (A matrix)
• linear model of sensor outputs as a function of
  states (H matrix)

If not
if non-linear, Taylor expansion (Jacobian) about
current estimate
measurement sensitivity matrix, H
implement filter algorithm with linearized
equations that must be re-computed at every
iteration.
state transition matrix, A
16
Extensions of the Kalman filter

• What do you need?

3. unbias, white, Gaussian disturbances on
measurements system dynamics

• The Kalman filter guarantees to be optimal under
  these conditions.
• But, some deviations from these constrains will
  still render good performance
• distribution needs to be unimodal gt multi-peak
  distributions are bad!
• mean should be at/near peak
• if bias is present and known, E(u,v) ? (0,0),
  then lump off-set into sensor model
• poor filter performance is expected if bias is
  significant and unaccounted for

Probability of robot position
(bad distribution for a Kalman filter very
non-Gaussian)
17
Extensions of the Kalman filter

• What do you need?

4. numerical estimates on variances of sensor
noise and system disturbances.
R measurement noise covariance matrix noise in
sensors Q process noise covariance matrix
unmodeled disturbances
In a real system you may not know the variance on
the sensor measurements or the process noise.
18
Where is stochastic estimation used?

• The Localization Problem

• Map m is known a priori
• Given the control actions Uk
• Make inferences about the unknown robot
  locations Xk

• At time k the robot observes 4 estimates of its
  pose
• 1 from its system model,
• 3 others from referencing the landmarks

The Kalman filter solves the over-constrained
problem
19
Where is stochastic estimation used?

• The Mapping Problem

• The vehicle locations Xk are provided (by some
  independent means).
• Given the control actions Uk
• Make inferences about (build) the map m

At time k, the robot measures the landmark
locations and combines this with its previous
estimate (Kalman filter)
Integrate measurement and model
20
Move to Sense

• What about moving for the sake of sensing?

Everything thus far has given a set of controls,
and then used optimal state estimation
Active Sensing?
How do we quantify uncertainty?
21
Move to Sense

• A tactile sensing example Bay, 1991.

measured
previous
Kalman gain
predicted
Error covariance of parameters, P.
Decrease the cost function
22
Move to Sense

• A tactile sensing example Bay, 1991.

What direction will get me the most information?
Decrease the cost function
Profile which way to move?
23
Move to Sense

• A tactile sensing example Bay, 1991.

The trace of the estimate error covariance
matrix, Q the estimate of uncertainty.
24
Move to Sense

• 3-DOF planar robot for localization

• Robot knows landmark map
• range and bearing sensors
• local controller
• minimize uncertainty
• over-constrained system
• 4 landmarks 5 estimates
• Kalman filter to combine

25
Move to Sense

• MATLAB simulation

Utility trace(inv(R)) (locally maximize)
26
(No Transcript)
27
Move to Sense

• Matlab Simulation

Utility 1/trace(R) (maximize locally)