2.160 System Identification, Estimation, and Learning
Lecture Notes No. 6
February 24, 2006
4.5.1 The Kalman Gain
Consider the error of a posteriori estimate xıt
et ≡
xı
t −
xt = xıt t −1 + K
t (
yt −
H
t xıt t −1 )
−
xt
=
xıt t −1 +
K t (
H
t xt +
vt −
H t xıt t −1 ) −
xt
(25)
=
( I − K t H t )ε t +
K t vt
where ε t is a priori estimation error, i.e. before assimilating the new measurement
yt.
ε t ≡
xıt t −1 −
xt
(26)
For the following calculation, let us omit the subscript t for brevity,
etT et =
[ε t −
K t H tε t + K t vt ] [ε t − K t H tε t +
K
t vt ]
T
= ε T ε + ε T H T K T KHε − 2ε T KHε + 2ε T Kv − 2vT K
T KHε + vT K T Kv
(27)
Let us differentiate the scalar function etT et with respect to matrix K by using the
following matrix differentiation rules.
i) f ≡
(a1 a2
⎡
K11 �
K1� ⎤⎛ b1 ⎞
⎜ ⎟
df
⎧⎪ ∂
f
⎫⎪
T
… an )⎢ � � � ⎥⎜ � ⎟ =
a T Kb →
=
⎨
⎬
= {
ai b j }
= a b
⎢
⎥
dK
K
∂
⎪
⎪⎩
ij ⎭
⎜ ⎟
⎣⎢ K n1 � K n� ⎥⎦⎝
b� ⎠
(28)
….. Rule 1
ii) g =
c
T K T Kb , b ∈
R � ×1 ,
c
∈
R � ×1 ,
K ∈
R n �
n
⎫ ⎧n
⎫
dg ⎧ ∂
� n n
T
T
=
⎨
K
c
K
b
=
c
K
b
+
⎨∑
m ij j ∑
Kik ck bm ⎬ =
K
b c +
K
c b
∑∑∑
ik k ij j ⎬
dK ⎩
∂
K im i =1 j =1 k =1
j =1
⎭ ⎩ j =1
⎭
(29)
…...Rule 2
1
Using these rules,
d T
d T T T
et et =
[�
H� K K �
Hε − 2vT K T KHε + v T K T Kv ] ← rule 2
dK
dK
cT
b
d T
+2
[ε Kv − ε T KHε ] ← rule 1
dK
= KH εε T H T + KHεε T H T − 2[ KHεv T + Kvε T H T ] + 2Kvv T + 2[εv T − εε T H T ]
(3)
0
he necessary condition for the mean sq
uared error of state estimate with respect to
T
the gain matrix K is:
dJ t
=0
dK
(3
1)
T
aking expectation of etT et , differentiating it w.r.t. K and setting it to zero yield:
[
]
E KHεε T H T − KHεvT − Kvε T H T + KvvT + εvT − εε T H T = 0
(3
2)
KH can be factored out,
KHE[εε T ]H T − KHE[εvT ] − KE[vε T ]H T + KE[vvT ] + E[εvT ] − E[εε T ]H T = 0 (3
3
)
Examine the term E[ε vT ] using (26) and (21),
E[ε t vt ] = E[(xı t t −1 − xt )vt ]
T
T
T
T
= E[ xı t t −1vt ] − E[ xt vt ]
For the first term xı t t −1 = At −1 xı t −1
xıt −1 = xıt −1 t − 2 + K t −1 ( yt −1 − Hxıt −1 t − 2 )
H ⋅ xt −1 + vt −1
Uncorrelated with vt
A ⋅ xt − 2 + wt − 2
Uncorrelated with vt
T
∴ E [ xı t t −1vt ] = 0
For the second term
xt = A ⋅ xt −1 + wt −1
Uncorrelated with vt
A ⋅ xt − 2 + wt − 2
T
Uncorrelated with vt
T
T
-∴ E[ xt vt ] = AE[ xt −1vt ] + E[ wt −1vt ] = 0
T
herefore
2
E[ε t vt ] = 0
T
(3
4)
Now note that the state xt has been driven by the process noise wt −1 , wt − 2 ,�, which
herefore, the second term vanishes:
are uncorrelated with the measurement noise vt . T
E[xt vtT ] = 0. In the first term, the previous state estimate xıt −1 is dependent upon the
previous process noise wt −2 , wt −3, �as well as on the previous measurement noise
vt −1 , vt −2 , � , both of which are uncorrelated with the current measurement noise vt .
T
herefore, the first term, too, vanishes. T
his leads to
E[εv
T ] = E[vε T ] = 0
(3
5)
Let us define the error covariance of a priori state estimation
Pt t −1 ≡ E[ε t ε tT ] = E[(xı
t t −1 − xt )(xıt t −1 − xt )T ]
(3
6)
.
5) and (3
Substituting (3
6) into (3
3
), we can conclude that the optimal gain must
satisfy
K t H t Pt t −1H tT + K t Rt − Pt t −1H tT = 0
∴
(3
7)
K t = Pt t −1H tT [H t Pt t −1H tT + Rt ]−1
(3
8)
T
his is called the Kalman Gain.
4.5.2 Updating the Error Covariance
T
he above Kalman gain contains the a priori error covariance Pt t −1 . T
his must be
updated recursively based on each new measurement and the state transition model.
Define the a posteriori state estimation error covariance
Pt = E[(xıt − xt )( xıt − xt )T ] = E[et etT ]
(3
9)
3
T
his covariance Pt can be computed in the same way as in the previous section. From
(25),
Pt = E[((I − KH )ε + Kv)((I − KH )ε + Kv)T ]
= E[(I − KH )εε T (I − KH )T ] + E[(I − KH )εvT K T ] + E[ Kvε T (I − KH )T ] + E[ KvvT K T ]
= (I − KH ) E[ε tε tT ](I − KH )T + KE[vvT ]K T
∴ Pt = ( I − KH ) Pt t −1 ( I − KH )T + KRt K T
(40
)
Substituting the Kalman gain (3)
8 into (40yields
)
Pt = (I − K t H t )Pt t −1
Exercise.
(41)
Derive (41)
uation
Furthermore, based on Pt we can compute
Pt +1 t by using the state transition eq
(8)
Consider
ε t +1 = xıt +1 t − xt +1
= At xıt − ( At xt + Gt wt )
(42)
= At et − Gt wt
From (3
6)
Pt +1 t = E[ε t +1ε tT+1 ]
= E[( At et − Gt wt )( At et − Gt wt )T ]
(43
)
= At E[et etT ]AtT − Gt E[ wt etT ]AtT − At E[et wtT ]GtT + Gt E[ wt wtT ]GtT
Evaluating E[ wt etT ]and
E[et wtT ]
E[et wtT ] = E[(xıt − xt )wtT ] = E[{xıt|t −1 + K t ( yt − yı t )}wtT ] − E[ xt wt
T ]
= E[ At−1 xıt−1wtT ] + E[
K t ( H t xt + vt )wtT ] − E[ K t H t xıt|t −1wtT ] − E[ xt wtT ]
(44)
= At−1E[ xıt−1wtT ] + ( K t H t − I ) E[ xt wtT ] + K t E[vt wtT ] − K t H t E[ xıt|t −1wtT ]
T
he first term: xıt −1 does not depend on wt , hence vanishes. For the second term,
using (8), we can write E[ xt wtT ] = E[( At−1 xt−1 + Gt−1wt−1 )wtT ] = 0 since E[ wt−1wtT ] = 0.
4
T
he third term vanishes since the process noise and measurement noise are not
correlated. T
he last term, too, vanishes, since xıt|t −1 does not include wt . T
herefore,
E
[
et wtT ] = E[wt etT ] = 0.
∴
Pt +1 t = At Pt AtT + Gt Qt GtT
(45)
4.5.3 The Recursive Calculation Procedure for the Discrete Kalman Filter
Initial Error Covariance P1 0 = P0, t = 1
Initial State
Estimate xı0
Measurement yt
Compute Kalman Gain
K t = Pt t −1H tT [ H t Pt t −1H tT + Rt ]−1
Update State Estimate with
new measurement
xıt t −1 = At −1 xıt −1
Update error covariance
Pt = ( I − K t H t ) Pt t −1
Pt +1 t = At Pt AtT + Gt Qt GtT
xıt = xıt t −1 + K t ( yt − H t xıt t −1 )
t ← t +1
State Estimate xıt
t ← t +1
On-Line or Off-Line
T
his does not depend on measurements
y1,y2,…yt,…
Real-T
ime
On-Line
5
4.6 Anatomy of the Discrete Kalman Filter
T
he Discrete Kalman Filter
Measurement:
yt = H t xt + vt
(9)
uared error
Minimizing the mean sq
T
J t = E[(xıt − xt ) (xıt − xt )]
(20
)
Uncorrelated measurement noise
t≠s
t=s
⎧ 0
E vt vsT = ⎨
⎩R t
[ ]
E[vt ] = 0,
Optimal Estimate
xı t = xı t t −1 + K t ( yt − yı t )
H t xı t
Noise Covariance
t −1
(23
)
Estimation output error
T
he Kalman Gain
(
K t = Pt t −1 H tT H t Pt t −1 H tT + Rt
)
−1
(3
8)
Error Covariance update (a priori to a posteriori):
Pt = (I − K t H t )Pt t −1
Δ
[
T
Pt = E (xıt − xt )( xıt − xt )
]
[
:a posteriori state estimation error covariance
Pt |t −1 = E (xıt |t −1 − xt )(xıt |t −1 − xt )
Δ
(41)
T
]
:a priori state estimation error covariance
Questions
Q1: How is the measurement noise covariance Rt used in the Kalman filter for
correcting (updating) the state estimate?
Rt … sensor q
uality
Q2:How is the state estimate error covariance Pt used for updating the state estimate?
8),
Post multiplying H
t Pt t −1H
tT + Rt to (3
(
)
K t H t Pt t −1 H tT + Rt = Pt t −1 H tT
6
From (41)
K t H t Pt t −1 = Pt t −1 − Pt
Pt t −1 H tT − Pt H tT + K t Rt = Pt t −1 H tT
∴ K t Rt = Pt H tT
(46)
T
he measurement noise covariance Rt is assumed to be non-singular,
K t = Pt H tT Rt−1
(47)
xı t =
xı t t −1 + Pt H tT Rt−1 Δy t
(48)
T
herefore
Q1. Without loss of generality, we can write
⎡σ 12
0⎤
⎥
⎢
2
σ2
⎥
Rt = ⎢
⎥
⎢
�
⎥
⎢
σ l2 ⎥⎦
⎢⎣ 0
⎡Δyt1 ⎤
⎢ � ⎥
⎥
Δyt = ⎢
⎢
⎥
⎢
⎥
⎣ Δytl ⎦
since if not diagonal we can change the coordinates.
⎡Δy t1 σ 12 ⎤
⎢
⎥
xı t = xı t t −1 + Pt H tT ⎢
�
⎥ (28)
⎢ Δy tl σ l2 ⎥
⎣
⎦
Depending on the measurement noise variance, σ i2 , the error correction term is
attenuated; Δy ti σ i2 . If the i-th sensor noise is large, i.e. large σ i2 , the error
correction based on that sensor is reduced.
Q2. By definition
[ ]
Pt = E et etT ; et = xıt − xt
7
Pt is the error covariance of a posteriori state estimation. Pt is interpreted as a
metric indicating the level of “expected confidence” in state estimation at the t-th
stage of correction.
Pt is large
n-dim
state
space
xtn
less confident
Large variance in this
direction � xı t t −1 is not
sure in this direction
Associated with matrix Pt , which is
λmax
⎛ xt1 ⎞
⎜ ⎟
xt = ⎜ � ⎟
⎜x ⎟
⎝ tn ⎠
positive-definite, we can consider an
ellipsoid with eigenvalues
More corrected
based on new data
λmin (Pt ) , λmax (Pt )
xı t = xı t t −1 + Pt Δq
xt 2
T −1
where Δq = H t Rt Δyt
λmin
xt1
Small variance � In average xıt t −1 is q
uite certain in this direction
No need to make a large change
l-dim
measurement
space
⎛ yt1 ⎞
⎜ ⎟
yt = ⎜ � ⎟ ∈ R �x1
⎜y ⎟
⎝ tl ⎠
�
ytl
λmax ( Rt )
uncertain measurement
(bad sensor) � Less correction
Rt−1 Δy t
yt 2
λmin : good sensor � dependable
y t1
T
he Kalman filter makes a clever trade-off between the intensity of sensor noise and
the confidence level of the state estimation that has been made up to the present
[ ]
time; Pt = E et etT .
8
How does the state estimation error covariance change over time?
Update Pt = Pt|t −1 − K t H t Pt|t −1
det(Pt )
det(Pt|t −1 )
Propagate
Pt +1|t = At Pt AtT + Gt Qt GtT
time
9