Kalman Filters - tams.informatik.uni-hamburg.de · Kalman Filters Kalman Filters Jonas Haeling and Matthis Hauschild Universit at Hamburg Fakult at fur Mathematik, Informatik und

Universität Hamburg

MIN-FakultätFachbereich Informatik

Kalman Filters

Kalman Filters

Jonas Haeling and Matthis Hauschild

Universität HamburgFakultät für Mathematik, Informatik und NaturwissenschaftenFachbereich Informatik

Technische Aspekte Multimodaler Systeme

November 9, 2014

J. Haeling and M. Hauschild - Kalman Filters 1



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




Motivation Kalman Filters

Table of Contents

1. Motivation




5. Conclusion





Robot localization scenario

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

I OdometryI Sonar sensor





Current estimation of position





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 KálmánI 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





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




The Discrete Kalman Filter Kalman Filters

Table of Contents

1. Motivation




5. Conclusion





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





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





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

State estimation

x̂−k = A · x̂k−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 kI uk−1: Control input at timestep k − 1I B: Relates optional control input to state x





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 kI Q: Process noise covariance





Time Update (“Predict”) Recap[3]





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

Kalman Gain computation

Kk = P−k · H

T · (H · P−k · HT + 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






State estimation update with measurement

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

−k )

I x̂k : A posteriori state estimate

I x̂−k : The observed state at timestep k


I zk : Measurement at timestep k







Error covariance update

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

I Pk : A posteriori estimate error covariance

I I: Identity matrix








Measurement Update (“Correct”) Recap[3]





Operation of the Kalman Filter[3]





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




Model Process of a Kalman Filter Kalman Filters

Table of Contents

1. Motivation




5. Conclusion




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




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





2. Model the state process[2]

I Water level L is constant

I State x̂k 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 · x̂k−1 + B · uk−1P−k = A · Pk−1 · A

T + Q





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 · HT + R)−1

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

−k )

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





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)





5. Test the filter[2]

I Simplified equations:

Predict

x̂−k = x̂k−1P−k = Pk−1 + q

Update

Kk = P−k · (P

−k + r)

−1

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

−k )

Pk = (1− Kk) · P−k

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






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 = 0P−1 = 1000 + 0.0001 = 1000.0001

Update

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






I Another step:

Predict

x̂−2 = 0.8999P−2 = 0.1000 + 0.0001 = 0.1001

I Hypothetical measurement of z2 = 0.8

Update

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






t x̂−k P−k zk Kk x̂k 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




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!





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

t x̂−k P−k zk Kk x̂k 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





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




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 the

estimated filling rate

I Ak =

(1 ∆k0 1

)represents the filling tank with timestep k

I B and u still ignored





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





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 plfplf pf

)I Discrete noise model: Q =

(qf /3 qf /2qf /2 qf

)with filling noise qf

(Q derived from the continuous Q, skipped here)





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





A Filling Model - example[2]





A Filling Model with a constant level[2]




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) + lc : scales the amplituder : cycle ratel : average level

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

I What do you notice?




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

Constant, but sloshing model - example[2]




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




Extended Kalman Filter Kalman Filters

Table of Contents

1. Motivation




5. Conclusion





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)





EKF Time Update Equations[3]

State estimation

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

Error covariance projection

P−k = Ak · Pk−1 · ATk + Wk · Qk−1 ·W Tk

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

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

(x̂k−1, uk−1, 0)

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

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

(x̂k−1, uk−1, 0)





EKF Measurement Update Equations[3]

Kalman Gain Computation

Kk = P−k · H

TK · (Hk · P

−k · H

Tk + Vk · Rk · V Tk )−1

State estimation update with measurement

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

−k , 0))

Error covariance update

Pk = (I − KkHk) · P−k

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

(x̃k , 0) and V[i ,j] =δh[i ]δv[j]

(x̃k , 0)

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




Predict-Correct-Cycle of EKF[3]





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




Conclusion Kalman Filters

Table of Contents

1. Motivation




5. Conclusion





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





Comparison to other filters

Particle Filter or Sequential Monte Carlo methods

I Estimate density represented with particles ⇒ MultimodalI 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





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





Thank you for your attention!

Jonas Haeling and Matthis [email protected] and

[email protected]

Universität HamburgFakultät für Mathematik, Informatik und NaturwissenschaftenFachbereich Informatik

Technische Aspekte Multimodaler Systeme





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.pdfhttp://www.cs.cornell.edu/courses/cs4758/2012sp/materials/mi63slides.pdfhttp://en.wikipedia.org/wiki/Extended_Kalman_filterhttp://en.wikipedia.org/wiki/Extended_Kalman_filterhttp://en.wikipedia.org/wiki/Kalman_filter

MotivationThe Discrete Kalman FilterModel Process of a Kalman FilterConstant ModelFilling TankNon-linear ModelSummary

Extended Kalman FilterConclusion

Documents

Kalman Filters - tams.informatik.uni-hamburg.de · Kalman Filters Kalman Filters Jonas Haeling and Matthis Hauschild Universit at Hamburg Fakult at fur Mathematik, Informatik und