Kalman Filters - uni-hamburg.de · Motivation Kalman Filters Robot localization scenario ... I Military: navigation of missiles, ... I Tries to estimate the state x 2Rn of a discrete-time

Universitat Hamburg

MIN-FakultatFachbereich Informatik

Kalman Filters

Kalman Filters

Jonas Haeling and Matthis Hauschild

Universitat HamburgFakultat fur Mathematik, Informatik und NaturwissenschaftenFachbereich Informatik

Technische Aspekte Multimodaler Systeme

November 9, 2014

J. Haeling and M. Hauschild - Kalman Filters 1

Universitat Hamburg


Kalman Filters

Table of Contents

1. Motivation

2. The Discrete Kalman Filter

3. Model Process of a Kalman FilterConstant ModelFilling TankNon-linear ModelSummary

4. Extended Kalman Filter

5. Conclusion


Universitat Hamburg


Motivation Kalman Filters

Table of Contents

1. Motivation




5. Conclusion


Universitat Hamburg



Robot localization scenario

I A robot drives along a one dimensional roadI It localizes itself using

I OdometryI Sonar sensor


Universitat Hamburg



Current estimation of position


Universitat Hamburg





Universitat Hamburg





Universitat Hamburg





Universitat Hamburg





Universitat Hamburg



History of the Kalman Filter[5]

The Kalman Filter is a linear filter producing an optimal estimateof the system state using noisy input data

I Named after Rudolf Emil KalmanI Born 1930 in BudapestI Hungarian-US-American electrical engineer & mathematician

I Invented in 1960 (with assistance from Richard Bucy)

I First use: trajectory estimation in the Apollo program

I Special case of non-linear filter by Stratonovich invented ealier


Universitat Hamburg



Applications of the Kalman Filter

I Generally position estimationI Robotics: robot localization, (moving) object or human trackingI Military: navigation of missiles, submarinesI Aeronautics: position of a plane, attitude control of the ISS

I Electronics: phase-locked loop

I Computer graphics: stabilizing depth measurements, fittingBezier patches


Universitat Hamburg


The Discrete Kalman Filter Kalman Filters

Table of Contents

1. Motivation




5. Conclusion


Universitat Hamburg



The Discrete Kalman Filter[3][1]

I Tries to estimate the state x ∈ Rn of a discrete-time controlledprocess

I Is an optimal linear filterI Incorporates all available dataI Produces a statistically minimized error

I Assumes white gaussian noise both for process prediction andmeasurement


Universitat Hamburg



The Discrete Kalman Filter[3]

I Time update projects the current state estimate ahead in time

I Measurement update adjusts the projected estimate by anactual measurement at that time


Universitat Hamburg



Time Update (“Predict”) - Step 1/2[3]

State estimation

x−k = A · xk−1 + B · uk−1

I x−k : The observed state at timestep k

I A: Relates the state at timestep k − 1 to the state at k

I uk−1: Control input at timestep k − 1

I B: Relates optional control input to state x


Universitat Hamburg



Time Update (“Predict”) - Step 2/2[3]

Error covariance projection

P−k = A · Pk−1 · AT + Q

I P−k : A priori estimate error covariance

I Pk : A posteriori estimate error covariance

I A: Relates the state at timestep k − 1 to the state at k

I Q: Process noise covariance


Universitat Hamburg



Time Update (“Predict”) Recap[3]


Universitat Hamburg



Measurement Update (“Correct”) - Step 1/3[3]

Kalman Gain computation

Kk = P−k · H

T · (H · P−k · H

T + R)−1

I Kk : Controls the influence of the measurement on the aposteriori state estimation at timestep k


I H: Relates measurement to state

I R: Measurement noise covariance


Universitat Hamburg




State estimation update with measurement

xk = x−k + Kk(zk − H · x−

k )

I xk : A posteriori state estimate

I x−k : The observed state at timestep k


I zk : Measurement at timestep k



Universitat Hamburg




Error covariance update

Pk = (I − Kk · H) · P−k

I Pk : A posteriori estimate error covariance

I I: Identity matrix





Universitat Hamburg



Measurement Update (“Correct”) Recap[3]


Universitat Hamburg



Operation of the Kalman Filter[3]


Universitat Hamburg



Summary: The Discrete Kalman Filter

I Is used for combining noisy data

I Is an optimal filter

I Has a cyclic recursive approach

I Assumes white gaussian noise

I Predicts an estimate of the current state x with a measurementscaled through the Kalman gain K


Universitat Hamburg


Model Process of a Kalman Filter Kalman Filters

Table of Contents

1. Motivation




5. Conclusion


Universitat Hamburg


Model Process of a Kalman Filter Kalman Filters

Model Definition Process[2]

The Kalman Filter removes noise by assuming a pre-defined modelof a system.

1. Understand the situation

2. Model the state process

3. Model the measurement process

4. Model the noise

5. Test the filter

6. Refine filter


Universitat Hamburg


Model Process of a Kalman Filter - Constant Model Kalman Filters

1. Understand the situation[2]

I Task: Measure the level of water in a tank

I Measurements obtained via floating device

I Average water level could be changing or static

I Water could be sloshing or stagnant


Universitat Hamburg



2. Model the state process[2]

I Water level L is constant

I State xk is the estimate of LI Constant model:

I Ak is 1 for any k ≥ 0I Control variables B and u are 0

Reminder: Time Update

x−k = A · xk−1 + B · uk−1

P−k = A · Pk−1 · AT + Q


Universitat Hamburg



3. Model the measurement process[2]

I Float gives us the measurement zkI Measurement scale is the same scale as state estimate

→ H = 1

Reminder: Measurement update

Kk = P−k · H

T · (H · P−k · H

T + R)−1

xk = x−k + Kk(zk − H · x−

k )Pk = (I − Kk · H) · P−

k


Universitat Hamburg



4. Model the noise[2]

I Error due to process→ Process variance matrix Q = q

I Noise from the measurement→ Measurement variance matrix R = r

I Noise from the estimation→ State variance matrix Pk = p (scalar)


Universitat Hamburg



5. Test the filter[2]

I Simplified equations:

Predict

x−k = xk−1

P−k = Pk−1 + q

Update

Kk = P−k · (P−

k + r)−1

xk = x−k + Kk(zk − x−

k )Pk = (1− Kk) · P−

k

I Filter is completely defined, let’s test it!


Universitat Hamburg




I True water level L = 1I Start state x0 arbitrarly initialized to 0I Start variance P0 is 1000, system noise q = 0.0001,

measurement noise r = 0.1 (z1 = 0.9)

Predict

x−1 = 0

P−1 = 1000 + 0.0001 = 1000.0001

Update

K1 = 1000.0001 ∗ (1000.0001 + 0.1)−1 = 0.9999x1 = 0 + 0.9999 ∗ (0.9− 0) = 0.8999P1 = (1− 0.9999) ∗ 1000.0001 = 0.1000


Universitat Hamburg




I Another step:

Predict

x−2 = 0.8999

P−2 = 0.1000 + 0.0001 = 0.1001

I Hypothetical measurement of z2 = 0.8

Update

K2 = 0.1001 ∗ (0.1001 + 0.1)−1 = 0.5002x2 = 0.8999 + 0.5002 ∗ (0.8− 0.8999) = 0.8499P2 = (1− 0.5002) ∗ 0.1001 = 0.0500


Universitat Hamburg




t x−k P−

k zk Kk xk Pk

3 0.8499 0.0501 1.1 0.3339 0.9334 0.03344 0.9334 0.0335 1 0.2509 0.9501 0.02515 0.9501 0.0252 0.95 0.2012 0.9501 0.02016 0.9501 0.0202 1.05 0.1682 0.9669 0.01687 0.9669 0.0169 1.2 0.1447 1.0006 0.01458 1.0006 0.0146 0.9 0.1272 0.9878 0.01279 0.9878 0.0128 0.85 0.1136 0.9722 0.0114

10 0.9722 0.0115 1.15 0.1028 0.9905 0.0103


Universitat Hamburg





Universitat Hamburg


Model Process of a Kalman Filter - Filling Tank Kalman Filters

Filling Tank Model[2]

I A 20 % error produced a 5 % inaccuracy

I But what if the true situation is not static?→ Static model, but the tank is filling at a constant rate

I Tank level at time k: Lk = Lk−1 + f

I Filling rate f = 0.1 per time step

I Tank level starts at L0 = 0

I Measurement and process noise remains the same

I Let’s see what happens!


Universitat Hamburg



Filling Tank model with q = 0.0001 and r = 0.1[2]

t x−k P−

k zk Kk xk Pk L

3 - - - - 0 1000 01 0.0000 1000.0001 0.11 0.9999 0.1175 0.1000 0.12 0.1175 0.1001 0.29 0.5002 0.2048 0.0500 0.23 0.2048 0.0501 0.32 0.3339 0.2452 0.0334 0.34 0.2452 0.0335 0.50 0.2509 0.3096 0.0251 0.45 0.3096 0.0252 0.58 0.2012 0.3642 0.0201 0.56 0.3642 0.0202 0.54 0.1682 0.3945 0.0168 0.6


Universitat Hamburg





Universitat Hamburg





Universitat Hamburg





Universitat Hamburg



Filling Tank model with q = 1 and r = 0.1[2]


Universitat Hamburg


Model Process of a Kalman Filter - Filling Model Kalman Filters

A Filling Model[2]

I You can relax a model by increasing your estimated error

I But a bad model will not give good estimates!

2. Model the state process

I State x = (xl , xf )T where xl is the estimated level and xf theestimated filling rate

I Ak =

(1 ∆k0 1

)represents the filling tank with timestep k

I B and u still ignored


Universitat Hamburg



A Filling Model[2]

I Cannot measure filling rate

I But noisy measurement of L

3. Model the measurement process

I Scaling remains the same: H =(1, 0

)I z =

(z , 0

)T


Universitat Hamburg



A Filling Model[2]

4. Model the noiseI Measurement process is unchanged: R = r

I State process is changed:

I Estimate error covariance no longer scalar: P =

(pl plf

plf pf

)I Discrete noise model: Q =

(qf /3 qf /2qf /2 qf

)with filling noise qf

(Q derived from the continuous Q, skipped here)


Universitat Hamburg



A Filling Model[2]

5. Test the modelI Measurement noise r = 0.1

I Process noise of qf = 0.0001, which is quite accurate

I Initial state x0 =(0, 0

)TI Initial variance P0 =

(1000 0

0 1000

)I True filling rate f = 0.1 per timestep


Universitat Hamburg



A Filling Model - example[2]


Universitat Hamburg



A Filling Model with a constant level[2]


Universitat Hamburg


Model Process of a Kalman Filter - Non-linear Model Kalman Filters

Constant, but sloshing model[2]

I Another model: Water level is constant, but it is sloshing

I Sloshing modeled as a sine wave:

L = c ∗ sin(2 ∗ π ∗ r ∗∆k) + l

c : scales the amplituder : cycle ratel : average level

I We use c = 0.5, r = 0.05, l = 1

I What do you notice?


Universitat Hamburg


Model Process of a Kalman Filter - Non-linear Model Kalman Filters

Constant, but sloshing model - example[2]


Universitat Hamburg


Model Process of a Kalman Filter - Summary Kalman Filters

Summary of the Three Tank Examples[2]

Six steps for defining a Kalman Filter model:

1. Understand the situation 2. Model the state process3. Model the measurement process 4. Model the noise5. Test the filter 6. Refine filter

I Filter will fit measurements to provided model

I May not always be desirable (sloshing could be just noise)

I Initialization and noise components affect the results

I Think of the outcome of your filter (linear model works, butlags)

I An Extended Kalman filter is required to model non-linearitycorrectly


Universitat Hamburg


Extended Kalman Filter Kalman Filters

Table of Contents

1. Motivation




5. Conclusion


Universitat Hamburg



Extended Kalman Filter[3]

I Previously: linear stochastic difference equation

I Process or measurement may be non-linear

I KF that linearizes about the current mean and covariance iscalled an Extended Kalman Filter

I Uses partial derivatives of the process and measurementfunction

Process with state x ∈ Rn

xk = f (xk−1, uk−1,wk−1)

Measurement with z ∈ Rm

zk = h(xk , vk)


Universitat Hamburg



EKF Time Update Equations[3]

State estimation

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

Error covariance projection

P−k = Ak · Pk−1 · AT

k + Wk · Qk−1 ·W Tk

I Jacobian matrix of partial derivatives of f with respect to x

A[i ,j] =δf[i ]δx[j]

(xk−1, uk−1, 0)

I Jacobian matrix of partial derivatives of f with respect to w

W[i ,j] =δf[i ]δw[j]

(xk−1, uk−1, 0)


Universitat Hamburg



EKF Measurement Update Equations[3]

Kalman Gain Computation

Kk = P−k · H

TK · (Hk · P−

k · HTk + Vk · Rk · V T

k )−1

State estimation update with measurement

xk = x−k + Kk · (zk − h(x−

k , 0))

Error covariance update

Pk = (I − KkHk) · P−k

H[i ,j] =δh[i ]

δx[j](xk , 0) and V[i ,j] =

δh[i ]

δv[j](xk , 0)

x : approximate state, w : process noise, v : measurement noiseJ. Haeling and M. Hauschild - Kalman Filters 53

Universitat Hamburg



Predict-Correct-Cycle of EKF[3]


Universitat Hamburg



Summary - Extended Kalman Filter[4][3]

I EKF are needed when you either have a non-linear process ormeasurement relationship

I Uses function f for difference equation and function h for themeasurement equation

I No longer optimal estimator (only in linear cases)

I Considered by some as the de facto standard for non-linearstate estimation

I Heavily used in navigation systems and GPS


Universitat Hamburg


Conclusion Kalman Filters

Table of Contents

1. Motivation




5. Conclusion


Universitat Hamburg



Advantages and Disadvantages

I AdvantagesI Optimal if you have a linear system with Gaussian noiseI Recursive ⇒ Real-time capableI EKF can handle non-linearityI Relatively easy to useI Wide use in practice speaks for itself

I DisadvantagesI Loses optimality in non-linear systemsI Unimodal because of Gaussians ⇒ Only one hypothesisI Models may be too complex⇒ Sensivity analysis required because of imprecisions⇒ Or Kalman Filters may not be useful at all


Universitat Hamburg



Comparison to other filters

Particle Filter or Sequential Monte Carlo methods

I Estimate density represented with particles ⇒ Multimodal

I Does not require Gaussian noise

I Often used in complex non-linear models

I Large state space dimensionality requires lots of particles

Hybrid

I Particle Filter superior for dealing with multi-modal data

I EKF superior for dealing with updates with little noise

I Use PF until variance is below a certain level, switch to KF


Universitat Hamburg



Conclusion

I The Kalman Filter is a good and easy to use filter to get morereliable output from your sensors

I The recursive approach makes it usuable for real-time purposessuch as in robots

I But: you have to be able to describe the underlying modelproperly


Universitat Hamburg



Thank you for your attention!

Jonas Haeling and Matthis [email protected] and

[email protected]

Universitat HamburgFakultat fur Mathematik, Informatik und NaturwissenschaftenFachbereich Informatik

Technische Aspekte Multimodaler Systeme


Universitat Hamburg



Bibliography

[1] Peter Maybeck. Stochastic models, estimation, and control. AirForce Institute of Technology, 1979.

[2] Ashutosh Saxena. Kalman Filter Applications. CornellUniversity, 2008. http://www.cs.cornell.edu/courses/cs4758/2012sp/materials/mi63slides.pdf.

[3] G. Welch and G. Bishop. An Introduction to the Kalman Filter.University of North Carolina at Chapel Hill, 2006.

[4] Wikipedia. Extended Kalman filter, 2014. http://en.wikipedia.org/wiki/Extended_Kalman_filter.

[5] Wikipedia. Kalman filter, 2014.http://en.wikipedia.org/wiki/Kalman_filter.


http://www.cs.cornell.edu/courses/cs4758/2012sp/materials/mi63slides.pdf

http://www.cs.cornell.edu/courses/cs4758/2012sp/materials/mi63slides.pdf

http://en.wikipedia.org/wiki/Extended_Kalman_filter

http://en.wikipedia.org/wiki/Extended_Kalman_filter

http://en.wikipedia.org/wiki/Kalman_filter

Documents

Kalman Filters - uni-hamburg.de · Motivation Kalman Filters Robot localization scenario ... I Military: navigation of missiles, ... I Tries to estimate the state x 2Rn of a discrete-time