Kalman Filters for Mapping and Localizationsleonard/week08.pdf · 2018-12-19 · Kalman Filters for...

600.436/600.636G.D. Hager

S. Leonard

Kalman Filters for Mapping and

Localization

600.436/600.636G.D. Hager

S. Leonard

Robot Arms

• Very accurate

– Encoders at each joint (0.04 degree

resolution)

– Industrial grade design and

assembled

– ISO standard for accuracy and

repeatability (9283)

– Some arms are even metrology

instruments

600.436/600.636G.D. Hager

S. Leonard

Mobile Robots

• Wheels and steering

– Wheel slip

– Wheel diameter

– Wheel alignment

– Steering alignment

– Integration of velocity

600.436/600.636G.D. Hager

S. Leonard

600.436/600.636G.D. Hager

S. Leonard

Sensors

IR rangefindersonar rangefinder

Light-field camera

KinectLaser

If you can’t model the world, then sensors

are the robot’s link to the external world

(obsession with depth)

600.436/600.636G.D. Hager

S. Leonard

Robots’ link to the external world...

Sensors, sensors, sensors!

and tracking what is sensed: world models

Force/

TorqueInertial measurement unit

(gyro + accelerometer)

Gyroscope

(orientatiation)

Compass

GPS

Sensors

600.436/600.636G.D. Hager

S. Leonard

16-735, Howie Choset with slides

from G.D. Hager and Z. Dodds

Infrared sensors“Noncontact bump sensor”

IR emitter/detector pair

(1) sensing is based on light intensity.

diffuse distance-sensing IR

“object-sensing” IR

looks for changes

at this distance

(2) sensing is based

on angle receved.

600.436/600.636G.D. Hager

S. Leonard

Infrared Calibration

in the dark

The response to white copy paper (a dull, reflective surface)

inches

15º increments

raw values

(put into 4 bits)

fluorescent light incandescent light



600.436/600.636G.D. Hager

S. Leonard

energy vs. distance for various materials

( the incident angle is 0º, or head-on )

( with no ambient light )

Infrared Calibration



600.436/600.636G.D. Hager

S. Leonard

IR Sensor on the Roomba

600.436/600.636G.D. Hager

S. Leonard

Sonar sensing

No lower range limit for paired sonars...

Polaroid sonar emitter/receivers

single-transducer sonar timeline

0

a “chirp” is emitted

into the environment

75ms

typically when

reverberations

from the initial

chirp have stopped

.5sthe transducer goes into

“receiving” mode and

awaits a signal...

limiting range sensing

after a short time, the

signal will be too weak

to be detected

blanking time



600.436/600.636G.D. Hager

S. Leonard

Sonar effects

(d) Specular reflections

cause walls to disappear

(e) Open corners produce a

weak spherical wavefront

(f) Closed corners measure to the

corner itself because of multiple

reflections --> sonar ray tracing

(a) Sonar providing an

accurate range measurement

(b-c) Lateral resolution is not very

precise; the closest object in the

beam’s cone provides the response



600.436/600.636G.D. Hager

S. Leonard

Sonar modeling

initial time response

spatial response

blanking time

accumulated

responses

cone width



600.436/600.636G.D. Hager

S. Leonard

600.436/600.636G.D. Hager

S. Leonard

Laser Ranging

LIDAR/Laser range finder



600.436/600.636G.D. Hager

S. Leonard

64 lasers

600.436/600.636G.D. Hager

S. Leonard

Recent...

Structured light:

Project a known dot

pattern with an IR

transmitter (invisible to

humans)

Infer depth from deformation to that pattern

depth from focus: Points far away are blurry

depth from stereo: Closer points are shifted

600.436/600.636G.D. Hager

S. Leonard

The Problem

• Mapping: What is the world around me (geometry, landmarks)

– sense from various positions

– integrate measurements to produce map

– assumes perfect knowledge of position

• Localization: Where am I in the world (position wrt landmarks)

– sense

– relate sensor readings to a world model

– compute location relative to model

– assumes a perfect world model

• Together, these are SLAM (Simultaneous Localization and Mapping)

– How can you localize without a map?

– How can you map without localization?

• All localization, mapping or SLAM methods are based on updating a state:

– What makes a state? Localization? Map? Both?

– How certain is the state?

600.436/600.636G.D. Hager

S. Leonard

Representations for Bayesian Robot

Localization

Discrete approaches (’95)• Topological representation (’95)

• uncertainty handling (POMDPs)

• occas. global localization, recovery

• Grid-based, metric representation (’96)

• global localization, recovery

Multi-hypothesis (’00)• multiple Kalman filters


Particle filters (’99)• sample-based representation


Kalman filters (late-80s?)• Gaussians

• approximately linear models

• position tracking

AI

Robotics

600.436/600.636G.D. Hager

S. Leonard

Gaussian (or Normal) Distribution

-s s

m

Univariate

Multivariate

𝑝 𝑥 ~𝑁 𝜇, 𝜎2

𝑝 𝑥 =1

2𝜋𝜎𝑒

−12(𝑥−𝜇)2

𝜎2

𝑝 𝒙 ~𝑁 𝝁, Σ

𝑝 𝒙 =1

(2𝜋)𝑑/2 Σ 1/2𝑒−

12

𝒙−𝝁 𝑇Σ−1(𝒙−𝝁)

600.436/600.636G.D. Hager

S. Leonard

Properties of Gaussians

• We stay in the “Gaussian world” as long as we start with

Gaussians and perform only linear transformations (closed under

linear transformation)

• Same holds for multivariate Gaussians

𝑋~𝑁 𝜇, 𝜎2

𝑌 = 𝑎𝑋 + 𝑏⟹ 𝑌~𝑁(𝑎𝜇 + 𝑏, 𝑎2𝜎2)

𝑋1~𝑁 𝜇1, 𝜎1

𝑋2~𝑁 𝜇2, 𝜎2

⟹ 𝑋1 + 𝑋2~𝑁(𝜇1 + 𝜇2, 𝜎1 + 𝜎2 )

2

22 2

600.436/600.636G.D. Hager

S. Leonard

• “System” that changes

• We will represent a system by

its state 𝒙𝑡 at time t

• We will also require that a

system be observable

Dynamical Systems

𝒙𝑡 𝒙𝑡+1 𝒙𝑡+2

𝒛𝑡 𝒛𝑡+1 𝒛𝑡+2

external command input

“process model” (state transition)

“observation model” (measurements of the

states)

𝒖𝑡 𝒖𝑡+1 𝒖𝑡+1

600.436/600.636G.D. Hager

S. Leonard

Kalman Filter• Seminal paper published in 1960

• Great web page at http://www.cs.unc.edu/~welch/kalman/

• Recursive solution for discrete linear filtering problems

– A state x Rn

– A measurement z Rm

– Discrete (i.e. for time t = 1, 2, 3, … )

– Recursive process (i.e. xt = f ( xt-1 ) )

– Linear system (i.e xt = A xt-1 )

• The system is defined by:

1) linear process model

xt = A xt-1 + B ut-1 + wt-1

2) linear measurement model

zt = H xt + vt

control

Input

(optional)

Gaussian

white

noise

state

transitionGaussian

white

noise

observation

model

How a state transitions into another state How a state relates to a measurement

600.436/600.636G.D. Hager

S. Leonard

Kalman Filter

1. Prior estimate 𝒙𝑡′ at step t : Use the process model to predict

what will be the next state of the robot

2. Posterior estimate 𝒙𝑡 at step t : Use the observation model to

correct the prediction by using sensor measurement

Compute posterior estimate as a linear combination of the prior

estimate and difference between the actual measurement and

expected measurement

Where the Kalman gain 𝐾𝑡 is a blending factor that adds a

measurement innovation.

)ˆ(ˆˆttttt

xHzKxx

600.436/600.636G.D. Hager

S. Leonard

Kalman Filter

Define prior error between true state and prior estimate

and the prior covariance as

Define posterior error between true state and posterior estimate

and it’s posterior covariance as

The gain K that minimizes the posterior covariance is defined by

Note that

– If 𝑅 → 0 then 𝐾𝑡 = 𝐻−1 and 𝒙𝑡 = 𝒙𝑡′ + 𝐾𝑡 𝒛𝑡 − 𝐻 𝒙𝑡

′ = 𝒙

– If Σ𝑡′ → 0 then 𝐾𝑡 = 0 and 𝒙𝑡 = 𝒙𝑡

′

tttxxe ˆ

)(T

tttE ee

tttxxe ˆ

)(T

tttE ee

1)(

RHHHKT

t

T

tt

600.436/600.636G.D. Hager

S. Leonard

Kalman Filter

• Recipe:

1. Start with an initial guess

𝒙0, Σ0

2. Compute prior (prediction)

𝒙𝑡′ = 𝐴 𝒙𝑡−1 + 𝐵𝒖𝑡−1

Σ𝑡′ = 𝐴Σ𝑡−1𝐴𝑇 + 𝑄

3. Compute posterior (correction)

𝐾𝑡 = Σ𝑡′𝐻𝑇 𝐻Σ𝑡

′𝐻𝑇 + 𝑅 −1

𝒙𝑡 = 𝒙𝑡′ + 𝐾𝑡 𝒛𝑡 − 𝐻 𝒙𝑡

′

Σ𝑡 = 1 − 𝐾𝑡𝐻 Σ𝑡′

Time update

(predict)

Measurement

update

(correct)RE

PE

AT

600.436/600.636G.D. Hager

S. Leonard

Initial

𝒙0, Σ0



Predict 𝒙1′ from 𝒙0 and 𝒖0

𝒙1′ = 𝐴 𝒙0 + 𝐵𝒖0

Σ1′ = 𝐴Σ0𝐴𝑇 + 𝑄

Correction using 𝒛1

𝐾1 = Σ1′ 𝐻𝑇 𝐻Σ1

′ 𝐻𝑇 + 𝑅 −1

𝒙1 = 𝒙1′ + 𝐾1 𝒛1 − 𝐻 𝒙1

′

Σ1 = 1 − 𝐾1𝐻 Σ1′

Predict 𝒙2′ from 𝒙1 and 𝒖1

𝒙2′ = 𝐴 𝒙1 + 𝐵𝒖1

Σ2′ = 𝐴Σ1𝐴𝑇 + 𝑄

600.436/600.636G.D. Hager

S. Leonard

Observability

𝒛𝑡 = 𝐻𝒙𝑡

• If H does not provide a one-to-one mapping

between the state and the measurement, then the

system is unobservable

𝒛𝑡 =0 00 1

𝒙𝑡

In this case H is singular, such that many states

can generate the same observation

𝐾𝑡 = Σ𝑡′𝐻𝑇 𝐻Σ𝑡

′𝐻𝑇 + 𝑅 −1

rank 𝐴𝐴𝑇 = rank 𝐴rank 𝐴 + 𝐵 ≤ rank 𝐴 + rank(𝐵)

600.436/600.636G.D. Hager

S. Leonard

Fully Observable vs Partially Observable

16-735, Howie Choset

600.436/600.636G.D. Hager

S. Leonard

Linearize it!

𝒙𝑡+1 ≈ 𝒙𝑡+1 + 𝐴 𝒙𝑡 − 𝒙𝑡 + 𝑊𝒘𝑡 𝒛𝑡 ≈ 𝒛𝑡 + 𝐻 𝒙𝑡 − 𝒙𝑡 + 𝑉𝒗𝑡

𝒙𝑡+1 = 𝑓 𝒙𝑡 , 𝒖𝑡 , 0 𝒛𝑡 = ℎ( 𝒙𝑡 , 0)

Kalman Filter Limitations

• Assumptions:

– Linear process model 𝒙𝑡+1 = 𝐴𝒙𝑡 + 𝐵𝒖𝑡 + 𝒘𝑡

– Linear observation model 𝒛𝑡 = 𝐻𝒙𝑡 + 𝒗𝑡

– White Gaussian noise 𝑁(0, Σ )

• What can we do if system is not linear?

– Non-linear state dynamics 𝒙𝑡+1 = 𝑓 𝒙𝑡 , 𝒖𝑡 , 𝒘𝑡

– Non-linear observations 𝒛𝑡 = ℎ 𝒙𝑡 , 𝒗𝑡

600.436/600.636G.D. Hager

S. Leonard

Extended Kalman Filter

• Where A, H, W and V are Jacobians defined by

𝐴 𝒙𝑡 =

𝜕𝑓1(, 𝒙𝑡𝒖𝑡, 0)

𝜕𝒙1⋯

𝜕𝑓1(𝒙𝑡, 𝒖𝑡, 0)

𝜕𝒙𝑀

⋮ ⋱ ⋮𝜕𝑓𝑀(𝒙𝑡, 𝒖𝑡, 0)

𝜕𝒙1⋯

𝜕𝑓𝑀(𝒙𝑡, 𝒖𝑡, 0)

𝜕𝒙𝑀

𝐻 𝒙𝑡 =

𝜕ℎ1(𝒙𝑡, 0)

𝜕𝒙1⋯

𝜕ℎ1(𝒙𝑡, 0)

𝜕𝒙𝑀

⋮ ⋱ ⋮𝜕ℎ𝑁(𝒙𝑡, 0)

𝜕𝒙1⋯

𝜕ℎ𝑁(𝒙𝑡, 0)

𝜕𝒙𝑀

𝑊 𝒙𝑡 =

𝜕𝑓1(𝒙𝑡, 𝒖𝑡, 0)

𝜕𝒘1⋯

𝜕𝑓1(𝒙𝑡, 𝒖𝑡, 0)

𝜕𝒘𝑁

⋮ ⋱ ⋮𝜕𝑓𝑀(𝒙𝑡, 𝒖𝑡, 0)

𝜕𝒘1⋯

𝜕𝑓𝑀(𝒙𝑡, 𝒖𝑡 , 0)

𝜕𝒘𝑁

𝑉 𝒙𝑡 =

𝜕ℎ1(𝒙𝑡, 0)

𝜕𝒗1⋯

𝜕ℎ1(𝒙𝑡, 0)

𝜕𝒗𝑁

⋮ ⋱ ⋮𝜕ℎ𝑀(𝒙𝑡, 0)

𝜕𝒗1⋯

𝜕ℎ𝑀(𝒙𝑡, 0)

𝜕𝒗𝑁

𝒇 𝒙, 𝒖, 𝑤 =𝑓1 𝒙, 𝒖, 𝑤

⋮𝑓𝑀 𝒙, 𝒖, 𝑤

𝒉 𝒙, 𝒗 =ℎ1 𝒙, 𝒗

⋮ℎ𝑁 𝒙, 𝒗

600.436/600.636G.D. Hager

S. Leonard

EKF for Range-Bearing Localization

• State 𝒔𝑡 = 𝑥𝑡 𝑦𝑡 𝜃𝑡𝑇 2D position and orientation

• Input 𝒖𝑡 = 𝑣𝑡 𝜔𝑡𝑇 linear and angular velocity

• Process model

𝒇 𝒔𝑡 , 𝒖𝑡 , 𝒘𝑡 =

𝑥𝑡−1 + (∆𝑡)𝑣𝑡−1cos(𝜃𝑡−1)𝑦𝑡−1 + (∆𝑡)𝑣𝑡−1sin(𝜃𝑡−1)

𝜃𝑡−1 + (∆𝑡)𝜔𝑡−1

+

𝑤𝑥𝑡

𝑤𝑦

𝑤𝜃𝑡

• Given a map, the robot sees N landmarks with coordinates

𝒍1 = 𝑥𝑙1 𝑦𝑙1𝑇 , ⋯ , 𝒍𝑁 = 𝑥𝑙𝑁 𝑦𝑙𝑁

𝑇

The observation model is

𝒛𝑡 =𝒉1(𝒔𝑡, 𝒗1)

⋮𝒉𝑁(𝒔𝑡, 𝒗𝑁)

𝒉𝑖 𝒔𝑡, 𝒗𝑡 =𝑥𝑡 − 𝑥𝑙𝑖

2+ 𝑦𝑡 − 𝑦𝑙𝑖

2

tan−1𝑦𝑡−𝑦𝑙𝑖

𝑥𝑡−𝑥𝑙𝑖

− 𝜃𝑡

+𝑣𝑟

𝑣𝑏

x

xtyt

y lN

l1

r1

b1

600.436/600.636G.D. Hager

S. Leonard

Linearize Process Model

𝒇 𝒔𝑡, 𝒖𝑡, 𝒘𝑡 =


𝜃𝑡−1 + (∆𝑡)𝜔𝑡−1

+

𝑤𝑥𝑡

𝑤𝑦

𝑤𝜃𝑡

𝐴 𝒙𝑡 =

𝜕𝑓1( 𝒙𝑡, 𝒖𝑡, 0)

𝜕𝒙1⋯

𝜕𝑓1( 𝒙𝑡, 𝒖𝑡, 0)

𝜕𝒙𝑁

⋮ ⋱ ⋮𝜕𝑓𝑀( 𝒙𝑡, 𝒖𝑡, 0)

𝜕𝒙1⋯

𝜕𝑓𝑀( 𝒙𝑡, 𝒖𝑡, 0)

𝜕𝒙𝑁

𝐴(𝒔𝑡−1) =1 0 −∆𝑡 𝑣𝑡−1sin(𝜃𝑡−1)0 1 ∆𝑡 𝑣𝑡−1cos(𝜃𝑡−1)

0 0 1

600.436/600.636G.D. Hager

S. Leonard

Linearize Observation Model



𝒉𝑖 𝒔𝑡 , 𝒗𝑡 =

𝑥𝑡 − 𝑥𝑙𝑖


2

tan−1𝑦𝑡 − 𝑦𝑙𝑖

𝑥𝑡 − 𝑥𝑙𝑖

− 𝜃𝑡

+𝑣𝑟

𝑣𝑏

𝐻 𝒔𝑡 =

𝜕𝒉1(𝒔𝑡, 0)

𝜕𝑥𝑡

𝜕𝒉1(𝒔𝑡, 0)

𝜕𝑦𝑡

𝜕𝒉1(𝒔𝑡, 0)

𝜕𝜃𝑡

⋮ ⋮ ⋮𝜕𝒉𝑁(𝒔𝑡, 0)

𝜕𝑥𝑡

𝜕𝒉𝑁(𝒔𝑡, 0)

𝜕𝑦𝑡

𝜕𝒉𝑁(𝒔𝑡, 0)

𝜕𝜃𝑡

600.436/600.636G.D. Hager

S. Leonard

KF vs EKF

xt

xt+1 xt+1 = Axt

xt

xt+1

xt+1=f (xt)xt+1 = Axt

600.436/600.636G.D. Hager

S. Leonard

• Extended Kalman Filter Recipe:

– Given

– Prediction

– Measurement correction

Extended Kalman Filter

• Kalman Filter Recipe:

– Given

– Prediction

– Measurement correction

600.436/600.636G.D. Hager

S. Leonard

Artificial Landmarks

Lippsett

600.436/600.636G.D. Hager

S. Leonard

Natural Visual Landmarks

• Visual landmarks: SIFT, SURF, corners,

Se IJRR

600.436/600.636G.D. Hager

S. Leonard

Data Association• From observation model, we have an

expected

• So if we have N landmarks l1, …, lN and

we are given a scan zt, how do associate

each landmark to a scan observation?

⋮ 𝒍𝑖𝑡

⋮

=

⋮ 𝒍𝑖𝑡′

⋮

+ 𝐾𝑡

⋮𝒛𝑖𝑡

⋮

−⋮

𝒉𝑖 𝒙𝑡′ , 0⋮

Observation of

landmark #1

Observation of landmark #i

must be in the ith row!

Observation of

landmark #Nmsdn.microsoft.com

600.436/600.636G.D. Hager

S. Leonard

From Localization to Mapping

• For us, the landmarks have been a known

quantity (we have a map with the

coordinates of the landmarks), but landmarks

are not part of the state

• Two choices:

– Make the state the location of the landmarks

relative to the robot (I also know exactly

where I am …)

• - No notion of location relative to past history

• - No fixed reference for landmarks

– Make the state the robot location now (relative

to where we started) plus landmark locations

• + Landmarks now have fixed location

• - Knowledge of my location slowly degrades

(but this is inevitable …)

iRobot

600.436/600.636G.D. Hager

S. Leonard

EKF for Range-Bearing Localization

• State 𝒔𝑡 = 𝑥𝑡 𝑦𝑡 𝜃𝑡𝑇 2D position and orientation

• Input 𝒖𝑡 = 𝑣𝑡 𝜔𝑡𝑇 linear and angular velocity

• Process model

𝒇 𝒔𝑡 , 𝒖𝑡 , 𝒘𝑡 =


𝜃𝑡−1 + (∆𝑡)𝜔𝑡−1

+

𝑤𝑥𝑡

𝑤𝑦

𝑤𝜃𝑡

• Given a map, the robot sees N landmarks with coordinates

𝒍1 = 𝑥𝑙1 𝑦𝑙1𝑇 , ⋯ , 𝒍𝑁 = 𝑥𝑙𝑁 𝑦𝑙𝑁

𝑇

The observation model is



𝒉𝑖 𝒔𝑡, 𝒗𝑡 =𝑥𝑡 − 𝑥𝑙𝑖


2

tan−1𝑦𝑡−𝑦𝑙𝑖

𝑥𝑡−𝑥𝑙𝑖

− 𝜃𝑡

+𝑣𝑟

𝑣𝑏

x

xtyt

y lN

l1

r1

b1

600.436/600.636G.D. Hager

S. Leonard

Kalman Filters and SLAM

• Localization: state is the location of the robot

• Mapping: state is the location of 2D landmarks

• SLAM: state combines both

• If the state is

then we can write a linear observation system

600.436/600.636G.D. Hager

S. Leonard

• State 𝒔𝑡 = 𝑥𝑡 𝑦𝑡 𝜃𝑡 𝒍1𝑇 ⋯ 𝒍𝑁

𝑇 𝑇 position/orientation of

robot and landmarks coordinates

• Input 𝒖𝑡 = 𝑣𝑡 𝜔𝑡𝑇 forward and angular velocity

• The process model for localization is

𝒔𝑡′ =

𝑥𝑡−1

𝑦𝑡−1

𝜃𝑡−1

+

∆𝑡𝑣𝑡−1cos(𝜃𝑡−1)∆𝑡𝑣𝑡−1sin(𝜃𝑡−1)

∆𝑡𝜔𝑡−1

This model is augmented for 2N+3 dimensions to

accommodate landmarks. This results in the process equation𝑥𝑡

′

𝑦𝑡′

𝜃𝑡′

𝒍1𝑡

′

⋮𝒍𝑁𝑡

′

=

𝑥𝑡−1

𝑦𝑡−1

𝜃𝑡−1

𝒍1𝑡−1

⋮

𝒍𝑁𝑡−1

+

1 0 00 1 00 0 10 0 0⋮ ⋮ ⋮0 0 0

∆𝑡𝑣𝑡−1cos(𝜃𝑡−1)∆𝑡𝑣𝑡−1sin(𝜃𝑡−1)

∆𝑡𝜔𝑡−1

EKF Range Bearing SLAM

Prior State Estimation

Landmarks don’t depend

on external input

600.436/600.636G.D. Hager

S. Leonard


Prior Covariance UpdateWe assume static landmarks. Therefore, the function

f(s,u,w) only affects the robot’s location and not the

landmarks.

Σ𝑡′ = 𝐴𝑡Σ𝑡−1𝐴𝑡

𝑇 + 𝑊𝑡𝑄𝑊𝑡𝑇

Jacobian of the

robot motion

The motion of the robot does not

affect the coordinates of

the landmarks

Jacobian of the

process model

2Nx2N identity

𝐴 =

𝜕𝑥

𝜕𝑥

𝜕𝑥

𝜕𝑦

𝜕𝑥

𝜕𝜃𝜕𝑦

𝜕𝑥

𝜕𝑦

𝜕𝑦

𝜕𝑦

𝜕𝜃𝜕𝜃

𝜕𝑥

𝜕𝜃

𝜕𝑦

𝜕𝜃

𝜕𝜃

0

0 𝐼

600.436/600.636G.D. Hager

S. Leonard


Kalman Gain

Compute the Jacobian Hi of each hi and then

stack them into one big matrix H. Note that hi

only depends on 5 variables: xt, yt, qt, xli, yliNeed to be in the

correct columns

of H

600.436/600.636G.D. Hager

S. Leonard


Measurement

• Observe N landmarks 𝒛𝑖𝑡 = 𝑟𝑖𝑡 𝜙𝑖𝑡𝑇

• Must have data association

– Which measured landmark corresponds to hi?

– If st contains the coordinates of N landmarks in the map, hi predicts the

measurement of each landmark

• Must figure which measured landmark corresponds to hi

𝒔𝑡 = 𝒔𝑡′ + 𝐾𝑡 𝒛𝑡 − 𝒉 𝒔𝑡

′ , 0

Make sure that each landmark observation 𝒛𝑖𝑡 appears in the correct rows of 𝒛𝑡

⋮ 𝒍𝑖𝑡

⋮

=

⋮ 𝒍𝑖𝑡′

⋮

+ 𝐾𝑡

⋮𝒛𝑖𝑡

⋮

−⋮

𝒉𝑖 𝒔𝑡′ , 0⋮

600.436/600.636G.D. Hager

S. Leonard

• From Kt and H update the posterior state estimate

𝒔𝑡 = 𝒔𝑡′ + 𝐾𝑡 𝒛𝑡 − 𝒉 𝒔𝑡

′ , 0

Σ𝑡 = 𝐼 − 𝐾𝑡𝐻𝑡 Σ𝑡′


Posterior Update

http://ais.informatik.uni-freiburg.de

600.436/600.636G.D. Hager

S. Leonard

Mono SLAM

• A visual landmark with a

single camera does not

provide range

• Data association is given by

tracking or matching visual

descriptors/patches

Robot Vision, Imperial College

http://homepages.inf.ed.ac.uk/

600.436/600.636G.D. Hager

S. Leonard

Submaps• As landmarks are added, the state vector grows

• For large maps only a few landmarks can be visible at any given

time

• Use submaps to reduce the number of landmarks to manageable

numbers

600.436/600.636G.D. Hager

S. Leonard

Navigation: RMS TitanicLeonard & Eustice

EKF-based system

866 images

3494 camera constraints

Path length 3.1km 2D / 3.4km 3D

Convex hull > 3100m2

344 min. data / 39 min. ESDF*

*excludes image registration time

600.436/600.636G.D. Hager

S. Leonard

Search of Flight 370

Inertial Navigation System (INS):

A glorified IMU with Kalman filter

600.436/600.636G.D. Hager

S. Leonard

Summary

• Basic system modeling ideas

• Kalman filter as an estimation method from a system model

• Linearization as a way of attacking a wider variety of problems

• Mapping localization and mapping into EKF

• Extensions for managing landmark matching and not-well-

constrained systems.

Kalman Filters for Mapping and Localizationsleonard/week08.pdf · 2018-12-19 · Kalman Filters for...

Documents

Transcript of Kalman Filters for Mapping and Localizationsleonard/week08.pdf · 2018-12-19 · Kalman Filters for...