Professional Documents
Culture Documents
Discrete Kalman Filter
Discrete Kalman Filter
Ian Reid
Hilary Term, 2001
We ended the rst part of this course deriving the Discrete-Time Kalman Filter as a recursive Bayes
estimator. In this lecture we will go into the lter in more detail, and provide a new derivation for
the Kalman lter, this time based on the idea of Linear Minimum Variance (LMV) estimation of
discrete-time systems.
1.1 Background
The problem we are seeking to solve is the continual estimation of a set of parameters whose values
change over time. Updating is achieved by combining a set of observations or measurements
which contain information about the signal of interest . The role of the estimator is to provide
at some time
. If
we have a prediction lter, if
a smoothing
an estimate
lter and if
the operation is simply called ltering.
Recall that an estimator is said to be unbiased if the expectation of its output is the expectation of
the quantity being estimated,
.
Also recall that a minimum variance unbiased estimator (MVUE) is an estimator which is unbiased and minimises the mean square error:
The term
, the so-called variance of error, is closely related to the error covariance
matrix,
. Specically, the variance of error of an estimator is equal to the trace
of the error covariance matrix,
trace
The Kalman lter is a linear minimum variance of error lter (i.e. it is the best linear lter over the
class of all linear lters) over time-varying and time-invariant lters. In the case of the state vector
and the observations being jointly Gaussian distributed, the MVUE estimator is a linear function
of the measurement set and thus the MVUE (sometimes written MVE for Minimum Variance of
Error estimator) is also a LMV estimator, as we saw in the rst part of the course.
Notation
The following notation will be used.
estimation error,
Covariance matrix.
(tilde notation)
(1)
where is the state at time , is an input control vector, is additive system or process noise,
is the input transition matrix and
is the state transition matrix.
We further assume that the observations of the state are made through a measurement system which
can be represented by a linear equation of the form,
(2)
where is the observation or measurement made at time , is the state at time , is the
observation matrix and is additive measurement noise.
w
+
u
G
unit
delay
1.3 Assumptions
We make the following assumptions;
The process and measurement noise random processes and are uncorrelated, zero-mean
white-noise processes with known covariance matrices. Then,
(3)
otherwise
(4)
otherwise
for all
(5)
The initial system state, is a random vector that is uncorrelated to both the system and
measurement noise processes.
The initial system state has a known mean and covariance matrix
and
(6)
Given the above assumptions the task is to determine, given a set of observations
, the
th instance in time generates an optimal estimate of the state ,
estimation lter that at the
which we denote by , that minimises the expectation of the squared-error loss function,
(7)
1.4 Derivation
Consider the estimation of state based on the observations up to time ,
, namely
. This is called a one-step-ahead prediction or simply a prediction. Now, the solution to
conditioned on the
the minimisation of Equation 7 is the expectation of the state at time
observations up to time . Thus,
(8)
(9)
where we have used the fact that the process noise has zero mean value and is known precisely.
The estimate variance
are uncorrelated:
(10)
Having obtained a predictive estimate suppose that we now take another observation .
How can we use this information to update the prediction, ie. nd ? We assume that the
estimate is a linear weighted sum of the prediction and the new observation and can be described by
the equation,
(11)
where and are weighting or gain matrices (of different sizes). Our problem now is to
reduced to nding the and that minimise the conditional mean squared estimation error
where of course the estimation error is given by:
(12)
(13)
Note that the last term on the right hand side of the equation is zero, and further note that the
prediction is unbiased:
(14)
and the condition that
(15)
(16)
and with
we obtain
(17)
Thus the covariance of the updated estimate is expressed in terms of the prediction covariance
trace
trace
(18)
trace
where
and then
Combining equations (17) and (18) and differentiating with respect to the gain matrix (using the
relation above) and setting equal to zero yields
(19)
Together with Equation 16 this denes the optimal linear mean-squared error estimator.
(20)
(21)
Update:
also known as the measurement update. This updates the state and variance using a
combination of the predicted state and the observation .
(22)
(23)
measurement
z
+
innovation
(whitening)
measurement
prediction
state
prediction
state
estimate
correction
u
G
+
unit
delay
x
F
Blending
(24)
Together with the initial conditions on the estimate and its error covariance matrix (equation 6)
this denes the discrete-time sequential, recursive algorithm for determining the linear minimum
variance estimate known as the Kalman lter.
the innovation
(25)
can be expressed by
(26)
The innovation or residual is an important measure of how well an estimator is performing. For
example it can be used to validate a measurement prior to it being included as a member of the
observation sequence (more on this later).
The process of transforming into is sometimes said to be achieved through the Kalman
whitening lter. This is because the innovations form an uncorrelated orthogonal white-noise process sequence which is statistically equivalent to the observations . This is important
because where as is in generally statistically correlated, the innovation is uncorrelated so
effectively provides new information or innovation.
The innovation has zero mean, since,
(27)
is given by
(28)
Using Equation 26 and 28 we can re-write the Kalman updates in terms of the innovation and its
variance as follows.
(29)
(30)
(31)
(32)
and
1.10 Example
Consider a vehicle tracking problem where a vehicle is constrained to move in a straight line with
a constant velocity. Let and represent the vehicle position and velocity. We assume that
observations of position can be made where the measurement noise is . Since the vehicle is
moving at constant speed,
.
where
(33)
where
Kalman lter:
(34)
(35)
(36)
Assume also that
are,
(37)
(38)
The Kalman lter involves sequential application of the recursive equations as given above for
. Here is some Matlab code to implement them, and an example program
%% innovation
%% innovation covariance
%% Kalman gain
%% new state
%% new covariance
%%% Matlab script to simulate data and process usiung Kalman filter
delT = 1;
F = [ 1 delT
0
1 ];
H = [ 1 0 ];
x = [ 0
10];
P = [ 10 0
0 10 ];
Q = [ 1 1
1 1 ];
R = [ 1 ];
z = [2.5 1 4 2.5 5.5 ];
for i=1:5
[xpred, Ppred] = predict(x, P, F, Q);
[nu, S] = innovation(xpred, Ppred, z(i), H, R);
[x, P] = innovation_update(xpred, Ppred, nu, S, H);
end
Results: The plots in Figure 3a-c illustrate the result of running the Kalman lter using
Some interesting observations can be made as follows.
1. Both
2. The estimates tend to follow the measurement values quite closely. Indeed since
is a weighting function acting on the measurement it is clear that this effect is more
prominent when is high.
3. From Equation 19 is decreases with and increases with . Thus the convergence properties are dependent on the relative magnitudes of the process and measurement
noise. Figure 4a illustrates this effect clearly. Here we have re-run the Kalman lter but decreased the elements of by a factor of 10 and 100 ( was kept at the original value).
It is clear from the gure that the net effect is that the estimates follow the measurements less
closely. Similar effects can be observed if the relative magnitude of is increased (Figure
4b). How do you explain what is observed in the latter case?
This example illustrates the fact that the performance of a Kalman lter is dependent on initialisation
conditions. In the next lecture we examine this observation in more detail when we consider the
performance of the discrete-time Kalman lter.
10
1.0
0.9
K0
0.8
^
x(k)
3
0.7
K1
2
0.6
z(k)
0.5
k
0
0
0.4
(a)
(b)
P(k|k)11
2
P(k|k)00
k
0
0
(c)
Figure 3: Example 1: (a) measurement and estimated state trajectories; (b) Kalman gain; (c) diagonal elements of error covariance matrix. Note how in the latter diagrams the curves tend to constant
values as gets large.
12
11
10
beta=100
9
4
8
7
alpha=0.01
6
5
alpha=0.1
alpha=1
z(k)
beta=1
z(k)
0
0
Figure 4: Example 1: Effect of changing the process and measurement noise covariance matrices
on the estimated position trajectory. (a) decreasing by a factor of 10 and 100; (b) increasing
by a factor of 100.
11
. In the
Consider a particle moving at constant velocity. Its ideal motion is described by
real world, the velocity will undergo some perturbation which we will assume to be randomly
distributed. Therefore the real motion equation is given by,
where
Recall from your 2nd year maths that this latter expectation is the auto-correlation function and that
an autocorrelation of this form corresponds to a constant power spectral density, more commonly
referred to as white noise.
The state vector is,
(39)
(40)
where
and
(41)
We obtain the discrete time equivalent for equation 40 for a sample interval of
over one sampling period. Taking Laplace transforms we obtain:
by integating up
Note that the integration on the right hand side is a consequence of the convolution rule for Laplace
transforms.
Shifting this forward in time to match our initial conditions we have
or
12
(42)
(43)
and
(44)
Note that we have derived the process noise covariance using a continuous-time white noise assumption. If, on the other hand we assume a piece-wise constant white noise model, then the target
undergoes a constant acceleration , with the accelerations independent from period to period, and
the process noise is slightly different. Bar-Shalom (p86) discusses (and derives the piecewise version) this and you should familiarise yourselves with both, but always keeeping in mind that both
are approximations.
1.11.2 Constant-acceleration particle
The analysis is similar to the previous case. In this case the particle moves at constant acceleration.
. In the real world, the acceleration will not
The ideal particle motion is described by
be perfectly constant, thus,
where as before,
(45)
13
(46)
where
and
(47)
is given by,
(48)
Once again taking Laplace transforms (etc) and using the assumption that is constant, we can
and the process noise covariance matrix :
determine the state transition matrix
and
(49)
In this lecture we consider how to evaluate the performance of a Kalman lter. We will focus on
understanding the following problems
1. how a Kalman lter of a perfectly modelled system with perfectly estimated noise behaves.
2. the effect that changes in the values of the noise sources have on the overall performance of a
Kalman lter.
3. how to recognise if the lter assumptions are met in practice. This is particularly important
in practical situations since many real systems are not well represented by a linear model and
measurement and system noise is non-Gaussian.
In the case of (3), here, we will only consider how to detect a problem with a Kalman lter. We consider how to modify the Kalman lter to accommodate nonlinear process and measurement models
and non-Gaussian noise in the nal lecture.
2.1 Example
We will illustrate Kalman lter performance using as an example a variant of the constant velocity
model we considered in section 1.10, in which we use the Kalman lter to track a vehicle moving in
a straight line.
14
70
60
50
40
30
20
10
0
10
20
30
40
50
60
70
80
90
100
-10
(a)
1.6
70
1.4
60
1.2
50
1.0
40
0.8
30
0.6
20
0.4
10
0.2
0.0
10
20
30
40
50
60
70
80
90
10
100
20
30
40
50
60
70
80
90
-10
-0.2
(b)
(c)
Figure 5: Input to Example 2: (a) true position; (b) true velocity; (c) observations.
System model
We assume that sampled observations are acquired at discrete time intervals
and output equations are of the form
Further, we assume that the process and observation noise are given by
(52)
.
We will take
,
and assume that the vehicle starts from rest so that
Figure 5 shows the true position and velocity and observations for a run of 100 samples computed
from the system equations using a pseudo-random number generator to generate normally distributed
15
100
and
we have
(53)
Equation (53) is known as the discrete-time matrix Ricatti equation. It turns out that if the system is
and are constant), and the measurement and process noise are stationary
time-invariant (i.e.
the solution to equation (53) converges to a positive denite
( and are constant) then as
matrix provided that the system model is completely observable and completely controllable (for
precise denitions of these see Jacobs). The corresponding gain matrix will also be
constant and called the steady-state gain.
The importance of this result is that in some applications you can assume that the Kalman lter
works under steady-state conditions. In this case you x the value of and hence from the start
and initial conditions do not need to be specied. Since is now xed it means that considerable
computational saving can be made since does not have to be recomputed at each time step.
2.2.2 Initialisation
Recall that part of the requirements for a Kalman lter is specication of initial conditions. Therefore, when considering implementation of a Kalman lter an important concern is how to set (guess!)
16
70
60
50
40
30
20
10
0
10
20
30
40
50
60
70
80
90
100
10
(a)
1.0
50
49
0.8
48
0.6
47
0.4
46
45
0.2
44
0.0
2
43
10
42
-0.2
41
-0.4
40
-0.6
39
74
(b)
76
78
80
82
84
(c)
Figure 6: Position track: (a) predicted, estimated positions and observation ; (b) initial track (zoom
of (a)); (c) steady-state track (zoom of (a).
1.6
1.4
estimate
1.2
1.0
0.8
prediction
0.6
0.4
0.2
0.0
10
20
30
40
50
60
70
0.2
17
80
90
100
86
2.0e01
2.0e01
Prediction
1.5e01
1.5e01
1.0e01
1.0e01
Predication
Estimate
Estimate
5.0e02
5.0e02
0.0e+00
0.0e+00
0
10
20
30
40
50
60
70
80
90
100
(a)
10
20
30
40
50
60
70
80
90
(b)
Figure 8: (a) Position error covariance; (b) Velocity error covariance.
values for and as they are not known. The obvious question to ask then, is does it matter
how good (or bad) your guess is?
One possibility is to initialise the state vector from the measurements
and a simple way to initialise the state covariance matrix is to set it to be a multiple of the process
noise matrix
).
Figure 9 illustrates the effect that changing initialisation parameters has on long term Kalman lter
performance. Note that regardless of the initial values both and tend to constant values in a few
iterations.
More formally, it can be shown that provided that the system is observable and controllable the error
. Finally note that although good initialisation
due to poor initialisation tends to zero as
is desirable for a linear Kalman lter it is not essential (the estimator merely takes longer to settle
down). However, good initialisation is critical in the implementation of Kalman lters for nonlinear
system models (see nal lecture).
2.2.3 Checking consistency
Since in practice we can not measure performance with respect to the state error measures (since
we dont know the true state values) how do we check that the lter is performing correctly? The
answer is that we can dene lter performance measures in terms of the innovation
We know that if the lter is working correctly then
is zero mean and white with a covariance
(see previous lecture). So we can verify that the lter is consistent by applying the following two
procedures.
1. check that the innovation is consistent with its covariance by verifying that the magnitude of
18
100
1.3e01
3.5
1.2e01
3.0
1.1e01
1.0e01
2.5
R=100
9.0e02
2.0
8.0e02
R=20
R=10
7.0e02
1.5
6.0e02
5.0e02
1.0
4.0e02
0.5
R=1
3.0e02
2.0e02
0.0
2
10
12
14
16
18
1.0e02
20
0.0e+00
-0.5
(a)
10
12
14
16
18
20
(b)
Figure 9: (a) Effect of using different position initialisation values; (b) Effect of changing
initialise .
to
1.2
1.0
0.8
0.6
0.4
0.2
0.0
10
20
30
40
50
60
70
80
90
100
0.2
0.4
0.6
0.8
1.0
1.2
test:
(54)
for a sequence of trials of a Kalman lter. If the lter assumptions are met then the are
each in degrees of freedom, where
in our case (the dimension of the measurement
19
0
0
10
20
30
40
50
60
70
80
90
100
(55)
This provides the test for unbiasedness. To estimate the mean we need to have
. The mean of this sequence,
samples of
can be used as a test statistic since
independent
is on degrees of freedom.
In our case, however, we can exploit the fact that the innovations are ergodic to estimate the sample
mean from the time average for a long sequence (ie. the moving average) rather than an ensemble
average. Thus we can estimate the mean as,
(56)
from a single run of a Kalman lter. Figure 11 shows the normalised innovation and the moving
average of the innovation. The latter tends to
as gets large. To test unbiasedness we need to
verify that lies in the condence interval dened by the hypothesis that is
(57)
,
, and let
For the example we are considering,
condence region). Using statistical tables we nd that,
20
1.0
0.8
0.6
0.4
0.2
0.0
10
20
30
40
50
60
70
80
90
100
-0.2
-0.4
(58)
(59)
distributed with mean zero and variance . Then we can compute the -gate as
and
of the values fall within this condence region. Again in our example the
check that at least
autocorrelation satises the hypothesis.
delT2/2
delT ];
zeros(2,N);
zeros(2,N);
zeros(1,N);
zeros(1,N);
i=2:N
w(:,i) = gennormal([0;0], Q);
x(:,i) = F*x(:,i-1) + w(:,i);
v(:,i) = gennormal([0], R);
z(:,i) = H * x(:,i) + v(:,i);
end
plot(x(1,:));
The matlab code to process the sequence and generate the various graphs is given below.
%%% Matlab script to assess Kalman filter performance
%%% The script assumes the existence of a vector z of
%%% noise corrupted observations
N = length(z);
Qfactor = 1;
Rfactor = 10;
delT = 1;
F = [ 1 delT
22
0
1 ];
H = [ 1 0 ];
% measurement matrix
sigmaQ = Qfactor*sqrt(0.01);
sigmaR = Rfactor*sqrt(0.1);
Q = sigmaQ2 * [ 1/3 1/2
1/2 1 ];
P = 10*Q;
R = sigmaR2 * [ 1 ];
xhat = zeros(2,N);
nu = zeros(1,N);
S = zeros(1,N);
q = zeros(1,N);
%
%
%
%
state estimate
innovation
innovation (co)variance
normalised innovation squared
for i=2:N
[xpred, Ppred] = predict(xhat(:,i-1), P, F, Q);
[nu(:,i), S(:,i)] = innovation(xpred, Ppred, z(i), H, R);
[xhat(:,i), P] = innovation_update(xpred, Ppred, nu, S, R);
q(:,i) = nu(:,i)*inv(S(:,i))*nu(:,i);
end
sumQ = sum(q)
r = xcorr(nu);
plot(xhat(1,:));
pause;
plot(nu)
% plot innovation and 2sigma confidence interval
hold on;
plot(2*sqrt(S),r);
plot(-2*sqrt(S),r);
hold off;
pause;
plot(q);
pause;
plot(r(N:2*N-1)/r(N));
Under-estimating : Refer to Figure 13. This illustrates the performance tests for the case when
the process noise is under-estimated by a factor of .
The normalised innovations squared are larger than expected and the sample mean falls outside
the condence bound dened by the test (for my trial the value came to 492.34/100 which is
clearly above the 95% condence region [74.22/100,129.6/100] computed above). This tells us that
the combined process and measurement noise levels are too low, i.e. too little weight is placed on
current measurements in the update process.
The autocorrelation sequence shows time correlations.
23
70
60
1.5
50
40
0.5
30
20
0.5
10
1.5
10
10
20
30
40
50
60
70
80
90
100
10
20
30
40
(a)
50
60
70
80
90
100
60
70
80
90
100
(b)
25
0.8
20
0.6
15
0.4
0.2
10
5
0.2
10
20
30
40
50
60
70
80
90
100
0.4
10
20
30
(c)
40
50
(d)
Figure 13: Effect of underestimating by a factor of . (a) state estimates; (b) innovation sequence; (c) normalised innovations squared; (d) normalised autocorrelation of the innovation sequence.
Over-estimating : Refer to Figure 14. This illustrates the performance tests for the case when
the process noise is over-estimated by a factor of . The innovations are well within the required
bounds.
The normalised innovations squared are smaller than expected and the sum (32.81, or eqivalently
the average) falls below the condence bound dened by the test. This tells us that the combined
process and measurement noise levels is too high.
The autocorrelation sequence shows no obvious time correlations.
Under-estimating : Refer to Figure 15. This illustrates the performance tests for the case when
the measurement noise is under-estimated by a factor of .
The innovations exceed the
The normalised innovations squared are larger than expected and the sample mean (3280/100) falls
outside the condence bound [0.74,1.3] dened by the test. This tells us that the combined
process and measurement noise levels is too low.
24
70
10
60
6
50
4
40
30
20
4
10
6
0
10
10
20
30
40
50
60
70
80
90
100
10
10
20
30
40
(a)
50
60
70
80
90
100
60
70
80
90
100
(b)
3.5
3
0.5
2.5
1.5
0
0.5
10
20
30
40
50
60
70
80
90
100
0.5
10
20
30
(c)
40
50
(d)
Figure 14: Effect of overestimating by a factor of . (a) state estimates; (b) innovation sequence;
(c) normalised innovations squared; (d) normalised autocorrelation of the innovation sequence.
The autocorrelation sequence shows no obvious time correlations.
Over-estimating : Refer to Figure 16. This illustrates the performance tests for the case when
the measurement noise is over-estimated by a factor of .
The innovations are below the
bounds.
The normalised innovations squared are smaller than expected and the sample mean (4.95/100) falls
outside the condence bound dened by the test. This tells us that the combined process and
measurement noise levels is too high.
The autocorrelation sequence shows time correlations.
General observations:
1. If the ratio of process to measurement noise is too low the innovation sequence becomes
correlated.
25
70
2.5
60
1.5
50
1
40
0.5
30
0
20
0.5
10
1
0
10
1.5
10
20
30
40
50
60
70
80
90
100
10
20
30
40
(a)
50
60
70
80
90
100
60
70
80
90
100
(b)
400
350
300
0.5
250
200
150
0
100
50
10
20
30
40
50
60
70
80
90
100
0.5
10
(c)
20
30
40
50
(d)
Figure 15: Effect of underestimating by a factor of . (a) state estimates; (b) innovation sequence; (c) normalised innovations squared; (d) normalised autocorrelation of the innovation sequence.
2. The absolute values of the process and measurement noise can be set by adjusting their values
so that the innovation test is satised.
3. In the example shown here, tuning is much more sensitive to changes in the measurement
noise rather than the process noise. In this example, this is because measurement noise affects
position, process noise only affects velocity (refer to the continuous system model in Lecture
1).
2.3.2 Detecting process modelling errors
We now consider what happens if we try to apply an estimator to measurement data that doesnt t
the model - the so-called mis-matched lter problem.
Specically, we consider the case of using a constant-velocity Kalman lter to track a particle which
has a true motion trajectory dened by a constant-acceleration model. Thus, the true motion is
26
70
60
50
40
30
20
10
10
10
20
30
40
50
60
70
80
90
100
10
20
30
40
(a)
50
60
70
80
90
100
70
80
90
100
(b)
0.25
0.8
0.2
0.6
0.15
0.4
0.2
0.1
0
0.05
0.2
10
20
30
40
50
60
70
80
90
100
0.4
10
20
(c)
30
40
50
60
(d)
Figure 16: Effect of overestimating by a factor of . (a) state estimates; (b) innovation sequence;
(c) normalised innovations squared; (d) normalised autocorrelation of the innovation sequence.
described by the transition equation
where the state transition matrix is
with
(60)
(61)
(62)
Figure 17 shows the result of applying the constant-velocity lter to the constant-acceleration model
and
.
where the lter noise parameters were
Observe that the innovation behaves like a rst order Gauss-Markov process (recall this implies that
in continuous-time
, where is white noise). The normalised squared values
27
12
4500
4000
10
3500
8
3000
2500
2000
4
1500
1000
500
0
0
500
10
20
30
40
50
60
70
80
90
100
10
20
30
40
(a)
50
60
70
80
90
100
70
80
90
100
(b)
500
1.2
450
1
400
0.8
350
300
0.6
250
0.4
200
150
0.2
100
0
50
10
20
30
40
50
60
70
80
90
100
0.2
10
20
(c)
30
40
50
60
(d)
Figure 17: Performance tests for an unmatched lter. (a) state estimates; (b) innovation sequence;
(c) normalised innovations squared; (d) normalised autocorrelation of the innovation sequence.
show a substantial drift in the mean and is not stationary. The autocorrelation reduces exponentially
in time - again typical of a rst-order Gauss-Markov process.
Boosting Q to reduce effects of modelling errors: one obvious thing to try in order to reduce the
effects of modelling errors is to boost the magnitude of the process noise articially to take into
account unmodelled errors. Recall that this should boost the value of the Kalman gain and hence let
the estimate follow the measurements more closely. The result of doing this where the process noise
was increased by a factor of 10 is shown in Figure 18. Some improvement is seen but this has not
totally compensated for the process model error.
So far we have considered the discrete-time formulation of the Kalman lter. This is the version
which nds the most wide-spread application in practice. The Kalman lter estimation approach
can also be derived for continuous-time. This is what we look at in this section. It is interesting to
28
20
6000
15
5000
10
4000
5
3000
0
2000
1000
10
1000
15
10
20
30
40
50
60
70
80
90
100
20
10
20
30
40
(a)
50
60
70
80
90
100
60
70
80
90
100
(b)
1.2
4.5
1
4
0.8
3.5
3
0.6
2.5
0.4
1.5
0.2
1
0
0.5
10
20
30
40
50
60
70
80
90
100
0.2
10
20
30
(c)
40
50
(d)
Figure 18: Performance tests for an unmatched lter with process noise boosted to compensate.
(a) state estimates; (b) innovation sequence; (c) normalised innovations squared; (d) normalised
autocorrelation of the innovation sequence.
study the continuous-time Kalman lter for two principal reasons;
1. to understand the asymptotic behaviour of discrete-time Kalman lters and,
2. to provide insight into the relationship between Kalman ltering and Wiener ltering.
We consider both of these factors in this section. In reading what follows you may nd it useful
to refer back to the equivalent results that we have previously derived for the discrete-time case in
Sections and .
29
(63)
(64)
Recall from the derivation of the discrete-time estimator that we also need to specify initial conditions for the state and its error covariance. Thus we assume that the initial conditions
(65)
are given.
(66)
(67)
which is known as the matrix Riccati equation. This matrix differential equation has been studied
extensively and an analytic solution exists for the constant parameter case.
Here the Kalman gain matrix is dened by
(68)
In summary Equations 66, 67 and 68 together with the initial conditions specied by Equation 65
describe the continuous-time Kalman lter algorithm. You should compare these equations with
the equivalent results for the discrete-time case.
3.3 Example
Consider the problem of estimating the value of a constant signal given measurements corrupted
by Gaussian white noise which is zero-mean and has constant spectral density . Derive (1) the
continuous-time Kalman lter; and (2) the discrete-time Kalman lter assuming a sampling time
interval of .
30
(69)
(70)
The scalar Riccati equation (Equation 67) governs error covariance propagation and is given by
where
. In this problem
Substituting for
. Therefore
(71)
and
(72)
Discrete-time solution: Let us consider what the result would have been if rather than analyse
the continuous-time measurements we had sampled the signals at instants in time
We have
, and .
(73)
(74)
The predicted state and error covariance are given by (see Section 1)
and
Using this result the update equation for the error covariance is
(75)
where . Making this substitution for into Equation 75
gives
If our initial error covariance was then it follows from Equation 76 that, at time
31
(76)
(77)
N
V
+
Hopt
+
H_1
H_2
(78)
where
Compare this with the result for the continuous-time case. Note again that as
tends to a constant value.
One nal point: unfortunately only simple types of continuous-time problems such as the example
given above can be solved analytically using the covariance equations. For more complicated problems, numerical methods are required. This is the main reason why the continuous-time Kalman
lter has not found wide-spread use as an estimation method.
(79)
Here is additive noise. If (1) we know the power spectral densities of the signal and noise,
and respectively; and (2) the observations have been acquired for sufcient time length so that
the spectrum of reects both and then the goal is to nd the optimal lter response
to recover the underlying signal.
Wiener lter solution: The Wiener lter solution to this problem is to nd the transfer function
corresponding to using frequency response methods. It can be shown (see for example Brown
chapter 4) that if the signal and noise are uncorrelated then the Wiener optimal lter takes on the
32
form
(80)
where and are the power spectral densities of the signal and noise. However, the lter
dened by Equation 80 denes a non-causal lter, meaning that the output depends on future values
of the input as well as the past, or
for some
For real-time operation this lter is not physically realisable (however it can be worth studying for
off-line processing applications). To generate a causal lter we need to dene the lter such that it
depends only on past and current values of the input , thus,
for all
Thus we want our optimal lter to satisfy this. The key is that if a transfer function
has only poles in the left half plane then its inverse is a positive time function (proof omitted).
Let
(81)
Then we can consider as being the cascade of two lters, (causal) and (noncausal) as illustrated in Figure 19. Let be the intermediate signal as shown. Then the power
spectral density of , is given by
(82)
In other words it is white noise and hence uncorrelated (independent of time). To make the whole
lter causal we can ignore the negative tail of the impulse response corresponding to . We do
this by taking partial fractions of and discarding the terms with poles in the right hand plane.
We denote this by . Hence the causal Wiener optimal lter is given by
(83)
Then,
(84)
33
(85)
It follows that,
is given by
(86)
(87)
Hence
(88)
(89)
(90)
The Kalman lter equivalent: We can solve the same LMV problem by using an alternative
approach based on state-space techniques that leads to a Kalman lter solution.
Specically, we can re-write Equation 79 in terms of a state-space equation as
We can nd as the solution to the steady-state continuous-time Kalman ltering problem and
hence nd the (optimal) transfer function between the signal and measurement. More details of
this approach are given next followed by a re-working of the example described earlier using this
alternative approach.
We have to make some assumptions about the Kalman ltering problem before we start. Let us
assume that the system and measurement equations have linear constant coefcients (i.e. and
are time-invariant). Let us also assume that the noise processes are time-invariant ( and are
constant). Further we assume that is controllable, and and are observable. Under these
conditions the Kalman lter will reach a steady-state condition where the error covariance matrix
. This means that the matrix Riccati equation (Equation 67) now becomes
34
(91)
(92)
Taking the Laplace transform and ignoring initial conditions (since we are in the steady state) we
have
or
(93)
This last equation denes a transfer function between the state estimate and the measurement which
when multiplied by gives the Wiener optimal lter,
(94)
Note that the causal Wiener lter and the continuous-time Kalman lter are equivalent under the
assumptions of time-invariance and one can be determined from the other. The key difference is that
one approach is based on state-space models and the other on frequency domain concepts (auto and
cross correlations).
Example: Let us re-work the example we considered before using the Kalman lter approach.
is decomposed as
Then this implies that
(95)
(96)
or
. Hence
The optimal transfer function between the state and measurement is therefore
35
giving
This section deals with some variants of the discrete Kalman lter which prove useful when some of
the assumptions of the conventional Kalman lter break down. Recall that three of the key problem
areas for a Kalman lter are,
1. Initialisation:
known.
we assume that the initial state vector and its error covariance matrix are
2. Modelling: we assume that we have an accurate linear model of the process and measurement
system.
3. Noise: we assume that the process and sensor noise processes are Gaussian.
In this section we look at how to deal with each of these problems.
We begin by considering the information lter which is a variant on the conventional Kalman lter
which gives more accurate results when there is no information about the initial state.
Next we consider how to cope with modelling error. In most practical cases the linear equations
describing the system and observation models of Equations 14 and 15 in Section are not a good
approximation to reality. Although, as we have seen, it is possible to detect that our assumptions
about modelling and noise are invalid it is clear that what we need to do is extend the estimation
approach to accommodate nonlinear models. Recall that for the Kalman lter algorithm, the estimate
. We showed in
is the conditional mean of the state given all the measurements up to time
Section 1 that under the assumption that the process was linear and the noise processes white this
led to linear, recursive solution of the form,
(97)
We show that there exists an equivalent version of Equation 97 that can be used in the case of a
nonlinear system model. We do this by linearising the (non-linear) state and observation matrices
about the estimated trajectory. This leads to the so-called Extended Kalman Filter (EKF) which is
the best linear estimator with respect to the minimum-mean-square error.
Finally we take a brief look at validating measurements where due to non-Gaussian sensor noise
some measurements could be confused with background clutter or outliers.
1. the measurement dimension is large compared to that of the process noise; or,
2. the initial system state is unknown.
As with the conventional algorithm, the information lter is a recursive linear estimator that repeatedly estimates the state of a variable called the information vector and its error covariance matrix
called the information matrix. This idea should be familiar from the rst part of the course where
we discussed recursive least squares.
The information vector at time
(98)
It is then straightforward to prove that its covariance is the inverse of the error covariance matrix,
, or information matrix.
Update equations: Recall that the conventional Kalman lter update equation for the error covariance matrix is given by
(99)
(100)
(101)
Proof:
Proof:
37
(102)
(103)
Note that this is exactly the form we derived from Bayes Rule in the rst part of the course: it is an
information weighted sum of prediction and measurement
The update can be written succinctly as
It follows that the corresponding prediction equations for the information lter are
(104)
(105)
Update:
Comments: As noted at the beginning of this section certain problems are better solved using the
information lter rather than the conventional Kalman lter.
In the case that there is no information about the initial state then the magnitude of should be
set very large in the conventional Kalman lter. This may lead to signicant loss in accuracy. The
information lter does not have this problem as it uses the inverse of the initial state covariance error
matrix.
When the dimension of the measurement vector is signicantly larger than that of the process
noise the information lter is computationally more efcient that the conventional Kalman lter.
This is because one of the computationally expense steps in either case is matrix inversion. In the
case of the conventional Kalman lter the inversion of the matrix
38
is required. In the case of the inverse covariance lter we compute the inverse of the
matrix dened by Equation 105.
Finally, note that given the output from one lter it is easy to nd the equivalent output of the other
lter using Equation 98 and the imformation matrix (inverse covariance).
where
(106)
(107)
We assume that the process and measurement noise are Gaussian, uncorrelated and zero-mean, and
have no cross-correlation. Thus
4.2.1 Prediction
As in the linear case, we assume that we have at time
To generate the prediction we expand Equation 106 in a Taylors series about the prediction
to the rst-order terms.
up
(108)
where the Jacobian of is evaluated at . Taking the Expectation of Equation 108, ignoring
higher than rst order terms, and assuming that is approximately equal to the conditional mean
and that the process noise has zero mean, yields
39
(109)
The state covariance can be found as follows. First the prediction error is given by
(110)
The prediction covariance is then found by taking the Expectation of the product of the prediction
error with its transpose:
(111)
Note that the prediction covariance has the same form as its linear equivalent with the Jacobian
playing the role of transition matrix .
Truncating to rst order and taking expectations yields the predicted observation
(112)
(113)
40
(114)
where the Jacobian of is evaluated at . Again note that Equation 114 is in the same form as
its linear counterpart except that has replaced .
4.2.3 Update
By similar reasoning to that made for the linear case it is possible to derive from rst principles the
equations for the lter gain, state update and covariance update. The forms turn out to be the same
as for the linear Kalman lter with replaced by . Thus, the Kalman gain is given by
(115)
(116)
(117)
(118)
(119)
Update:
(120)
(121)
where
(122)
and
(123)
and
are functions of both the state and timestep; they are not con-
41
2. Stability: Since we are dealing with perturbation models of the state and observation matrices
about the predicted trajectory, it is important that predictions are close enough to the true states
otherwise the lter will be poorly matched and possibly diverge.
3. Initialisation: Unlike in the linear case, special care has to be taken when initialising the
Extended Kalman lter.
4. Computational cost: The Extended Kalman lter is computationally signicantly more expensive than its linear counterpart. This limited its early use in applications. However, today
real-time computing implementations of the EKF can be achieved using moderate computing
resources.
4.4 Implementation
Implementation issues are similar to those of the linear Kalman lter and you can test the performance of the lter using all the techniques introduced in Section .
In particular, special care has to be taken to check whether the system and noise process modelling
assumptions are met. There are obviously some errors introduced by using a linearised model.
A further important point to note is that the state covariance matrix is only an approximation to the
mean square error and not a true covariance. Recall that determines the weight given to
new measurements in the updating procedure. Thus, if is erroneous and becomes small,
the measurements have little affect on the estimation and it is quite possible that the EKF will diverge.
The motion of the vehicle can be described by the nonlinear state transition equation
(124)
Here, is the time interval between time steps, is the wheel baseline and is the noise vector
which combines errors in modelling the process and control.
We assume that measurements of range (depth) and bearing are made to a set of beacons at xed
locations
. The nonlinear observation model is therefore
42
(125)
Bi
z(k)=(r,)
x(k)=(x,y,)T
origin
is given by
(126)
(127)
where
(128)
Update:
The equations for the updating of the state and its covariance are:
(129)
where
43
(130)
and
(131)
and
Here
(132)
(133)
distribution tables.
In the context of the Kalman Filter algorithm, Equation 133 constrains the region of space where we
look for a measurement. We assume that the correct measurement will be detected in this region. It
is possible, however, that more than one measurement will be in the valid region. The problem of
distinguishing between the correct measurement and measurements arising from background clutter
and other targets (false alarms) is called data association. This topic falls outside of the scope of
the course (see, for example, Bar-Shalom and Fortmann).
44