Professional Documents
Culture Documents
Autonomous Systems
The meaning of the error covariance matrix The Extended Kalman Filter
2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering
Controls
System
System state (desired but not know) Observed
Measuring devices
measurements
Kalman filter
Autonomous Systems
Kalman Filtering
Autonomous Systems
Kalman Filtering
Autonomous Systems
Kalman Filtering
Based on this conditional pdf the best estimate of the position given by the trained sailor and the variance of the error in the estimate is
Autonomous Systems
Kalman Filtering
is Gaussian
The uncertainty in the position estmate decreased by combining the two pieces of information
Kalman Filtering
Autonomous Systems
gain
error
The best prediction at time t2, is given by the previous estimate plus an error multiplied by a gain Interpret the error
Kalman Filtering
gain
error
Which is the uncertainty of the estimate at time t2 given z1 and z2? The variance of the conditional pdf
Autonomous Systems
Kalman Filtering
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
Autonomous Systems
Kalman Filtering
10
Kalman Gain
Autonomous Systems
Kalman Filtering
11
Autonomous Systems
Kalman Filtering
12
the
13
Autonomous Systems
Kalman Filtering
14
Why base the state estimation on the conditional probability density function ?
Autonomous Systems
Kalman Filtering
15
represents all the information we have on x(i) based (conditioned) on the measurements acquired up to time i given the value of all measurements taken up time i, this conditional pdf indicates what the probability would be of x(i) assuming any particular value or range of values.
Autonomous Systems
Kalman Filtering
16
Autonomous Systems
Kalman Filtering
17
What happens if these assumptions are relaxed? Is the KF still an optimal filter? In which class of filters?
Autonomous Systems
Kalman Filtering
18
Given noisy measurements of linear combinations of the plant state variables Determine the best estimate of the system state variable STATE DYNAMICS AND MEASUREMENT EQUATION
Autonomous Systems
Kalman Filtering
19
Autonomous Systems
Kalman Filtering
20
STATE AND MEASUREMENT NOISE zero mean E[wk]=E[vk]=0 {wk}, {vk} - white Gaussian sequences
21
Let k denote present value of time Given the sequence of past inputs
Autonomous Systems
Kalman Filtering
22
. . .
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima
Kalman Filtering
23
. . .
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering
24
. . .
. . .
. . .
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima
. . .
Kalman Filtering
25
is Gaussian
Autonomous Systems
26
. . .
. . .
. . .
. . .
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima
. . .
. . .
Kalman Filtering
27
? ?
As
is Gaussian
Autonomous Systems
Kalman Filtering
28
What can you say about xk+1 before we make the measurement zk+1
How can we improve our information on xk+1 after we make the measurement zk+1
Autonomous Systems
Kalman Filtering
29
prediction
filtering
prediction
filtering
filtering
Autonomous Systems
Kalman Filtering
30
?
assumed known Is Gaussian
Autonomous Systems
Kalman Filtering
31
prediction error
Autonomous Systems
Kalman Filtering
32
1st Step
Measurement prediction
What can you say about zk+1 before we make the measurement zk+1
Autonomous Systems
Kalman Filtering
33
Required result
Autonomous Systems
Kalman Filtering
34
Autonomous Systems
Kalman Filtering
35
prediction
filtering
P ( k + 1 | k + 1) = P (k + 1 | k ) K ( k + 1)Ck +1P ( k + 1 | k )
Initial conditions
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering
36
Does the Kalman gain matrix converges to a constant matrix? In which conditions?
Autonomous Systems
Kalman Filtering
37
Autonomous Systems
Kalman Filtering
38
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
39
Autonomous Systems
Kalman Filtering
40
is an unbiased estimate of the state x(k+1), providing that the initial conditions are
Is this still true if the filter initial conditions are not the specified ?
Autonomous Systems
Kalman Filtering
41
Filter dynamics
42
is independent of
provided that
Autonomous Systems
Kalman Filtering
43
n=1
n=2
Autonomous Systems
Kalman Filtering
44
n=2 ellipse 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
Autonomous Systems
Kalman Filtering
45
Autonomous Systems
Kalman Filtering
46
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 with
At the new coordinate system, the ellipsoid axis are aligned with the axis of the new referencial
Autonomous Systems 2007 - M. Isabel Ribeiro, Pedro Lima Kalman Filtering
47
MEANING OF THE COVARIANCE MATRIX Generals on Gaussian pdf -Error elipsis and referencial axis
n=2
ellipse
Autonomous Systems
Kalman Filtering
48
MEANING OF THE COVARIANCE MATRIX Generals on Gaussian pdf -Error ellipse and referencial axis
n=2
1 2 2 2 2 2 2 2 x + y + ( x y ) + 4 2 x y 2 1 2 2 2 2 2 2 2 2 = x + y ( x y ) + 4 2 x y 2
1 =
[ [
] ]
Autonomous Systems
Kalman Filtering
49
Given and it is possible to define the locus where, with a given probability, the values of the random vector x(k) lie.
Autonomous Systems
Kalman Filtering
50
51
xk Rn
distribution
Probability = 90% n=1 K=2.71 n=2 K=4.61 Probability = 95% n=1 K=3.84 n=2 K=5.99
Kalman Filtering
Autonomous Systems
52
DISCRETE KALMAN FILTER The error ellipsoid and the filter dynamics
Prediction cycle
Autonomous Systems
Kalman Filtering
53
DISCRETE KALMAN FILTER The error ellipsoid and the filter dynamics
Filtering cycle
Autonomous Systems
Kalman Filtering
54
55
Autonomous Systems
Kalman Filtering
56
linearize
Apply KF to the linear dynamics
around
linearize around
Apply KF to the linear dynamics
Autonomous Systems
Kalman Filtering
57
k | k , uk ) + f k ( x k x k | k ) + .... f k ( x k , uk ) f k ( x
Prediction cycle of KF
known input
Autonomous Systems
Kalman Filtering
58
Update cycle of KF
known input
Autonomous Systems
Kalman Filtering
59
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)
Autonomous Systems
Kalman Filtering
60