tx - home page corsi

Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
KALMAN FILTER
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
1
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
Dealing with input-output models, we studied the ARMAX models:
y (t ) =
B( z )
C ( z)
u (t − 1) +
e(t ), e(t ) ≈ WN (0, λ2 )
A( z )
A( z )
The prediction theory for this kind of linear-system representation is
known as “Kolmogorov–Wiener”.
Kalman theory, on the contrary, concerns systems described by state
representations:
disturbance
y(t)
u(t)
x(t)
x(t + 1) = F ⋅ x(t ) + Γ ⋅ u (t ) + v1 (t )
v1 ≈ WN (0, V1 )
y (t ) = H ⋅ x(t ) + v 2 (t )
v 2 ≈ WN (0, V2 )
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
2
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
First, we briefly discuss about the passage from one representation to
another.
y (t ) =
B( z )
C ( z)
u (t − 1) +
e(t ), e(t ) ≈ WN ( µ , λ2 )
A( z )
A( z )
⇑⇓
x(t + 1) = F ⋅ x(t ) + Γ ⋅ u (t ) + v1 (t )
v1 ≈ WN (0, V1 )
y (t ) = H ⋅ x(t ) + v 2 (t )
v 2 ≈ WN (0, V2 )
(assuming a SISO system)
Let’s see an example
Example
Given a system with state representation:
1
1
x(t ) + u (t ) + v1 (t )
2
4
y (t ) = x(t ) + v 2 (t )
x(t + 1) =
with v1 ≅ WN (0,1), v 2 ≅ WN (0,1), v1 ⊥ v 2 ,
write the system in its ARMAX-equivalent representation.
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
3
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
1
1
( z − ) x(t ) = u (t ) + v1 (t )
2
4
⇓
1
1
x(t ) = 4 u (t ) +
v (t )
1
1 1
z−
z−
2
2
⇓
1
1
y (t ) = 4 u (t ) +
v1 (t ) + v 2 (t )
1
1
z−
z−
2
2
Passing to negative powers:
1
z −1
4
y (t ) =
u (t − 1) +
v (t ) + v2 (t )
1 −1
1 −1 1
1− z
1− z
2
2
B( z )
u (t − 1)
A( z )
C ( z)
e(t ) ??
A( z )
Notice: the state representation is seemingly richer.
In practice, however, we can find a spectral-equivalent
representation
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
4
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
Recall that the noise-spectrum is:
⎛
⎞
1
⎜⎜
+
1
⎟⎟
−1
(
1
−
0
.
5
)(
1
−
0
.
5
)
z
z
⎝
⎠ z = e jω
⇓
⎛ 9 1 −1
⎞
⎜ − (z + z ) ⎟
⎜4 2
⎟
⎜ 5 − 1 (z −1 + z ) ⎟
⎜
⎟
⎝4 2
⎠ z =e jω
This spectrum therefore is equivalent to a spectrum in a process as:
α + βz −1
1
1 − z −1
2
e(t ), e ≈ WN (0,1)
Where (e.g.):
⎧α = −0.34
⎨
⎩β = 1.46
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
5
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
Then:
1
1
x(t ) + u (t ) + v1 (t )
2
4
y (t ) = x(t ) + v 2 (t )
x(t + 1) =
with v1 ≅ WN (0,1), v 2 ≅ WN (0,1), v1 ⊥ v 2 ,
⇓⇑
1
1.46 − 0.34 z −1
4
e(t )
y (t ) =
u (t − 1) +
1 −1
1 −1
1− z
1− z
2
2
e ≈ WN (0,1)
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
6
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
Which problem classes shall we deal with Kalman theory?
Given a data set until time N {y (1), y (2),..., y ( N )} (in case even
{u (1), u (2),..., u ( N )}) find the output predictor
yˆ ( N + 1 | N ) (in general
yˆ ( N + k | N ) )
(problem already faced by Kolmogorov – Wiener theory)
Given a data set until time N {y (1), y (2),..., y ( N )} (in case even
{u (1), u (2),..., u ( N )}) find the state predictor xˆ ( N + 1 | N ) (in general
xˆ ( N + k | N ) )
Given a data set until time N, find xˆ ( N | N ) , i.e. carry out a state
filtering.
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
7
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
Remark
Generic dynamic system MIMO:
u1
u2
…
um
y1
x1
x2
…
xN
S
y2
…
yp
Problem:
• Usually p<<n, ( much fewer sensors than the state variables
number)
• Very often to face control problems or fault-detection problems
on S we need to measure one or more state variables.
• The sensor is often very expensive or cannot be used (i.e. no
suitable sensor is available or the measurement is not accessible).
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
8
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
Kalman filter: “SW sensor”
x1
x2
x3
…
S
xˆ ( N | N )
⇒ Example: estimated vehicle speed
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
9
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
STATEMENT OF THE BASIC PROBLEM OF KALMAN
FILTERING
(one-step prediction of state and output, with no exogenous input)
⎧ x(t + 1) = Fx(t ) + v1 (t )
S =⎨
⎩ y (t ) = Hx(t ) + v 2 (t )
F is the state matrix: dimension n x n
H is the output matrix: dimension p x n
MIMO system (m x p)
state vector:
⎡ x1 (t ) ⎤
⎢ x (t ) ⎥
x(t ) = ⎢ 2 ⎥
⎢... ⎥
⎢
⎥
⎣ x n (t ) ⎦
⎡ y1 (t ) ⎤
⎢ y (t ) ⎥
2
⎥
output vector: y (t ) = ⎢
⎢... ⎥
⎥
⎢
y
t
(
)
p
⎦
⎣
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
10
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
v (t ) is the “process” noise (noise “polluting” the state equations).
1
It must be a noise-vector with n components:
⎡v11 (t ) ⎤
⎢v (t ) ⎥
v1 (t ) = ⎢ 12 ⎥ ,
⎢... ⎥
⎥
⎢
⎣v1n (t )⎦
v1 ≅ WN (0, V1 )
We suppose a white vector noise, with null mean.
(we can always get a null mean by signal depolarization)
1) E[v1 (t )] = 0
2) E[v1 (t ) ⋅ v1 (t ) T ] = V1 ≥ 0
3) E[v1 (t ) ⋅ v1 (t − τ ) T ] = 0, ∀t ,τ ≠ 0
NB: V1 is symmetric and semi-defined positive (Covariance matrix)
NB: V is diagonal if and only if single noise components are not
1
correlated
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
11
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
v (t ) is the “measurement” noise (it pollutes the output equation).
2
⎡v 21 (t ) ⎤
⎢v (t ) ⎥
22
⎥,
v 2 (t ) = ⎢
⎢...
⎥
⎢
⎥
(
)
v
t
2
p
⎣
⎦
v 2 ≅ WN (0, V2 )
We suppose a white vector noise, with null mean.
1) E[v 2 (t )] = 0
2) E[v 2 (t ) ⋅ v 2 (t ) T ] = V2 > 0
3) E[v 2 (t ) ⋅ v 2 (t − τ ) T ] = 0, ∀t ,τ ≠ 0
NB: On V : (restrictive) assumption that it is defined positive ( V >0).
2
2
Correlation assumptions between v and v :
1
⎧0
E[v1 (t ) ⋅ v 2 (t − τ ) T ] = ⎨
⎩V12
2
se τ ≠ 0
se τ = 0
i.e. suppose v and v can be correlated only at the same instant.
1
2
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
12
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
Assumptions about initial conditions:
E[ x(1)] = x 0
VAR[ x(1)] = P0 , i.e E[( x(1) − x 0 ) ⋅ ( x (1) − x 0 ) T ] = P0
Notice: if P0 = 0 we know exactly the initial condition value (without
any uncertainty)
We assume that:
x(1) ⊥ v1 (t )
x(1) ⊥ v 2 (t )
The problem we want to solve is:
given {y (1), y (2),..., y ( N )},
estimate the optimal xˆ ( N + 1 | N ) (and yˆ ( N + 1 | N ) )
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
13
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
Remark
No more the restrictive assumption of a steady-state system (as in KW theory)
Remark
In practice the noise disturbing state and output is rarely white.
However, it’s easy to go back to the standard formulation, if we know
the characteristics of spectral noises.
Let’s see by a simple example.
x (t + 1) = ax (t ) + η (t ) , with η (t ) =
1
1
y (t ) = bx (t ) + v (t ) ,
1
2
1
e(t ) ,
1 − cz
−1
with v (t ) ≅ WN (0,1) ,
2
e(t ) ≅ WN (0,1)
v 2 (t ) ⊥ e(t )
Re-write the noise equation:
η (t ) = cη (t − 1) + e(t )
define
v(t ) = e(t + 1) , v ≅ WN (0,1) e v ⊥ v 2
η (t ) = cη (t − 1) + v(t − 1)
η (t + 1) = cη (t ) + v(t )
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
14
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
Define now a second state variable and extend the system:
x (t ) = η (t )
2
The systems changes into:
⎧ x (t + 1) = ax (t ) + x (t )
⎪
⎨ x (t + 1) = cx (t ) + v(t )
⎪ y (t ) = bx (t ) + v (t )
⎩
1
1
2
2
1
2
2
Namely:
⎧ x(t + 1) = Fx(t ) + v (t )
S =⎨
⎩ y (t ) = Hx(t ) + v (t )
1
2
⎡ 0 ⎤
, v ≅ WN (0,1) v1 ⊥ v 2
v (t ) ≅ WN (0,1) v = ⎢
⎥
⎣v(t )⎦
2
⎡ a 1⎤
F =⎢
⎥
⎣0 c⎦
1
H = [b 0]
We went back to a standard formulation of the system: we had to
enlarge the state space (from a order 1 system to an order 2 system).
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
15
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
PREMISE TO A SOLUTION OF THE KALMAN FILTERING
PROBLEM: BAYESIAN ESTIMATE
The proof of the solution of the Kalman filtering problem is based on
the theory of the Bayesian estimate.
Short recall.
Assume a vector of stochastic variables
⎡d 1 ⎤
⎢d ⎥
d =⎢ 2 ⎥
⎢... ⎥
⎢ ⎥
⎣d N ⎦
and suppose we want to estimate –starting from a measure of d – a
stochastic variable ϑ .
Assume also (model assumption) that the estimated value of ϑ
depends linearly on d :
⎡a ⎤
⎢a ⎥
A=⎢ ⎥
⎢... ⎥
⎢ ⎥
⎣a ⎦
1
ϑˆ = AT d = a1 d 1 + a 2 d 2 + ... + a N d N
2
N
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
16
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
The Bayesian estimate ϑ , given d, is:
ϑˆ = E[ϑ | d ] = [Λ ϑd ⋅ Λ dd −1 ] ⋅ d
Λ ϑd = E[ϑd ]
Λ dd = E[dd T ]
(this result is valid only in case of casual variables all with expected
null value)
Exercise (Bayesian estimate)
Suppose we measure a stochastic variable y obtained as follows:
y = x + w1
• x ≅ N (0,2) ,
• w ≅ N (0,1) ,
1
• x⊥w.
1
Problem: estimate x̂ starting from a measurement of y. Compute the
corresponding error variance.
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
17
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
NB:
• x and w are stochastic variables normally distributed (they are
1
not “white noises”). They are simply stochastic variables, not
successions of casual variables (not stochastic processes).
• y is the measured variable, x is the variable to be estimated, and
w is the measurement disturbance.
1
−1
xˆ = E[ x | y ] = [Λ xy ⋅ Λ yy ] ⋅ y
Λ xy = E[ xy ] = E[ x( x + w1 )] = E[ x 2 ] + E[ xw1 ] = 2 + 0 = 2
2
Λ yy = E[ y 2 ] = E[( x + w1 ) 2 ] = E[ x 2 ] + E[ w1 ] + 2 E[ xw1 ] = 2 + 1 + 0 = 3
xˆ = (2 ⋅ 3 −1 ) y =
2
y
3
Error variance:
2 2
2
2
y ) ] = E[( x − x − w1 ) 2 ] =
3
3
3
1
2
1
4
6 2
= E[( x − w1 ) 2 ] = ⋅ 2 + ⋅ 1 = =
3
3
9
9
9 3
E[( x − xˆ ) 2 ] = E[( x −
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
18
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
Suppose now a second measurement:
y = x+w
1
⎡y ⎤
y=⎢ ⎥
⎣y ⎦
1
1
y = x+w
2
2
2
with x ≅ N (0,2) , w ≅ N (0,1) , w ≅ N (0,1) , x ⊥ w ⊥ w
1
2
1
2
This is a vector problem:
xˆ = E[ x | y ] = (Λ ⋅ Λ
xy
1x1
1x2
−1
yy
2x2
Λ xy = E[ xy T ] = E[ x[ y1
)⋅ y
2x1
y 2 ]] = E[[x( x + w1 ) x( x + w2 )] = [2 2]
⎡y ⎤
Λ yy = E[ yy T ] = E[ ⎢ 1 ⎥[ y1
⎣ y2 ⎦
y 2 ]] =
⎡ ( x + w1 ) 2
( x + w1 )( x + w2 )⎤ ⎡3 2⎤
= E[ ⎢
⎥ = ⎢ 2 3⎥
2
( x + w2 )
⎦
⎣( x + w1 )( x + w2 )
⎦ ⎣
1 ⎡ 3 − 2⎤
−1
Λ yy = ⎢
5 ⎣− 2 3 ⎥⎦
⎛
1 ⎡ 3 − 2⎤ ⎞ ⎡ y1 ⎤ ⎡ 2
⎟⎟ ⋅ ⎢ ⎥ = ⎢
xˆ = ⎜⎜ [2 2] ⋅ ⎢
⎥
5 ⎣− 2 3 ⎦ ⎠ ⎣ y 2 ⎦ ⎣ 5
⎝
2 ⎤ ⎡ y1 ⎤ 2
2
y
y2
⋅
=
+
1
⎢
⎥
⎥
5 ⎦ ⎣ y2 ⎦ 5
5
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
19
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
2
2
2
2
2
2
y1 − y 2 ) 2 ] = E[( x − x − w1 − x − w2 ) 2 ] =
5
5
5
5
5
5
1
2
2
1
4
4
10 2
= E[( x − w1 − w2 ) 2 ] =
⋅ 2 + ⋅1 + ⋅1 =
=
5
5
5
25
25
25
25 5
E[( x − xˆ ) 2 ] = E[ x −
NB:
2 2
<
5 3
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
20
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
SOLUTION OF THE BASIC PROBLEM OF KALMAN
FILTERING (ONE-STEP PREDICTION, NO EXOGENOUS
INPUT)
System (see all previously-defined assumptions):
⎧ x(t + 1) = Fx(t ) + v (t )
S :⎨
⎩ y (t ) = Hx(t ) + v (t )
1
2
KALMAN filtering
⎧ xˆ (t + 1 | t ) = Fxˆ (t | t − 1) + K (t ) ⋅ e(t )
⎨
⎩ yˆ (t | t − 1) = Hxˆ (t | t − 1)
e(t ) : prediction error on output,
e(t ) = y (t ) − yˆ (t | t − 1)
K (t ) : filter gain
K (t ) = (FP(t ) H + V
T
12
)(HP(t ) H
T
+V
2
)
−1
P(t ) : solution of the Difference Riccati Equation (DRE)
P(t +1) = FP(t )F T + V1 − (FP(t )H T + V12 )(HP(t )H T + V2 ) −1 (FP(t )H T + V12 ) T
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
21
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
Remark
The P(t ) matrix, which solves the DRE, is a matrix n × n with this
meaning:
P (t ) =
E[( x(t ) − xˆ (t | t − 1)( x(t ) − xˆ (t | t − 1)) T ] =
VAR[ x(t ) − xˆ (t | t − 1)]
P(t ) : variance of one-step prediction error of the state .
For every t, P(t ) is a symmetric, semi-definite and positive ( ≥ 0 )
matrix.
Remark
The state equation and the DRE are two difference equations
Therefore it’s necessary to initialize them:
xˆ (1 | 0) = E[ x(1)] = x 0
P (1) = VAR[ x(1)] = P0
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
22
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
Remark
Graphic representation of Kalman filter
v (t )
v (t )
2
1
+
x(t + 1)
z
−1
x(t )
+
H
y (t )
+
+
F
+
e(t )
K (t )
−
+
z
−1
xˆ (t / t − 1)
H
+
yˆ (t / t − 1)
F
⇒ It makes a replica (without noises) of the system
⇒ It makes a correction proportional to e(t ) = y (t ) − yˆ (t )
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
23
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
This idea was already known ⇒ Luemberg Observer
The theory of Kalman Filter made a (great) progress: it computes
K (t ) optimal value.
K (t ) optimal value is crucial:
• K (t ) too low value ⇒ too feeble correction
• K (t ) too high value ⇒ exaggerated correction
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
24
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
EXTENSIONS TO BASIC CASE (I): MULTI-STEP
PREDICTION
Starting from xˆ (t + 1 | t ) and yˆ (t + 1 | t ) :
xˆ (t + 2 | t ) = Fxˆ (t + 1 | t )
xˆ (t + 3 | t ) = F 2 xˆ (t + 1 | t )
...
...
xˆ (t + k | t ) = F k −1 xˆ (t + 1 | t )
⇓
yˆ (t + k | t ) = Hxˆ (t + k | t )
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
25
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
EXTENSIONS TO BASIC CASE (II): FILTERING
Starting from xˆ (t + 1 | t ) (NB: filtering on y is nonsense):
xˆ (t + 1 | t ) = Fxˆ (t | t )
⇓⇑
xˆ (t | t ) = F xˆ (t + 1 | t ) ,
−1
We can obtain xˆ (t | t ) from xˆ (t + 1 | t ) by this method only if F is
invertible.
If F is NOT invertible, we can compute xˆ (t | t ) all the same.
The formula in case V12 = 0 (we just consider this case) is:
⎧ xˆ (t | t ) = Fxˆ (t − 1 | t − 1) + K 0 (t )e(t )
⎪ yˆ (t | t − 1) = Hxˆ (t | t − 1)
⎪
⎨
⎪e(t ) = y (t ) − yˆ (t | t − 1)
⎪⎩ K 0 (t ) = ( P (t ) H T )( HP (t ) H T + V2 ) −1
P(t ) is the DRE solution.
NB: the filter gain is different ( F is missing)
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
26
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
EXTENSIONS TO BASIC CASE (III): TIME-VARIANT
SYSTEMS
⎧ x(t + 1) = F (t ) x(t ) + v (t )
S =⎨
⎩ y (t ) = H (t ) x(t ) + v (t )
1
2
All the filter equations:
- State equation
- Output equation
- Error equation
- Gain equation
- DRE equation
Are exactly the same, provided that we replace F and H by F (t ) and
H (t ) .
NB: remarkable difference in comparison with Kolmogorov – Wiener
theory (everything steady-state; time-invariant systems)
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
27
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
EXTENSIONS TO BASIC CASE (IV): SYSTEMS WITH
EXOGENOUS INPUT
It can be explained clearly by this graphical representation:
v (t )
v (t )
1
u (t )
Γ
2
+ x(t + 1)
z
x(t )
−1
+
H
+
y (t )
+
F
+
K (t )
−
u (t )
+
Γ
z
−1
xˆ (t / t − 1)
yˆ (t / t
H
+
F
⇒ K (t ) is the same as in the case without exogenous input
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
28
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
STEADY-STATE SOLUTION
Problem: in case the system is time-invariant, we should ask whether a
steady-state solution exists (and how it is built up), i.e.:
• Does P(1), P(2),..., P(t ) sequence converge to a steady-state
value P ?
• In case P(t ) converges to P , does the corresponding gain
K = (FP H T + V12 )(HP H T + V2 ) make Kalman filter
−1
asymptotically stable?
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
29
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
Remark (filter stability)
Consider the steady-state solution ( K )
Analyze the filter stability (filter state equation):
xˆ (t + 1 / t ) = Fxˆ (t | t − 1) + K e(t )
xˆ (t + 1 / t ) = Fxˆ (t | t − 1) + K ( y (t ) − yˆ (t | t − 1))
xˆ (t + 1 / t ) = Fxˆ (t | t − 1) + K ( y (t ) − Hxˆ (t | t − 1))
xˆ (t + 1 / t ) = ( F − K H ) xˆ (t | t − 1) + K y (t )
⇓
F − K H : dynamic filter K matrix
⇓
Kalman filter is asymptotically stable if and only if all F − K H
eingevalues are strictly inside the unit circle.
NB: Kalman filter can be asimptotically stable even if the system is
not ( F − K H ≠ F )
⇓
Kalman filter theory can be applied even to unstable systems (unlike
Kolmogorov – Wiener theory)
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
30
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
Remark (DRE convergence)
To understand if P (t ) → P we have to analyse DRE.
Notice that DRE is a matrix non-linear difference equation ⇒ very
difficult analysis.
Notice that if a steady-state P solution exists, at steady-state the
following must hold:
P (t + 1) = P (t ) = P
⇓
P must satisfy this equation:
P = FP F T + V1 − ( FP H T + V12 )( HP H T + V2 ) −1 ( FP H T + V12 ) T
Algebraic equation known as ARE (Algebraic Riccati Equation).
Therefore if a steady-state P exists (in such a way that P (t ) → P ), it
should necessarily be a solution to ARE.
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
31
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
The problems to solve are:
• Has ARE a semi-definite-positive (s.d.p.) solution?
• If ARE has a s.d.p. solution, will DRE converge to such solution?
• If ARE has a s.d.p. solution and if DRE converges to such
solution, is the corresponding Kalman filter is asymptotically
stable? ( are F − K H eigenvalues strictly inside the unit circle?)
⇒ Two theoretic results exsist, known as first and second theorem
of asymptotic convergence (“asymptotic theorems”) – they
provide sufficient convergence conditions.
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
32
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
FIRST THEOREM OF ASYMPTOTIC CONVERGENCE
If the following conditions hold:
• V =0
12
• the system S is asymptotically stable (i.e. all F eigenvalues are
strictly inside the unitary circle ray)
⇓
• ARE has just one solution P ≥ 0 (s.d.p.)
• DRE converges asymptotically in P ∀ P
0
• Gain K corresponding to P grants the asymptotical stability of
Kalman filter (i.e. all the matrix eigenvalues F − K H are strictly
inside the unit circle).
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
33
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
As a premise to the Second theorem of asymptotic convergence, we
recall the notions of Oservability and Reachability (and the conditions
to check them)
Observability
⎧ x(t + 1) = Fx(t )
S :⎨
⎩ y (t ) = Hx(t )
System S is called Observable (the ( F , H ) pair is observable) if two
different initial states do not exist, such that their corresponding
outputs are exactly the same, for each t≥0.
Condition to check Observability
S is observable if and only if the observability matrix O is full rank,
i.e. it has rank n, where n is the system order.
⎡ H ⎤
⎢ HF ⎥
⎢
⎥
O = ⎢ HF 2 ⎥
⎢ ... ⎥
⎢ HF n −1 ⎥
⎣
⎦
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
34
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
(Noise) reachabilty
⎧ x(t + 1) = Fx(t ) + v (t )
S :⎨
⎩
1
v ≅ WN (0,V ) .
1
1
⇒ it’s always possible to factorize V as follows: V = GG
1
T
1
This is not the only possible factorization, but Reachability doesn’t
depend on the G we choose.
Then we can re-write the system as:
x(t + 1) = Fx(t ) + Gw(t ) con w(t ) ≅ WN (0, I )
Notice:
VAR[Gw(t )] = E[Gw(t ) w(t ) T G T ] = GE[ w(t ) w(t ) T ]G T =
= GIG = GG = V = VAR[v ]
T
T
1
1
System S is Reachable (from v1 (t ) ) ( ( F , G ) pair is Reachable) if,
starting from a null initial state, we can always find an input signal
w(t ) which leads the systems the to any a-priori selected final state in
finite time.
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
35
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
Condition to check Reachability
S is Reachable (from v1 (t ) ) if and only if the Reachability matrix R is
full rank, i.e. its rank is n.
R = [G
FG
F 2 G ... F n −1G ]
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
36
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
SECOND THEOREM OF ASYMPTOTIC CONVERGENCE
If the following conditions hold:
• V =0
12
• ( F , G ) is Reachable (V = GG )
T
1
• ( F , H ) is Observable
⇓
• ARE has one single solution P > 0 (d.p.)
• DRE converges asymptotically to P ∀ P
0
• gain K corresponding to P grants asymptotical stability of
Kalman filter (i.e. all eigenvalues of F − K H matrix are strictly
inside the unit circle).
Remark
The theorems of asymptotic convergence are just sufficient
conditions. Therefore if their assumptions are not met, we cannot get
any conclusion ⇒ we have to make a direct analysis of DRE.
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
37
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
EXTENDED KALMAN FILTER
⎧ x(t + 1) = f ( x(t )) + v1 (t )
S :⎨
⎩ y (t ) = h( x(t )) + v 2 (t )
f (⋅) e h(⋅) generic, static and non-linear functions (supposed class C 1
at least) ⇒ the system is NON-linear.
Ex.
1
⎧
2
⎪ x(t + 1) = x(t ) + v1 (t )
S :⎨
2
⎪⎩ y (t ) = e x ( t ) + v (t )
2
⇒ How can we solve Kalman filter problem in this case?
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
38
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
Kalman NON-linear filtering:
v (t )
v (t )
1
2
+ x(t + 1)
z
−1
x(t )
+
h(⋅)
+
y (t )
+
f (⋅)
+
?
−
+
z
−1
xˆ (t / t − 1)
h(⋅)
yˆ (t | t − 1)
+
f (⋅)
How can we compute the filter gain in this case?
• Non-linear function? [ K (e(t ))]
• Linear but time-variant? [ K (t )e(t )]
The second way (linear but time variant gain) is the easiest and most
natural.
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
39
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
To find K (t ) at each instant:
P (t + 1) = F (t ) P(t ) F (t ) T + V1 − ( F (t ) P (t ) H (t ) T + V12 ) ⋅
⋅ ( H (t ) P (t ) H (t ) T + V2 ) −1 ⋅ ( F (t ) P(t ) H (t ) T + V12 ) T
⇓
K (t ) = ( F (t ) P(t ) H (t ) T + V12 )( H (t ) P(t ) H (t ) T + V2 ) −1
⇒ Problem: what are F (t ) and H (t ) ?
F (t ) =
∂f ( x(t ))
∂x(t ) x(t ) = xˆ (t | t − 1)
H (t ) =
∂h( x(t ))
∂x(t ) x(t ) = xˆ (t | t − 1)
I.e.: we obtain F (t ) and H (t ) as linear approximation of f (⋅) and
h(⋅) around x(t ) = xˆ (t / t − 1)
NB: xˆ (t / t − 1) is known from the previous step
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
40
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
To implement an extended Kalman filter, each step it’s necessary to
do the following, at each time step:
• take xˆ (t | t − 1) , i.e. the prediction made previously
• compute matrices F (t ) and H (t ) using xˆ (t | t − 1)
• compute P (t ) from Riccati equation (which uses F (t ) and H (t ) )
• compute K (t ) using P (t )
• using K (t ) for the filter (which adopts F (t ) and H (t ) ), we can
compute a new prediction xˆ (t + 1 | t )
Kalman filter for non-linear systems is called Extended Kalman Filter
(EKF)
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
41
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
EKF FOR GREY-BOX IDENTIFICATION
EKF is used for the identification known as “grey-box”.
⎧ x(t + 1) = f ( x(t ),ϑ ) + v (t )
S :⎨
⎩ y (t ) = h( x(t ),ϑ ) + v (t )
1
2
The problem is to estimate the state and a few unknown parameters of
the model (ϑ ).
Define a new state equation:
ϑ (t + 1) = ϑ (t ) + v (t )
ϑ
⇓
⎧ x(t + 1) = f ( x(t ),ϑ (t )) + v (t )
⎪
⎨ϑ (t + 1) = ϑ (t ) + vϑ (t )
⎪ y (t ) = h( x(t ),ϑ (t )) + v (t )
⎩
1
Equazioni
di stato
2
⎡ x(t ) ⎤
Define the “extended state vector”: X E (t ) = ⎢
⎥
⎣ϑ (t ) ⎦
(add as many state variables as the unknown parameters are and we
obtain a standard problem of extended Kalman filtering)
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
42
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
Problem: choice of vϑ (t ) (this noise doesn’t exist).
This is a fictitious noise that we have to consider to obtain a good
estimate of the parameters.
Normally we choose vϑ (t ) in such a way that:
• it’s uncorrelated with v (t ) e v (t )
1
2
• it has a diagonal covariance matrix (i.e. the fictitious noises per
single parameter are uncorrelated)
⎤
⎡λ1
⎥
⎢
0
λ2
⎥
Vϑ = ⎢
...
⎥
⎢
⎥
⎢ 0
λ
nθ ⎦
⎣
Choice of values λ1 , λ 2 ,..., λ nθ empirically made:
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
43
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
Example (mass-spring-friction):
K
M
x
C
(Physical) model:
&x&M = − Kx − Cx&
⇓
x = x1
x& = x 2
⎧ x&1 = x 2
⎪⎪
K
C
S : ⎨ x& 2 = − x1 −
x2
M
M
⎪
⎪⎩ y = x1
Linear 2nd-order model written in standard form (in continuous-time:
to be discretized!)
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
44
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
Discretization example:
x& 2 (t ) ≈
x 2 (t + 1) − x 2 (t )
,
∆t
x&1 (t ) ≈
x1 (t + 1) − x1 (t )
∆t
∆t : sampling interval.
⇓
⎧ x (t + 1) − x (t )
⋅ M = − Kx (t ) − Cx (t )
⎪⎪
∆t
⎨
⎪ x (t + 1) − x (t ) = x (t )
⎪⎩
∆t
2
2
1
1
2
1
2
⇓
⎧ x1 (t + 1) = x1 (t ) + x2 (t )∆t
⎪⎪
C∆t
∆t ⋅ K
) x2 (t )
x1 (t ) + (1 −
⎨ x2 (t + 1) = −
M
M
⎪
⎪⎩ y (t ) = x1 (t )
Linear 2nd-order model written in standard form with discrete-time
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
45
Materiale ad uso esclusivo degli studenti del corso. Vietata ogni forma di copia o distribuzione. Per la preparazione all’esame
questo materiale va integrato con le spiegazioni del docente durante le lezioni, e con i libri di testo del corso.
Assume that parameters M and K are known ( easy to measure), while
C is unknown (we want an estimate)
Set the problem as follows:
x3 (t ) = C (t )
x3 (t + 1) = x3 (t ) + v13 (t )
⇓
⎧ x1 (t + 1) = x1 (t ) + x2 (t )∆t + v11 (t )
⎪
∆t ⋅ K
∆t
⎪⎪ x2 (t + 1) = −
x1 (t ) + (1 − x3 (t ) ) x2 (t ) + v12 (t )
M
M
⎨
⎪ x3 (t + 1) = x3 (t ) + v13 (t )
⎪
⎪⎩ y (t ) = x1 (t ) + v2 (t )
NB: we obtained a NON-linear system (then EKF must be used)
We have to choose (empirically) the noise variance values.
Identificazione dei Modelli ed Analisi dei Dati - KALMAN FILTER
46