| Date post: | 03-Apr-2018 |
| Category: |
Documents |
| Upload: | arduinostrapdown |
| View: | 227 times |
| Download: | 0 times |
of 60
7/28/2019 Kalman Introduction
1/60
Introduction to Kalman Filtering
Maria Isabel Ribeiro, Pedro Lima
with revisions introduced by Rodrigo Ventura
Instituto Superior Tcnico / Instituto de Sistemas eRobtica
October 2008
All rights reserved
7/28/2019 Kalman Introduction
2/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 2
INTRODUCTION TO KALMAN FILTERING
What is a Kalman Filter ? Introduction to the Concept A simple example Which is the best estimate ? Basic Assumptions
Discrete Kalman Filter Problem Formulation From the Assumptions to the Problem Solution Towards the Solution Filter dynamics
Prediction cycle Filtering cycle Summary
Properties of the Discrete KF A simple example
The meaning of the error covariance matrix The Extended Kalman Filter
7/28/2019 Kalman Introduction
3/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 3
WHAT IS A KALMAN FILTER?
Optimal Recursive Data Processing Algorithm
Measuring
devices
System
Kalman filter
System state(desired but
not know)
Measurementerror sources
Controls
System errorsources
Observed
measurements
Optimal estimate ofsystem state
Typical Kalman filter application
7/28/2019 Kalman Introduction
4/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 4
WHAT IS A KALMAN FILTER?
Introduction to the Concept
7/28/2019 Kalman Introduction
5/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering
A simple (static) example (from Maybeck)
You are an inexperient sailor at sea and you do not know your location Take a (one-dimensional) star sighting to establish your position At time t1 you establish your position to be z1 (you measure) Because there is ineherent measurement device innacuracies you say that the
precision is z1 (standard deviation)
You can establish the conditional probability of x(t1) (your position at time t1)conditioned on the observed value of measurement z
1.
5
Based on this conditional pdf
the best estimate of your
position is
and the variance of the error in
the estimate is
7/28/2019 Kalman Introduction
6/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering
A simple (static) example (from Maybeck)
You have a friend that is a trained sailor At time instant t2 ( ) (the boat did not move) this trained sailor measures z2 with a
variance
As this second sailor has larger skills, assume that the variance in his measurementis smaller than the measurement in yours.
6
Based on this conditional pdf
the best estimate of the
position given by the trainedsailor
and the variance of the error in
the estimate is
7/28/2019 Kalman Introduction
7/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering
A simple (static) example (from Maybeck)
At this point we have two measurements, with different uncertainty How to combine these data? What do we want to know?
Conditional position at time t2 ( ) given both z1 and z2
7
is Gaussian
The uncertainty in the position
estmate decreased by
combining the two pieces ofinformation
7/28/2019 Kalman Introduction
8/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering
A simple (static) example (from Maybeck)
Which is the best estimate at time t2 given z1 and z2? The mean (also the maximum) of the conditional pdf
8
In the previous time instant
The best prediction at time t2, is given bythe previous estimate plus an error
multiplied by a gain
Interpret the errorThis is the same as the Kalman filterimplementation
gainerror
7/28/2019 Kalman Introduction
9/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering
A simple (static) example (from Maybeck)
Which is the best estimate at time t2 given z1 and z2? The mean (also the maximum) of the conditional pdf
9
gain error
Which is the uncertainty of the estimate at time t2 given z1 and z2? The variance of the conditional pdf
7/28/2019 Kalman Introduction
10/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering
A simple (dynamic) example (from Maybeck)
10
From now on observations are done at different time instants andthe boat travels between the time instants where measurementsare taken
Motion modelu= velocityw = noise term representing uncertainty in the actualknowledge of velocity, assumed zero mean
At time t2 we already have and Now the vehicle is travelling Before doing a measurement at time instant t3 (i.e., at ) which
is the best prediction that we can do about the position and
associated uncertainty?
Prediction
7/28/2019 Kalman Introduction
11/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering
A simple (dynamic) example (from Maybeck)
11
A measurement z3 is done at time instant t3 with an assumed variance Which is now the best estimate ?
Once again we have two Gaussian probability density functions One associated with information up to one provided by the measurement itself
Combining bothFiltering
Kalman Gain
7/28/2019 Kalman Introduction
12/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 12
WHAT IS A KALMAN FILTER?
Introduction to the concept
7/28/2019 Kalman Introduction
13/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 13
WHAT IS A KALMAN FILTER?
Introduction to the concept
To evaluate theKF only requires
and z(k+1)
7/28/2019 Kalman Introduction
14/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 14
WHAT IS A KALMAN FILTER?
Introduction to the concept
7/28/2019 Kalman Introduction
15/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 15
WHAT IS THE KALMAN FILTER ?
Which is the best estimate?
Any type of filter tries to obtain an optimal estimate of desiredquantities from data provided by a noisy environment.
Best = minimizing errors in some respect. Bayesian viewpoint - the filter propagates the conditional probability
density of the desired quantities, conditioned on the knowledge ofthe actual data coming from measuring devices
Why base the state estimation on the conditional probability densityfunction ?
7/28/2019 Kalman Introduction
16/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 16
WHAT IS A KALMAN FILTER?
Which is the best estimate?
Example
x(i) one dimensional position of a vehicle at time instant i z(j) two dimensional vector describing the measurements of position
at time j by two separate radars
If z(1)=z1, z(2)=z2, ., z(j)=zj
represents all the information we have on x(i) based (conditioned) onthe measurements acquired up to time i
given the value of all measurements taken up time i, this conditional pdfindicates what the probability would be of x(i) assuming any particular
value or range of values.
7/28/2019 Kalman Introduction
17/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 17
WHAT IS A KALMAN FILTER?
Which is the best estimate?
The shape of conveys theamount of certainty we have in the knowledge of the value x.
Based on this conditional pdf, the estimate can be: the mean - the center of probability mass (MMSE) the mode - the value of x that has the highest probability (MAP) the median - the value of x such that half the probability weight lies to the left
and half to the right of it.
7/28/2019 Kalman Introduction
18/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 18
WHAT IS THE KALMAN FILTER ?
Basic Assumptions
Under these assumptions, the conditional pdf is Gaussian mean=mode=median there is a unique best estimate of the state the KF is the best filter among all the possible filter types
What happens if these assumptions are relaxed? Is the KF still an optimal filter? In which class of filters?
7/28/2019 Kalman Introduction
19/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 19
DISCRETE KALMAN FILTER
Problem Formulation
MOTIVATION
Given a discrete-time, linear, time-varying plant with random initial state driven by white plant noise
Given noisy measurements of linear combinations of the plant statevariables
Determine the best estimate of the system state variableSTATE DYNAMICS AND MEASUREMENT EQUATION
7/28/2019 Kalman Introduction
20/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 20
DISCRETE KALMAN FILTER
Problem Formulation
VARIABLE DEFINITIONS
7/28/2019 Kalman Introduction
21/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 21
DISCRETE KALMAN FILTER
Problem Formulation
INITIAL CONDITIONS
x0 is a Gaussian random vector, with mean covariance matrix
STATE AND MEASUREMENT NOISE
zero mean E[wk]=E[vk]=0 {wk}, {vk} - white Gaussian sequences
x(0), wk and vj are independent for all k and j
7/28/2019 Kalman Introduction
22/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 22
DISCRETE KALMAN FILTER
Problem Formulation
DEFINITION OF FILTERING PROBLEM
Let k denote present value of time
Given the sequence of past inputs
Given the sequence of past measurements
Evaluate the best estimate of the state x(k)
7/28/2019 Kalman Introduction
23/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 23
DISCRETE KALMAN FILTER
Problem Formulation
Given x0 Nature apply w0 We apply u0 The system moves to state x1 We make a measurement z1
Question: which is the best estimate of x1?
Nature apply w1 We apply u1 The system moves to state x2 We make a measurement z2
Question: which is the best estimate of x2?
.
.
.
Answer: obtained from
Answer: obtained from
7/28/2019 Kalman Introduction
24/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 24
DISCRETE KALMAN FILTER
Problem Formulation
.
.
.
Question: which is the best estimate of xk-1?
Nature apply wk-1 We apply uk-1 The system moves to state xk We make a measurement zk
Question: which is the best estimate of xk?
.
.
.
Answer: obtained from
Answer: obtained from
7/28/2019 Kalman Introduction
25/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 25
DISCRETE KALMAN FILTER
Towards the Solution
The filter has to propagate the conditional probability densityfunctions
.
.
.
.
.
.
.
.
....
7/28/2019 Kalman Introduction
26/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 26
DISCRETE KALMAN FILTER
From the Assumptions to the Problem Solution
is Gaussian
Uniquely characterized by
the conditional mean the conditional covarianceP(k | k) = cov[x
k;x
k|Z1
k,U0
k1]
7/28/2019 Kalman Introduction
27/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 27
DISCRETE KALMAN FILTER
Towards the Solution
.
.
.
.
.
.
.
.
....
.
.
.
.
.
.
7/28/2019 Kalman Introduction
28/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 28
DISCRETE KALMAN FILTER
Towards the Solution
We stated that the state estimate equals the conditional mean
Why? Why not the mode of ? Why not the median of ?
As is Gaussian mean = mode = median
7/28/2019 Kalman Introduction
29/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 29
DISCRETE KALMAN FILTER
Filter dynamics
KF dynamics is recursive
What can you say about
xk+1 before we makethe measurement zk+1
How can we improve
our information on xk+1after we make the
measurement zk+1
7/28/2019 Kalman Introduction
30/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 30
DISCRETE KALMAN FILTER
Filter dynamics
prediction predictionfiltering filtering
predictionprediction
filtering
filtering
7/28/2019 Kalman Introduction
31/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 31
DISCRETE KALMAN FILTER
Filter dynamics - Prediction cycle
Prediction cycle
assumed known
?
Is Gaussian
7/28/2019 Kalman Introduction
32/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 32
DISCRETE KALMAN FILTER
Filter dynamics - Prediction cycle
Prediction cycle
prediction
error
7/28/2019 Kalman Introduction
33/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 33
DISCRETE KALMAN FILTER
Filter dynamics - Filtering cycle
Filtering cycle
+ ?
1st Step Measurement prediction What can you say about zk+1 before wemake the measurement zk+1
7/28/2019 Kalman Introduction
34/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 34
DISCRETE KALMAN FILTER
Filter dynamics - Filtering cycle
Filtering cycle2nd Step
If x, y and z are jointly Gaussian and y and z are
statistically independent
Required result
Z1k+1
and {Z1k
,z(k+1 | k)} Equivalent from the point of viewof the contained information
7/28/2019 Kalman Introduction
35/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 35
DISCRETE KALMAN FILTER
Filter dynamics - Filtering cycle
Filtering cycle
Kalman Gain
measurement
prediction
7/28/2019 Kalman Introduction
36/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 36
DISCRETE KALMAN FILTER
Dynamics
Linear System
Discrete Kalman Filter
P(k+1 | k+1) = P(k+1 | k)K(k+1)Ck+1P(k+1 | k)
prediction
filtering
Initial
conditions
7/28/2019 Kalman Introduction
37/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 37
DISCRETE KALMAN FILTER
Properties
The Discrete KF is a time-varying linear system
even when the system is time-invariant and has stationary noise
the Kalman gain is not constant
Does the Kalman gain matrix converges to a constant matrix? Inwhich conditions?
7/28/2019 Kalman Introduction
38/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 38
DISCRETE KALMAN FILTER
Properties
The state estimate is a linear function of the measurementsKF dyamics in terms of the filtering estimate
Assuming null inputs for the sake ofsimplicity
7/28/2019 Kalman Introduction
39/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 39
DISCRETE KALMAN FILTER
Properties
Innovation process
z(k+1) carries information on x(k+1) that was not available on this new information is represented by r(k+1) - innovation process
Properties of the innovation process the innovations r(k) are orthogonal to z(i)
the innovations are uncorrelated/white noise
this test can be used to acess if the filter is operating correctly
7/28/2019 Kalman Introduction
40/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 40
DISCRETE KALMAN FILTER
Properties
Covariance matrix of the innovation process
7/28/2019 Kalman Introduction
41/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 41
DISCRETE KALMAN FILTER
Properties
The Discrete KF provides an unbiased estimate of the state
is an unbiased estimate of the state x(k+1), providing that theinitial conditions are
Is this still true if the filter initial conditions are not the specified ?
7/28/2019 Kalman Introduction
42/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 42
DISCRETE KALMAN FILTER
Steady state Kalman Filter
Time invariant system; stationary white system and observation noise
Filter dynamics
P(k +1|k) = AP(k | k -1)ATAP(k | k1)C
T[CP(k | k1)C
T+R]
1CP(k | k1)A
T+GQG
T
Discrete Riccati Equation
7/28/2019 Kalman Introduction
43/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 43
DISCRETE KALMAN FILTER
Steady state Kalman Filter
If Q is positive definite, is controllable, and (A,C) isobservable, then
the steady state Kalman filter exists the limit exists is the unique, finite positive-semidefinite solution to the algebraic
equation
is independent of provided that the steady-state Kalman filter is assymptotically unbiased
7/28/2019 Kalman Introduction
44/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 44
MEANING OF THE COVARIANCE MATRIX
Generals on Gaussian pdf
Let z be a Gaussian random vector of dimension n
P - covariance matrix - symmetric, positive definite Probability density function
n=1 n=2
7/28/2019 Kalman Introduction
45/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 45
MEANING OF THE COVARIANCE MATRIX
Generals on Gaussian pdf
Locus of points where the pdf is greater or equal than a giventhreshold
n=1 line segment n=2 ellipse and inner points
n=3 3D ellipsoid and inner points n>3 hiperellipsoid and inner points
If the ellipsoid axis are aligned with the axis of the referencial where the
vector z is defined
length of the ellipse semi-axis =
7/28/2019 Kalman Introduction
46/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 46
MEANING OF THE COVARIANCE MATRIX
Generals on Gaussian pdf - Error elipsoid
Examplen=2
7/28/2019 Kalman Introduction
47/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 47
MEANING OF THE COVARIANCE MATRIX
Generals on Gaussian pdf -Error ellipsoid and axis orientation
Error ellipsoid P=PT - to distinct eigenvalues correspond orthogonal eigenvectors Assuming that P is diagonalizable
Error ellipoid (after coordinate transformation)
At the new coordinate system, the ellipsoid axis are aligned with theaxis of the new referencial
with
7/28/2019 Kalman Introduction
48/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 48
MEANING OF THE COVARIANCE MATRIX
Generals on Gaussian pdf -Error elipsis and referencial axis
n=2
ellipse
7/28/2019 Kalman Introduction
49/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 49
MEANING OF THE COVARIANCE MATRIX
Generals on Gaussian pdf -Error ellipse and referencial axis
n=2
1 =
1
2
x
2+
y
2+ (
x
2
y
2)2+ 42
x
2y
2[ ]
2 =1
2x2 +y2 (x2 y2)2 + 42x2y2[ ]
7/28/2019 Kalman Introduction
50/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 50
DISCRETE KALMAN FILTER
Probabilistic interpretation of the error ellipsoid
Given and it is possible to define the locus where, with agiven probability, the values of the random vector x(k) lie.
Hiperellipsoid with center in and with semi-axis
proportional to the eigenvalues of
7/28/2019 Kalman Introduction
51/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 51
DISCRETE KALMAN FILTER
Probabilistic interpretation of the error ellipsoid
Example for n=2
is a function of Ka pre-specified values of this probability can be obtained by anapropriate choice of K
7/28/2019 Kalman Introduction
52/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 52
DISCRETE KALMAN FILTER
Probabilistic interpretation of the error ellipsoid
How to chose K for a desired probability? Just consult a Chi square distribution table
xkR
n
(Scalar) random variable with a distribution
with n degrees of reedom
Probability = 90%
n=1 K=2.71
n=2 K=4.61
Probability = 95%
n=1 K=3.84
n=2 K=5.99
7/28/2019 Kalman Introduction
53/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 53
DISCRETE KALMAN FILTER
The error ellipsoid and the filter dynamics
Prediction cycle
7/28/2019 Kalman Introduction
54/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 54
DISCRETE KALMAN FILTER
The error ellipsoid and the filter dynamics
Filtering cycle
7/28/2019 Kalman Introduction
55/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 55
Extended Kalman Filter
are non Gaussian
7/28/2019 Kalman Introduction
56/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 56
Extended Kalman Filter
7/28/2019 Kalman Introduction
57/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 57
Extended Kalman Filter
linearize
aroundApply KF to thelinear dynamics
linearize
around
Apply KF to thelinear dynamics
7/28/2019 Kalman Introduction
58/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 58
Extended Kalman Filter
linearize
around
fk(xk,uk) fk(xk|k,uk) +fk(xk xk|k) + ....
known inputPrediction cycle of KF
7/28/2019 Kalman Introduction
59/60
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering 59
Extended Kalman Filter
linearize
around
known inputUpdate cycle of KF
7/28/2019 Kalman Introduction
60/60
References
Anderson, Moore, Optimal Filtering, Prentice-Hall, 1979. M. Athans, Dynamic Stochastic Estimation, Prediction and Smoothing, Series of Lectures,
Spring 1999.
E. W. Kamen, J. K. Su, Introduction to Optimal Estimation, Springer, 1999. Peter S. Maybeck, The Kalman Filter: an Introduction to Concepts Jerry M. Mendel, Lessons in Digital Estimation Theory, Prentice-Hall, 1987. Isabel Ribeiro, Kalman and Extended Kalman Filters: Concept, Derivation and Properties
Institute for Systems and Robotics, IST, February 2004 (http://www.isr.ist.utl.pt/~mir)
Isabel Ribeiro, Gaussian Probability Density Functiosn: Properties and Error Characterization,Institute for Systems and Robotics, IST, February 2004 (http://www.isr.ist.utl.pt/~mir)
