introduction to sensing and recursive state estimation introduce the basics of recursive state...

T D Barfoot Intro to State Estimation

University of Toronto Institute for Aerospace Studies

Copyright © 2016 by T D Barfoot

Introduction to Sensing and Recursive

State Estimation

1


Purpose of Lecture

‣ To introduce the basics of recursive state estimation

‣ sensors and sensing principles

‣ sensor uncertainty → probability

‣ Bayes filter

‣ Extended Kalman filter

‣ particle filter

‣ Sigmapoint Kalman filter

2

T D Barfoot Intro to State Estimation 3

UTIASWhat do these have in common?

Pioneer UAV

Radarsat-1

Sojourner rover

Apollo Lunar Module

Boeing 777


UTIASIs estimation really important? Yes!

!"#$%#&'(

)&'*+&,(


Sense-Think-Act Cycle

5

World

Perception Action?

Robot


Perception = Sensors

6

stereo camera

2D laser

rangefinder

RGB-D camera

omnidirectional

cameramonocular

camera

3D laser

rangefinder

global

positioning

system

inertial

measurement

unit

encoder radarsonar

sun sensor

(also star tracker)switch

infrared

rangefinder

compassinclinometer

flash lidar


Monocular Camera

7

monocular

camera

lensesCCD or CMOS

imager

-imaging principles haven’t really changed since

300 BC, with the camera obscura (pinhole

camera)

-today we have sophisticated lens systems to

achieve different fields of view, focus, etc.

-in robotics we often calibrate our cameras to

make them appear as though the images came

from an idealized pinhole camera, so that we can

use simple equations in our algorithms


Stereo Camera

8

stereo camera

u v2b

f

z

x

Focal Plane

Image Plane

Feature

Baseline

Focal Length

Right PinholeLeft Pinhole

bb

z = 2f bu−v

x = (u + v) bu−v

-a stereo camera is just two monocular

cameras in a known configuration, typically

facing in the same direction

-by finding correspondences between the

two captured images, it’s possible to

triangulate for three-dimensional

information


Laser Rangefinder

9

2D laser

rangefinder

3D laser

rangefinder

-pulsed laser rangefinder determines

range by measuring the time of flight

-2D or 3D ‘scans’ are generated by

either steering the laser with mirrors, or

using multiple lasers in an array

-note, height of returning pulse also

provides an intensity, which depends on

range, surface material, angle of

incidence


Flash Lidar

10

flash lidar

-sometimes called a ToF camera

-consists of a near infrared light source (not a

laser) and an optical sensor (i.e., camera)

-light source emits a phase-modulated signal

-the sensor captures intensity and phase of the

returned signal (at every pixel)

-range is computed by comparing the current

phase modulation (at the time of reception) with

the phase of the received signal (at every pixel)

-due to modulation, there is a range ambiguity

beyond about 7 m in typical flash lidars

-these sensors tend to blur edges of objects

-they used to have problems working well in

direct sunlight (but this is getting better)


RGB-D Camera

11

RGB-D camera

-RGB-D cameras (Microsoft Kinect) uses the

structured lighting principle to obtain a depth

image, in addition to a regular RGB camera

image

-a known pattern of structured dots is projected

onto the scene

-these dots are identified in a camera image and

depth is computed through triangulation


Omnidirectional Camera

12

omnidirectional

camera

-there are a few clever ways to increase the

field of view of a camera so that it can look

in all directions at once: several cameras,

special mirrors

-omnidirectional cameras typically require

careful calibration to handle these increased

fields of view

-stereo omnidirectional cameras are

possible also


Radar

13

radar

-radar has been around a long time

but is under-subscribed for robotic

applications

-may become more popular due to

current automotive applications

using millimetre-wave radar

-transmitted radio waves are

reflected off objects and detected to

determine range via time of flight

-requires very sophisticated signal

processing to understand the

returned signal and process into a

useable image of the scene


Inertial Measurement Unit

14

-inertial measurement units (IMU)

measure three linear accelerations

and three angular rates

-gravity is measured by the

accelerometers and typically needs

to be subtracted off

-position/orientation can be

determined by integrating the IMU

data (twice), but this is very

inaccurate except for quite short

movements

-there are biases on all the

measurements that vary with time

(e.g., due to temperature change),

meaning bias needs to be estimated

in addition to the position/

orientation

inertial

measurement

unit


GPS

15

global

positioning

system

-global positioning system (GPS) is a

network of US satellites with known

orbits that transmit signals that can be

used to triangulate position of a receiver

-there is also GLONASS (Russia), Galileo

(Europe), and other satellite networks

-differential GPS techniques can be used

to improve performance of regular GPS

(10 m accuracy) to about a few cm

accuracy in the best implementations

-Real-Time Kinematic GPS additionally

uses a satellite’s carrier phase and

compares this to that of a stationary

reference receiver in order to greatly

improve accuracy

-post-processing GPS data can also help

improve accuracy as an entire batch of

data may be used

-GPS can be used live to determine robot

positions, or as groundtruth to assess other

position-determination techniques


Compass, Inclinometer, Sun/Star Tracker

16

sun sensor

(also star tracker)

compass

inclinometer

-references a magnetic field to determine heading

-must account for local variations of field on Earth

-no usable magnetic field on Mars or the Moon

-references a gravity field to determine pitch and roll

-also measures vehicle accelerations so be careful

-provides a body-frame vector to a celestial body such as the sun or

nighttime stars in the case of a star tracker

-with an ephemeris (map of sun/stars in sky over time), it is possible to

determine attitude information

-typically combined with an inclinometer to obtain three-axis attitude

-need to account for refraction through the Earth’s atmosphere

-not common on mobile robots


UTIASWhere does uncertainty come from?

! In general, there are several sources of uncertainty we must

overcome in estimation

Environment

Dynamics

Random

Action Effects

Sensor

Limitations

Inaccurate

Models

Approximate

Computation

! Our estimation framework must acknowledge these sources of

uncertainty so that we are aware of the limitations


UTIASLet’s look at an example: GPS


UTIASError Propagation I

x

y

a

b

c

d

l1 l2

σr

Problem: Allocate uncertainty (tolerance) to

l1, l2, and d to ensure the uncertainty in the

location of the top point is within σr (one

standard deviation).

Nominal geometry is

l2

1= a

2 + b2

l2

2= b

2 + (d− a)2

Take first-order perturbations:

2 l1 δl1 = 2 a δa + 2 b δb

2 l2 δl2 = 2 b δb + 2 (d− a)(δd− δa)


UTIASError Propagation II

x

y

a

b

c

d

l1 l2

σr

Re-arranging in matrix form we have

[δa

δb

⇥

=

[a b

a− d b

⇥−1 [

l1 0 00 l2 a− d

⇥

✏ ︷︷ ⇣

A

⇧

⌥

δl1δl2δd

⌃

which is now a linear model for error

propagation in our truss.

If we think of (δl1, δl2, δd) as independent

Gaussian random variables, we can use

standard deviations to model the uncertainty

in their values:

Q =

⇧

⌥

σ2

l10 0

0 σ2

l20

0 0 σ2

d

⌃

= E

⇤

↵

⇧

⌥

δl1δl2δd

⌃

⇧

⌥

δl1δl2δd

⌃

T⌅

⌦


UTIASError Propagation III

x

y

a

b

c

d

l1 l2

σr

σa

σb

We then have that

R =

[

σ2

aσaσb

σaσb σ2

b

⇥

= E

⇤

[

δa

δb

⇥ [

δa

δb

⇥T⌅

= AQAT

In general this will be an uncertainty ellipse

like the red one on the left. We need to

allocate uncertainties in (δl1, δl2, δd), by

assigning values to (σl1, σl2

, σd), to ensure the

red ellipse stays within our requirement (the

black circle).

This error budget model is just one example of

the propagation of error through a (nonlinear)

model.


UTIASA similar example: stereo camera

u v2b

f

z

x

Focal Plane

Image Plane

Feature

Baseline

Focal Length

Right PinholeLeft Pinhole

bb

z = 2f bu−v

x = (u + v) bu−v

! Two cameras with known

baseline, 2b, focal length, f .

! By observing the same feature

in both images, can triangulate

for the three-dimensional

position

! Uncertainties in the image

coordinates, (u, v), result in

uncertainties in the feature

location, (x, z).


Point Uncertainties from Stereo

23

Matthies L, Shafer S, “Error Modeling in Stereo Navigation”.

IEEE Journal of Robotics and Automation, 3(3):239-248, 1987

MATTHIES AND SHAFER: ERROR MODELING IN STEREO NAVIGATION 24 1

Camera Caiera

Fig. 2. Stereo geometry showing triangulation uncertainty.

tions were found by linear programming. In our application,

statistical minimization and methods are more appropriate

because of the stochastic nature of measurement errors and the need to filter time sequences of measurements.

The motivation for using scalar weights is that uncertainty

grows with distance, so it can be modeled by weighting points

inversely with distance [18]. However, as Fig. 2 shows, the

uncertainty induced by triangulation is not a simple scalar

function of distance to the point; it is also skewed and oriented.

Nearby points have a fairly compact uncertainty, whereas

distant points have a more elongated uncertainty that is

roughly aligned with the line of sight to the point. Scalar error

measures do not capture these distinctions in shape.

These distinctions can be captured by using 3D probability

distributions to characterize the uncertainty in point locations.

Our approach is to assume 2D, normally distributed (i.e.,

Gaussian) error in the measured image coordinates and to derive 3D Gaussian distributions describing the error in the

inferred 3D coordinates. Similar approaches have been used in

photogrammetry [20] and elsewhere in computer vision [ 111, [5], [ 121, [ 151, [9]. The use of Gaussian distributions to model

image coordinate error is a common [l 11, [5], convenient

approximation that gives adequate performance, as will be seen in Section VI. For the 3D coordinates, the true

distribution will be non-Gaussian because triangulation is a

nonlinear operation; we approximate this as Gaussian for simplicity and because it gives an adequate approximation

when the distance to points is not extreme. We will discuss

shortly the cases where this breaks down.

We will now show the details of the triangulation and error

model calculation for the general case of 3D points projecting

onto 2D images. We assume a camera geometry with parallel

image planes, aligned epipolar lines, and image coordinate

systems centered at the piercing point of each camera. Let the

image coordinates be given by 1 = [xl, yr] and r = [x,, y,] in

the left and right image, respectively. Consider these as

normally distributed random vectors with means pl and p , and

covariance matrices VI and V,. From 1 and r we need to

estimate the coordinates [ X , Y, Z ] of the 3D point P. We

take the simple approach of using the ideal noise-free

triangulation equations P = [ X , Y, Z] = f (Z , r), or

X=b(x ,+x , ) / (x [ -xr )

Y=b(Yl+Yr) / (x [ -xr )

(assuming a unit focal length and a baseline of 2b) and

inferring the distributions of X , Y, and Z as functions of

random vectors 1 and r. If (1) was linear, P would be normal

[8] with mean p p = f ( p L I , p r ) and covariance

vP=J [o J T VI 0

where J is the matrix of first partial derivatives of f or the

Jacobian. Since f is nonlinear these expressions do not hold

exactly, but we use them as satisfactory approximations.

The true values of the means and covariances of the image

coordinates needed to plug into (1) and (2) are unknown. We

approximate the means with the coordinates returned by the

stereo matcher and the covariances with identity matrices.

This is equivalent to treating the image coordinates as

uncorrelated with variances of one pixel. Better covariance

approximations can be obtained by several methods [2], [ 111.

What does this error model mean geometrically? Constant

probability contours of the distribution of P describe ellipsoids

about the nominal mean that approximate the true error

distribution. This is illustrated in Fig. 3 where the ellipse

represents the contour of the error model and the diamond represents quantization error of Fig. 2. For nearby points the

contours will be close to spherical; the farther the points, the

more eccentric they become. A covariance matrix with

structure V = w1, equal to a scalar times the identity matrix,

describes only spherical contours. This is the difference

between attaching scalar weights to 3D coordinate vectors and

using the full 3D distribution; that is, scalar weights are

equivalent to spherical covariances whereas the full distribution permits ellipsoidal covariances. In the balance of the

paper we will often refer to scalar weights as a spherical error

model and the full distribution as an ellipsoidal error model.

Where the Gaussian approximation breaks down is in failing to represent the longer tails of the true error distribution. The

true distribution is skewed not unlike the diamond in Fig. 3,

whereas normal distributions are symmetric. The skew is not significant when points are close, but becomes more pro-

nounced the more distant the points. A possible consequence is


UTIASIt’s an uncertain world

There are several sources of uncertainty we must overcome in

estimation:

Environment

Dynamics

Random

Action Effects

Sensor

Limitations

Inaccurate

Models

Approximate

Computation

We’ll need some probabilistic machinery to acknowledge and manage

uncertainty.

24


UTIASProbability densities as likelihood

We say that a random variable, x, is distributed according to a

particular probability density function. Let p(x) be a probability density

function (PDF) for the random variable, x, over the interval[

a, b⇥

. This

is a non-negative function that satisfies

⇤

b

a

p(x) dx = 1.

That is, it satisfies the axiom of total probability.

We use PDFs to represent the likelihood of x being in all possible

states in the interval, [a, b].

25


UTIASRobot in a hallway

If this robot has a prior map of the hallway (i.e., knows where the doors

are) and then detects a door,

Image from Fox et al. (1999).

it will have a PDF, p(x), with three peaks representing that it is likely

that it is near a door.

26


UTIASRepresentations

As we’ll see, we can’t represent PDFs perfectly in a computer so we

need to choose an approximation:

Mixture of Gaussians

Particles

Gaussian

Histograms (metric, topological)

Each has its pros and cons, depending on the application.

27


UTIASProbability is area under the curve

Note that probability density is not probability. Probability is given by

the area under the density function. For example, the probability that x

lies between c and d, Pr(c ≤ x ≤ d), is given by

Pr(c ≤ x ≤ d) =

∫d

c

p(x) dx.

p(x)

xa b

∫b

a

p(x) dx = 1p(x)

xc d

Pr(c ≤ x ≤ d)

28


UTIASThere are often strings attached

We can introduce a conditioning variable:

Let p(x|y) be a probability density function over x ∈[

a, b⇥

conditioned

on y ∈[

r, s⇥

such that

(∀y)

⇤

b

a

p(x|y) dx = 1.

29


UTIASThe curse of dimensionality

We may also denote joint probability densities for N -dimensional

continuous variables in our framework as p(x) where

x =[

x1 · · · xN

⇥Twith xi ∈

[

ai, bi

⇥

. Note that we can also use the

notation

p(x1, x2, . . . , xN )

in place of p(x). Sometimes we even mix and match the two and write

p(x, y)

for the joint density of x and y.

30


UTIASDimensions don’t trump axioms

In the N -dimensional case, the axiom of total probability requires

⇤

b

a

p(x) dx =

⇤

bN

aN

· · ·

⇤

b2

a2

⇤

b1

a1

p (x1, x2, . . . , xN ) dx1 dx2 · · · dxN = 1

where a :=[

a1 · · · aN

⇥Tand b :=

[

b1 · · · bN

⇥T.

31


UTIASBayes’ rule

Bayes’ rule (Bayes, 1764) can be used to factor a joint probability

density into a conditional and a non-conditional factor:

p(x, y) = p(x|y)p(y) = p(y|x)p(x)

Technically speaking, Bayes’ rule should be written as

p(y|x) =p(x|y)p(y)

p(x).

In the specific case that x and y are statistically independent, we can

factor the joint density as follows:

p(x, y) = p(x)p(y)

32


UTIASOn the margins

If we integrate over all possible values of one of the joint variables, we

can compute the density over only the other:

∫ b

a

p(x, y) dx =

∫ b

a

p(x|y)p(y) dx = p(y)

∫ b

a

p(x|y) dx = p(y)

This is called marginalization.

33


UTIASInference

A key issue when working with stochastic equations is inferring one

probability density from another using Bayes’ rule. To do this, we

compute the integral

p(y) =

∫ b

a

p(y|x)p(x) dx

which can be quite expensive to do in the general nonlinear case. The

above equation is also called the Chapman-Kolmogorov equation

when the conditional density is the update density for a sequential

Markov process (Papoulis, 1965). We will see how approximations can

be used to make this easier.

34

p(x|y) =p(y|x)p(x)

p(y)


UTIASJust a moment

When working with mass distributions (a.k.a., density functions) in

dynamics, we often keep track of only a few properties called the

moments of mass (e.g., mass, center of mass, inertia matrix).

The same is true with probability density functions. The zeroeth

probability moment is always 1 since this is exactly the axiom of total

probability. The first probability moment is known as the mean, x:

x := E [x] =

∫b

a

xp(x) dx

where E[·] denotes the expectation operator.

35


UTIASTo infinity and beyond!

The second probability moment is known as the covariance matrix, C:

C := E[

(x − x)(x − x)T⇥

The next two moments are called the skewness and kurtosis, but for

the multivariate case, these get quite complicated and require tensor

representations. We will not need them here, but it should be

mentioned that there are an infinite number of these probability

moments.

36


UTIASGauss’ namesake

We’ll be working with Gaussian probability density functions. In one

dimension, a Gaussian PDF is given by

p(x|µ, σ2) :=

1√2πσ2

exp

(

−1

2

(x − µ)2

σ2

⇥

where µ is the mean and σ2 is the variance (σ is called the standard

deviation).

x

p(x|µ, σ2) =

1√

2πσ2

exp

(

−

1

2

(x − µ)2

σ2

⇥p(x)

µ

σ σ

37


UTIASMultivariate

A multivariate Gaussian probability density function (PDF), p(x|x, C),over the random variable, x ∈ R

M , may be expressed as

p(x|x, C) :=1

⇤

(2π)M det Cexp

(

−1

2(x − x)T

C−1(x − x)

⇥

where x ∈ RM is the mean and C ∈ R

M×M is the (symmetric

positive-definite) covariance matrix.

38


p(x|µ, σ2) :=1√

2πσ2exp

✓

−1

2

(x − µ)2

σ2

◆

Probability Theory in 1 Slide

39

p(x)

xa b

∫b

a

p(x) dx = 1p(x)

xc d

Pr(c ≤ x ≤ d)

- probability density functions represent the likelihood of

random variables taking on different values; can be used to

quantify our uncertainty in various quantities

- they can be continuous (like above) or discrete (not shown)

- they satisfy the axiom of total probability:

- joint PDFs (over two or more variables) factor as follows:

- this leads us to Bayes rule:

- and also marginalization (dropping x) and mapping x to y:

Zb

a

p(x) dx = 1.

p(x, y) = p(x|y) p(y) = p(y|x) p(x)

example multimodal PDF from Fox et al. (1999)

p(y) =

Zb

a

p(x, y) dx =

Zb

a

p(y|x) p(x) dx

- joint PDFs over independent variables factor like this:

- mean, expected value:

- variance:

- Gaussian (normal) PDF:

p(x, y) = p(x) p(y)

µ = E[x] =

Zb

a

x p(x) dx

σ2 = E[(x− µ)2)] =

Zb

a

(x− µ)2 p(x) dx

x

p(x|µ, σ2) =

1√

2πσ2exp

(

−

1

2

(x − µ)2

σ2

⇥p(x)

µ

σ σ

Gaussian PDF

a b

xp(x)

p(x|y) =p(y|x)p(x)

p(y)


Predictor-Corrector

40

Prediction

-high frequency

-internal sensors

-odometry

-inertial sensors

-low computational cost

Correction

-low frequency

-external “map” needed

-vision, GPS, laser

-high computational cost

Fusion

-optimal combination


UTIASNonlinear Motion and Observation Models

We will consider only discrete-time time-invariant equations, but this

time we will allow nonlinear equations. We define the following motion

and observation models:

motion model: xk = h (xk−1, uk, wk)

observation model: yk = g (xk, nk)

where k = 1 . . . K is again the discrete-time index and K its maximum.

For now we will be purposely vague about the properties of wk and nk,

but these are again noise variables taking on similar roles to the linear

case (they may or may not be Gaussian). The function h(·) is the

nonlinear motion model and the function g(·) is the nonlinear

observation model.

41


UTIASMarkov property

Definition: In the simplest sense, a stochastic process has

the Markov property if the conditional probability density

functions of future states of the process, given the present

state, depend only upon the present state, but not on any

other past states, i.e. they are conditionally independent of

these older states. Such a process is called Markovian or a

Markov process.

42


UTIASMarkov process

x0

g g g

h h hx1xk−1 xk

uk

yk

wk

nknk−1

wk−1

uk−1

yk−1y1

u1

w1

n1

· · · · · ·

43


UTIASBayes filter

44

The Bayes filter seeks to come up with a entire probability density

function to represent the likelihood of the state, xk, using only

measurements up to and including the current time. Using our notation

from before, we want to compute

p(xk|u1:k, y1:k).

which is also sometimes called the belief for xk. Recall from the

section on factoring the batch linear-Gaussian solution that


UTIASFactor out latest measurements

By employing the independence of all the measurements1, we may

factor out the latest exteroceptive measurement to have

p(xk|u1:k, y1:k) = η p(yk|xk) p(xk|u1:k, y1:k−1),

where we have employed Bayes’ rule to reverse the dependence and η

serves to preserve the axiom of total probability.

45


UTIASIntroduce hidden state

Turning our attention to the second factor, we introduce the hidden

state, xk−1, and integrate over all possible values:

p(xk|u1:k, y1:k−1) =

∫p(xk, xk−1|u1:k, y1:k−1) dxk−1

=

∫p(xk|xk−1, u1:k, y1:k−1) p(xk−1|u1:k, y1:k−1) dxk−1

The introduction of the hidden state can be viewed as the opposite of

marginalization. So far we have not introduced any approximations.

46


UTIASEmploy the Markov property

The next step is subtle and is the cause of many limitations in

recursive estimation. Since our system enjoys the Markov property, we

use said property (on the estimator) to say that

p(xk|xk−1, u1:k, y1:k−1) = p(xk|xk−1, uk),

p(xk−1|u1:k, y1:k−1) = p(xk−1|u1:k−1, y1:k−1),

which seems entirely reasonable. However, we’ll come back to

examine this step later.

x0

g g g

h h hx1xk−1 xk

uk

yk

wk

nknk−1

wk−1

uk−1

yk−1y1

u1

w1

n1

· · · · · ·

47


UTIASSubstitute in

Substituting in the previous steps, we have the Bayes filter:

p(xk|u1:k, y1:k)⌅ ⇤⇥ ⇧

posterior

belief

= η p(yk|xk)⌅ ⇤⇥ ⇧

observation

correction

using g(·)

∫

p(xk|xk−1, uk)⌅ ⇤⇥ ⇧

motion

prediction

using h(·)

p(xk−1|u1:k−1, y1:k−1)⌅ ⇤⇥ ⇧

prior

belief

dxk−1

We can see that it takes on a predictor-corrector form:

! First, the prior belief, p(xk−1|u1:k−1, y1:k−1) is propagated forwards

in time using the interoceptive measurement, uk, and the motion

model, h(·).

! This new estimate is then corrected using the exteroceptive

measurement, yk, and the observation model, g(·).

! The result is the posterior belief, p(xk|u1:k, y1:k).

48


UTIASTaxonomy of Bayes filter approximations

Bayes

Filter

Gaussian

Filters

Maximum

Likelihood Filters

Particle

Filters

Extended

Kalman

Filter

Unscented

Kalman

Filter

Hybrid

Filters

Rao-Blackwellized

Particle

Filter

approximate PDFs using

mean only


mean and covariance


finite # of samples

approximate PDFs using combination of

mean/cov and samples

linearize the motion and observation models

and pass Gaussians through exactly

use the unscented trans- formation to pass Gaussians

through nonlinear motion and observation models

49


UTIASExtended Kalman filter setup

! We now show that if

(i) the belief is constrained to be Gaussian,

(ii) the noise Gaussian,

(iii) and we linearize the motion and observation models in order to

carry out the integral (and also the direct product) in the Bayes filter,

we arrive at the famous extended Kalman filter (EKF)2.

! The EKF is still the mainstay of estimation and data fusion in

many circles, and can sometimes be effective for mildly nonlinear

non-Gaussian systems.

! For a good reference on the EKF see Maybeck (1994).

2The EKF is called ‘extended’ because it is the extension of the Kalman filter to

nonlinear systems.

50


UTIASApproximation 1

We first limit (i.e., constrain) our belief function for xk to be Gaussian:

p(xk|u1:k, y1:k) → N(

xk, Pk

⇥

where xk is the mean and Pk the covariance. Next, we assume that the

noise variables, wk and nk (∀k), are in fact Gaussian as well:

wk ∼ N (0, Qk)

nk ∼ N (0, Rk)

Note, a Gaussian PDF can be transformed through a nonlinearity to be

non-Gaussian.

51


UTIASApproximation 2

With g(·) and h(·) nonlinear, we still cannot compute the integral in the

Bayes filter in closed form so we turn to linearization. We linearize the

motion models about the current state estimate mean:

h (xk−1, uk, wk) ≈ x−k + Hx,k (xk−1 − xk−1) + Hw,kwk

g (xk, nk) ≈ yk + Gx,k

(

xk − x−k

⇥

+ Gn,knk

where

x−

k:= h

`

xk−1, uk, 0´

, Hx,k :=∂h(xk−1, uk, wk)

∂xk−1

˛

˛

˛

˛

˛

xk−1,uk,0

, Hw,k :=∂h(xk−1, uk, wk)

∂wk

˛

˛

˛

˛

˛

xk−1,uk,0

,

andyk := g

“

x−

k, 0

”

, Gx,k :=∂g(xk, nk)

∂xk

˛

˛

˛

˛

˛

x−

k,0

, Gn,k :=∂g(xk, nk)

∂nk

˛

˛

˛

˛

˛

x−

k,0

.

52


UTIASMotion model PDF

From here the statistical properties of the current state, xk, given theold state and interoceptive measurement, are

xk ≈ x−

k+ Hx,k (xk−1 − xk−1) + Hw,kwk

E [xk] = x−

k+ Hx,k (xk−1 − xk−1) + Hw,k E [wk]

| {z }

0

Eh

(xk − E [xk]) (xk − E [xk])Ti

= Hw,k Eh

wkwTk

i

| {z }

Qk

HTw,k

p(xk|xk−1, uk) → N“

x−

k+ Hx,k (xk−1 − xk−1) , Hw,kQkHT

w,k

”

.

53


UTIASObservation model PDF

For the statistical properties of the current exteroceptive measurement,yk, given the current state, we have

yk ≈ yk + Gx,k

“

xk − x−

k

”

+ Gn,knk

E [yk] = yk + Gx,k

“

xk − x−

k

”

+ Gn,k E [nk]| {z }

0

Eh

(yk − E [yk]) (yk − E [yk])Ti

= Gn,k Eh

nknTk

i

| {z }

Rk

GTn,k

p(yk|xk) → N“

yk + Gx,k(xk − x−

k), Gn,kRkGT

n,k

”

54


UTIASBayes filter

Substituting in these results, the Bayes filter then looks like

p(xk|y1:k, u1:k)| {z }

N(xk,Pk)

= η p(yk|xk)| {z }

N

“

yk+Gx,k(xk−x−

k),Gn,kRkGT

n,k

”

×

Z

p(xk|xk−1, uk)| {z }

N

“

x−

k+Hx,k(xk−1−xk−1),Hw,kQkHT

w,k

”

p(xk−1|y1:k−1, u1:k−1)| {z }

N(xk−1,Pk−1)

dxk−1.

55


UTIASConvolve the GaussiansWe have

p(xk|y1:k, u1:k)| {z }

N(xk,Pk)

= η p(yk|xk)| {z }

N

“

yk+Gx,k(xk−x−

k),Gn,kRkGT

n,k

”

×

Z

p(xk|xk−1, uk)| {z }

N

“

x−

k+Hx,k(xk−1−xk−1),Hw,kQkHT

w,k

”

p(xk−1|y1:k−1, u1:k−1)| {z }

N(xk−1,Pk−1)

dxk−1.

Use our identity for Gaussian inference (see Lecture 2):

p(xk|y1:k, u1:k)| {z }

N(xk,Pk)

= η p(yk|xk)| {z }

N

“

yk+Gx,k

“

xk−x−

k

”

,Gn,kRkGTn,k

”

×

Z

p(xk|xk−1, uk) p(xk−1|y1:k−1, u1:k−1) dxk−1

| {z }

N

“

x−

k,Hx,k Pk−1HT

x,k+Hw,kQkHT

w,k

”

.

56

Requires an identity for passing a Gaussian through a nonlinearity (not presented):


UTIASMultiply the GaussiansWe are now left with the direct product of two Gaussian PDFs, whichwe also discussed previously:

p(xk|y1:k, u1:k)| {z }

N(xk,Pk)

= η p(yk|xk)| {z }

N

“

yk+Gx,k

“

xk−x−

k

”

,Gn,kRkGTn,k

”

×

Z


| {z }

N

“

x−

k,Hx,k Pk−1HT

x,k+Hw,kQkHT

w,k

”

.

Applying our direct product identity (see Lecture 2), we find that

p(xk|y1:k, u1:k)| {z }

N(xk,Pk)

= η p(yk|xk)

Z


| {z }

N

“

x−

k+Kk(yk−yk),(1−KkGx,k)(Hx,k Pk−1HT

x,k+Hw,kQkHT

w,k)”

where Kk = P−k GT

x,k

(

Gx,kP−k GT

x,k + Gn,kRkGTn,k

⇥−1.

57

Requires another identity for direct product of Gaussian (not presented)


UTIASCanonical EKF

Comparing left and right sides of our posterior expression above we

have

Predictor:P−k = Hx,kPk−1HT

x,k + Hw,kQkHTw,k

x−k = h(xk−1, uk, 0)

Kalman Gain: Kk = P−k GTx,k

⇤

Gx,kP−k GTx,k + Gn,kRkGT

n,k

⌅−1

Corrector:Pk = (1−KkGx,k) P−k

xk = x−k + Kk

(yk − g(x−k , 0)

⇥

︷⌥ ⌦

innovation

These are the classic recursive update equations for the EKF. The

update equations allow us to compute⇧

xk, Pk

⌃

from⇧

xk−1, Pk−1

⌃

.

58


UTIASEKF final thoughts

! There is no proof that the EKF will work in general for any

nonlinear system 3.

! In order to gauge the performance of the EKF on a particular

nonlinear system, it often comes down to simply trying it out.

! The main problem with the EKF is the operating point of the

linearization is the mean of our estimate of the state, not the true

state.

! This seemingly small difference can cause the EKF to diverge

wildly in some cases. Sometimes the result is less dramatic, with

the estimate being biased or inconsistent, or most often, both.

3To the best knowledge of the author.

59


Predictor-Corrector

60

Prediction

-high frequency

-internal sensors

-odometry

-inertial sensors

-low computational cost

Correction

-low frequency

-external “map” needed

-vision, GPS, laser

-high computational cost

Fusion

-optimal combination


EKF Mobile Robot Example

61

! We’ll apply the EKF to this nonlinear-non-Gaussian problem: a

mobile robot driving around in a room full of round landmarks


UTIAS‘Lost in the Woods Dataset’

The mobile robot was driven around for 20 minutes and three streams

of data were logged:

! laser rangefinder scans consisting of 681 range measurements

spread over a 240◦ horizontal field of view centered on straight

ahead (Hokuyo URG-04LX sensor), logged at 10 Hz

! robot’s speed based on wheel odometry, logged at 10 Hz

! groundtruth position of a marker on the laser rangefinder origin,

logged at 40 Hz


UTIASLaser scanner

−4 −2 0 2 4 6 8 10 12−5

−4

−3

−2

−1

0

1

2

3

4

5

6

x [m]

y [

m]

Ground−truth Map, Laser Scan at True Robot Position

! Range/bearing to landmarks already extracted from raw laser data

! Data association is provided; you know which measurement

belongs to which landmark


UTIASOdometry

−2 0 2 4 6 8 10−6

−5

−4

−3

−2

−1

0

1

2

3

4

x [m]

y [

m]

Ground−truth Map, Odometry Robot Path


UTIASMotion and observation models

For this nonlinear estimation problem, use the following vehicle and sensor models:

motion:

2

4

xk

yk

θk

3

5

| {z }

xk

=

2

4

xk−1

yk−1

θk−1

3

5

| {z }

xk−1

+T

2

4

cos θk−1 0sin θk−1 0

0 1

3

5

„»vk

ωk

–

| {z }

uk

+wk

«

| {z }

h(xk−1,uk,wk)

observation:

»rk,l

φk,l

–

| {z }

yk,l

=

» p(xl − xk − d cos θk)2 + (yl − yk − d sin θk)2

atan2(yl − yk − d sin θk, xl − xk − d cos θk)− θk

–

+ nk,l

| {z }

g(xk,nk,l)

where (xk, yk, θk) is the robot’s position/orientation, (vk, ωk) is the robot’s translational/rotational

speed (derived from odometers), wk is the process noise, T is the sampling period, (rk,l, φk,l) is

the range/bearing to landmark l (derived from the laser rangefinder), (xl, yl) is the position of the

center of landmark l, and nk,l is the exteroceptive sensor noise. The distance between the robot

center and the laser rangefinder is d (the laser is at the front of the robot). Note that at each

timestep, k, there are multiple exteroceptive measurements.


UTIASPrepare the Jacobians

Q2. Write out expressions for all the Jacobians (of the motion and

observation models) that are required in the extended Kalman filter:

Hx,k :=∂h(xk−1, uk, wk)

∂xk−1

˛

˛

˛

˛

xk−1,uk,0

, Hw,k :=∂h(xk−1, uk, wk)

∂wk

˛

˛

˛

˛

xk−1,uk,0

,

Gx,k :=∂g(xk, nk)

∂xk

˛

˛

˛

˛

x−

k,0

, Gn,k :=∂g(xk, nk)

∂nk

˛

˛

˛

˛

x−

k,0

.


UTIASSet up the EKF

Q3. Modify and write out the equations of the extended Kalman filter,

modified to handle a variable number of exteroceptive measurements

at each timestep.

Predictor:P−

k= Hx,kPk−1HT

x,k + Hw,kQkHTw,k

x−

k= h(xk−1, uk, 0)

Kalman Gain: Kk = P−

kGT

x,k

“

Gx,kP−

kGT

x,k + Gn,kRkGTn,k

”−1

Corrector:Pk = (1− KkGx,k) P

−

k

xk = x−

k+ Kk

“

yk − g(x−

k, 0)

”

| {z }

innovation


Taxonomy of Filters

69

Bayes

Filter

Gaussian

Filters

Maximum

Likelihood Filters

Particle

Filters

Extended

Kalman

Filter

Unscented

Kalman

Filter

Hybrid

Filters

Rao-Blackwellized

Particle

Filter


mean only


mean and covariance


finite # of samples








Particle Filter

70

1. we draw M samples from the prior density

2. for each particle, draw samples from the motion noise density

(often just one motion noise sample is used per particle)

3. generate a prediction of the posterior PDF using the inputs; we do this by passing each

prior particle/noise sample through the nonlinear motion model

(this gives us an approximation of )

4. the last step is to incorporate the measurements, which we show on the next slide

x(m)k−1 ← p (xk−1|u1:k−1, y1:k−1)

w(m,lm)k ← p (wk)

x(m,lm)−

k := h⇣

x(m)k−1, uk, w

(m,lm)l

⌘

, (∀m, lm)∀

ximate p (xk|u1:k, y1:k−1).


Particle Filter

71

4. the last step is to incorporate the measurements; this happens in two steps

a. assign a scalar weight to each predicted particle based on the divergence between the

desired posterior and the predicted posterior (for each particle)

which is typically accomplished in practice by simulating an expected sensor reading

using the nonlinear observation model

We then assume that and the right-hand side is a

known density (e.g., Gaussian)

b. resample the posterior based on the weight assigned to each predicted posterior particle

ticle:

w(m,lm)k

:=

p

„

x(m,lm)−

k|u1:k, y1:k

«

p

„

x(m,lm)−

k|u1:k, y1:k−1

« = η p

„

yk|x(m,lm)−

k

«

, (∀m, lm) ,

y(m,lm)k

= g

„

x(m,lm)−

k, 0

«

, (∀m, lm) .

„ «

“ ”

„ «

assume p

„

yk|x(m,lm)−

k

«

= p“

yk|y(m,lm)k

”

,

x(m,lm)k

resample←−

⇢

x(m,lm)−

k, w

(m,lm)k

ff

several different ways.


MCL Example with Just 2 Range Sensors

72

red = wheel odometry


Taxonomy of Filters

73

Bayes

Filter

Gaussian

Filters

Maximum

Likelihood Filters

Particle

Filters

Extended

Kalman

Filter

Unscented

Kalman

Filter

Hybrid

Filters

Rao-Blackwellized

Particle

Filter


mean only


mean and covariance


finite # of samples








UTIASUKF Prediction: Steps 1-2To go from the prior belief,

⇤

xk−1, Pk−1

⌅

, to the predicted belief,⇤

x−k

, P−k

⌅

:

1. Both the prior belief and the motion noise have uncertainty so these are

stacked together in the following way:

z :=

[

xk−1

0

⇥

, Y :=

[

Pk−1 0

0 Qk

⇥

where we see that {z, Y} is still a Gaussian representation. We let

L = dim z.

2. Convert {z, Y} to a sigma-point representation:

SST:= Y (Cholesky decomposition, S lower-triangular)

Z0 := z

Zi := z +√

L + κ coliSi = 1 . . . LZi+L := z −

√L + κ coliS


UTIASUKF Prediction: Steps 3-4

3. Unstack each sigma-point into state and motion noise,

Zi =:

[

Xi,k−1

Wi,k

⇥

,

and then pass each sigma point through the nonlinear motion model

exactly:

X−

i,k := h (Xi,k−1, uk,Wi,k) i = 0 . . . 2L.

Note that the motion measurement, uk, is required.

4. Recombine the transformed sigma points into the predicted belief,⇤

x−

k , P−

k

⌅

, according to

x−

k:=

1

L + κ

κX−

0,k+

1

2

2LX

i=1

X−

i,k

!

,

P−

k:=

1

L + κ

κ

“

X−

0,k− x

−

k

”“

X−

0,k−

ˆx−

k

”T

+1

2

2LX

i=1

“

X−

i,k− x

−

k

”“

X−

i,k− x

−

k

”T

!

.


UTIASUKF Correction and Kalman Gain: Steps 1-2To go from the predicted belief,

⇤

x−

k, P−

k

⌅

, to the posterior belief,⇤

xk, Pk

⌅

:

1. Both the predicted belief and the observation noise have uncertainty so

these are stacked together in the following way:

z :=

[

x−

k

0

⇥

, Y :=

[

P−

k0

0 Rk

⇥

where we see that {z, Y} is still a Gaussian representation. We let

L = dim z.

2. Convert {z, Y} to a sigma-point representation:

SST

:= Y (Cholesky decomposition, S lower-triangular)

Z0 := z

Zi := z +√

L + κ coliSi = 1 . . . LZi+L := z −

√L + κ coliS


UTIASUKF Correction and Kalman Gain: Steps 3-4

3. Unstack each sigma-point into state and observation noise,

Zi =:

⇤

X−i,kNi,k

⌅

,

and then pass each sigma point through the nonlinear observation

model exactly:

Yi,k := g(

X−i,k,Ni,k

⇥

i = 0 . . . 2L.

4. Recombine the transformed sigma points into the predictedmeasurement and covariance, {yk, Vk}, according to

yk :=1

L + κ

κY0,k +1

2

2LX

i=1

Yi,k

!

,

Vk :=1

L + κ

κ`

Y0,k − yk

´ `

Y0,k − yk

´T+

1

2

2LX

i=1

`

Yi,k − yk

´ `

Yi,k − yk

´T

!

.

Note, the matrix Vk takes on the role of Gx,kP−k GTx,k + Gn,kRkGT

n,k in the

EKF.


UTIASUKF Correction and Kalman Gain: Steps 5-6

5. Build the ‘state-measurement covariance’ and ‘Kalman gain’ accordingto

Uk :=1

L + κ

κ

“

X−

0,k− x

−

k

”

`

Y0,k − yk

´T+

1

2

2LX

i=1

“

X−

i,k− x

−

k

”

`

Yi,k − yk

´T

!

,

Kk := UkV−1

k.

Note, the matrix Uk takes on the role of P−k GTx,k in the EKF.

6. Compute the posterior belief,{

xk, Pk

⇥

, according to

xk := x−k + Kk (yk − yk) ,

Pk := P−k −KkUTk .

Note, we have used the observation measurement, yk, in the update.

The update equation for the posterior mean is identical to the EKF.

Moreover, we can see that if Uk were indeed P−k GTx,k, we would recover

the usual EKF covariance update: Pk = (1−KkGx,k) P−k .


UTIASUKF thoughts

The advantages of the UKF are:

! It doesn’t require any analytical derivatives.

! It uses only basic linear algebra operations in the implementation.

! We don’t need the nonlinear motion and observation models in

closed form; they could just be black-box software functions.

! It requires about the same computational cost as using numerical

derivatives with linearization.

The UKF should be used instead of the EKF in almost all

situations!


UTIASUKF Example: load-haul-dump vehicle

SICK Laser SICK Laser Angle Sensor

Odometer Tachometer,

Gear

ST1010


UTIASUKF Example: LHD control system

{st−1,Pt−1}

Old Pose

Estimate

Pose Prediction(using Eqs. (3))

ut−1, γt−1

Odometry

Predicted

Pose

Estimate

{

s−

t,P−

t

⇥

Pose Correction(using UKF)

Current

Local

Metric

Map

Laser

Rangefinder

Readingszt

Corrected

Pose

Estimate

{st,Pt}Find Closest

Path Point andCalculate Errors(using Eqs. (7))

εL, εH

Path

Profile

Path-

Tracking

Errors


UTIASUKF Example: LHD

(Open document in Acrobat Reader to play this movie.)

! Motion model was easy to express in closed form.

! Observation model was just a software function that traced laser

rays to an occupancy grid map.


UTIASUKF Example: sigma-point illustration

S0,t

Center

S1,t

Forward

S4,t

Backward

S2,t

Left

S5,t

Right

S6,t

Rotated Right

S3,t

Rotated Left

Actual laser

readings are

attached to

vehicle

pose.

Simulated

laser

readings will

lie on map

walls.

The vehicle pose represented

by σ-point S5,t will have

the best correspondence

between simulated and actual

laser readings in this case.


UTIASUKF Example: poor initial prior

(Open document in Acrobat Reader to play this movie.)


Summary of Lecture

‣ Introduced the basics of recursive state estimation

‣ sensors and sensing principles

‣ sensor uncertainty → probability

‣ Bayes filter

‣ Extended Kalman filter

‣ particle filter

‣ Sigmapoint Kalman filter

‣ Did not cover

‣ batch state estimation

‣ visual state estimation (e.g., visual odometry, vSLAM)

86

introduction to sensing and recursive state estimation introduce the basics of recursive state...

Documents