Non-Parametric Filtering

Non-Parametric Filtering Thomas Schön Division of Automatic Control Linköping University Linköping, Sweden www.control.isy.liu.se/~schon/ Non-Parame...
Author: Clifton Cross
10 downloads 0 Views 2MB Size
Non-Parametric Filtering

Thomas Schön Division of Automatic Control Linköping University Linköping, Sweden www.control.isy.liu.se/~schon/

Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

2

Linköping University

Linköping

Norrköping

145 000 inhabitants

128 000 inhabitants

27 600 students 3 800 employees 1 300 research students 140 study programmes 350 partner universities in more than 50 countries June 9, 2009

• • • • •

www.liu.se

Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

3

Division of Automatic Control Formed by Prof. Lennart Ljung Some 50 employees Conducts research within the areas of • System identification • Sensor fusion and robotics • Automatic control (especially optimization based app.)

www.control.isy.liu.se

• Industrial robotics

Some of our industrial partners: Saab, Ericsson, ABB, Volvo, Nira dynamics, Xsens Technologies. Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

4

The Setting We are dealing with dynamic systems! Mathematics

Application examples State variable

Stochastic noise

June 9, 2009

Measurement

“The output of a dynamical system depends on all the previous inputs”

Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

Introductory Example – Fighter Aircraft Navigation (I/II) Aim: Find the position, velocity and orientation of a fighter aircraft. Sensors Inertial sensors

Sensor Fusion Estimation problem

Radar

Pose estimate

World model

Barometer

Dynamic model

Terrain elevation

Sensor model

Industrial partner: Saab

Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

5

Introductory Example – Fighter Aircraft Navigation (II/II)

Green (O) Marginalized particle filter estimate Red (+) True horizontal position

Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

6

7

Outline (Today and Tomorrow) Background • State-space models • Define the nonlinear estimation problem Particle Filter Derivation and Implementation

e=repmat(y(:,t),1,N) - C*x; w=exp(-0.5*sum(e.*(inv(R)*e))); w=w/sum(w); index=sysresampling(w); x=x(:,index);

• Random number generation

• Implementation

• A first particle filter

• Rao-Blackwellized PF

Applications • Underwater positioning

• Fighter aircraft positioning

• GPS free positioning of cars

• UAV SLAM

Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

8

Dynamical Systems – Modelling Nonlinear state-space model (stochastic difference equation) State variable Input signal

Process model

Stochastic process noise Stochastic measurement noise

Measurement model Measurement

Alternative formulation

The two model descriptions are related,

Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

9

Dynamical Systems – Estimation The aim is to obtain information about the state present in the measurements

using the information

Compute

Three different estimation problems: • The filtering problem,

Filtering density

• The prediction problem, • The smoothing problem,

Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

10

Theorem From Last Lecture We have now shown that for the dynamic model

the filtering and the one-step ahead prediction densities are Previous prediction density Likelihood

Measurement update

Time update Dynamics

Non-Parametric Filtering Thomas Schön ([email protected])

Previous filtering density

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

11

The Filtering Problem The aim is to obtain information about the state the measurements

using the information present in

linear equations Gaussian noise General case

The Kalman filter is the best solution Particle filter Point mass filters Extended Kalman filter Sigma point Kalman filter

Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

Linear, Gaussian Models – Kalman Filter Important special case, linear equations with Gaussian noise

This allows for a closed form solution

Gaussian variables and linear transformations implies that complete information is provided by the mean and the covariance. Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

12

13

The Kalman Filter

innovation

Measurement update Decrease uncertainty

Time update Increase uncertainty Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

Working With the Multivariate Gaussian Multivariate Gaussian’s are important building blocks in more sophisticated models. It is a very good skill to be able to ”freely” manipulate Gaussians.

For more details, proofs and an example where the Kalman filter is derived using basic facts about Gaussians is provided in Thomas B. Schön and Fredrik Lindsten, Manipulating the Multivariate Gaussian Density. Division of Automatic Control, Linköping University, Sweden, January 2011. http://www.rt.isy.liu.se/~schon/Publications/SchonL2011.pdf

Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

14

15

Different Filtering Approaches

Linear, Gaussian

Approximate model

Approximate solution (pdf)

Kalman filter

Extended Kalman filter

Particle Filter

Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

Illustration of Different Filtering Methods

Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

16

The Key Idea Underlying the Particle Filter

17

The particle filter produce estimates of the filtering PDF The key idea is to form a nonparametric estimate,

particles

The estimate converge as This implies that the multidimensional integrals are reduced to finite sums

“ Non-Parametric Filtering Thomas Schön ([email protected])

” Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

June 9, 2009

weights

Random Number Generation – Perfect Sampling Assumption: We have access to samples from the density we seek to estimate,

We are seeking an estimate according to

The strong law of large numbers

Central limit theorem

Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

18

19

Perfect Sampling – Example and Problem

5000 samples

50000 samples

Obvious problem: In general we are NOT able to sample from the density we are interested in! Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

20

Random Number Generation Proposal density

Target density Acceptance prob.

Target density – We seek samples distributed according to this density Proposal density – This density is simple to generate samples from Acceptance probability – Used to decide whether the sample is OK Three common algorithms based on this idea: 1. Rejection sampling 2. Importance sampling 3. Metropolis-Hastings Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

Random Number Generation – Importance Sampling

This algorithm provides the following approximation of the target density

Central limit theorem and a law of large numbers can be formulated Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

21

Sampling Importance Resampling

Resampling Generates an unweighted set of samples by drawing according to

Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

22

Sampling Importance Resampling – Algorithm

This part is exactly the same as the importance sampling algorithm

Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

23

The Importance of a Good Importance Density

(50 000 samples were used in both experiments)

Lesson learned: It is very important to be careful in selecting the importance density. Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

24

25

The Particle Filter Idea: Make use of SIR in order to find a first particle filter, i.e., an algorithm that provides approximations of

Recall that we have previously shown that

This implies that SIR can be used to produce estimates of Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

26

A First Particle Filter

State estimate:

Gordon, N. J., Salmond, D. and Smith, A. F. M. (1993) Novel approach to nonlinear non-Gaussian Bayesian state estimation. IEE Proceedings on Radar and Signal Processing, 140: 107–113. Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

The Particle Filter in Terms of the Kalman Filter The Kalman filter

The particle filter

1. Initialize

1. Initialize:

2. Time update

2. Time update

3. Measurement update 3. Measurement update

Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

27

The Particle Filter – Simple Example Let us study one-dimensional motion and build a simple positioning system. Continuous-time model:

Discrete-time model:

Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

28

29

Velocity error (m/s2)

Kalman filter

Particle filter

Time (s)

Using 100 particles.

Velocity error (m/s2)

The Particle Filter – Simple Example

Kalman filter Particle filter

Time (s)

Using 10 000 particles.

Clearly, the result improves as we increase the number of particles. Xiao-Li Hu, Thomas B. Schön and Lennart Ljung. A Basic Convergence Result for Particle Filtering. IEEE Transactions on Signal Processing, 56(4):1337-1348, Apr. 2008. Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

Motivating Toy Example – Revisited in MATLAB x=repmat(x0,1,N)+sqrtm(P0)*randn(nx,N); for t=1:t_final

1. Initialize:

2. Time update:

x=A*x + Bw*sqrtm(Q)*randn(1,N); e=repmat(y(:,t),1,N) - C*x; w=exp(-0.5*sum(e.*(inv(R)*e))); w=w/sum(w); index=sysresampling(w); x=x(:,index);

3. Measurement update:

xHat(:,t)=mean(x,2);

Obtaining the estimate

end; A 5 minute MATLAB implementation is available if you are interested. Thomas B. Schön, Solving Nonlinear State Estimation Problems Using Particle Filters - An Engineering Perspective. Division of Automatic Control, Linköping University, Sweden, Technical Report nr: LiTH-ISY-R-2953, May 2010. http://www.rt.isy.liu.se/~schon/Publications/SchonPF2010.pdf Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

30

31

A General Particle Filter

The algorithm is reduced to the “first particle filter” Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

Illustration of the Depletion Problem

32

The particle filter provides an estimate of the joint smoothing density. However, the approximation is only good ”close” to the current time. This is referred to as the particle depletion problem. This leads to problems when the PF is used to solve the SLAM problem. Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy

33

Literature This lecture is to a large extent based on the following material Thomas B. Schön, Solving Nonlinear State Estimation Problems Using Particle Filters An Engineering Perspective. Division of Automatic Control, Linköping University, Sweden, Technical Report nr: LiTH-ISY-R-2953, May 2010. http://www.rt.isy.liu.se/~schon/Publications/SchonPF2010.pdf Thomas B. Schön and Fredrik Lindsten, Manipulating the Multivariate Gaussian Density. Division of Automatic Control, Linköping University, Sweden, January 2011. http://www.rt.isy.liu.se/~schon/Publications/SchonL2011.pdf

Some additional introductory texts on the particle filter are Arulampalam, M. S., Maskell, S., Gordon, N., and Clapp, T. (2002). A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking. IEEE Transactions on Signal Processing, 50(2):174–188. Doucet, A. and Johansen, A. M. (2011) A tutorial on particle filtering and smoothing: fifteen years later. In Handbook of Nonlinear Filtering (eds D. Crisan and B. Rozovsky). Oxford University Press. Non-Parametric Filtering Thomas Schön ([email protected])

Doctoral Course on Least Squares Estimation and SLAM Dip. Informatica e Sistemistica Sapienza Università di Roma Rome, Italy