EECE 5550 Mobile Robotics

Lecture 5

Uncertainty & Kalman Filter

Dr. Xian Li

Department of Electrical and Computer Engineering

Northeastern University

Example: measure weight with imperfect scales

An example to understand fusing two noisy measurements

Fact: Scale A is good to ±2 lb, scale B to ±5 lb, and neither is biased.

Measurement: Scale A reads "160 lbs", while Scale B reads "170 lbs".

What can we conclude about your weight?

Well, what are our choices?

Choose a number less than both A and B.
Choose a number greater than both A and B.
Choose to only trust A, and assign 160lbs to our weight estimate.
Choose to only trust B, and assign 170lbs to our weight.
Choose an average number: 165 lb ± 2.7 lb
Weight each by how much you trust it: 161.4 lb ± 1.9 lb

➔ If neither scale is biased, each one's error is as likely to be high as low.

➔ Nothing points you outside the two readings.

➔ Ignoring a measurement throws away information, and it only makes sense if that scale is useless.

➔ Probably wrong, but reasonable

➔ A better estimation: weighted average!

Try it: fuse two scales

Move the readings and uncertainties. The weighted average trusts each scale by 1/σ².

Scale A

Reading Good to ±

Scale B

Reading Good to ±

\hat{x} = \frac{z_{A} / σ_{A}^{2} + z_{B} / σ_{B}^{2}}{1 / σ_{A}^{2} + 1 / σ_{B}^{2}}

EECE 5550 Mobile Robotics

Where is the robot?

The robot faces a wall. Two estimates of the distance disagree.

2.00 m

odometry

Integrated from the wheels since a known start. Its error has been growing the whole way: σ ≈ 5 cm.

2.03 m

LiDAR

One beam, straight at the wall, right now. Your Lab 2 sheet says about 1% of range: σ ≈ 2 cm.

your answer

Pick one, average them, or something better. You will learn an “optimal” way: Kalman Filter

By the end of today you can answer this with a number and an error bar, and say why the error bar is smaller than either one.

EECE 5550 Mobile Robotics

Today

How to combine two estimates that are both wrong, and end up less wrong than either?

❏Belief is a distribution

❏Propagating uncertainty

❏Bayes’ rule, working form

❏The recursive Bayes filter

❏The Kalman filter

Reading

[1] How a Kalman filter works, in pictures

[2] Kalman FIlter Tutorial

[3] Probabilistic Robotics, by Dieter Fox, Sebastian Thrun, and Wolfram Burgard

[4] GitHub Repo: Kalman-and-Bayesian-Filters-in-Python

Problem Set 2 and Lab 2 & 3

SECTION 1

Belief is a distribution

Mean, variance, covariance, the ellipse

A simple example

Belief: A probability distribution that models uncertainty over possible states X of the world

Suppose I roll a fair die, but don’t tell you the result X

Q1: What should your belief be about X?

A1: Prior: p(X=x)=⅙ for all x in {1, …, 6}

Q2: Suppose that now I look at X, and tell you that its value is even. What should your belief be now?

A2: Posterior (conditional): p(X=x|X in {2,4,6}) = $\frac{p (X = x)}{p ({2, 4, 6})} = 1 / 3$

Key point: In this example, it is not the world that is changing, but rather our information about the world!

Remember: Beliefs model our state of knowledge of the world

A blue dice cup tipped over next to two dice

Try it: update a belief

I roll a hidden die. Ask questions and watch your belief change. The die never changes; only what you know about it does.

Ask about the roll

EECE 5550 Mobile Robotics

Systematic error V.S. Random error

Lecture 4 removed the shift. Today is about the spread

Systematic error: the shift

Same every run. UMBmark measures it and calibration removes it. Lecture 4 and Lab 2 Task 3.

Random error: the spread

Different every run, so no calibration can remove it. The best you can do is know how big it is, carry that through every calculation, and combine sources to shrink it.

Question: Difference between Accuracy & Precision?

EECE 5550 Mobile Robotics

Systematic error V.S. Random error

Lecture 4 removed the shift. Today is about the spread

Systematic error: the shift

Same every run. UMBmark measures it and calibration removes it. Lecture 4 and Lab 2 Task 3.

Random error: the spread

Different every run, so no calibration can remove it. The best you can do is know how big it is, carry that through every calculation, and combine sources to shrink it.

Question:

What does “high accuracy, low precision” look like?

EECE 5550 Mobile Robotics

The robot's belief is a distribution, not a number

p (x) = \frac{1}{σ \sqrt{2 π}} \exp (- \frac{{(x - μ)}^{2}}{2 σ^{2}})

Mean μ: the best single guess

Variance σ²: how unsure; σ is in the same units as x

Gaussian (Normal) Distribution

EECE 5550 Mobile Robotics

The robot's belief is a distribution, not a number

Every estimate on a robot should come with its σ. A position without an error bar cannot be combined with anything.

p (x) = \frac{1}{σ \sqrt{2 π}} \exp (- \frac{{(x - μ)}^{2}}{2 σ^{2}})

Mean μ: the best single guess

Variance σ²: how unsure; σ is in the same units as x

Within 1σ, 2σ, 3σ: 68.27%, 95.45%, 99.73% of the time

Gaussian (Normal) Distribution

EECE 5550 Mobile Robotics

Why Gaussian?

Assume Gaussian because it is usually close and always convenient. Then check whether your data agree.

EECE 5550 Mobile Robotics

Why Gaussian?

Assume Gaussian because it is usually close and always convenient. Then check whether your data agree.

Central limit theorem

Odometry adds thousands of small, independent wheel errors. Their sum tends to a Gaussian whatever each one looks like

EECE 5550 Mobile Robotics

Why Gaussian?

Assume Gaussian because it is usually close and always convenient. Then check whether your data agree.

The math closes

A linear map of a Gaussian is Gaussian. A product of two Gaussians is Gaussian. So a filter that starts Gaussian stays Gaussian, and two numbers, μ and σ, describe it completely.

EECE 5550 Mobile Robotics

Why Gaussian?

Assume Gaussian because it is usually close and always convenient. Then check whether your data agree.

When it fails

A slip is one big error, not many small ones.

Kidnapped Robot Problem.

EECE 5550 Mobile Robotics

Demo: beam_watch.py

The robot sits 2 m from a flat wall. Nothing moves. Watch the one beam that points at it.

The wall does not move; the number does

Your beam_watch.py, simulated: one LiDAR beam at a wall 2.00 m away, noise 4 mm.

Python starts when the lecture gets close to this slide.

Run

Beam noise 4 mm

Your script uses 4 mm. Changing it restarts the run.

Python not started yet.

Gaussians

Univariate

p (x) \sim N (μ, σ^{2}) :

p (x) = \frac{1}{\sqrt{2 π} σ} e^{- \frac{1}{2} \frac{{(x - μ)}^{2}}{σ^{2}}}

Multivariate

p (𝐱) \sim N (𝛍, 𝚺) :

p (𝐱) = \frac{1}{{(2 π)}^{d / 2} {| 𝚺 |}^{1 / 2}} e^{- \frac{1}{2} {(𝐱 - 𝛍)}^{T} 𝚺^{- 1} (𝐱 - 𝛍)}

EECE 5550 Mobile Robotics

Gaussians

2D case

Σ = [\begin{matrix} σ_{x}^{2} & σ_{x y} \\ σ_{x y} & σ_{y}^{2} \end{matrix}], ρ = \frac{σ_{x y}}{σ_{x} σ_{y}}

Diagonal: the variance of each coordinate on its own

Off-diagonal σxy: whether an error in x comes with an error in y

ρ between −1 and 1: 0 means independent; ±1 means one determines the other

In 2D, the 2σ ellipse holds 86% of the samples, not 95%. The 1σ ellipse holds only 39%.

Gaussians

2D case

Σ = [\begin{matrix} σ_{x}^{2} & σ_{x y} \\ σ_{x y} & σ_{y}^{2} \end{matrix}], ρ = \frac{σ_{x y}}{σ_{x} σ_{y}}

σ_x²σ_y²σ_xy

samples 2σ ellipse

shading: the joint density p(x, y); top and right: the marginals p(x) and p(y)

SECTION 2

Propagating uncertainty

Model. Why odometry error grows sideways, and the banana

EECE 5550 Mobile Robotics

Uncertainty

Two rules carry it through any calculation

Errors scale

Convert a wheel's ticks to meters with a = 2πr / 508.8, and the tick noise is scaled by the same a. A 1% error in r is a 1% error in every distance.

Variances add

Two independent errors add in variance, not in size. Five steps of 1 cm each give √5 = 2.2 cm, not 5 cm. That is why random error grows as √n.

Odometry is a long sum of small steps. Its random error grows like a square root, and its systematic error grows in a straight line.

y = a x + b \Rightarrow σ_{y} = | a | σ_{x} z = x_{1} + x_{2} \Rightarrow σ_{z}^{2} = σ_{1}^{2} + σ_{2}^{2}

EECE 5550 Mobile Robotics

Uncertainty

The same rule, for vectors

𝐲 = A 𝐱 \Rightarrow Σ_{y} = A Σ_{x} A^{T}

A maps the old variables to the new ones

A Σ A-transpose is the only formula you need: it covers scaling, adding, and the cross terms

Check: with A = [a] it gives σ² = a²σx²; with A = [1 1] and independent inputs it gives σ1² + σ2²

EECE 5550 Mobile Robotics

Motion model

Your TurtleBot uses this one

\begin{matrix} x' = x + d \cos (θ + \frac{Δ θ}{2}) \\ y' = y + d \sin (θ + \frac{Δ θ}{2}) \\ θ' = θ + Δ θ \end{matrix} d = \frac{d_{r} + d_{l}}{2}, Δ θ = \frac{d_{r} - d_{l}}{w}

Inputs u: the two wheel distances

dr and dl, from /wheel_ticks. Each carries its own small random error.

State x: the pose

x, y, θ. The cos and sin of θ are what make the model nonlinear.

+xr

+yr

(x,y)

EECE 5550 Mobile Robotics

Motion model

Linearize: the Jacobian of one step

\begin{matrix} x' = x + d \cos (θ + \frac{Δ θ}{2}) \\ y' = y + d \sin (θ + \frac{Δ θ}{2}) \\ θ' = θ + Δ θ \end{matrix} d = \frac{d_{r} + d_{l}}{2}, Δ θ = \frac{d_{r} - d_{l}}{w}

+xr

+yr

(x,y)

F_{x} = \frac{\partial f}{\partial (x, y, θ)} = [\begin{matrix} 1 & 0 & - d \sin ϕ \\ 0 & 1 & d \cos ϕ \\ 0 & 0 & 1 \end{matrix}], ϕ = θ + \frac{Δ θ}{2}

Derivative

A heading error moves the end of the step in x & in y

An error you already have is carried forward unchanged.

If you were 2 cm off before the step, you are still 2 cm off after it.

A position error cannot change the heading.

Question: Why ?

(θ + \frac{Δ θ}{2})

EECE 5550 Mobile Robotics

Motion model

One step of covariance propagation

P' = F_{x} P F_{x}^{T} + F_{u} Q F_{u}^{T}, Q = [\begin{matrix} k | d_{r} | & 0 \\ 0 & k | d_{l} | \end{matrix}]

First term: old uncertainty, carried forward

Each robot drives d along its own heading, so a heading error becomes a sideways error. Nothing new is added: the ellipse is sheared, not enlarged.

Second term: new uncertainty, added

Each wheel rolls a little more or less than measured; that is Q. F_u turns those wheel errors into pose errors, mostly heading. This is the only term that makes the robot less certain.

Motion model

One step of covariance propagation, drawn two ways. Errors are exaggerated so the effect is visible.

step d 0.50 mstart σθ 8°wheel noise k 1.0e-3

1. Before the step

before: y and θ unrelated

2. First term: carried forward

first term: a shear

3. Second term: new wheel noise

second term: it grows

dots: 700 simulated robots; color: each one’s heading error, right (blue) to left (amber); ellipses: 2σ. Click to switch between the robots’ positions and the y, θ plane.

EECE 5550 Mobile Robotics

Drive straight, and the ellipse grows sideways

	1 m	2 m	4 m	doubling
along the path, σx	0.22 cm	0.32 cm	0.45 cm	×1.414
sideways, σy	1.1 cm	3.11 cm	8.79 cm	×2.828
heading, σθ	1.09°	1.54°	2.18°	×1.414

The pattern

σx grows like √s, but σy like s√s.

Problem Set 2 Problem 2 derives both.

Odometry's uncertainty, propagated live

Your cov_odom.py on a simulated TurtleBot: every step adds wheel noise to P, and the 2σ ellipse grows.

Python starts when the lecture gets close to this slide.

Drive

Uncertainty

Python not started yet.

EECE 5550 Mobile Robotics

The true shape is a banana

Run the noisy motion 4000 times

Heading errors bend each run into an arc, so the cloud curves back toward the start.

➔Linearizing gives an ellipse.

➔Without linearizing, the true spread is a banana whose two ends curl back toward the start,

(Noise exaggerated here so the shape shows: σθ = 21.7°.)

Question: Why bending backwards?

➔Taylor Series

➔Linearizing keeps only the first-order terms.

➔The first term the linearization drops is −dθ²/2, in x.

➔It's never positive. θ² ≥ 0

➔So, backwards

linearizing gives an ellipse, which is the banana's middle extended straight. Without linearizing, the true spread is a banana whose two ends curl back toward the start, by an amount that grows with the square of the heading error.

The first term the linearization drops is −dθ²/2, in x. It explains everything about the bend:

It's never positive. θ² ≥ 0, so a heading error can only reduce forward progress. Any error at all moves the robot backward, never forward.

It's even in θ. θ = +20° and θ = −20° give the same θ², so the same shortfall. Left and right ends bend back by the same amount, which is why the banana is symmetric.

It grows like θ². Twice the heading error gives four times the bend. At θ = 20° with d = 4 m, dθ²/2 = 24.4 cm, against the exact 24.1 cm in the earlier table. At 2°, it's 2.4 mm.

SECTION 3

Bayes' rule, working form

Measurement. Two Gaussians become one

EECE 5550 Mobile Robotics

Bayes' rule: how a measurement changes a belief

\underset{posterior}{\underset{⏟}{p (x ∣ z)}} = \frac{\overset{likelihood}{\overset{⏞}{p (z ∣ x)}} \overset{prior}{\overset{⏞}{p (x)}}}{p (z)}

prior

What you believed before looking.

Here: odometry.

likelihood

How probable this reading is, for each possible true distance.

Here: the LiDAR model.

posterior

What you believe after. Prior times likelihood, rescaled to integrate to 1.

p(z) does not depend on x. It only rescales, so in practice: posterior ∝ likelihood × prior.

Rev. Thomas Bayes

EECE 5550 Mobile Robotics

The prior: what odometry says

Odometry: 2.00 m, σ = 5 cm. Wide, because it has been accumulating error since the start.

EECE 5550 Mobile Robotics

The likelihood: what the LiDAR says

LiDAR: 2.03 m, σ = 2 cm. Narrow, because it measures the wall directly, right now.

EECE 5550 Mobile Robotics

The posterior: both, multiplied

Both: 2.026 m, σ = 1.9 cm. Closer to the LiDAR, because it is more certain, and narrower than either.

EECE 5550 Mobile Robotics

Two Gaussians multiply into one: the precision-weighted mean

μ = \frac{σ_{l}^{2} μ_{o} + σ_{o}^{2} μ_{l}}{σ_{o}^{2} + σ_{l}^{2}} = μ_{o} + \underset{K}{\underset{⏟}{\frac{σ_{o}^{2}}{σ_{o}^{2} + σ_{l}^{2}}}} (μ_{l} - μ_{o}), \frac{1}{σ^{2}} = \frac{1}{σ_{o}^{2}} + \frac{1}{σ_{l}^{2}}

	value	where it comes from
weight on the LiDAR, K	0.862	25 / (25 + 4), in cm²
posterior mean	2.026 m	2.00 + 0.862 × 0.03
posterior σ	1.9 cm	1/σ² = 1/25 + 1/4, in cm⁻²

The weights are not 50/50.

Each estimate is weighted by its precision, 1/σ², so a sensor twice as certain counts four times as much.

SECTION 4

The recursive Bayes filter

Predict, update, repeat

EECE 5550 Mobile Robotics

Example: measure weight with imperfect scales

What if we only have one imperfect scale?

Go on

Multiple measurements!

Easimate based on prediction and measurement.

Example: measure weight with imperfect scales

Day 0Day 1Day 2Day 3Day 4Day 5Day 6Day 7Day 8Day 9Day 10

Initialization!

Question: What are Day 2’s Estimate & Prediction for Day 3?

Question: What are Day 4’s Estimate & Prediction for Day 5?

Day	Measurement	Prediction	Estimate	Truth
0	-	-	-160.0	160.0
1	158.0	161.0	159.8	161.0
2	164.2	160.8	?¹162.16	162.0
3	160.3	?²163.16	162.02	163.0
4	159.9	163.02	?161.77	164.0
5	162.1	?162.77	162.50	165.0
6	164.6	163.50	163.94	166.0
7	169.6	164.94	166.80	167.0
8	167.4	167.80	167.64	168.0
9	166.4	168.64	167.75	169.0
10	171.0	168.75	169.65	170.0

Predict:

prediction = estimate + gain_rate * time_step

Sensor reading:

measurement

Update:

estimate = prediction + 410(measurement − prediction)

Giving:

gain_rate = 1.0 lb/daytime_step = 1.0 day

EECE 5550 Mobile Robotics

Example: measure weight with imperfect scales

Go on

Day N

Try it: tune the filter

Same predict and update loop. Change how much you trust the scale and how fast you think the weight grows.

Filter settings

Update gain Assumed gain_rate

Measurements

EECE 5550 Mobile Robotics

“Predict-Measure-Update” in 1D

EECE 5550 Mobile Robotics

“Predict-Measure-Update” in multivariate

Transformed prediction from Prior belief

Giving a new measurement

Update a Posterior belief

EECE 5550 Mobile Robotics

The recursive Bayes filter: predict, update, repeat

\begin{matrix} predict: & 𝑏𝑒𝑙 (x_{t}) = \int p (x_{t} ∣ u_{t}, x_{t - 1}) 𝑏𝑒𝑙 (x_{t - 1}) d x_{t - 1} \\ update: & 𝑏𝑒𝑙 (x_{t}) = η p (z_{t} ∣ x_{t}) 𝑏𝑒𝑙 (x_{t}) \end{matrix}

Predict is a blur

For every place the robot could have been, spread it out by the motion model, and add up. A blur can only widen the belief.

Update is a reweighting

Multiply by how well each place explains the reading, then rescale with η. That can only sharpen the belief, or move it.

\underset{posterior}{\underset{⏟}{p (x ∣ z)}} = \frac{\overset{likelihood}{\overset{⏞}{p (z ∣ x)}} \overset{prior}{\overset{⏞}{p (x)}}}{p (z)}

The recursive Bayes filter: predict, update, repeat

TurtleBot 4
• odometry predicts at 20 to 62.5 Hz
• LiDAR updates at about 5 to 10 Hz

SECTION 5

The Kalman filter

The gain as a trust ratio; where Q and R come from

1D Example

Measure weight with an imperfect scale

Predict:

prediction = estimate + gain_rate * time_step

Sensor reading:

measurement

Update:

estimate = prediction + 410(measurement − prediction)

Giving:

gain_rate = 1.0 lb/daytime_step = 1.0 day

What if we have a bad guess?

EECE 5550 Mobile Robotics

1D Example

Measure weight with an imperfect scale

Previous: we predict a weight gain of 1 lb/day as perfect fitting to the actual gain of truth

Now: we predict a weight loss of 1 lb/day (-1 lb/day)

gain_rate: 1 lb/day

gain_rate: -1 lb/day

1D Example

Measure weight with an imperfect scale

How to fix?

Compute the gain from existing measurements and estimates!

new gain = old gain + \frac{1}{3} \frac{measurement − predicted weight}{1 day}

Predict:

prediction = estimate + gain_rate * time_step

Sensor reading:

measurement

Update:

estimate = prediction + 410(measurement − prediction)

Giving:

gain_rate = 1.0 lb/daytime_step = 1.0 day

2D Example

Tracking the constant velocity of an aircraft

A radar dish tracking an aircraft at two times, t1 and t2, along a straight line

\dot{x} = v = \frac{d x}{d t}

n	1	2	3	4	5	6	7	8	9	10
$z_{n}$	30171	30353	30756	30799	31018	31278	31276	31379	31748	32175
${\hat{x}}_{n, n}$	30194.2	30383.64	30612.73	30818.93	31025.7	31242.3	31418.8	31566.3	31739.4	31964.1
${\hat{\dot{x}}}_{n, n}$	39.42	38.65	42.2	41.7	41.55	42.44	38.9	34.2	34.4	39.67
${\hat{x}}_{n + 1, n}$	30391.3	30576.9	30823.9	31027.6	31233.4	31454.5	31613.15	31737.24	31911.4	32162.45
${\hat{\dot{x}}}_{n + 1, n}$	39.42	38.65	42.2	41.7	41.55	42.44	38.9	34.2	34.4	39.67

Δ t = 5 s

{\hat{x}}_{0, 0} = 30,000 m

Predict:

{\hat{x}}_{n, n - 1} = {\hat{x}}_{n - 1, n - 1} + Δ t {\hat{\dot{x}}}_{n - 1, n - 1}

{\hat{\dot{x}}}_{n, n - 1} = {\hat{\dot{x}}}_{n - 1, n - 1} = 40 m/s

State Update Equation

α = 0.2

β = 0.1

{\hat{x}}_{n, n} = {\hat{x}}_{n, n - 1} + α (z_{n} - {\hat{x}}_{n, n - 1})

{\hat{\dot{x}}}_{n, n} = {\hat{\dot{x}}}_{n, n - 1} + β (\frac{z_{n} - {\hat{x}}_{n, n - 1}}{Δ t})

z_{n}

Measurement at time n

{\hat{x}}_{n, n}

Estimation of x at time n

{\hat{x}}_{n + 1, n}

Prediction of x at time n

{\hat{x}}_{1, 0} = 30,200 m

{\hat{\dot{x}}}_{1, 0} = 40 m/s

Try it: tune α and β

α sets how far a residual moves the position estimate; β sets how much it changes the velocity.

n	1	2	3	4	5	6	7	8	9	10
$z_{n}$
${\hat{x}}_{n, n}$
${\hat{\dot{x}}}_{n, n}$
${\hat{x}}_{n + 1, n}$
${\hat{\dot{x}}}_{n + 1, n}$

α (position) β (velocity)

Random flights speed up after n = 5

g-h filter or α-β filter

g, h or α, β refer to scaling factors

{\hat{x}}_{n, n} = {\hat{x}}_{n, n - 1} + α (z_{n} - {\hat{x}}_{n, n - 1})

{\hat{\dot{x}}}_{n, n} = {\hat{\dot{x}}}_{n, n - 1} + β (\frac{z_{n} - {\hat{x}}_{n, n - 1}}{Δ t})

new gain = old gain + \frac{1}{3} \frac{measurement − predicted weight}{1 day}

Predict:

prediction = estimate + gain_rate * time_step

Sensor reading:

measurement

Update:

estimate = prediction + 410(measurement − prediction)

Scaling factor g or α

Scaling factor h or β = 0

Giving:

gain_rate = 1.0 lb/daytime_step = 1.0 day

Kalman filter

A special case of α-β filter

Rudolf E. Kalman
1930-2016

α-β filter

State predict

State update

x_{n + 1} = x_{n} + Δ t {\dot{x}}_{n}

{\dot{x}}_{n + 1} = {\dot{x}}_{n}

{\hat{x}}_{n, n} = {\hat{x}}_{n, n - 1} + α (z_{n} - {\hat{x}}_{n, n - 1})

{\hat{\dot{x}}}_{n, n} = {\hat{\dot{x}}}_{n, n - 1} + β (\frac{z_{n} - {\hat{x}}_{n, n - 1}}{Δ t})

Kalman filter, one dimension

State predict

State update

{\hat{x}}_{n + 1, n} = {\hat{x}}_{n, n} + Δ t {\hat{\dot{x}}}_{n, n}

{\hat{\dot{x}}}_{n + 1, n} = {\hat{\dot{x}}}_{n, n}

for constant velocity dynamics

p_{n + 1, n}^{x} = p_{n, n}^{x} + Δ t^{2} p_{n, n}^{v}

p_{n + 1, n}^{v} = p_{n, n}^{v}

for constant velocity dynamics

{\hat{x}}_{n, n} = {\hat{x}}_{n, n - 1} + K_{n} (z_{n} - {\hat{x}}_{n, n - 1})

p_{n, n} = (1 - K_{n}) p_{n, n - 1}

K_{n} = \frac{p_{n, n - 1}}{p_{n, n - 1} + r_{n}}

Kalman filter

Prediction Step

A general case of prediction matrix $F_{k}$ :
Control matrix $B_{k}$ and control vector $u_{k}$

x_{k + 1} = F_{k} x_{k} + B_{k} u_{k}

Consider both states and covariances:
Covariance matrix $Σ_{k}$ and process noise matrix $Q_{k}$

μ_{k + 1} = F_{k} μ_{k} + B_{k} u_{k},

Σ_{k + 1} = F_{k} Σ_{k} F_{k}^{T} + Q_{k}

Kalman filter

Update Step

A general case of measurement model
Measurement $z_{k}$ , observation matrix $H_{k}$ , measurement noise vector $δ_{k}$ and measurement noise covariance matrix $R_{k}$

z_{k} = H_{k} x_{k} + δ_{k}, δ_{k} \sim N (0, R_{k})

The posterior belief for $x_{k}$ given $z_{k}$ is $N (μ_{k}, Σ_{k})$ , where:

μ_{k} = {\hat{μ}}_{k} + K_{k} (z_{k} - H_{k} {\hat{μ}}_{k}),

Σ_{k} = (I - K_{k} H_{k}) {\hat{Σ}}_{k}

And Kalman Gain as

K_{k} = {\hat{Σ}}_{k} H_{k}^{T} {(H_{k} {\hat{Σ}}_{k} H_{k}^{T} + R_{k})}^{- 1}

Kalman filter

Predict: Given belief

x_{t} \sim N (μ_{t}, Σ_{t})

for the current state

x_{t}

, control

u_{t}

, and process model:

x_{t + 1} = A_{t} x_{t} + B_{t} u_{t} + ε_{t}, ε_{t} \sim N (0, R_{t})

the belief for the next state

x_{t + 1}

N (μ_{t + 1}, Σ_{t + 1})

, where:

μ_{t + 1} = A_{t} μ_{t} + B_{t} u_{t}, Σ_{t + 1} = A_{t} Σ_{t} A_{t}^{T} + R_{t}

Update: Given prior belief

x_{t} \sim N ({\hat{μ}}_{t}, {\hat{Σ}}_{t})

for the current state

x_{t}

, measurement

z_{t}

, and measurement model:

z_{t} = C_{t} x_{t} + δ_{t}, δ_{t} \sim N (0, Q_{t})

the posterior belief for

x_{t}

given

z_{t}

N (μ_{t}, Σ_{t})

, where:

μ_{t} = {\hat{μ}}_{t} + K_{t} (z_{t} - C_{t} {\hat{μ}}_{t}), Σ_{t} = (I - K_{t} C_{t}) {\hat{Σ}}_{t}

and

K_{t} = {\hat{Σ}}_{t} C_{t}^{T} {(C_{t} {\hat{Σ}}_{t} C_{t}^{T} + Q_{t})}^{- 1}

EECE 5550 Mobile Robotics

The gain is a trust ratio

K near 1

The prediction is unsure, or the sensor is good. Jump to the measurement.

K near 0

The prediction is sure, or the sensor is noisy. Barely move.

K is not a tuning knob. It is computed every step from P⁻ and r, so it changes as the filter's confidence changes.

EECE 5550 Mobile Robotics

On the wall: the band collapses on every scan

Odometry alone keeps its starting error: 3.71 cm RMS. The filter: 0.67 cm. With the LiDAR off, the band widens from 0.51 to 0.85 cm in 5 s, then snaps back.

A Kalman filter on the distance to a wall

Your wall_kf.py, running in this page: predict on every odometry step, update on every LiDAR scan.

LiDAR ON r 2 cm q 1 cm/√m K - σ -

Python starts when the lecture gets close to this slide.

Run

Lecture run: LiDAR off from 12 to 17 s, as on the slides

Filter keys from wall_kf

Python not started yet.

What wrong noise values do

Three Kalman filters on one run of wall_kf.py: the same odometry, the same LiDAR scans. Only one noise value differs.

Mistune by

Python starts when the lecture gets close to this slide.

An honest filter keeps the truth inside its own ±2σ band about 95% of the time; far less means a noise value is too small. The values in wall_kf.py come from measurement: r from the LiDAR's spread at a fixed distance, q from the odometry scatter per meter driven.

Python not started yet.

EECE 5550 Mobile Robotics

What to take away

Every estimate carries a σ.

A position without an error bar cannot be combined with anything.

Uncertainty propagates through the math.

A Σ A-transpose; odometry error grows sideways, as s√s.

Combining two estimates beats either one.

Weight by precision, 1/σ², and the result is tighter than the best input.

The Kalman filter is predict and update, forever.

Its gain is a trust ratio, computed from Q and R that you measure.