Introkalman e 151211
Introkalman e 151211
D.Alazard
Contents
Introduction 5
2 Kalman filter 27
2.1 Principle of Kalman filtering . . . . . . . . . . . . . . . . . . . . . . 27
2.1.1 Kalman model . . . . . . . . . . . . . . . . . . . . . . . . . . 27
2.1.2 Assumptions: . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
2.1.3 Structure of an unbiased estimator . . . . . . . . . . . . . . . 30
2.2 Minimal error variance estimator . . . . . . . . . . . . . . . . . . . . 32
2.2.1 General solution . . . . . . . . . . . . . . . . . . . . . . . . . . 33
2.2.2 Kalman filter steady state . . . . . . . . . . . . . . . . . . . 34
2.2.3 Kalman filter tuning . . . . . . . . . . . . . . . . . . . . . . 35
2.3 Corrected exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
References 59
Introduction
trol). The signal processing principles on which is based Kalman filter will be
also very useful to study and perform test protocols, experimental data processing
and also parametric identification, that is the experimental determination of some
plant dynamic parameters.
Chapter 1
where :
• w(t) ∈ Rq is the vector of unknown random signals (noises) that perturb the
state equation through the input matrix Mn×q (wx = M w denotes also the
state noise),
Example 1.1 The ”quarter vehicle” model used to study a car active suspension
can be depicted by Figure 1.1. The control device (piston, jack) allows a force u
to be applied between the wheel (with a mass m and a vertical position z) and the
body of the car (with a mass M and a vertical position Z). K and f denote the
stiffness and the damping of the passive suspension. k is the tire stiffness between
wheel and ground. Finally, w denotes the vertical position of the point of contact
tire/ground excited by road unevenness (rolling noise). Body vertical acceleration
K f u
g
m
Z
k
z
ground w
˙ δz]
Let x = [δZ, δz, δZ, ˙ T be the state vector, then the following state space repre-
sentation can be derived:
0 0 1 0 0 0 0
0 0 0 1 0
ẋ = x + 0 u + 0 w
−K/M K/M −f /M f /M 1/M 0 g 0
K/m −(K + k)/m f /m −f /m −1/m 0 1/k
u
y = −K/M K/M −f /M f /M x + [1/M − 1] +v .
g
(1.2)
This model is in the form of (1.1) with n = 4, m = 2, q = 1 and p = 1.
1
y: :
F (s) = D + C(sI − A)−1 B .
The response of this model to a deterministic input u(t) over a time range t ∈ [t0 , t]
and to initial conditions x(t0 ) reads :
Z t
A(t−t0 )
x(t) = e x(t0 ) + eA(t−τ ) Bu(τ ) dτ (1.3)
t0
y(t) = Cx(t) + Du(t) (1.4)
• to state some assumptions on the stochastic properties of noises w(t) and v(t)
(gaussian white noise) to facilitate the determination of the response of model
1.1,
The various definitions and background given below are extracted from refer-
ence [5] (chapter II and appendix A.I).
F : R → [0 1] / F (x) = P [X < x] .
Properties:
dF (x)
p(x) = soit : p(x)dx = P [x ≤ X < x + dx] .
dx
To characterize a random variable X , one can also used the moments of this
variable. The first moment is called mean value or expected value. The second
central moment is called variance and is denoted varx = σx2 where σx is the
standard deviation. That is:
• expected value:
Z +∞ Z +∞
E[X ] = x p(x) dx = x dF (x) , (1.5)
−∞ −∞
• k-th moment: Z +∞
k
E[X ] = xk p(x) dx , (1.6)
−∞
2
• k-th central moment:
Z +∞
k
E[(X − E[X ]) ] = (x − E[X ])k p(x) dx .
−∞
Example 1.2 [Uniform distribution] The probability density function of the random
variable X is constant between two values a and b with b > a.
Z b b
x 1 1 2 1 b 2 − a2 a+b
E[X ] = dx = x = =
a b−a b−a 2 a 2 b−a 2
2
Recall: varx = E[X 2 ] − E[X ]2 .
F(x) p(x)
1 1/(b−a)
a 0 b x a 0 b x
Figure 1.2: Probability distribution and density functions for a uniform distribution.
Z b 2 " 3 #b
1 a+b 1 1 a+b
varx = E[(X − E[X ])2 ] = x− dx = x−
b−a a 2 b−a 3 2
a
3 2
1 1 b−a (b − a)
⇒ varx = = .
3b−a 2 12
Density function
∂ q F (x1 , · · · , xq )
p(x1 , · · · , xq ) = .
∂x1 · · · ∂xq
Moments
Define x = [x1 , · · · , xq ]T . Only the vector or first moments (that is the mean
vector) and the matrix of second central moments (that is the covariance matrix)
are considered:
• covariance: Covx = E[(X − E[X ])(X − E[X ])T ]. The component Covx (i, j) at
row i and column j of this covariance matrix verifies:
Z
Covx (i, j) = (xi − E[Xi ])(xj − E[Xj ]) dF (xi , xj ) .
R2
1 1 T −1
p(x) = √ e− 2 (x−m) ∆ (x−m) .
(2π)q/2 det∆
The gaussian random vector X with mean m and covariance ∆ can be build from
the normal gaussian vector N (that is the vector with a null mean and an unitary
covariance) in the following way:
X = m + GN
Independence
1
⇒ vary = .
6
In the same manner:
T 1 0
covx1 ,y = Gcovx1 ,x2 G , with : G =
1/2 1/2
.
1/3 1/6
=
1/6 1/6
Remark : At the cost of more tedious calculus, one can also give a full caracterization
of the random variable Y by its distribution function F (y) or density function p(y)
and then compute first and second moments:
Z
X1 + X2
F (y) = P < y = P (X1 < 2y − X2 ) = P (X1 < 2y − x2 )p(x2 )dx2 .
2 Dx2
F(y) p(y)
1 1
−1 0 1 y −1 0 1 y
In the following, random variable (resp. signal) stands for both single (q = 1)
or multivariate (q components) variable (resp. signal).
1.2.5 Stationarity
A random signal is defined to be wide sense stationary 4 if its mean is constant
(m(t) = m) and if its auto-correlation function depends only on τ (φww (t, τ ) =
φww (τ )).
The quadratic mean or variance 5 of a stationary random signal is the auto-
correlation function value at the origin:
Exercise 1.2 A random signal b(t) is generated in the following way: from the
initial time t0 = 0, the signal b(t) is hold every dt seconds on the value (sample) of
a gaussian centered random variable X with a standard deviation σ; all samples xi
of the variable X are independent to each others. So: b(t) = xi ∀t ∈ [idt, (i + 1)dt[
(see Figure 1.4).
• Compute the mean m(t) and the auto-correlation function φbb (t, τ ) of signal
b(t). Is b(t) wide sense stationary ?.
• Answer again previous questions assuming now that the initial instant is a
random variable with a uniform distribution between 0 and dt.
Solution keys:
4
One can also defined the strict sense stationary (see reference[3]).
5
or covariance matrix in the case of a vectorial random signal.
b(t)
2 σ
dt
σ
Probability
(−2σ < b(t) < 2σ)
=0.95
0
− σ
−2 σ
0 10 dt 20 dt .... t
• φbb (t, τ ) = E[b(t)b(t + τ )] = σ 2 if t and t + τ are in the same time interval dt;
0 else (because the samplings are independent and centered).
– for −dt < τ < 0, φbb (i dt + ε, τ ) = σ 2 iff −τ < ε < dt, 0 else. That is:
Z dt
σ2 σ2
φb0 b0 (t, τ ) = dε = (dt + τ ) ∀τ / − dt < τ < 0 .
dt −τ dt
−dt 0 dt
So φb0 b0 (t, τ ) = σ 2 (1− |τdt| ) ∀τ ∈ [−dt dt], 0 else (see Figure 1.6) and depends
only on τ , b0 (t) is now wide sense stationary.
6
The inverse bilateral Laplace transform inverse is defined by:
Z +j∞
1
φww (τ ) = L−1
II Φww (s) = Φww (s)esτ ds
2π j −j∞
Remark 1.2 :
Z ∞ Z ∞
−τ s
Φww (s) = φww (τ )e dτ + φww (−u)eus du
0 0
−
= Φ+
ww (s) + Φww (−s)
with :
Φ+ +
ww (s) = L[φww (τ )] and φ+
ww (τ ) = φww (τ ) if τ ≥ 0, φ+
ww (τ ) = 0 else ,
Φ− −
ww (s) = L[φww (τ )] and φ−
ww (τ ) = φww (−τ ) if τ ≥ 0, φ−
ww (τ ) = 0 else .
−
If the function φww (τ ) is pair then Φ+ ww = Φww and the spectrum in the s-plane
Φww (s) is a function of s2 (that is, the PSD is real).
The initial value theorem of the mono-lateral Laplace transform allows the
variance to be computed directly from the spectrum in the s-plane:
The variance of the noise w is (with a factor 1/2π) the integral of the PSD in the
frequency domain.
Example 1.4 Let us consider again the signal b0 (t) of section 1.2 whose autocor-
relation function is: φb0 b0 (t, τ ) = σ 2 (1 − |τdt| ) (pair function). The spectrum in the
s-plane of this signal is:
Z ∞
|τ | −τ s
Φb0 b0 (s) = σ 2 (1 − )e dτ = Φ+ +
b0 b0 (s) + Φb0 b0 (−s) with:
−∞ dt
Z dt
τ
Φ+
b0 b0 (s) = σ 2 (1 − )e−τ s dτ
0 dt
( dt Z dt )
σ2 dt − τ −τ s 1
= e − e−τ s dτ (integration by parts)
dt −s 0 s 0
( −τ s dt )
σ2 dt e σ2 −s dt
= + = s dt + e − 1 .
dt s s2 0 dt s2
σ2
Φb0 b0 (s) = dt s2
e−s dt + es dt − 2 .
Thus the PSD is:
σ2 −jω dt jω dt
2σ 2 4σ 2 ω dt
Φb0 b0 (ω) = − 2
e + e − 2 = 2
(1 − cos(ω dt)) = 2
sin2 ( )
dt ω dt ω dt ω 2
sin2 ( ω2dt )
Φb0 b0 (ω) = σ 2 dt ( ω2dt )2
.
Φb0 b0 (ω) is plotted in Figure 1.7 7 . From a practical point of view, the logarithmic
scale representation given in Figure 1.8 is preferred and highlights that the PSD
can be considered as constant for pulsations very lower than the sampling pulsation
2π/dt. This signal can be used as a white noise signal with a PSD R in a
given pulsation range if 2 π/dt is chosen very large w.r.t. this pulsation
range and if σ 2 = R/dt (see next section).
Φ (ω)
b’b’
2
σ dt
Φb’b’(ω) |dB
20 log10(σ2 dt)
0 2π/dt ω
−∞ 4π/dt
Matrices W (t) and V (t) become constant in the case of stationary gaussian white
noises. The normalized gaussian white noise is such that W (t) = Iq×q (q components
in the noise).
allows to assert that the state x and the output y are also gaussian multivariate
signals and are therefore entirely characterized by first and second moments. The
following theorem will allow these characteristics to be computed. It is assumed
that the deterministic input u is null (u(t) = 0).
• mean vector:
m(t) = E[x(t)] = eA(t−t0 ) m(t0 )
Remark 1.5 If the output equation is considered without noise i.e. y(t) = Cx(t)
then the covariance matrix Py (t) of y(t) is:
Py (t) = CP (t)C T
(if the white noise on the measurement is considered i.e. y(t) = Cx(t) + v(t), then
the covariance of y is infinite).
Theorem 1.2 (Transmission of a white noise in the linear system) Let us con-
sider a stable linear continuous-time system defined by the transfer matrix G(s)p×q
between the input w and the output y. The steady state response y to a station-
ary random signal w, characterized by a spectrum Φww (s)q×q in the s-plane, is a
stationary random signal characterized by a spectrum Φyy (s) in the s-plane such
that:
Φyy (s)p×p = G(−s)Φww (s)GT (s) .
G(s)
Exercise 1.3 Let us consider again example 1.1 on the 1/4 vehicle model. Some
experiments in a real environment have shown the PSD of the rolling noise for a
vehicle at 90 km/h on a secondary road could be approximated by:
ω 2 + 104
Φww (ω) =
ω 4 − 1.75 104 ω 2 + 108
8
That is: a spectrum with only exponents of s2 ; that is: the correspondent auto-correlation
function φww (τ ) is pair.
Solution :
−s2 +104
a) By definition: Φww (s) = s4 +1.75 104 s2 +108
.
All the stable poles of Φww (s) must be gathered in G(s), that allows the de-
nominator of G(s) to be determined but there is no condition on the stability
of G(s) zeros. So there are 2 different filters that provide an output signal with
the spectrum Φww (s) when the input is a normalized white noise:
102 − s 102 + s
G(s) = and G0 (s) =
s2 + 50s + 104 s2 + 50s + 104
If G(s) is chosen then a Markov representation of w(t) is (an horizontal
companion form is proposed, for instance):
˙ 0 1 0
xG (t) = 4 xG (t) + b(t)
−10 −50 1 . (1.15)
2
w(t) = [10 − 1]xG (t)
– from the spectrum in the s-plane and the initial value theorem (see remark
1.2):
– from the Markov model and the theorem 1.1: during steady-state, the
covariance matrix P of the state vector xG is the solution of the Lya-
punov equation:
0 1 0 −104 0 0 0 0
P +P + =
−104 −50 1 −50 0 1 0 0
10−6 0
⇒ P =
0 10−2
−6 2
2 2 10 0 10
⇒ σw = [10 − 1] −2 = 1/50 .
0 10 −1
%=============================================================
clear all close all
% Filter definition:
G=tf([-1 100],[1 50 10000])
% State space realization:
[A,B,C,D]=ssdata(G);
% Variance determination using Lyapunov equation:
P=lyap(A,B*B’); var_w=C*P*C’
% ==> on can find the wanted result: variance=1/50.
% (standard deviation: sigma=0.14)
% Choice of a sampling period fast enough w.r.t. the filter dynamics (100 rd/s):
dt=0.0001;
% Simulation:
9
Feel free to send an e-mail to [email protected] with ”Introduction Kalman” for the subject
if you want a copy of the 2 files bruit.m and simule bruit.mdl.
sim(’simule_bruit’);
% Results plotting:
plot(b.time,b.signals.values,’k’);
% Numerical computation of the variance of b(t):
var(b.signals.values) % One can find again: var_b=1/dt=10000.
figure
plot(w.time,w.signals.values,’k’)
% Numerical computation of variance of w(t):
var(w.signals.values) % One can find: var_w=1/50 (approximately).
To Workspace1
Figure 1.10: SIMULINK file simule bruit.mdl for the rolling noise simulation.
1.5 Conclusion
Mathematical tools used to analyse continuous-time random signals and their trans-
mission in linear systems were presented in this chapter. The notion of gaussian
white noise was also introduced (this assumption is required in the Kalman filter
developed in next chapter). The interest of such gaussian (centered) noises lies in
the fact that they are entirely characterized by their autocorrelation function (or
their spectrums in the s-plane if they are stationary).
The reader will find in appendix B some additional developments for the anal-
ysis of discrete-time random signals.
400
300
200
100
b(t)
−100
−200
−300
−400
0 0.5 1 1.5 2
Temps (s)
0.4
2σ =0.28
w
0.3
0.2
0.1
0
w(t)
−0.1
−0.2
−0.3
−0.4
−0.5
0 0.5 1 1.5 2
Temps (s)
Figure 1.12: Responses of rolling noise w(t) (with dt = 0.0001 s: black; with dt =
0.001 s: grey).
Chapter 2
Kalman filter
Example 2.1 Let us consider the motion of an aerospace vehicle (with a mass m)
along a single translation degree-of-freedom. Its position along this degree-of-freedom
is named p(t). To control the postion p(t), the spacecraft is fitted with an actuator
(thruster). This actuator can be considered itself as a dynamic sub-system with
a transfer function A(s) between its input uc (t) (in fact the output of the control
algorithm) and the output F (t): the real thrust applied to the spacecraft (N ). The
spacecraft is instrumented with:
d2 p(t)
• a linear accelorometer which measures the acceleration p̈(t) = dt2
with a
noise w(t), that is: p̈m (t) = p̈(t) + w(t),
• a position sensor (star tracker) which measures the position p(t) with a noise
v(t), that is: pm (t) = p(t) + v(t).
The control algorithm (out of the scope of this document) requires estimates of the
position pb(t) and the velocity vb(t) = ḃp(t) of the spacecraft in order to compute the
control signal uc (t) to be sent to the actuator. Note that the velocity is not measured.
To solve such a problem, one can use a Kalman filter involving the accelerometer
as primary measurement u(t) = p̈m (t) (seen as the input of the Kalman model)
and the position sensor as secondary mesurement y(t) = pm (t) (seen as the output
of the Kalman model). Obviously, the Kalman model between u (acceleration)
and y (position) is a double integration and reads:
ṗ(t) = v(t)
v̇(t) = p̈(t) = u(t) − v(t)
y(t) = p(t) + v(t)
or using the general state-space representation (2.1) with x(t) = [p(t) v(t)]T :
˙ 0 1 0 0
x(t) = x(t) + u(t) + w(t)
0 0 1 −1 . (2.2)
y(t) = 1 0 x(t) + 0u(t) + v(t)
The block-diagram sketch of the whole system is represented in Figure 2.1. Of course
such a Kalman filter cannot be used to estimate the internal state of the actua-
tor sub-system A(s) as the model of the actuator is not taken into account in the
Kalman model.
2.1.2 Assumptions:
The Kalman model (2.1) must meet the following assumptions:
H1: the (A, C) pair is detectable, i.e. there is no unstable and unobservable eigen-
value in the model,
H2: signals w(t) and v(t) are centered gaussian white noise with Power Spec-
tral Densities (PSD) W and V respectively, that is:
uc F p̈ ṗ p
Control Actuator: A(s) 1 1
algoritm 1/m s s
w + p̈m = u
pb
+ (primary)
Kalman
v + pm = y filter vb
+ (secondary)
Plant model
Figure 2.1: Block-diagram sketch for the control of an aerospace vehicle using a
Kalman filter to hybridize primary (acceleration) and secondary (posi-
tion) measurements.
Remarks:
• Although the whole theory of Kalman filter can be applied to the non-
stationary case (for the plant and the noises), we will assume in the following
that the plant and noises are stationary, i.e: matrices A, B, M , C, D, W are
V independent of time t.
The new state noise w̄(t) = w(t) − E[w(t)] is now centered. If the bias E[w(t)]
is unknown then it can be modelled as an initial condition on a new integral
state and the Kalman filter will allow this bias to be estimated (see example
in section 2.3).
• If noises are colored and characterized by spectrums in the s-plane then re-
sults of section 1.3.2 will allow the ”color” (or frequency response) of the noises
to be taken into account with a Kalman model augmented by the Markov
representation of these noises. For instance: if the spectrum in the s-plane
Φww (s) of the centered random signal w(t) in (2.1) is known, then the decom-
position Φww (s) = G(−s)GT (s) will allow a Markov representation of w(t)
to be determined, that is: a state space realization of G(s) (see example 1.3):
xG˙(t) = AG xG (t) + BG b(t)
w(t) = CG xG (t)
In fact all the deterministic information it is possible to know in the system must
be gather in the model (i.e. ẋ = Ax + Bu, y = Cx + Du and matrix M ). All
the random information must be gather in noises w(t) and v(t). The state noise
wx = M w represents all the external perturbations (wind in the case of an aircraft,
road unevenness in the case of a car, ...) and/or also modelling errors or uncertain-
ties (difference, due to linearization, between the tangent model and the non-linear
model, neglected dynamics, ...): wx is an upper bound of all what makes that the
state does not evolve exactly as the deterministic model predicts (ẋ = Ax + Bu).
Let:
u(t)
ḃ
x(t) b(t) + [Bf Kf ]
= Af x (2.3)
y(t)
b(t) + Bf u(t) + Kf y(t)
= Af x (2.4)
be the state space realization of this filter. Of course this filter must be initialized
with x b(t0 ): the estimate of the state of the plant at the initial time t0 .
Let us denote ε(t) = x(t) − x b(t) the state estimation error and ε(t0 ) = x(t0 ) −
b(t0 ) the initialization error.
x
Substituting the equation (2.4) to the state equation of (2.1) and using the
measurement equation, we can write:
b − Bf u − Kf (Cx + Du + v)
ε̇ = Ax + Bu + M w − Af x
b + (B − Bf − Kf D)u + M w − Kf v
= (A − Kf C)x − Af x
= (A − Kf C)ε + (A − Kf C − Af )b
x + (B − Kf D − Bf )u + M w − Kf v . (2.5)
As noises w and v are gaussian and as the system is linear, on can state that
ε(t) is a gaussian random signal. We are going to characterize the expected value
of ε(t).
Unbiased estimator: first of all, we wish the estimator to be unbiased, that is:
• whatever the input profile u(τ ) applied in the time interval τ ∈ [t0 , t],
b(t0 ),
• whatever the initialization x
we wish the estimation error expected value tends towards 0 when t tends towards
infinity.
As noises w and v are centered, we can write:
˙
E[ε(t)] = E[ε̇(t)] = (A−Kf C)E[ε(t)]+(A−Kf C −Af )E[b
x(t)]+(B −Kf D−Bf )u(t)
Af = A − Kf C, B f = B − Kf D (2.6)
ḃ = (Ab
x x + Bu) + Kf (y − Cb
x − Du) . (2.8)
We recognize, in the first term of the right-hand member of equation (2.8), the deter-
ministic model of the plant (Ab x + Bu). This model is used to predict the evolution
of the plant state from the current estimation xb. This prediction is in fact an in-line
simulation of the plant model. This model being wrong, the prediction is corrected
(updated) with the difference between the measurement y and the prediction of the
measurement yb = Cb x + Du through the filter gain Kf . This difference y − yb is
also called the innovation. The block-diagram of such a filter (in the case D = 0)
is depicted in Figure 2.2. This structure ensures that the estimator is unbiased
whatever the matrices A, B, C, D of the plant and the gain Kf such that A − Kf C
is stable (that justifies the assumption H1: the existence of an unstable and unob-
servable mode in the plant does not allow a stabilizing gain Kf to be determined
and so an unbiased estimator to be built).
u y + −
Plant
Kf
B x^
Z
C
+
+
+
A
Kalman filter
^
x
b(t))(x(t) − x
P (t) = E[(x(t) − x b(t))T ] is the estimation error covariance matrix.
Returning (2.6) in (2.5), the evolution of ε(t) is governed by the following state
space equation:
w(t)
ε̇(t) = (A − Kf C)ε(t) + [M − K] , (2.12)
v(t)
with:
w(t) T T Wq×q 0q×p
E [w (t + τ ) v (t + τ )] = δ(τ ) .
v(t) 0p×q Vp×p
The theorem 1.1 (page 21) can be applied and allows to conclude that the estimation
error covariance P (t) is governed by the differential equation:
T W 0 MT
Ṗ (t) = (A − Kf C)P (t) + P (t)(A − Kf C) + [M − Kf ]
0 V −KfT
= (A − Kf C)P (t) + P (t)(A − Kf C)T + M W M T + Kf V KfT . (2.13)
b(t0 ))T ] .
b(t0 ))(x(t0 ) − x
P (t0 ) = E[(x(t0 ) − x
2
Indead: starting from the prescribed initial condition P (t0 ), to minimize trace P (t) ∀ t > t0 ,
it is sufficient to minimize its time-domain derivative at each time t > t0 .
Ṗ (t) = 0 .
P is a constant positive definite matrix (the covariance of the state estimation er-
ror in steady state) and is the only positive solution of the algebraic Riccati
equation:
AP + P AT − P C T V −1 CP + M W M T = 0 (2.16)
The filter gain Kf = P C T V −1 becomes also stationary.
On can easily verify (from the steady state of Lyapunov equation (2.13)) that
the positiveness of P implies the stability of the filter, that is all the eigenvalues of
A − Kf C have a negative real part.
The reader will find in [1], a general method to compute the positive solution
of an algebraic Riccati equation. From a practical point of view, keep in mind
that such solvers are available in most softwares for computer-based control design:
macro-function lqe of Matlab or Scilab control packages. This function provides
directly P and Kf from matrices A, B, M , C, D, W and V (see also macro-function
care and kalman under Matlab).
In steady state, if the system and the noises are stationary, the Kalman gain Kf
is constant and its value depends only on W and V .
Furthermore, the gain Kf and the estimation error response ε(t) depend only on
the relative weight of P (t0 ) w.r.t. V (during the transient response) and the relative
weight of W w.r.t. V (during the steady state). Indeed, it can be easy to check that
the gain Kf (and so the filter) does not change if these 3 tuning matrices W , V and
P (t0 ) are multiplied by a scaling factor α. But keep in mind that the estimation
error covariance P will be multiplied by α. So the designer must be sure that these
3 matrices are realistic if he wants to use P (t) to analyse the estimation quality and
conclude for instance:
p the probability forpthe i-th component of the state xi (t) to be
between x bi (t) − 2 P(i,i) (t) and x
bi (t) + 2 P(i,i) (t) is equal to 0.95 (gaussian variable
property). From a practical point of point, the Kalman filter must be validate on
a validation model which takes into account realistic measurement noises and a
modelling, as accurate as possible, of all the disturbances (non-linearities, external
perturbations,...) that have been overvalued by a state noise w(t) in the Kalman
model also called synthesis model. Such a validation model cannot be used as
synthesis model for 2 reasons: fisrtly, it does not generally fulfill the assumptions
H1, H2, H3 and secondly it will lead to a too much complex Kalman filter from
the implementation point of view.
Whatever the tuning, one can check that the Kalman estimation error covari-
ance P (t) is always lower than the covariance propagated in the state equation (that
is without using measurement and governed by Ṗ = AP + P AT + M W M T ) and the
estimation error covariance of the output yp (without noise: yp = Cx + Du), that is:
CP (t)C T , is always lower that the measurement noise covariance (which is infinite
in the continuous-time case !! but this property is also true in the discrete-time case
where the measurement noise covariance is finite, see section 2.4). So it is always
better to use ybp = Cb x + Du rather than the direct measurement y to estimate the
real output yp of the plant.
The designer could use the following trade-off to tune qualitatively the estima-
tion error time-response.
Influence of P (t0 ) ./. V : during the transient response, the initial estimation
error ε0 will be corrected as faster (and the Kalman gain will be as greater) as
P (t0 ) is greater w.r.t. V . But the estimate will be affected by the measurement
noise because we have a good confidence in this measurement.
Influence of W ./. V : during the steady state, the gain Kf will be as lower,
and the estimation response will be as smoother, as W is weaker w.r.t. V (we have
confidence in the model). But, if the plant is subject to a perturbation which has
been under-estimated by this too weak choice of W , the estimate x b will not track
the real state x or will have a too long updating time. Furthermore the estimation
error covariance P will not be representative of this phenomena.
These behaviors will be illustrated in the following example. Although this is a
first order example, it allows to highlight general trade-offs that can be encountered
on more complex applications (continuous or discrete-time).
w v
u + 1 x + y
+ s−a +
Questions:
a) Set up the Kalman model for this problem and compute the Kalman filter,
to estimate the state (or output) x of the plant. Give results as a function
of a, W and P0 (the initial estimation error variance). Study the asymptotic
behavior of the filter gain Kf when W tends towards 0.
b0 = 0,
• a = −1, x0 = 20, x
• T = 0, 0.1, 0.5, 1(s) (these various values will be tested),
• u(t): square signal with a magnitude of 10 and a period of 5 s,
• dt = 0.01 s (integration step for the simulator).
• looking for a constant particular solution (because the coefficients of the dif-
ferential equation do not depend on time t). This solution corresponds in fact
to steady-state solution p = p∞ which must solve:
2
P∞ − 2aP∞ − W = 0 .
√
The only positive solution is P∞ = a + a2 + W ,
The integration of this differential equation using the varying constant method
gives: h √ i
1 √
2 2
z(t) = √ e2 a +W (t−t0 ) − 1 + e2 a +W (t−t0 ) z0
2
2 a +W
where t0 is the initial time and z0 = z(t0 ) is the initial condition on z(t) :
1
z0 =
P0 − P ∞
√
where P0 is the initial condition on P (t). Let us denote k = 2 a2 + W , then:
k(P0 − P∞ )
P (t) = P∞ +
(P0 − P∞ )(ek(t−t0 ) − 1) + kek(t−t0 )
k(P0 − P∞ )
= P∞ + k(t−t0 ) . (2.18)
e (P0 − P∞ + k) + P∞ − P0
Lastly: Kf (t) = P (t).
⇒ lim Kf = 0 .
t→∞
Once the initial error is corrected, only the model is used to estimate x. This is
logic because the model is assumed to be perfect when W is set to 0. That property
holds only if the system is stable. Indeed, if the system is unstable (a > 0) then:
⇒ lim Kf = 2a .
t→∞
This is the value of the gain which allows the filter dynamics to be stable while
minimizing the effect of the measurement noise on the estimation error variance.
Question b): the Matlab/Simulink simulator is depicted in Figure 2.4. The
various parameters used in this Simulink file (simuKalman.mdl) are written in this
Figure. The Matlab function Kf_t.m used to implement the non-stationary gain
Kf is given in appendix D with the script-file demoKalman.m to declare the various
parameters and to plot results 3 . Different simulation results are presented in Figures
2.5 to 2.9 :
• Figure 2.7: If now a 1 s delay is taken into account in the validation model,
the (good) confidence we have in the synthesis model (W = 1) does not allow
to be representative of real model errors. The filter is confident in a wrong
model and does not use the measurement enough: this filter has a very long
reaction time.
• Figure 2.8: If now it is specified that the model is not so good (W = 100), the
filter is more confident with measurements: the estimate is more sensitive to
the measurement noise but have a good reaction time.
• Figure 2.9 (answer to question c)): if now the delay is known (T = 1 s), it
is possible to take this delay into account in the Kalman synthesis model in
two different ways: firstly in propagating this delay in the prediction equation
or secondly (what it is proposed here: see file demoKalman.m) in using a Pade
filter in series with the model.
v
SIMULATION PARAMETERS
Band−Limited Start time: 0s Stop time: 10s
White Noise: Solver: ode4 (Runge Kutta)
Noise power=V; Fixed step=dt
Sample Time=dt; y
u x
x’ = Ax+Bu
y = Cx+Du
Signal Transport Delay 1/(s+1)
Generator Time Delay=T A=−1;B=1;C=1;
D=0;X0
Scope
Clock
output
MATLAB To Workspace
Kf_t Function StructureWithTime
1
b c
s
Integrator
Initial Condition=0
Figure 2.4: SIMULINK file simuKalman.mdl for the Kalman filter simulation.
50
y(t)
x(t)
u(t)
40
xhat(t)
xhat(t)+2 σ
xhat(t)−2 σ
30
20
10
−10
−20
−30
−40
0 1 2 3 4 5 6 7 8 9 10
50
y(t)
x(t)
u(t)
40
xhat(t)
xhat(t)+2 σ
xhat(t)−2 σ
30
20
10
−10
−20
−30
−40
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
50
y(t)
x(t)
40 u(t)
xhat(t)
xhat(t)+2 σ
30 xhat(t)−2 σ
20
10
−10
−20
−30
−40
−50
0 1 2 3 4 5 6 7 8 9 10
50
y(t)
x(t)
40 u(t)
xhat(t)
xhat(t)+2 σ
30 xhat(t)−2 σ
20
10
−10
−20
−30
−40
−50
0 1 2 3 4 5 6 7 8 9 10
40
y(t)
x(t)
u(t)
30
xhat(t)
xhat(t)+2 σ
xhat(t)−2 σ
20
10
−10
−20
−30
−40
−50
0 1 2 3 4 5 6 7 8 9 10
Figure 2.9: Simulation of the stationary Kalman filter designed on a model taking
into account a second order Pade approximation of the delay (W = 1,
T = 1 s).
1) Find the state space equations of the Kalman model with x and b as state
variables, vm as input variable, xm as output variable.
In fact the bias b(t) can derive with time. To take into account these variations, it
is assumed that the time-derivative of the bias is perturbed by a white noise w(t)
with a PSD W = q 2 (independent of v(t)):
˙ = w(t) .
b(t)
3) Give the new Kalman model equation, compute the steady state Kalman
gain (as a function of q) and give the state space representation of the filter
b and bb to be computed from xm and vm .
allowing estimates x
ḃ
4) How would you proceed to estimate the velocity of the vehicle x?
ḃ
6) Comment this transfer (particularly X(s)/V m (s)) as a function of q. Demon-
strate that this filter F (s) provides perfect estimates if the measurements are
perfect.
The bias is modelled as an integral effect (ḃ = 0) with an unknown initial condition
b0 . Thus we have:
ẋ 0 −1 x 1
= + vm
ḃ 0 0 b 0
(2.19)
x
xm = [ 1 0 ] +v
b
b0
s v
− x
. x +
vm xm
+ +
Question 3. If the Kalman filter is designed on model 2.19, we will find a null gain
in steady state because the state equation is not perturbed by a noise: once the bias
is estimated, the filter will not be able to detect an eventual drift of this parameter.
To overcome this problem and to take into account the bias is not constant, a noise
w is introduced on ḃ(t):
ẋ 0 −1 x 1 0
= + vm + w
ḃ 0 0 b
0 1
(2.20)
x
xm = [ 1 0 ] +v
b
with E[w(t)wT (t + τ )] = q 2 δ(τ ). This model is in the form of (2.1) and matrices A,
B, C, M can be identified.
T −1 p1 p12
The steady state solution reads: Kf = P C V with P = positive
p12 p2
solution of the algebraic Riccati equation:
0 −1 p1 p12 p1 p12 0 0 p1 p12 1 0 p1 p12 0 0 0 0
+ − + =
0 0 p12 p2 p12 p2 −1 0 p12 p2 0 0 p12 p2 0 q2 0 0
So:
2p12 + p21 = 0
p + p1 p12 = 0 .
2
p212 = q 2
" # √ √
ḃ
x − 2q −1 b
x 2q 1 xm
or: = bb + −q 0 .
ḃb q 0 vm
That is: √
" # 2qs + q s
√
b
X(s) qs s2 + 2qs Xm (s)
ḃ = √ . (2.21)
X(s) s2 + 2qs + q Vm (s)
Question 6.
ḃ √
X s2 + 2qs
(s) = 2 √ .
Vm s + 2qs + q
√
This is a second√ order high pass filter with a cutting pulsation q (rd/s) and a
damping ratio 2/2 (∀q). The steady state (D. C.) gain of this filter is null (the
null frequency component, that is the bias, is filtered). The cutting frequency is as
greater as q is great, that is, as the bias may derive a lot.
If measures are perfect then xm = x and vm = ẋ. So we have:
• Xm (s) = X(s) and Vm (s) = sX(s). Reporting this result in the first row of
(2.21), we get:
√
b ( 2qs + q)X(s) + s2 X(s) b
X(s) = √ ⇒ X(s) = X(s) .
s2 + 2qs + q
• Vm (s) = Ẋ(s) and Xm (s) = 1/s Ẋ(s). Reporting that in second row of (2.21),
we get:
√
ḃ Ẋ(s) + (s2 + 2qs)Ẋ(s) ḃ
X(s) = √ ⇒ X(s) = Ẋ(s) .
s2 + 2qs + q
This filter does not induce any degradation of the quality of the measurement. Such
a filter is called a complementary filter.
2
Remark 2.1 : While in the continuous-time case, white noises in the Kalman
model are defined by their PSD W and V (variances are infinite), discrete-time
Kalman model noises are defined by their covariance matrices Wd and Vd (variances
are finite). PSD of these discrete-time noises are constant (and equal to Wd and Vd )
but on limited range of the reduced pulsation θ ∈ [−π π] (see appendix B.1); this
is why this noise is said to be pseudo-white.
w(t)
v(t)
dt dt
u(k) u(t) y(t) y(k)
Bo(s) PLANT
Figure 2.11: Continuous-time plant with zero-order holds on the inputs and sampled
outputs.
u(k) u(t)
Remark 2.2 : like in the continuous-time case, if the noise wd is a colored noise
and characterized by a spectrum in the z-plane Φww (z) the factorization Φww (z) =
G(z −1 )GT (z) (see section B.3.2) allows a Markov representation of wd to be de-
rived and to be taken into account in an augmented Kalman model.
The measurement equation is a static equation and then, at each sampling time, we
Φvv(ω)
−π/dt 0 π/dt ω
can write:
y(k) = Cx(k) + Du(k) + v(k)
The discrete time state equation is expressed under the form of (2.22) with:
R dt
Ad = eAdt , Bd = 0
eAv Bdv, Md = In , Cd = C, Dd = D (2.23)
As the sampling of a signal does not change its variance, the discrete-time measure-
V
ment equation is: y(k) = Cx(k) + Du(k) + vd (k) with E[vd (k) vd (k + l)T ] = dt δ(l).
Now we have to compute the covariance matrix Wd of the state noise wd (k):
Z dt Z dt
T Av T T AT τ
Wd = E[wd (k)wd (k)] = E e M w((k + 1)dt − v)dv w ((k + 1)dt − τ )M e dτ
0 0
Z Z dt
T
= eAv M E[w((k + 1)dt − v)wT ((k + 1)dt − τ )]M T eA τ dvdτ
0
Z Z dt
T
= eAv M W δ(τ − v)M T eA τ dvdτ
0
R dt T
Wd = 0
eAv M W M T eA v dv . (2.25)
Remark 2.3 : We mention in appendix B (see remark B.2, equation (B.13)) how
Wd can be computed; but we can also use the approximation Wd ≈ dtM W M T if dt is
small with respect to the settling time of the plant. Lastly, we have to keep in mind
that all these formulaes are embedded in Matlab macro-functions lqed or kalmd.
These functions allow a discrete-time Kalman filter to be designed directly from
the continuous-time data (A, B, C, D, M , W and V ) and a sampling period dt (see
the Matlab illustration in appendix D.3). Such an approach (and such functions)
cannot bear correlation between measurement and state noises.
• the predicted state at time k + 1, denoted x b(k + 1|k), knowing all the mea-
sures until time k; which is associated with the prediction error covariance
matrix denoted:
h T i
P (k + 1|k) = E x(k + 1) − x b(k + 1|k) x(k + 1) − x b(k + 1|k) .
• the estimated (or updated) state, denoted x b(k + 1|k + 1), knowing all the
measures until time k + 1 (just after the correction or the updating); which is
associated with the estimation error covariance matrix, denoted:
h T i
P (k + 1|k + 1) = E b(k + 1|k + 1) x(k + 1) − x
x(k + 1) − x b(k + 1|k + 1) .
b(k + 1|k) = Ad x
x b(k|k) + Bd u(k) . (2.26)
At time k, the estimation error was characterized by P (k|k). The prediction model
being wrong, the error can only increase and this error at time k + 1 will be char-
acterized by (see theorem B.1 in appendix) :
T
P (k + 1|k + 1) = In − Kf (k + 1)Cd P (k + 1|k) In − Kf (k + 1)Cd + Kf (k + 1)Vd KfT (k + 1)
= P (k + 1|k) − Kf (k + 1)Cd P (k + 1|k) − P (k + 1|k)CdT KfT (k + 1) · · ·
· · · + Kf (k + 1) Cd P (k + 1|k)CdT + Vd KfT (k + 1) . (2.29)
The set of equations (2.26), (2.27), (2.28), (2.30) and (2.31) constitutes the discrete-
time Kalman filter. Equations (2.26) and (2.28) are initialized with x b(0|0), the
initial estimate and equations (2.27), (2.30) and (2.31) are initialized with P (0|0),
the confidence we have in the initialization.
If the model and noises are stationary, equations (2.27), (2.30) and (2.31) can
be integrated off-line. Removing (2.30) and (2.31) in (2.27), one can find a recurrent
Riccati equation in the prediction error covariance:
−1
T T T
P (k+1|k) = Ad P (k|k−1)Ad −Ad P (k|k−1)Cd Cd P (k|k−1)Cd +Vd Cd P (k|k−1)ATd +Md Wd MdT .
(2.32)
Lastly, in steady state, Kf (k + 1) = Kf (k) = Kf , but on can distinguish:
Then, the state space realization of the stationary (steady state) Kalman filter is
(with (2.28) in (2.26)):
y(k)
b(k + 1|k) = Ad (I − Kf Cd )b
x x(k|k − 1) +[Ad Kf Bd − Ad Kf Dd ]
u(k)
y(k)
b(k|k) = (I − Kf Cd )b
x x(k|k − 1) +[Kf − Kf Dd ]
u(k)
(2.34)
The state of this realization is the predicted state, the output is the estimated state.
Remark 2.4 :
The second term of the right-hand member is always positive, thus: Pe < Pp ,
that is the estimation error variance is always lower than the prediction error
variance (or the state noise variance propagated in the state equation).
Cd Pe CdT = Cd Pp CdT −Cd Pp CdT (Cd Pp CdT +Vd )−1 Cd Pp CdT = Cd Pp CdT (Cd Pp CdT +Vd )−1 Vd .
That is:
Cd Pe CdT −Vd = Cd Pp CdT (Cd Pp CdT +Vd )−1 −I Vd = −Vd (Cd Pp CdT +Vd )−1 Vd < 0 .
So: Cd Pe CdT < Vd i.e. ybp (k) is a better estimate of yp that the direct measure-
ment.
• The way to solve Discrete-time Algebraic Riccati Equations (DARE) are not
detailed here. Macro-functions (lqe in Scilab or dlqe, dare, kalman in Mat-
lab) allow such equations to be solved, that is: to compute Kf and to provide
the state space realization (2.34) of the filter in steady state. The way to use
these function is illustrate in the Matlab session presented in appendix D.3.
Exercise 2.1 Demonstrate that in the case of a continuous-time plant with discrete-
time sampled output (with a sampling period dt),
• and discrete-time Kalman filter designed from equations (2.23), (2.24) and
(2.25)
tend towards the same solution as dt tends towards 0 (first order calculus in dt).
Solution : The continuous-time Kalman filter is defined by equations (2.8), (2.14)
and (2.15). The Euler method consists in removing x(t) ˙ and x(t) by x(k+1)−x(k)
dt
and x(k), respectively. Removing (2.14) in (2.8), this first discrete-time filter is
defined by:
P (k + 1) = P (k) + dt AP (k) + P (k)AT − P (k)C T V −1 C(k)P + M W M T
x b(k + 1) = x b(k) + dt Abx(k) + Bu(k) + P (k)C T V −1 y(k) − Cb x(k) − Du(k)
(2.35)
The prediction error covariance of the discrete-time Kalman filter is given by equa-
tion (2.32), let us denote P (k|k − 1) = P (k), then using equations (2.23), (2.24)
and (2.25) and with the first order approximation Wd ≈ dtM W M T , we get:
T dt
−1 T
P (k+1) ≈ eAdt P (k)eA −eAdt P (k)C T CP (k)C T +V /dt CP (k)eA dt +dtM W M T .
So one can recognize (at the first order) the first equation of (2.35). The gain Kf
becomes:
−1
Kf (k) = P (k)C T CP (k)C T V −1 dt + I V −1 dt ≈ P (k)C T V −1 dt,
and the state equation of the filter (equation (2.34)) becomes (we denote x b(k|k −1) =
b(k) and we use also the first order approximation: Bd ≈ dtB):
x
b(k + 1) = eAdt I − dtP (k)C T V −1 C x
x b(k) + dteAdt P (k)C T V −1 y(k)
+ Bd − dt eAdt P (k)C T V −1 D u(k)
T −1
≈ (I − Adt) I − dtP (k)C V C x b(k) + dt(I − Adt)P (k)C T V −1 y(k)
+ Bd − dt(I − Adt)P (k)C T V −1 D u(k)
T −1
≈ x b(k) + dt Ab x(k) + Bu(k) + P (k)C V y(k) − Cbx(k) − Du(k) + dt2 (· · ·) .
In the first order approximation, we recognize the second equation (2.35). So both
discrete-time filters are equivalent when dt tends toward 0.
2.4.4 Example
Let us consider again exercise 2.3.2 on the bias estimation. We wish to implement
the filter on a numerical computer, both measurements vm and xm being sampled
with the sampling period dt.
• Provide state-space equations of the discrete-time Kalman model with [x(k), b(k)]T
as state vector.
• Compute using Matlab the gain Kf and the estimation error covariance in
steady state.
Numerical application: dt = 0.01 s, V = 1, q = 1.
The reader will find in appendix D.3 the Matlab file demoKalmand allowing the
various model parameters to be computed and also the gain Kf and the steady state
estimation error covariance (from function dlqe). We also show how the function
lqed can be used directly from the data of the continuous-time model.
2.5 Exercises
2.5.1 Second order system:
A mobile with a mass m is moving along Ox axis due to a force (command) u(t).
A perturbation force w(t) acts also on this mass. w(t) is modelled as a gaussian
centered white noise with a PSD W . The position x(t) of this mass is measured.
We denote xm (t) this measurement which is perturbed by a gaussian centered white
noise v(t) with a PSD V .
A.N.: m = 1 (Kg); W = 1 (N 2 /Hz); V = ρ2 (m2 /Hz) .
From the measurement xm and the command u, we wish to design a Kalman
˙ of the mobile to be estimated.
filter allowing the position x(t) and the velocity x(t)
These estimates are denoted x ḃ
b(t) and x(t).
Chapter 3
In the case of a discrete-time random signal, the reader will consult Table 3.2
(see also appendix B.1).
One can remark a factor time (s) between the continuous-time PSD (or spec-
trum in the s-plane) and the discrete-time PSD (or spectrum in the z-plane). Equa-
tion (2.24), used for the discretization of the measurement equation of a continuous-
time system is homogeneous with that. To characterize a continuous-time noise on a
signal expressed√in u, some authors specify the square root of the PSD which is thus
expressed in u/ Hz (and which is wrongly considered as a standard deviation).
If we consider now the continuous-time Kalman model (2.1) and if u stands
for the physical unit of the state variable x(t), then the physical unit of the state
noise wx (t) = M w(t) is u/s and its DSP Wx = M W M T is expressed in u2 /s.
In the case of discrete-time Kalman filter (2.22), if u stands for the physical
unit of x(k) (and also of x(k + 1)) then Md wd (k) is also expressed in u and the PSD
of the state noise Md Wd MdT is expressed in u2 . Equation (2.25) used to discretize
a continuous-time state noise or the approximation Wd = W dt respect also this
homogeneity.
References
Appendix A
Proof: the general solution of the state equation ”without second member”
ẋ(t) − Ax(t) = 0 is:
x(t) = eAt K .
A particular solution can be derived using the ”varying constant method” (K →
K(t)) :
AeAt K(t) + eAt K̇(t) = AeAt K(t) + Bu(t) (A.4)
K̇(t) = e−At Bu(t) (A.5)
Z t
K(t) = e−Aτ Bu(τ ) dτ + K0 (A.6)
t0
Z t
At
⇒ x(t) = e K0 + eA(t−τ ) Bu(τ ) dτ . (A.7)
t0
Taking into account the initial condition at t = t0 yields to K0 = e−At0 x(t0 ) and
allows to find a unique solution (A.2). Lastly, the output equation is static : y(t) =
Cx(t) + Du(t).
2
Such a solution involves the exponential matrix eA(t−t0 ) ([7]) which can be computed
using a eigen-value/eigen-vector decomposition of matrix A if A is diagonalizable
(see example in section C.2 or using a power series expansion (see example below).
Remark A.1 Using (A.2) with t0 =0, x0 = 0 and u(t) = δ(t) ( Dirac function),
the impulse response of the system defined by (A, B, C, D) is:
f (t) = CeAt B + Dδ(t) ∀t ≥ 0 (f (t) = 0 si t < 0) .
R +∞
The Laplace transform: F (s) = 0 f (τ )e−τ s d τ allows to find the well-known
result:
Z +∞
F (s) = (CeAτ B+Dδ(τ ))e−τ s d τ = C(sI−A)−1 B+D (∀s ∈ convergence domain).
0
(A.8)
Y (s) 1
Example A.1 Let us consider the second order model: U (s)
= (s+1)2
.
• a) compute the system impulse response,
• b) compute the response of the system to initial conditions y(0) = y0 and ẏ(0) =
y˙0 (with t0 = 0).
Correction: If the use of Laplace transform pair table allows the question a) to be
solved directly: y(t) = te−t , it is strongly recommended to use a state space approach
and equation (A.2) to solve question b). Let us consider a Jordan realization of
the plant:
−1 1 0
ẋ = x+ u
0 −1 1 . (A.9)
y= [ 1 0 ]x
As the dynamic matrix A is not diagonalizable (one multiple eigenvalue of order 2),
the computation of the matrix exponential eAt can be performed using a power series
expansion:
A2 t2 A3 t3 A4 t4
eAt = I + At + + + + ··· .
2! 3! 4!
Then we get:
−t t
t2 t3 3 4
1−t+ − + · · · t − t2 + t2! − t3! + · · · e−t te−t
e 0 −t = 2! 3!
2 3 = .
0 1 − t + t2! − t3! + · · · 0 e−t
Impulse response: with t0 = 0, x(t0 ) = 0 and u(t) = δ(t) ( Dirac function), equation
(A.2) becomes :
Z t −t
(t − τ )eτ −t te
x(t) = τ −t δ(τ )dτ = −t et y(t) = [1 0]x(t) = te−t .
0 e e
Response to initial conditions (u(t) = 0): a relationship between the state vector x =
[x1 , x2 ]T and the vector composed of the output y and its derivative ẏ, on which the
initial conditions are formulated, must be established. The measurement equation
and its derivation allow to write:
y 1 0 1 0 y0
= x ⇒ x(t0 ) = .
ẏ −1 1 1 1 y˙0
So:
e−t te−t 1 0 y0
At
y(t) = Ce x(t0 ) = [1 0] = e−t (1 + t)y0 + te−t y˙0 .
0 e−t 1 1 y˙0
Remark: an horizontal companion realization can be also used: then the state vector
is composed of the output and its derivative:
ẏ 0 1 y 0
= + u. (A.10)
ÿ −1 −2 ẏ 1
The computation of the matrix exponential is now:
0 t
e−t (1 + t) te−t
e −t −2t = .
−te−t e−t (1 − t)
2
Remark A.2 Using (A.12) with k0 =0, x0 = 0 and u(i) = δ(i) (discrete Dirac
function: δ(0) = 1; δ(i) = 0 if i 6= 0), the impulse response of the system defined by
(Ad , Bd , Cd , Dd ) is therefore:
Appendix B
The following proofs are extracted from reference [5] and adapted to the notation
of this document.
The variance is equal (with a factor dt/2/π) to the integral of the PSD between
−π/dt et π/dt.
If the system is stable (all the eigenvalues of A have a negative real part) a
steady state is reached: Ṗ = 0 and P (t) = P is then solution of the continuous-
time Lyapunov equation:
AP + P AT + M W M T = 0 . (B.3)
Proof: The integration of equation (B.1) from initial time t0 and running time t
yields to: Z t
A(t−t0 )
x(t) = e x(t0 ) + eA(t−τ ) M w(τ )dτ
t0
x(t) is thus a combination of random gaussian signals (x(t0 ) and w(τ )), so x(t) is
also a gaussian random signal. Let us compute its mean m(t) = E(x(t)] and its
covariance matrix P (t) = E[(x(t) − m(t))(x(t) − m(t))T ].
Mean m(t):
Z t
A(t−t0 )
m(t) = e E[x(t0 )] + eA(t−τ ) M E[w(τ )]dτ ,
t0
Covariance P (t):
Z t
A(t−t0 )
x(t) − m(t) = e x(t0 ) − m(t0 ) + eA(t−τ ) M w(τ )dτ
t
Z t0
= eA(t−t0 ) x(t0 ) − m(t0 ) + eA(t0 −τ ) M w(τ )dτ . (B.4)
t0
T T
A(t−t0 )
x(t) − m(t) x(t) − m(t) = e x(t0 ) − m(t0 ) x(t0 ) − m(t0 ) +
Z t T
+ eA(t0 −τ ) M w(τ ) x(t0 ) − m(t0 ) dτ +
t
Z 0t T
+ x(t0 ) − m(t0 ) wT (τ )M T eA (t0 −τ ) dτ +
t
Z Z0 t
A(t0 −τ ) T T AT (t0 −u) T
+ e M w(τ )w (u)M e dτ du eA (t−t0 ) . (B.5)
t0
Z t T
A(t−t0 ) A(t0 −τ )
P (t) = e P (t0 ) + e M E w(τ ) x(t0 ) − m(t0 ) dτ +
t0
Z t
T
+ E x(t0 ) − m(t0 ) w (τ ) M T eA (t0 −τ ) dτ +
T
t
Z Z0 t
A(t0 −τ ) T T AT (t0 −u) T
+ e M E w(τ )w (u) M e dτ du eA (t−t0 ) . (B.6)
t0
From assumption (x(t0 ) and w(τ ) are independent ∀ τ > t0 and w(t) is a centered
white noise), one can state:
T
• E w(τ ) x(t0 ) − m(t0 ) =0
T
• E w(τ )w (u) = W δ(τ − u).
So:
Z t
A(t−t0 ) A(t0 −τ ) T AT (t0 −τ ) T
P (t) = e P (t0 ) + e MWM e dτ eA (t−t0 ) . (B.7)
t0
and
˙ dP (t) A(t−t0 ) AT (t−t0 ) A(t−t0 ) T
P (t) = = Ae P (t0 ) + · · · e +e P (t0 ) + · · · eA (t−t0 ) AT
dt
A(t−t0 ) A(t0 −t) T AT (t0 −t) T
+e e MWM e eA (t−t0 ) . (B.8)
That is:
˙ = AP (t) + P (t)AT + M W M T .
P (t) (B.9)
Indeed:
Z ∞
T Tt T
AP + P A = AeAt M W M T eA + eAt M W M T eA t AT dt
0
Z ∞ T
d[eAt M W M T eA t ]
= dt
0 dt
T
= [e M W M T eA t ]∞
At
0 = 0 − MW M
T
(iff A is stable) .
Let us denote h(t) = eAt B, ∀t ≥ 0, the impulse response of the state x, one can
write:
Z ∞ Z ∞
T 1
P = h(t)W h (t)dt = H(−jω)W H T (jω)dω ( Parseval equality)
0 2π −∞
= [L−1 Φ
II xx (s)]s=0 with: Φ T
xx (s) = H(−s)W H (s) .
These last equalities allow to do the link with the following frequency-domain ap-
proach (theorem 1.2) and to show that the variance is (with a factor 2π) the integral
of the noise frequency response square.
Proof: (this proof is simpler than in the continuous-time case). In equation (A.12),
x(k) is a linear combination of gaussian random variable. Then one can conclude
that x(k) is a gaussian random variable. wd (k) being centered (∀k), one can state
that E[x(k)] = Ak−k
d
0
m(0).
Lastly, one can note that x(k + 1) − m(k + 1) = Ad (x(k) − m(k)) + Md wd (k).
The independence, at each instant k, of centered variables x(k) − m(k) et wd (k)
allow us to conclude that P (k + 1) = Ad P (k)ATd + Md Wd MdT .
Remark B.2 From equation B.7, one can derive the discrete-time Lyapunov equa-
tion for a continuous system sampled with a sampling period dt. It is sufficient to
choose t0 = n dt et t = (n + 1) dt where dt is the sampling period. Then we denote
P (t) = P (n + 1) and P (t0 ) = P (n), we have:
Z dt
A dt AT dt T
P (n + 1) = e P (n)e + eAu M W M T eA u du
0
Let us remark :
• eAdt = Ad : discrete-time dynamic matrix,
R dt T
• 0 eAu M W M T eA u du = Wd is the covariance matrix integrated on a sampling
period,
Then we can write:
Pn+1 = Ad Pn ATd + Wd
(whose steady state is: Pn = Ad Pn ATd + Wd ). One find again equation (B.11) with
Md = I.
One can verify that :
Example B.1 (Example with Matlab) Let us consider again exercise 1.3 and
complete it by a discrete-time analysis of the variance of the noise w:
% Filter definition:
G=tf([-1 100],[1 50 10000])
% State space realization:
[A,B,C,D]=ssdata(G);
% variance determination using continuous-time Lyapunov equation:
P=lyap(A,B*B’); var_w=C*P*C’ % ==> var_w=1/50.
% Discrete-time Analysis:
dt=0.001; A_d=expm(A*dt);
Wd=lyap(A,B*B’-A_d*B*B’*A_d’); %In this example: W=I; M=B.
Pd=dlyap(A_d,Wd);var_wd=C*Pd*C’
% ==> We find excatly the variance of w(t): var_w=1/50.
Proof : without loss of generality the proof will consider a strictly proper system
(without direct feed-through). Let (A, M , C) be a state space realization of the
transfer G(s), i.e.: G(s) = C(sI − A)−1 M .
By definition:
Z ∞ Z ∞
−τ s
Φyy (s) = φyy (τ )e dτ = E y(t)y T (t + τ ) e−τ s dτ . (B.14)
−∞ −∞
To compute y(t) during the steady state, the formulae (A.2) will be used with
x(t0 ) = 0 and t0 = −∞:
Z t Z ∞
A(t−u)
y(t) = Ce M w(u)du = CeAv M w(t−v)dv (change of variable: t − u = v) .
−∞ 0
Z +∞ Z ∞ Z ∞
T AT u
Φyy (s) = E Av
Ce M w(t − v)dv w (t + τ − u)M e C du e−τ s dτ
T T
−∞ 0 0
Z +∞ Z Z ∞
T AT u T
= Ce M E w(t − v)w (t + τ − u) M e C du dv e−τ s dτ
Av T
−∞ 0
Z +∞ Z Z ∞
Av vs −(τ +v−u)s T AT u T −us
= Ce M e φww (τ + v − u)e M e C e du dv dτ
−∞ 0
Z ∞ Z +∞ Z ∞
−(τ +v−u)s T
= Av
Ce M e dvvs
φww (τ + v − u)e dτ M T eA u C T e−us du
0 −∞ 0
T
= G(−s)Φww (s)G (s) from (A.8) and (B.14) .
To compute y(k) during the steady state, the formulae (A.12) will be used with
x0 = 0 and k0 = −∞:
X
k−1 X
+∞
y(k) = Cd Aj−k
d
0
Md w(k − 1 − j + k0 ) = Cd Ajd Md w(k − 1 − j) (j ← j − k0 ) .
j=−∞ j=0
" +∞ #
X
+∞ X X
+∞
Tl
Φyy (z) = E Cd Ajd Md w(k − 1 − j) wT (k + i − 1 − l)MdT Ad CdT z −i
i=−∞ j=0 l=0
( +∞ +∞ )
X
+∞ XX l
= Cd Ajd Md E w(k − 1 − j)wT (k + i − 1 − l) MdT ATd CdT z −i
i=−∞ j=0 l=0
X
+∞ X
+∞ X
+∞
l
= Cd Ajd Md z j+1 φww (i − l + j)z −(i−l+j) MdT ATd CdT z −l−1
i=−∞ j=0 l=0
X
+∞ X
+∞ X
+∞
l−1
= Cd Aj−1
d Md z
j
φww (k)z −k MdT ATd CdT z −l (k = i − l + j)
j=1 k=−∞ l=1
−1 T
= G(z )Φww (z)G (z) from (A.14) and (B.15) .
Appendix C
Analytical solution of
continuous-time differential
Riccati equation
Then:
X(t) I Φ11 (t0 , t) Φ12 (t0 , t) I
= Φ(t0 , t) = (C.3)
Y (t) P0 Φ21 (t0 , t) Φ22 (t0 , t) P0
P (t) = [Φ21 (t0 , t) + Φ22 (t0 , t) P0 ][Φ11 (t0 , t) + Φ12 (t0 , t) P0 ]−1 . (C.4)
This analytical solution of P (t) is still valid in the non-stationnary case, that
is when equation matrix data A, C, M , W and V are functions of time t. In the
stationnary case:
Φ(t0 , t) = eH(t−t0 ) .
This matrix exponiential can be computed very efficiently using linear algebra library
(function expm in Matlab).
In the next section, this general solution is used to find the solution presented
in the exercice of section 2.3.1.
C.2 Exemple
Let us consider the Riccati differential equation (2.17):
where:
λ1 0 · · · 0
0 λ2 0 · · ·
• Λ is the eigenvalue diagonal matrix Λ = .. ... ,
. 0
0 · · · 0 λn
• M is the eigenvector matrix M = [v1 , v2 · · · vn ].
Eigen-values:
Eigen-vectors:
−a − λ 1 1
• λ1 = λ then v1 / v1 = 0 ⇒ v1 = ,
W a−λ a+λ
−a + λ 1 1
• λ2 = −λ then v1 / v2 = 0 ⇒ v2 = ,
W a+λ a−λ
λ−a 1
1 1 −1 2λ 2λ
Then: M = and M = λ+a 1 ,
a+λ a−λ 2λ
− 2λ
and:
λ(t−t ) λ−a 1
H(t−t0 ) 1 1 e 0
0 2λ 2λ
e =
a+λ a−λ 0 e−λ(t−t0 ) λ+a
2λ
− 2λ1
1 eλ(t−t0 ) (λ − a) + e−λ(t−t0 ) (λ + a) eλ(t−t0 ) − e−λ(t−t0 )
= ,
2λ eλ(t−t0 ) (λ2 − a2 ) + e−λ(t−t0 ) (a2 − λ2 ) eλ(t−t0 ) (λ + a) + e−λ(t−t0 ) (λ − a)
and finally:
eλ(t−t0 ) (λ2 − a2 ) + e−λ(t−t0 ) (a2 − λ2 ) + eλ(t−t0 ) (λ + a)P0 + e−λ(t−t0 ) (λ − a)P0
P (t) =
eλ(t−t0 ) (λ − a) + e−λ(t−t0 ) (λ + a) + eλ(t−t0 ) P0 − e−λ(t−t0 ) P0
λ2 − a2 + (a + λ)P0 + e−2λ(t−t0 ) [a2 − λ2 + (λ − a)P0 ]
=
λ − a + P0 + e−2λ(t−t0 ) (λ + a − P0 )
λ2 −a2 +(a+λ)P0
Steady state: limt→∞ P (t) = P∞ = λ−a+P0
=a+λ
√
P∞ = a + λ = a + a2 + W (P∞ is independant of initial condition P0 )
2λ(P0 −P∞ )
P (t) = P∞ + e2λ(t−t0 ) (P0 −P∞ +2λ)+P∞ −P0
.
One can recognize the solution proposed in equation (2.18) with k = 2λ.
Appendix D
% Compute P(t):
Pinf=a+sqrt(a^2+W);
k=2*sqrt(a^2+W);
P=Pinf+k*(P0-Pinf)./(exp(k*u(1))*(P0-Pinf+k)+Pinf-P0);
% Compute Kf(t):
Kf=P*c’;
% Output
y=Kf*u(2:length(u));
% Start simulation:
sim(’simuKalman’);
% Output plots:
figure plot(output.time,output.signals.values(:,1),’g-’) hold on
plot(output.time,output.signals.values(:,2),’k-’,’LineWidth’,2)
plot(output.time,output.signals.values(:,3),’k-’)
plot(output.time,output.signals.values(:,4),’r-’,’LineWidth’,2)
% Compute state estimation error variance as a function of time:
t=output.time;
Pinf=a+sqrt(a^2+W);
k=2*sqrt(a^2+W);
Pdet=Pinf+k*(P0-Pinf)./(exp(k*t)*(P0-Pinf+k)+Pinf-P0);
% plot estimation+2*sigma:
plot(output.time,output.signals.values(:,4)+2*sqrt(Pdet),’r-’)
% plot estimation-2*sigma:
plot(output.time,output.signals.values(:,4)-2*sqrt(Pdet),’r-’)
legend(’y(t)’,’x(t)’,’u(t)’,’xhat(t)’,...
’xhat(t)+2\sigma’,’xhat(t)-2 \sigma’)
return
[Kf,P]=lqe(a,b,c,W,1);
% Output estimation error variance:
var=c*P*c’;
% Output plots:
figure plot(output.time,output.signals.values(:,1),’g-’) hold on
plot(output.time,output.signals.values(:,2),’k-’,’LineWidth’,2)
plot(output.time,output.signals.values(:,3),’k-’)
plot(output.time,output.signals.values(:,4),’r-’,’LineWidth’,2)
% plot estimation+2*sigma:
plot(output.time,output.signals.values(:,4)+2*sqrt(var),’r-’)
% plot estimation-2*sigma:
plot(output.time,output.signals.values(:,4)-2*sqrt(var),’r-’)
legend(’y(t)’,’x(t)’,’u(t)’,’xhat(t)’,...
’xhat(t)+2\sigma’,’xhat(t)-2 \sigma’)
dt=0.01;
% Compute Discrete-time model:
sysd=c2d(sysc,dt,’zoh’);
[A_d,B_d,C_d,D]=ssdata(sysd)
Vd=V/dt
Wd=[1/3*q^2*dt^3 -1/2*q^2*dt^2;-1/2*q^2*dt^2 q^2*dt] % Wd can not
% be computed by a Lyapunov equation (equation B.13) since A
% is not invertible !! (see how Wd can be computed in LQED)
% Discrete-time Kalman filter using dlqe:
[Kfd1,Pd1]=dlqe(A_d,eye(2),C_d,Wd,Vd)
% Discrete-time Kalman filter using lqed (from continuous-time data):
[Kfd2,Pd2]=lqed(A,M,C,W,V,dt) %==> same solution !