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