Robo$cs:

Science and Systems

State Es$ma$on
Subramanian Ramamoorthy
School of Informa$cs

7 October, 2013

Mo#va#on Sensors are Noisy!

Consider the following problem:
You have been gi?ed an AIBO
You want to design behaviours using visual observa#ons
So, you want accurate es#mates of object loca#ons
How will you go about this?
The output of your sensor looks like this Clip.

7/10/2013

Robo#cs: Science and Systems

Control Implica#ons of Sensor Noise

Robots u#lize strategies based
on sensory feedback
Example: Posi#oning a disk
using a magne#c actuator
modulate the force based on
height above actuator
Height is obtained from edge
detec#on in the image
1. Where is the edge?
2. What is the eect of object
orienta#on?
3. What happens when we
make mistakes here?

7/10/2013

Robo#cs: Science and Systems

How high is
the
object?
3

Tools for Dealing with Uncertainty

The following are some major techniques for such problems,
based on probabilis#c and sta#s#cal ideas:

(Parameter) Es#ma#on extrac#ng true signal from noise
Sensor Fusion combining mul#ple sensory modali#es
Kalman Filters state es#ma#on in noisy dynamic systems
SLAM (Simultaneous Localiza#on and Mapping) algorithms
Where am I and what does the world look like?

7/10/2013

Robo#cs: Science and Systems

Es#ma#on: Basic Ques#on

Consider this simple experimental scenario:
You make mul#ple measurements (y) from a system
What you observe is a transformed and noise-corrupted
version of a true underlying state (x)
So, you want to solve the following problem:

7/10/2013

Robo#cs: Science and Systems

A Simple Es#ma#on Procedure

Minimize the sum of squared errors between model and data.

7/10/2013

Robo#cs: Science and Systems

Batchwise Least Squares Method

Do you see how?

7/10/2013

Robo#cs: Science and Systems

Worked Example Es#mate Coecients of a

Cubic Polynomial
Use k measurements at dierent t
to es#mate coecients x :

7/10/2013

Robo#cs: Science and Systems

Batchwise Weighted Least Squares

7/10/2013

Robo#cs: Science and Systems

Sequen0al Computa#on of LS Es#mates

The least squares loss func#on can be dened recursively:





Then, the es#mates can be computed recursively:

7/10/2013

Robo#cs: Science and Systems

10

Alterna#ves to the Least Squares Method

- Maximum Likelihood

7/10/2013

Robo#cs: Science and Systems

11

Sequen#al State Es#ma#on

Consider a ship in choppy waters
Needs to navigate using a distant landmark (star)
To keep things simple, assume the model x = z + e
Can make mul#ple measurements of the same ground truth
(let us say successively in #me)
We know (a priori) one crucial fact the variance for each
measurement/sensor
Then, what is the true value of the
distance to the landmark?
7/10/2013

Robo#cs: Science and Systems

12

Data Fusion from Two Es#mates

(e.g., two dierent sensors)
How do you combine two unbiased es#mates with arbitrary
covariance matrices?

Lower variance
= higher confidence
= higher weight

7/10/2013

Robo#cs: Science and Systems

13

Fusion in terms of Informa0on

(Inverse Covariance)

7/10/2013

Robo#cs: Science and Systems

14

De-Fusion

7/10/2013

Robo#cs: Science and Systems

15

Apply to Sequen#al Problem:

Upda#ng Es#mated State
How to combine mul#ple sequen0al measurements?

7/10/2013

Robo#cs: Science and Systems

16

Answer: Variance-weighted Sum

Remember this!
Well see something similar again.

7/10/2013

Robo#cs: Science and Systems

17

Same Idea in a Figure

7/10/2013

Robo#cs: Science and Systems

18

What Happens when we Include Dynamics?

7/10/2013

Robo#cs: Science and Systems

19

Eect of Adding System Dynamics

Uncertainty is amplied & density func#on is squashed

Why?

7/10/2013

Robo#cs: Science and Systems

20

Structure of the Kalman Filter

We know a
statistical description

7/10/2013

Robo#cs: Science and Systems

21

Key Variables: First Two Sta#s#cal Moments

7/10/2013

Robo#cs: Science and Systems

22

Discrete Kalman Filter: Key Ingredients

1. Discrete process model
State change over #me
Linear dierence equa#on

2. Discrete measurement model

Rela#onship between state and measurement
Linear func#on

3. Model parameters (assump#on men#oned in #21)

Process noise characteris#cs
Measurement noise characteris#cs
7/10/2013

Robo#cs: Science and Systems

23

Discrete Kalman Filter: Two Models

7/10/2013

Robo#cs: Science and Systems

24

Discrete Kalman Filter: Model Equa#ons

7/10/2013

Robo#cs: Science and Systems

25

Complete Model Specica#on

7/10/2013

Robo#cs: Science and Systems

26

State Es#ma#on using Kalman Filter

Predic#on: Project state and error
covariance forward in #me:

e.g., during a time interval, you

expect the ball to go from 1m to 1.5 m,

with some uncertainty increase

Correc#on: Update es#mate
a?er measurement:

In fact, vision sees ball going to 1.7 m

so you update your estimates to

Conclude ball must be at 1.6 m with

some new level of uncertainty

7/10/2013

Robo#cs: Science and Systems

27

State Es#ma#on using Kalman Filter

Project state and error
covariance forward in #me:

Update es#mate a?er measurement:

7/10/2013

Same idea as in

slide # 17

Robo#cs: Science and Systems

28

Discrete Kalman Filter: The Complete Loop

7/10/2013

Robo#cs: Science and Systems

29

Toy Example: Es#ma#ng a (Noisy) Constant

Of course, this is a trivial

dynamic system

7/10/2013

Robo#cs: Science and Systems

30

So, What is the Kalman Filter?

An op#mal, recursive/on-line data processing algorithm
Op#mal in what sense?

If the system dynamics are linear

And system/sensor noise is Gaussian
Then, there is no alternate algorithm with lower MSE
Op#mal Best Linear Unbiased Es0mator

Bayesian view a Kalman Filter:

Propagate condi#onal probability density of desired
quan##es, condi#oned on knowledge of actual data

7/10/2013

Robo#cs: Science and Systems

31

Nonlinear Dynamics Whats the Problem?

Not Gaussian any more!

7/10/2013

Robo#cs: Science and Systems

32

Extended Kalman Filter - Setup

We have a model, so we
can compute this exactly

(a) Noise free approximation (b) Linear (local) dynamics

around that approximation

7/10/2013

Robo#cs: Science and Systems

33

Extended Kalman Filter - Computa#on

We can dene a Kalman lter over these error terms

Then, use that linear error es#mate to drive the main es#ma#on
loop

7/10/2013

Robo#cs: Science and Systems

34

Limita#ons of the Kalman Filter

Op#mal state es#mator for Linear systems & Gaussian noise
Most robots involve nonlinear dynamics (simple example: s#ck-slip
fric#on and slippage in tyres)
Many commonly used sensors, e.g., sonar, involve more complex type
of noise
In complex scenarios (e.g., es#ma#ng posi#ons of obstacles in a
room), one is dealing with mul#-modal distribu#ons

Standard extensions for nonlinearity may not be sa#sfactory:

If ini#al state es#mate is wrong, or if process is incorrectly modeled,
the lter could quickly diverge
Covariance is underes#mated
7/10/2013

Robo#cs: Science and Systems

35

Summary
All realis#c robo#cs systems suer from sensor noise
This manifests itself in a sequence of problems. We looked at:
Es#ma#ng a signal from noise
State es#ma#on in the form of a Kalman lter
Nonlineari#es and non-Gaussian distribu#ons will need extensions

In future, we will look at uses of these methods in localiza#on and

mapping
Also, you will see some of these methods again in the context of
stochas#c control

7/10/2013

Robo#cs: Science and Systems

36

Acknowledgments
Some pictures/slides are adapted from the following sources:
Tutorial by Welch and Bishop (
hsp://www.cs.unc.edu/~welch/kalman/)

7/10/2013

Robo#cs: Science and Systems

37