Linear-Optimal State Estimation!
Robert Stengel!
Optimal Control and Estimation MAE 546 !
Princeton University, 2017
Linear-optimal Gaussian estimator for discrete-time
system (Kalman filter)
2nd-order example
Alternative forms of the Kalman filter equations
Ill conditioning
Correlated inputs and measurement noise
Time-correlated measurement noise
Copyright 2017 by Robert Stengel. All rights reserved. For educational use only.
http://www.princeton.edu/~stengel/MAE546.html
http://www.princeton.edu/~stengel/OptConEst.html
1
Uncertain Linear, Time-Varying
(LTV) Dynamic Model
x k = !k"1x k"1 + # k"1u k"1 + $ k"1w k"1
z k = Hk x k + nk
dim ( w k ) = s ! 1
dim ( z k ) = r ! 1
•!
•!
Initial condition and disturbance inputs are not known precisely
Measurement of state is transformed and is subject to error
2
State Estimation
Actual System
•! Goals
–! Minimize effects of measurement error on knowledge of the state
–! Reconstruct full state from reduced measurement set (r " n)
–! Average redundant measurements (r # n) to estimate the full state
•! Method
–! Provide optimal balance between measurements and estimates
based on the dynamic model alone
3
Linear-Optimal !
State Estimation
•! Kalman filter is the optimal estimator for discrete-time
linear systems with Gaussian uncertainty
•! It has five equations
1)$
2)$
3)$
4)$
5)$
State estimate extrapolation
Covariance estimate extrapolation
Filter gain computation
State estimate update
Covariance estimate update
•! Notation
x̂ k ( ! ) : Estimate at k th instant before measurement update
x̂ k ( + ) : Estimate at k th instant after measurement update
4
Equations of the Kalman Filter
1) State estimate extrapolation (or propagation)
x̂ k ( ! ) = "k !1 x̂ k !1 ( + ) + # k !1u k !1
2) Covariance estimate extrapolation (or propagation)
Pk ( ! ) = "k !1 Pk !1 ( + ) "kT!1 + Qk !1
5
Equations of the Kalman Filter
3) Filter gain computation
K k = Pk ( ! ) H "# H k Pk ( ! ) H + R k $%
T
k
T
k
!1
4) State estimate update
x̂ k ( + ) = x̂ k ( ! ) + K k "# z k ! H k x̂ k ( ! ) $%
5) Covariance estimate update
Pk ( + ) = "# Pk!1 ( ! ) + HTk R !1
k Hk $
%
!1
6
Diagram of State Estimate
Fig. 4.3-2, OCE
[1]
[4]
7
Covariance Estimate and
Filter Gain Matrix
Fig. 4.3-3, OCE
[3]
[2]
[5]
8
Alternative Expressions for Kk
K k = Pk ( ! ) HTk "# H k Pk ( ! ) HTk + R k $%
I r = R !1
k Rk
Matrix
Identity
!1
T
= Pk ( ! ) HTk R !1
k Rk "
# H k Pk ( ! ) H k + R k $%
T
!1
K k = Pk ( ! ) HTk R !1
k "
# H k Pk ( ! ) H k R k + I r $%
!1
!1
T
!1
K k "# I r + H k Pk ( ! ) HTk R !1
k $
% = Pk ( ! ) H k R k
T
!1
K k = Pk ( ! ) HTk R !1
k ! K k H k Pk ( ! ) H k R k
K k = ( I n ! K k H k ) Pk ( ! ) HTk R !1
k
Multiply by
inverse
Subtract from
both sides
Collect terms
9
Alternative Expressions
for Kk and Pk(+)
•! From matrix inversion lemma
Pk ( + ) = "# Pk!1 ( ! ) + HTk R !1
k Hk $
%
!1
= Pk ( ! ) ! Pk ( ! ) HTk "# H k Pk ( ! ) HTk + R k $% H k Pk ( ! )
= ( I n ! K k H k ) Pk ( ! )
!1
•! This covariance update does not
inherently preserve symmetry
•! With Pk(+) known, estimation gain is
T
!1
K k = ( I n ! K k H k ) Pk ( ! ) HTk R !1
=
P
+
H
R
(
)
k
k
k
k
10
Second-Order Example
of Kalman Filter
•! Rolling motion of an airplane, continuous-time
# !!p & # L
%
(=% p
!
"
!
%$
(' %$ 1
# Lp
0 & # !p & # L) A &
(+%
(%
( !) A + %
0 (' %$ !" (' %$ 0 ('
%$ 0
&
( !pw
('
•! Rolling motion of an airplane, discrete-time
L T
#
ep
# !pk & % L T
%
( = % e p )1
!
"
%$
k (
' %
Lp
%$
(
)
0 &
( # !pk)1
(%
1 ( %$ !"k)1
('
# +11 0 & # !pk)1
(%
=%
%$ + 21 1 (' %$ !"k)1
& # ~L T
*A
(+%
0
(' %$
& # ,
(+% 1
(' %$ 0
# !p & # Roll rate, rad/s &
%
(=%
(
%$ !" (' %$ Roll angle, rad ('
!) A = Aileron deflection, rad
!pw = Turbulence disturbance, rad/s
# ~ L pT
&
( !* Ak)1 + %
0
('
%$
&
( !pwk)1
('
&
# -1 &
( !* Ak)1 + %
( !pwk)1
('
%$ 0 ('
T = sampling interval, s
11
Second-Order Example of
Kalman Filter
Rate and Angle Measurement
# !pM
%
%$ !" M
& # !p + !n p
( =%
(' %$ !" + !n"
k
&
( = I!x k + !n k
(
'k
1) State Estimate Extrapolation
$ !p̂ ( " )
k
&
& !#̂ k ( " )
%
' $ *
0 ' $ !p̂k "1 ( + )
11
)=&
)&
) & * 21 1 ) & !#̂ k "1 ( + )
(%
( %
' $ + '
) + & 1 ) !, Ak "1
) &% 0 )(
(
12
Second-Order Example of
Kalman Filter
2) Covariance Extrapolation
" p (!)
$ 11
$ p21 ( ! )
#
p12 ( ! ) % " (11 0 % " p11 ( + )
' =$
'$
p22 ( ! ) ' $# ( 21 1 '& $ p21 ( + )
&k
#
# L
Qk !1 " % p
%$ 0
&
( Q 'C # L p
$
('
p12 ( + ) % " (11 ( 21 % " ) 2p
' $
'+$
p22 ( + ) ' $# 0
1 '& $# 0
& k !1
0 %
'
0 '&
0 &
(
0 ('
# )2
0 &T = % p
'
%$ 0
3) Gain Computation
! k11
#
#" k21
k12
k22
$ ! p11 ( ' )
& =#
&% #" p21 ( ' )
k
*
p12 ( ' ) $ , ! p11 ( ' )
& +#
p22 ( ' ) & , # p21 ( ' )
%k -"
$ "2
pM
R k! jk = &
& 0
%
0
" #2M
2
p12 ( ' ) $ ! ( pM
#
& +
p22 ( ' ) & # 0
%k "
0
( )2M
'
)
)
(k
$ .
& ,/
& ,
%k 0
'1
13
Second-Order Example of
Kalman Filter
4) State Estimate Update
# !p̂ ( + ) & # !p̂ ( ) ) & # k
k
k
%
(=%
( + % 11
% !"ˆk ( + ) ( % !"ˆk ( ) ) ( % k21
$
' $
' $
k12 & *, # !pM k
( +%
k22 ( , % !" M k
'k -$
& # !p̂k ( ) ) & .
( ,/
()%
( % !"ˆk ( ) ) ( ,
' $
'0
5) Covariance Update
! p (+)
# 11
# p21 ( + )
"
p12 ( + ) $
&
p22 ( + ) &
%k
*
,
, ! p11 ( ' )
= +#
, #" p21 ( ' )
,
-
p12 ( ' ) $
&
p22 ( ' ) &
%k
! 1
# 2
# ( pM
+#
# 0
#
"
0
1
( )2M
$ .
& ,
& ,
& /
& ,
& ,
%k 0
'1
14
Example: Propagating a Scalar
Probability Density Function
Scalar LTI system with zero-mean Gaussian random input
xk +1 = bxk + 1 ! b 2 uk + 1 ! b 2 wk , x0 given
Propagation of the mean value
xi +1 = bxi + 1 ! b 2 ui , x0 given
Propagation of the variance
(
)
Pk +1 = b 2 Pk + 1 ! b 2 Qk , P0 given
( )
Qk = E wk2
15
Example: System Response to
Small Disturbance
Large estimate of initial uncertainty
16
Example of Kalman Filter Estimation
Q = 0.001
R=1
!!
!!
!!
Low disturbance input
High measurement error
State estimate emphasizes
system model
17
Example: System Response to
Larger Disturbance
18
Example of Kalman Filter Estimation
Q = 0.01
R = 0.1
!!
!!
!!
Moderate disturbance input
Low measurement error
State estimate emphasizes
measurement
19
Linear-Optimal Predictor
t k : Current time, sec
t K : Future time, sec
•! State estimate propagation
from last estimate
x̂ K = ! ( t K " t k ) x̂ k ( + ) + # ( t K " t k ) u k
•! Covariance estimate propagation
PK = ! ( t K " t k ) Pk ( + ) ! T ( t K " t k ) + Qk ( t K " t k )
•! Predictor analogous to Kalman filter without
measurement
20
Prediction
Depends Entirely
on the Model
•! Bad model – bad prediction
•! High disturbance covariance – bad
prediction
•! Prediction of mean might seem good
•! … but if covariance grows, uncertainty may
render prediction meaningless
21
Alternative Forms of the
Kalman Filter!
22
Simplifying the Gain Calculation
Filter gain computation for r measurements
K k = Pk ( ! ) HTk "# H k Pk ( ! ) HTk + R k $%
!1
r x r inversion required
For r = 1
Pk ( ! ) HTk
Kk =
H k Pk ( ! ) HTk + rk
scalar division
23
Consider One Measurement at
Each Sampling Interval
•! With r measurements and diagonal R, consider
just one measurement at a time
!
#
z = ##
#
#"
!
z1 $
&
#
z2 &
; R = ##
&
!
&
#
zr &
#"
%
!
0 $
&
#
0 r22 ! 0 &
; H = ##
&
! ! ! !
&
#
0 0 ! rrr &
#"
%
r11
0
!
H1 $
&
H2 &
! &
&
Hr &
%
z k +1 = z1
z k + 2 = z2
!
z k + r = zr
Scalar Update
x̂ k ( ! ) = "k!1 x̂ k!1 ( + ) + # k!1u k!1
x̂ k ( + ) = x̂ k ( ! ) + Kik $% z ik ! Hik x̂ k ( ! ) &'
Kik =
Pk ( ! ) HTik
Hik Pk ( ! ) HTik + rik
•! Cycle through all measurements varying H, and
repeat cycle
•! Wasteful, as it may not use all available information
24
Sequential Processing of Measurements
at Each Sampling Interval
•! With r measurements, form an
inner loop of calculations,
processing one sample at a time
for i = 1, r
Kik = Pi !1k ( + ) HTik "# Hik Pi !1k ( + ) HTik + rik $%
(
z1k = z1
z 2 k = z2
!
z rk = zr
)
Pik ( + ) = I n ! Kik Hik Pi !1k ( + ) , P0 k ( + ) = Pk ( ! )
x̂ ik ( + ) = x̂ i !1k ( + ) + Kik "# z ik ! Hik x̂ i !1k ( + ) $%
end
x̂ k ( + ) = x̂ rk ( + )
Pk ( + ) = Prk ( + )
H1k = H1
H2k = H2
!
H rk = H r
Joseph Form of the Filter
(Stabilized Kalman Filter
)
Guaranteed to retain positivedefiniteness and symmetry
State update is
x̂ k ( + ) = [ I n ! K k H k ] x̂ k ( ! ) + K k z k
Pre- and post-update measurement errors
! k ( " ) = x k " x̂ k ( " ); ! k ( + ) = x k " x̂ k ( + )
Measurement error is updated by
! k ( + ) = [ In " K k H k ] ! k ( " ) + K k n k
26
25
Joseph Form of the Filter
(Stabilized Kalman Filter
)
! k ( + ) = [ In " K k H k ] ! k ( " ) + K k n k
(
)
Definitions
(
)
(
)
E ! k ! Tk = Pk ; E n k nTk = R k ; E ! k nTk = 0
Then, covariance update is the outer product of
the expected error
Pk ( + ) = [ I n ! K k H k ] Pk ( ! )[ I n ! K k H k ] + K k R k KTk
T
•! Update equation is symmetric
•! Equation updates covariance whether or not K is optimal
–! Evaluate error covariance of a sub-optimal filter
–! Design and evaluate a low-order filter for a high-order system
•! Does not require an (n x n) inversion
27
Information Matrix Form
of the Kalman Filter
•! Filter based on the inverse of P
P : State error covariance (small is good)
I = P!1 : Information matrix (large is good)
•! Fewer continuing inversions
Pk ( + ) = "# Pk!1 ( ! ) + HTk R !1
k Hk $
%
!1
I k ( + ) = I k ( ! ) + HTk R !1
k Hk
28
Information Matrix Propagation
and Gain Matrix
I k ( ! ) = #$ I n ! B k!1 " k!1T %& A k!1 ,
I 0 = P0 !1
where
A k!1 = " k!1!T I k!1 ( + ) " k!1!1
B k!1 = A k!1 $% #
T
k!1
A k!1 # k!1 + Q'k!1 &'
!1 !1
and
K k = I k !1 ( + ) HTk R k !1
29
Ill Conditioning of the Filter
Computation
•! Calculations may be inaccurate if
system contains
–! very fast and slow modes
–! very noisy and near-perfect measurements
–! very large and small disturbance inputs
•! Condition number of a matrix, P, is the
ratio of singular values
(
(
)
)
1/2
" !max PT P %
k ( P) = $
'
T
!
P
P
'&
$# min
=
( max ( P) ( ( P)
=
( min ( P) ( ( P)
30
Solutions to Ill Conditioning
•! Double, triple, ..., precision arithmetic, or
•! Formulate the equations to solve for the
square root of P
•! Define
P ! SST or ST S
then k ( P ) = k ( SST ) = k 2 ( S ) , which is order 10 x
k ( S ) = k ( P) , which is order 10 x/2
31
U-D Formulation of the Kalman Filter
Factorization of P
P = UDU T
! ( UD1/2 ) ( UD1/2 )
T
where
! SST
! 1 " " $
U : Unit upper triangular matrix: # 0 1 " &
#
&
#" 0 0 1 &%
! " 0 0 $
D : Diagonal matrix: # 0 " 0 &
#
&
#" 0 0 " &%
•! No square roots in the factorization, but formulation has squareroot conditioning
•! Covariance update uses sequential processing
•! Algorithm originally expressed in pseudo-code (Bierman and
Thornton, 1977)
•! See OCE (pp. 357-360) for equations and pseudo-code
32
Kalman Filters for More
Complex Systems*
•! Correlated Disturbance Input and Measurement Error
–! Measurements may be corrupted by the same processes that force
the system (e.g., turbulence)
–! Consider estimation-error dynamics, as in Joseph form
–! Stepwise minimization of estimation cost function w.r.t. filter gain
matrix
–! See OCE
•! Time-Correlated (Colored
) Measurement Error
–!
–!
–!
–!
Augment system with measurement error dynamics
Use measurement differencing
Two-step state and covariance estimates
See OCE
* Arthur Bryson and students
33
Next Time:!
Kalman-Bucy Filters for
Continuous-Time Systems!
34
!"##$%&%'()$**
+)(%,-)$*
35
Descriptions of
Random Variables
T
E ( x 0 ) = x̂ o ; E "( x 0 ! x̂ o ) ( x 0 ! x̂ o ) $ = P0
#
%
(
)
E ( w k ) = 0; E w j w Tk = Q'k ! jk
{
E [ u k ] = u k ; E [ u k ! u k ][ u k ! u k ]
T
(
}=0
)
E ( n k ) = 0; E n j nTk = R k! jk
(
)
E w j nTk = 0
36
Program for Covariance Propagation and
Kalman Filter Estimatation
%
%
%
%
%
Scalar Propagation of Mean and Variance
Kalman Filter
Copyright by Robert Stengel. All rights reserved.
4/12/2011
xm(1)
xp(1)
Pm(1)
Pp(1)
z(1)
Q
R
u
clear
'=========================='
'Propagation of Uncertainty'
date
b
b2
bb
sqrb
w
x
xbar
=
=
=
=
=
=
=
0.99
b*b
1 - b^2
sqrt(bb)
[];
[];
[];
x(1)
xbar(1)
P(1)
Q
u
=
=
=
=
=
1
x(1)
3
0.01
0
for
end
k =
1:999
w(k)
=
x(k+1)
=
xbar(k+1)
=
P(k+1)
=
k
=
w(1000) =
Kalman Filter
'============='
'Kalman Filter'
for
end
=
=
=
=
=
=
=
=
k =
n(k)
xm(k+1)
Pm(k+1)
K
z(k+1)
xp(k+1)
Pp(k+1)
0
0
1
1
xm(1)
Q
0.1
0
1:999
=
sqrt(R)*randn(1);
=
b*xp(k) + sqrb*u;
=
b2*Pp(k) + Q;
=
Pm(k+1) / (Pm(k+1) + R);
=
x(k+1) + n(k);
=
xm(k+1) + K*(z(k+1) - xm(k+1));
=
1 / ((1/Pm(k+1)) + 1/R);
k
=
[1:1000];
n(1000) =
sqrt(R)*randn(1);
figure
plot(k,z,'c',k,(xp + sqrt(Pp)),'g',k,(xp sqrt(Pp)),'g',k,xp,'r',k,x,'b'),grid
title(['Kalman Filter, Q =:',num2str(Q),', R =:',num2str(R)]),
xlabel('Sampling Index, k'), ylabel('x, xp, xp ± sqrt(Pp)')
legend('Measurement','Estimate + Sigma','Estimate Sigma','Estimate','Actual')
sqrt(Q)*randn(1);
b*x(k) + sqrb*u + sqrb*w(k);
b*xbar(k) + sqrb*u;
b2*P(k) + bb*Q;
%
[1:1000];
sqrt(Q)*randn(1);
figure
plot(k,w,'c',k,x,'b',k,(xbar + sqrt(P)),'k',k,(xbar sqrt(P)),'k',k,xbar,'r'),grid
title(['Propagation of Uncertainty, b =:', num2str(b), ', Q
=:',num2str(Q)]), xlabel('Sampling Index, k'), ylabel('x, xbar, xbar
± sqrt(P)')
legend('Disturbance','State','Mean + Sigma','Mean Sigma','Mean')
Covariance Comparison
figure
SP =
sqrt(P);
SPp =
sqrt(Pp);
plot(k,n,'c',k,w,'g',k,SP,'b',k,SPp,'r'), grid
title(['Covariance Comparison, Q =:',num2str(Q),', R =:'
num2str(R)]), xlabel('Sampling Index, k'),ylabel('w, n, Sqrt(P),
Sqrt(Pp)')
legend('Measurement Error','Disturbance','Estimate of Actual
State Std Dev','Estimate Error Std Dev')
37
Example of Kalman Filter Estimation
Q = 0.001
R=1
Dist./Meas.
Legends reversed
38
Example of Kalman Filter Estimation
Legends reversed
Q = 0.01
R = 0.1
39
Example of Kalman Filter Estimation
Q = 0.01
R=1
!! Moderate disturbance input
!! High measurement error
40