Robotics: Science and Systems

Zhibin (Alex) LiSchool of Informatics

University of Edinburgh

State Estimation & Kalman Filter

1

Outline 1. Quick review2. Basics of state estimation and sensor fusion3. Kalman filter for state estimation4. Kalman filter vs low-pass filter5. *Extended Kalman filter

2

Recap: What Tools Do We Need?

3

State Estimation

Quick review❏ System identification

❏ Model based control

❏ Build model based on dynamics

❏ Identify model parameters

❏ Least square estimation

❏ Signal filtering & filter design❏ Types of filters

❏ Why filtering is important?

4

M

State estimation

5

Sensor fusionSensor fusion solves the problem of combining the data from different sensors to obtain an estimate of a system.We first start with the basic concept of sensor fusion, which is an essential part of fusing the prediction and real measurement inside the Kalman filter.

6

Sequential state estimation❏ A car needs to navigate using different information, we have

multiple measurements for the location: 1.GPS; 2. Odometry.❏ We know (a priori) one crucial fact – the variance (errors) for each

measurement and sensor, then, what is the true position of the car?

7Picture source: Intelligent Vehicles and Systems Group, Pennsylvania State University

Covariance and varianceProbability density of the normal distribution (Gaussian):

: mean of the distribution : standard deviation : varianceRecap:

8Picture source: en.wikipedia.org/wiki/Normal_distribution

σ σ

Covariance and varianceCovariance: a measure of the level of correlation between two or more random variates. is the mean of , let ,

The sign of the covariance shows the approx. linear relationship between the variables (hint: we can deduce from the above equations): Positive covariance: the larger , the larger , and vice versa; Negative covariance: the larger , the smaller , and vice versa.

9

Covariance and varianceIf two variables, and , are independent, ie these two are uncorrelated, then their covariance is 0, .For the same set of variable , its covariance is the same as its variance. ,

Or

10

Recall:

Data fusion from two estimates (e.g., two different sensors)❏ How do you combine two unbiased estimates , with their

covariances? Assuming , are independent variables.

11

Lower variance= higher confidence= higher weight

Similar to resistors in a parallel circuit

Fusion: weighting by the inverse of covariance❏ The fused estimate is weighted by the quality (variance) of the

measurements.❏ This fusion formula can be extended to more than two terms

recursively

12

More accuracy, more trust

Sequential update of estimated state Now, here is a new case that shows how to combine multiple sequential measurements. Define for the estimate, for the measurement.❏Given the first measurements: ❏As initialization, the first estimate is directly the measurement:

❏Given a 2nd measurement:❏What is the 2nd estimate

13

Recall: the bigger the variance is, the less we can trust the measurement.

Variance-weighted sum

14

This will show up in Kalman filter.

Conditional density based on two data sets

15

Mean of the estimate

Uncertainty of the estimate

Maybeck 1979, Stochastic Models: Estimation and Control

Kalman filter

16

Use dynamics in estimationPrevious sensor fusion builds on the assumption that measurements are given and then weight them in some way to yield an estimate, without questioning how these signals are generated. So what if we do have knowledge of the dynamics that produces this signal?However, what if measurement goes a bit wrong, how the dynamic property can be used to obtain a more reliable prediction, instead of relying only on direct measurement of signals?

17

Why dynamics in estimation?

18

What happens when we include dynamics?Using the physical laws of motion, use the control inputs to the system and multiple sequential measurements to predict better estimates.

19

System state

Measurement

Effect of Adding System Dynamics❏Uncertainty is amplified & density function is “squashed”

20

Why?

Maybeck 1979, Stochastic Models: Estimation and Control

Move

Gaussian functionProbability density of the normal distribution:

: mean of the distribution : standard deviation : variance* Normal distribution is denoted by 21Picture source: en.wikipedia.org/wiki/Normal_distribution

Kalman FilterKalman filter is suitable to model systems with normal/gaussian distributions using their variances.

22

Structure of the Kalman Filter

23

We know astatistical description

Adapted from Maybeck 1979, Stochastic Models: Estimation and Control

System

Transducers& sensors

Kalman filter

Measurement error source

Observedfeedback

Optimal estimates of system state

System state(true state is

not measurable)

Controls

System error source

Key variables: first two statistical moments

24

Discrete Kalman Filter: key ingredients❏ Discrete process model

❏ State change over time❏ Linear difference equation

❏ Discrete measurement model❏ Relationship between state and measurement❏ Linear function

❏ Model parameters ❏ Process noise characteristics❏ Measurement noise characteristics

25

Discrete Kalman Filter: two models

26

Discrete Kalman Filter: model equationsSystem dynamics by state space equation

The equation of measurement process

Here, and are the random noises in system dynamics and measurements, and their probability distributions are Gaussian that

27

Complete model specificationDenote by to be a priori state estimate at step k given knowledge of the process before to step k;Similarly, denote by to be a posteriori state estimate at step k given measurement . Define a priori and a posteriori estimate errors are

28

Complete model specificationThe error covariance of a priori estimate is

The error covariance of a posteriori estimate is

A posteriori state estimate is a linear combination of an a priori estimate and a weighted difference between an actual measurement and a measurement prediction.The difference is the residual, so the next step is to add the residual with a weight related with trust.

29

State estimation using Kalman Filter❏Prediction: project state and error covariance forward in time:

❏Correction: update estimate after measurement:

30

e.g., during a time interval, youexpect the ball to go from 1m to 1.5 m,with some uncertainty increase

in fact, vision sees ball going to 1.7 mso you update your estimates toconclude ball must be at 1.6 m with some new level of uncertainty

State estimation using Kalman FilterHow to weight the residual ? The gain K that minimize the covariance of posteriori error is

A quick examination:If the measurement is accurate that R=0, then

31

State estimation using Kalman Filter❏Project state and error covariance forward in time:

❏Update estimate after measurement:

32

Same idea as in slide “Variance-weighted Sum”

To use in next step

Discrete Kalman Filter: a two-step process

33

Illustration of beliefsIllustration of Kalman filters: (a) initial belief, (b) a measurement (in bold) with the associated uncertainty, (c) belief after integrating the measurement into the belief using the Kalman filter algorithm.

34

Example: estimating a noisy constant

35

Of course, this is a trivial dynamic system

Disturbance magnitude w: 0.0; Noise magnitude v: 15;Q: [1.0e-04 0 0 1.0e-04];R: 15^2.

How Q and R affect the estimates?

36

Disturbance magnitude w: 0.01; Noise magnitude v: 15;Q: [1.0e-04 0 0 1.0e-04];R: 15^2.

Compared to low-pass filter

37

Offline low-pass filtering approach:Disturbance magnitude w: 0.01; Noise magnitude v: 15;

3-order Butterworth low-pass filter;Cutoff frequency: 0.4 Hz;Demo code shown in last lecture, online low-pass filtering will result in more lag.

Compared to low-pass filter

38

Offline low-pass filtering approach:Disturbance magnitude w: 0.01; Noise magnitude v: 15;

3-order Butterworth low-pass filter;Cutoff frequency: 0.2 Hz;Demo code shown in last lecture, online low-pass filtering will result in more lag.

Compared to low-pass filter

39

Offline low-pass filtering approach:Disturbance magnitude w: 0.01; Noise magnitude v: 15;

3-order Butterworth low-pass filter;Cutoff frequency: 0.1 Hz;Demo code shown in last lecture, online low-pass filtering will result in more lag.

How Q and R affect the estimates?

40

Disturbance magnitude w: 0.01; Noise magnitude v: 15;Q: [1.0e-04 0 0 1.0e-04];R: 15^2*100.

How Q and R affect the estimates?

41

Disturbance magnitude w: 0.01; Noise magnitude v: 15;Q: [0.01 0 0 0.01];R:15^2.

Discussion

Effects of Q and R?

42

Summary: what is kalman filter?❏ An optimal, recursive/on-line data processing algorithm❏ Optimal in what sense?

❏If it is a linear dynamical system ❏And system/sensor noise is Gaussian❏Then, there is no alternate algorithm with lower MSE❏Optimal ⬄ Best Linear Unbiased Estimator

43

Kalman Filter: a linear filter!

So when is suitable to use Kalman Filter?Applications:

1. There is a dynamic model that you can use to predict the future state of the system, e.g. tracking position of real objects (cars, boats, rockets);

2. Fuse multiple data from Lidar, RGB-camera for depth and velocity measurements;

3. For linear systems, i.e. linear dynamics/models.4. For a dynamical system, Kalman Filter can also be used for

localisation, eg, for a self-driving car to estimate the speed of other cars.

44

Assuming both the system and observation models are linear might not be realistic in many real life situations. Signals and measurements are not necessarily Gaussian. In real life scenarios, situations are always nonlinear, it depends on the level of the nonlinearity, whether or not it can be approximated well by linearization. Sometimes the model is nonlinear; and even if a model is linear, it may not be well known yet. Then the uncertainty of the model dynamics can not be handled.

Limitations?

45

Nonlinear dynamics – What’s the Problem?

46

Not Gaussian any more!

Extended Kalman Filter (EKF)❏ Extended Kalman filter (EKF) is the nonlinear version of the Kalman

filter which linearizes about an estimate of the current mean and covariance.

❏ The key underlying feature of EKF is linearization using partial differentials, logically EKF is suboptimal.

❏ An important limitation of the EKF arises from the fact that it approximates state transitions and measurements using the first order of linear Taylor expansions. (Alternative improvement: second order EKF by taking Taylor expansions until the second order for better accuracy)

47

Extended Kalman Filter (EKF)

48

(a) Noise free approximation

(b) Linear (local) dynamics around that approximation. Note the incremental change of state

We have a model, so wecan compute this exactly

Instead of a linear transition matrix, we have a general nonlinear function to represent the change of state

Extended Kalman Filter - computation❏We can define a Kalman filter over these error terms❏Then, use that linear error estimate to drive the main estimation loop

49

Summary of sensor fusion❏ State estimation

❏ How to fuse measurements?❏ Easy way to remember, analogous to parallel resistors

50

More accuracy, more weight

Further readingA practical use of Kalman filter in Darpa Robotics Challenge: https://sites.google.com/site/xxinjile/home

51