Robotics Research Laboratory 1 Chapter 7 Multivariable and Optimal Control
Dec 13, 2015
Robotics Research Labo-ratory
2
Time-Varying Optimal Control- deterministic systems
0 1 12
2
( ) ( ) ( ) time-varying gain
Notes : ( ) ( ) constant control gain in pole
So
placement
0, 0, 0 (non-negative definite matrix)
0 (posit
lution
iv
u k K k x k
u k Kx k
Q Q Q
Q
e definite matrix)
1
1 12 2 00
- a quadratic form
1 1 minimize ( ) ( ) 2 ( ) ( )
Cost function (performance ind
( ) ( ) ( ) ( )2 2
ex)
Constra
NT T T T
k
J x k Q x k x k Q u k u k Q u k x N Q x N
subject to given system ( 1) ( ) (
int
)x k Φx k Γu k
Robotics Research Labo-ratory
3
1 20
1 20
1 12 2
Cost functions
1 minimize ( ) ( ) ( ) ( )
2
1 minimize ( ) ( ) ( ) ( )
2
Stochastic systems
1 minimize ( ) ( ) 2 ( ) ( ) ( ) ( )
2
NT T
k
T T
k
T T T
k
J x k Q x k u k Q u k
J x k Q x k u k Q u k
J E x k Q x k x k Q u k u k Q u k
0
1 12 2
1
or minimize ( ) ( ) 2 ( ) ( ) ( ) ( )
subject to given system ( 1) ( ) ( ) ( )
where is white noise with ( )
N
T T T
T
J E x k Q x k x k Q u k u k Q u k
x k Φx k Γu k v k
v E vv R
Robotics Research Labo-ratory
4
1 20
1Minimize
2
subject to ( 1) ( ) ( ) 0; 1,2,3, .... ,
NT T
k
J x Q x u Q u
x k Φx k Γu k k N
LQ problem (Linear Quadratic)–Finite time problem
Using Lagrange multipliers
'1 2
0
'
1 1( ) ( ) ( ) ( ) ( 1) ( 1) ( ) ( )
2 2
Find the minimum of with respect to ( ), ( ), ( )
NT T T
k
J x k Q x k u k Q u k λ k x k Φx k Γu k
J x k u k λ k
'
2
'
'
1
0 ( ) ( 1) 0 ; control eq.( )
0 ( 1) ( ) ( ) 0 ; state eq.( 1)
0 ( ) ( ) ( 1) 0 ; adjoint(costate) eq.( )
T T
T T T
Ju k Q λ k Γ
u k
Jx k Φx k Γu k
λ k
Jx k Q λ k λ k Φ
x k
Robotics Research Labo-ratory
5
2
1
1
( 1) ( ) ( ) (0) is given
(
( ) ( 1) (1)
(2) .
1) ( ) ( ) (0) is not kn(3) . ownT T
T
x k Φx k Γu k x
λ k
u k Q Γ λ k
Φ λ k Φ Q λx k
( ) ( ), ( )
.
Because has no effect on should be zero in order
to minimize
u N x N u N
J
2
1
( ) ( 1) 0
( 1) 0
(
( ) ( 1)
( ) ( 1
( ) ( )
)
4)
Two Points Boundary Value Problem (TPBVP)
T T
x k x k
λ
λ N
k λ k
u N Q λ
N
Q x
N
λ
N
Γ
Robotics Research Labo-ratory
6
2
1
2
1
2
Assume that (5)
( ) ( 1) ( 1)
( 1) ( ) ( )
( ) ( 1) ( 1) ( )
( 1) ( ) (6)
where (7)
(5
( ) ( ) ( )
) (3)
( 1)
( )
T
T
T T
T
T
Q u k Γ S k x k
Γ S k Φx k Γu k
u k Q Γ S k Γ Γ S k Φx k
R Γ S k Φx k
λ k
λ k S k x k
R Q Γ S k Γ
Φ
1
1
1
11
( 1) ( )
( ) ( ) ( 1) ( 1) ( )
( 1) ( ) ( ) ( )
( 1) ( ) ( 1) ( ) ( )
T
T
T
T T
λ k Q x k
S k x k Φ S k x k Q x k
Φ S k Φx k Γu k Q x k
Φ S k Φx k ΓR Γ S k Φx k Q x k
Robotics Research Labo-ratory
7
11
11
11
1
( ) ( 1) ( 1)
( ) ( 1) ( 1
( ) ( ) ( ) ( )
( )
discrete
) ( 1) 0
The above equation i
(
s c
)
Riccati
( 1) ( 1) ( 1)
( )
ealled t quahe
T T
T T
T T
T
T
S k Φ S k Φ ΓR Γ S k Φ Q
S k Φ S k Φ Φ S k ΓR Γ S k Φ Q
S k Φ S k Φ Φ S k ΓR Γ S k Φ Q
S
x k x k x k x
Q
k
N
x k
1
-1
1 2 3
1
1
2
( ) ( 1)
( 1) ( 1) - ( 1) ( 1),
.
or
where
( ) ( )
( )
Note: Jacopo Francesco Riccat
tion
( 1) ( 1
i (1676 - 1754)
( )
( )
)
( )
)
(
T
T
T TQ Γ S k Γ Γ
S k Φ M k Φ Q
M k S k S k ΓR Γ S k S N
S k Φu k x k
x k
dyq x q x y q
dx
k
Q
K
2( )x y
Robotics Research Labo-ratory
8
1
1
2
1
2
1
Procedure ( . 367)
1. ( ) and ( ) 0 since ( 1) 0
2. Let
3. Let ( ) ( ) ( ) ( ) ( )
4. Let ( 1) ( ) ( )
5. Store ( 1)
6. Let ( 1) ( )
7.
T T
T T
T
p
S N Q K N S N
k N
M k S k S k Γ Q Γ S k Γ Γ S k
K k Q Γ S k Γ Γ S k Φ
K k
S k Φ M k Φ Q
Let 1
8. Go to 3
k k
Robotics Research Labo-ratory
9
0 5 10 15 20 25 30 35 40 45 500
2
4
6
8
10
K t
heta
k
Control gains vs. time
Q2 = 1.0
Q2 = 0.1
Q2 = 0.01
0 5 10 15 20 25 30 35 40 45 500
1
2
3
4
5
K t
heta
dot
k
Q2 = 1.0
Q2 = 0.1
Q2 = 0.01
Robotics Research Labo-ratory
10
'1 2
0
1 2 10
2
Remark:
1( ) ( ) ( ) ( ) ( 1)( ( 1) ( ) ( ))
2
1 ( ) ( ) ( ) ( ) ( 1) ( 1) ( ( ) ( ) ) ( )
2
( ( ) ) ( )
1 ( ) ( ) ( 1) (
2
NT T T
k
NT T T T T
k
T
T T
J x k Q x k u k Q u k λ k x k Φx k Γu k
x k Q x k u k Q u k λ k x k λ k x k Q x k
u k Q u k
λ k x k λ k x
0
'
1)
1 (0) (0) ( 1) ( 1)
2
Since ( 1) 0,
1
1(0) (0) (0(0) (0)
2)
2
N
k
T T
T T
k
λ x λ
x
N
S x
x N
λ N
J J λ x
Robotics Research Labo-ratory
11
LQR (Linear Quadratic Regulator)-Infinite time problem
ARE(Algebraic Riccati Equation)– analytic solution is impossible in most cases.– numerical solution is required.
12
12 1
1 12 1 2
1
( 1) ( ) ( ) ( ) ( 1)
( ) ( ) ( )
( ) ( )
( 1) ( ) ( )
T
T T T
T T T T
T T
x k Φx k Γu k Φx k Γ Q Γ λ k
Φx k Γ Q Γ Φ λ k Φ Q x k
Φ ΓQ Γ Φ Q x k ΓQ Γ Φ λ k
λ k Φ Q x k Φ λ k
11 1
1 11 1
( ) ( 1) ( 1) ( 1) , ( )
IT T T T
T T
S Φ S S ΓR Γ S Φ Q Φ S Γ
S k Φ S k S k ΓR Γ S k Φ Q
R Γ S Φ
S N
Q
Q
Robotics Research Labo-ratory
12
1 2 20
Consider ( 1) ( ) ( ) and the performance index is
1given by ( ) ( ) ( ) ( ) , Q >0.
2
Assume that a positive-definite
T T
k
x k Φx k Γu k
J x k Q x k u k Q u k
Stability of the Closed - loop System (LQ controller)
steady-state solution of ARE exists.
Then the steady-state optimal solution law ( ) ( ) gives
an (closed -loop system 1) ( ) ( ).
Note:
In LQ controller, the poles
asymptotically sta
are
ble
u k Kx k
x k Φ ΓK x k
obtained from det( ) 0.
And the poles are the stable eigenvalues of the generalized
eigenvalue problem. Euler equation of LQ problem
λI Φ ΓK
n
Robotics Research Labo-ratory
13
1 12 1 2
1
How to obtain ( ) ( ) from ARE?
From the state equation and costate equation, we have
( 1) ( )
( 1) ( )
These equations are called Hamilton
T T T T
T T
u k K x k
x k x kΦ ΓQ Γ Φ Q ΓQ Γ Φ
λ k λ kΦ Q Φ
1 12 1 2
1 2 2
's equations
or the Euler equations.
is called a Hamiltonian matrix (constant matrix).
How to obtain ( ) ( ) from ?
T T T T
c T T
n n
c
c
Φ ΓQ Γ Φ Q ΓQ Γ ΦH
Φ Q Φ
H
u k K x k H
Robotics Research Labo-ratory
14
12
1
12
11
12
11
12
1 1 11 2
( ) ( ) ( )
( ) ( )
( ) ( ) ( )
( ) 0
( ) 0
det 0
det 00 ( )
T
T
T
T
T
T
T
T T
zX z ΦX z ΓU z
ΦX z Γ zQ Γ Λ z
Λ z Q X z zΦ Λ z
X zzI Φ ΓQ Γ
zΛ zQ z I Φ
zI Φ ΓQ Γ
Q z I Φ
zI Φ ΓQ Γ
z I Φ Q zI Φ ΓQ Γ
Remark : Using reciprocal root properties in p372
Robotics Research Labo-ratory
15
1 1 11 2
1 1 1 1 11 2
1
2
1
1
11 1
1
det( )det ( ) 0
det( )det ( ) ( ) ( ) 0
Let det( ) ( ), det( ) ( )
( ) ( )det ( ) 0
where a d
( )
n
T T
T T
T T T
T
T
T
zI Φ z I Φ Q zI Φ ΓQ Γ
zI Φ z I Φ I z I Φ Q zI Φ ΓQ Γ
zI Φ a z z I Φ a z
a z a z I ρ H zI Φ ΓΓ
Q
z I Φ H
ρH H ΓQ
1
1
1 1 1
Using the property of det
( )
( ) det( )
( ) ( )det 1 ( ) 0T T
T T
n m
T
Γ ΓΓ
I BA I AB
a z a z ρH zI Φ ΓΓ z I Φ H
Robotics Research Labo-ratory
16
-1.5 -1 -0.5 0 0.5 1 1.5-1.5
-1
-0.5
0
0.5
1
1.5
Real Axis
Imag
Axi
s
Symmetric root locus
Robotics Research Labo-ratory
17
1 12 1 2
1
How to obtain ( ) ( ) from ARE?
From the state equation and costate equation, we have
( 1) ( )
( 1) ( )
These equations are called Hamilton
T T T T
T T
u k K x k
x k x kΦ ΓQ Γ Φ Q ΓQ Γ Φ
λ k λ kΦ Q Φ
1 12 1 2
1 2 2
's equations
or the Euler equations.
is called a Hamiltonian matrix (constant matrix).
How to obtain ( ) ( ) from ?
T T T T
c T T
n n
c
c
Φ ΓQ Γ Φ Q ΓQ Γ ΦH
Φ Q Φ
H
u k K x k H
Robotics Research Labo-ratory
18
Eigenvector Decomposition
1
Recall: Coordinate change by similarity transformation
For ( 1) ( ),
( ) ( ), ( 1) ( 1)
The above state equation becomes
( 1) ( )
( 1) ( )
Let
x k Ax k
x k Mξ k x k Mξ k
Mξ k AMξ k
ξ k M AMξ k
Λ M
1 . ( 1) ( ).
In order to make a diagonalized matrix,
should consist of eigenvectors of .
AM ξ k Λξ k
Λ
M A
Robotics Research Labo-ratory
19
1*
* 1
0
0
where is a similarity transformation matrix of eigenvectors of .
c c
c c
c
EH H
E
H W H W
W H
0
0
I
I
X XW
Λ Λ
inside the unit circle
outside the unit circle
*1
*
* *0
* *0
* *
* *0
i.e.,
0 = from Hamilton's equations.
0
I
I
N
NN
xxW
λλ
X Xx x xW
Λ Λλ λ λ
x E x
λ E λ
Robotics Research Labo-ratory
20
* *
* *
* *
* 1
*
1
*
As goes to infinity, ( ) (0) 0 and ( ) (0)
Only sensible solution (0) 0, therefore ( ) 0
( ) ( ) (0)
(0) ( )
( ) ( ) (0)
( ) ( ) ( )
is
N N
k
I
I I
kI
kI I
I
N x N E x λ N E λ
λ λ k k
x k X x k X E x
x
Λ X S
E X x k
λ k Λ x k Λ E x
λ k x k x k
S
@
12( ) ( ), whe
re ( )
1Remark: (
the steady-state solution of ARE
0) (
.
0)2
T
T
u k K x k K Q Γ S Γ S Φ
J x S x
Robotics Research Labo-ratory
21
1 2
Procedure ( . 377)
1. Compute eigenvalues of .
2. Compute eingenvectors associated with the stable eigenvalues of .
3. Compute control gain with .
Riccati equation
( ) ( ) (
c
c
p
H
H
K S
y t q t q
23
2 2
22
) ( )
)
( ) 1 2 , ( )
1 1 ( ) , ( )
p
p
t y q t y
ex
y t t ty y y t t
yy t y y t
t tt
Robotics Research Labo-ratory
22
12 1
12 1
1
continuous-time ( ) ( ) ( ) ( ) ( )
discrete-time ( 1) ( ) ( 1)
Matrix Riccati equation
( 1
T
T
T T T
A S t S t A S t ΓQ ΓS t S t Q
A S S A S ΓQ ΓS Q
Φ S k Φ S k Φ S k ΓR Γ S k
1
T 11
T
Lyapunov equatio
) ,
continuous-time
discrete-time
n
T T
T
Φ Q
Φ S Φ S Φ S ΓR Γ S Φ Q
A P PA Q
Φ PΦ P Q
Robotics Research Labo-ratory
23
1 20
1 1
2 2
1 2
T 11
1ex) Minimize ( ) ( ) ( ) ( )
2
( 1) ( )1 1 1 subject to ( )
( 1) ( )1 0 0
where I and 1
o
T
k
T
J x k Q x k u k Q u k
x k x ku k
x k x k
Q Q
S Φ S S ΓR Γ S Φ Q
T1
1T 11
1T 12 1
2
1r I
By matrix inversion lemma
I ( )
i.e., I
since
Note:
T T
T
T
T
S Φ S Φ Q
S Φ S Γ R Γ S Γ Γ S Φ Q
S Φ S ΓQ Γ S Φ Q
R Q Γ S Γ
Γ SRΓ
-1 1 1 1 1 1 1 ( ) ( )
A BCD A A B C DA B DA
Robotics Research Labo-ratory
24
1 11 2
11 12 11 12 11 12
12 22 12 22 12 22
12 11 11 12
1
( )
1 0 0 1 1 0 1 1 11 1 0
0 1 1 1 0 1 0 1 0
(1 )
T TS Q Φ I ΓQ Γ S Φ S
s s s s s s
s s s s s s
s s s s
s
22 11 12 11 22
22 11 11
22 12 12 22 12
1
( 1)(1 )
( 1) 1
3.7913 1.0000
1.0000 1.7913
s s s s
s s s
s s s s s
S
Robotics Research Labo-ratory
25
1 102 1 2
01 2 2
,
1 2 0 1
1 0 0 0
0 1 0 1
1 1 1 1
Eigenvectors
0.3548
T T T TI
c T TIn n
c
X XΦ ΓQ Γ Φ Q ΓQ Γ ΦH W
Λ ΛΦ Q Φ
H
W
Same problem but different approach
0.8463 0.2433 0.1233
0.1621 0.3866 0.5326 0.2699
0.4429 0.2830 0.3899 0.7374
0.8073 0.2329 0.7107 0.6068
Robotics Research Labo-ratory
26
*
1
1
Corresponding eigenvalues
2.1889 0 0 0
0 2.1889 0 0
0 0 0.4569 0
0 0 0 0.4569
0.3899 0.7374 0.2433 0.1233
0.7107 0.6068 0.5326 0.2699
3.7913 1.
c
I I
H
S Λ X
0000
1.0000 1.7913
Robotics Research Labo-ratory
27
Cost Equivalents
1 20
1 ( 1)
1 20
Analog cost function (Refer . 580 in Ogata's)
1 ( ) ( ) ( ) ( )
21
( ) ( ) ( ) ( )2
Because ( ) ( ) ( ) ( ) ( )
where ( )= , (
NT T Tc c c
N k T T Tc ckT
k
Fτ
p
J x t Q x t u t Q u t dt
x τ Q x τ u τ Q u τ dτ
x kT τ Φ τ x kT Γ τ u kT
Φ τ e Γ τ
0
111 12
0 21 22
111 12
0221 22
)
( )1 ( ) ( )
( )2
0 ( ) ( )( ) 0where
0 0( )
τ Fη
NT T
ck
TT c
Tc
e dηG
Q Q x kJ x k u k
Q Q u k
QQ Q Φ τ Γ τΦ τdτ
QQ Q IΓ τ I
Robotics Research Labo-ratory
28
1 111 12 22 21 22 21
1
220
Remarks :
i) Cross terms that weight the product of and
ˆii) Define and ( ) ( ) ( )
1 ˆ ( ) ( ) ( ) ( )2
subject to ( 1) ( )
NT T
k
x u
Q Q Q Q Q v k Q Q x k u k
J x k Qx k v k Q v k
x k Φx k Γ v
122 21
122 21
( ) ( )
( ) ( )
ˆ ( ) ( )
k Q Q x k
Φ ΓQ Q x k Γv k
Φx k Γv k
Robotics Research Labo-ratory
29
Least Squares Estimation
y Hx v
( ) ( )1 1
2 2T TJ v v y Hx y Hx
p1 measurement vector
p1 measurement error vector
pn matrixn1 unknown
vector
1
minimize to determine the best estimate of
given the measurements .
( ) ( ) 0
ˆ
ˆ is a best estimate of
T
T T
T T
J x
y
Jy Hx H
x
H y H Hx
x H H H y
x x
Robotics Research Labo-ratory
30
1 1 1
1
1
ˆ ( )
ˆ
ˆIf is zero mean, is an unbiased estimate (zero mean).
The covariance of the estimate error
ˆ ˆ
( ) (
T T T T T T
T T
T
T T T T
x H H H Hx v H H H H x H H H v
x x H H H v
v x x
P E x x x x
H H H E vv H H
1
2
1 2
)
If ,
( )
T
T
H
E vv Iσ
P H H σ
Robotics Research Labo-ratory
31
)
. . . . . . . . . . . .
20 1 2
21 11 1
202 22 2
213 33 3
2
ex Least- sqaures estimation
0 2 0 5 1 1 1 2 1 1 1 3 1 1 1 2 2 0 1 2 2 2 4 0
1
1
1
T
i i i i
y
y a a t a t v
y vt t
ay vt t
ay vt t
a
ˆ . . .
21 1
202 2
213 3
2
1
1
where and 1
0 7432 0 0943 0 0239T
t t
at t
H x at t
a
x
Robotics Research Labo-ratory
32
0 5 10 15 20 250
2
4
6
8
10
12
14Sales fit and prediction
Sal
es (
$100
0)
Months
. . . 20 7432 0 0943 0 0239y t t
Robotics Research Labo-ratory
33
Weighted Least Squares
ˆ1
1
2
1
2
From the previous result,
Note:
1. The covariance is used in weighting matrix, i.e.,( ) .
2. Covariance indicates the degree of uncertainty of measurem
T
T T
i
J v Wv
x H WH H Wy
W R
R σ I
ˆ
ˆ
11 1
11
ent error.
3.
is a best linear unbiased estimate.
4. The covariance of the estimate error is
T T
T
x H R H H R y
x
P H R H
Robotics Research Labo-ratory
34
Recursive Least Square
1 1
1 1
1 1
1 1
: old data, : new data
0 0ˆ
0 0
ˆ (old data only)
( )
o o o
n n n
T T
o o o oo o
n n n nn n
T To o o o o o o
T To o o n n n
y H Vx
y H V
o n
H H H yR Rx
H H H yR R
H R H x H R y
H R H H R H
1 1
1 1 1 1
1
ˆ + (old data and new data)
ˆ ˆ ˆLet
ˆ ˆ
where terms are cancelled out.
T To o o n n n
o
T T T Tn n n o o o o n n n n n n
To o o
x H R y H R y
x x δx
H R H x H R H H R H δx H R y
H R y
Robotics Research Labo-ratory
35
ˆ ˆ
ˆ
11 1 1
11 1 1
T T To o o n n n n n n n o
T To n n n n n n n o
δx H R H H R H H R y H x
P H R H H R y H x
11 1Define
Tn o n n nP P H R H
ˆ ˆ ˆ1To n n n n n ox x P H R y H x
old esti-mate
old estimatenew estimate
covariance of old esti-mate
Robotics Research Labo-ratory
36
?
:
11 1 1 1 1 1
1 11 1
1
1
1
Remarks :
i) How to calculte
- Matrix Inversion Lemma in Appendi
N
C
o
x
te
n
T T To n n
T
n o o n n n
Tn o o n n n o
o n n o
To
n n o
n nn
P
P H R H P P H R H P H H P
P H R
A BCD A A B C DA B
P P P
DA
P H H P
P
H R H
( ) ( ) (
( )
) ( )
)
)
(
(
1
1 11 1
11
1
1
ii) Comput
Note:
ation complexity
1
1
1 1 1
n
T To n n n n n o n
T T
Tn n n
P k P k
H
P H R H R H P H
PP
P k H HP k H R HP
k H R Hk
k
Sometime, it is a scalar. That is if we use just one new information.
Robotics Research Labo-ratory
37
Stochastic Models of DisturbanceWe have dealt with well-known well-defined, ideal systems.
- disturbance (process, load variation)- measurement noise
0real line
new (range) sample space
sample point in sample space(event)
s
iX
( )iX s
Some sample space and a probability distribution defined on
events in that sample space are give A single-valued real function
is then defined on the sample space so that to each
n.
point of
X
S s
S P
( ).
therefore corresponds a single real number
The function is called a random variable.
S
x X s
X
Robotics Research Labo-ratory
38
) : head-indicator function
- at least one head occurs in the two independent flips
of an unbiased coin.
- unity value for sample points in the set of particular i
Hex I
nterest
and zero elsewhere.
Robotics Research Labo-ratory
39
, ,
, ,
single point 1 range sample space
its inverse image in sample space (event)
single point 0 range sample sapce
its inverse image in sample space (event)
1
H
H
I
I
H
S
HH HT TH S
S
TT S
P I P HH HT TH
1 1 1 3 + +
4 4 4 41
0 4H
P HH P HT P TH
P I P TT
Robotics Research Labo-ratory
40
( ) ( ( ), ( ) ( , )
( , ) : ( ), ( ),
( , ).( , ),( , )
2D vector ) for any
Range and
0 1 1 0 11
H T
I H T
I s I s I s x y s S
S x y x I s y I s s S
) : set-indicator function (2D random vector) ex I
1
2
3
random vecto
Note:
is called a
whose components are random variables , 's.
r
i
X
X X
X
X
Robotics Research Labo-ratory
41
,
(0,1) range sample space
its inverse image in sample space (event)
(1,0) range sample sapce
its inverse image in sample space (event)
(1,1) range sample sapce
its invers
I
I
I
S
TT S
S
HH S
S
HT TH
( , )
( , )
( , ) ,
e image in sample space (event)
10 1
41
1 0 4
1 1 111 + =
4 4 2
S
P I P TT
P I P HH
P I P HT TH P HT P TH
Robotics Research Labo-ratory
42
Suppose that ( ) has a probability density ( ).
( ) ( ) ( ) or ( ) 0
Probability distribution of ( )
( ) ( ) where is a real number.
( ) ( ) , : ( ) ,
X
xx
x X X
X
X s f x
dF xF x f ξ dξ f x
dx
X s
F x P X s x x
P X s x P X s x P s X s x
( ) 1 and ( ) 0, ( ) ( ) if
[ ( ) ] , 0 1
X X X X
i i i
s S
F F F b F a b a
P X s x p p
Robotics Research Labo-ratory
43
Ex)
3[ 1]
41
[ 0]4
0 for - < < 0
1( ) for 0 < 1
41 for 1 <
This distribution function is a step function,which has two possible values.
It is called a Bernoull
H
H
H
I
P I
P I
x
F x x
x
1 2 1 1 2 2
i randon variable.
Note:
( , , ... , ) , , ... ,X n n nF x x x P X x X x X x
Robotics Research Labo-ratory
44
Let be an -random vector with probability density ( ).
[ ] ( ) or
Remarks :
i) [ ] is a linear operator
ii) [ ] is called a mean (statistical av
[
erage) o
]
f
kk
X
X k
X n f x
E X E X x P Xxf x dx
E
E X
x
m X
2
iii) [ ] is a first moment
[ ] ( ) or the moment of
the central moment of
( ) ( )( )
[
]k kj j
k kX
k
T
j
E X
E X x f x dx kth X
E X E X kth X
Var X E X E X E X m X m
E X x P X x
2 2 2 2
the variation (matrix) of
( ) [ 2 ] [ ] [ ]
X
Var X E X XE X E X E X E X
Robotics Research Labo-ratory
45
Let :
and ( ) is a function of random variables and a random -vector.
( ) ( ) ( )
Let be a random -vector with mean and
be a random -vector with mean .
Let ( , ) be their
n m
X
X
Y
XY
g R R
g X m
E g X g x f x dx
X n m
Y m m
f x y
joint probability density.
( )( )
( )( ) ( , )
is covariance matrix between and .
TX Y
TX Y XY
E X m Y m
x m y m f x y dx dy
X Y
Robotics Research Labo-ratory
46
The random vectors and are independent
if ( , ) ( ) ( )
or ( , ) ( ) ( ).
The random vectors and are uncorrelated
if [ ] [ ] [ ]
Let be an n-random vector with normal (or Gauss
XY X Y
XY X Y
T T
X Y
F x y F x F y
f x y f x f y
X Y
E XY E X E Y
X
1/ 2 1/ 2
ian) density
1 1( ) exp ( ) ( )
(2 ) (det ) 2
where is a constant -vector, is an symmetric
and positive definite matrix.
[ ] (mean)
( )( ) (varianc
only first and s
)
e
e
TX n
T
f x x m P x mπ P
m n P n n
E X m
E X m X m P
cond moments are required.
Robotics Research Labo-ratory
47
1
2
1 1
A vector random process is a family of vector time function
denoted by
( )
( ) ( ) , 0
( )
A random process is characterized by specifying its distribution
function
( , ... , ,
n
m
X t
X tX t t
X t
F x x t
1 1 2 2
1 2
1 2
, )
( ) , ( ) , ... , ( )
for all vector , , ... ,
for all , ,
for all
... ,
...
m
m m
m
m
P X t x X t x X t x
x x x
t t
m
t
t
Robotics Research Labo-ratory
48
F(X
, t2) F
(X, t
1)
F(X
, t3)
01
X
X
0 t1 t2 t3
t
X(•, 1)
X(•, 2)
X(•, 3)
Robotics Research Labo-ratory
49
Remark:
1
1
1 111 1 1
( , ) : random vector
( , ) : time function vector
The density function for a random process ( ) is
, , , , ,
covariance
nm
n mn n nm
T
T
X t
X ω
X t
Ff x x t t
x x x x
E X t m t
E X t m t X τ m τ
E X t m t X t m t
,
v
autocorrela
ari
tion
ance
TE X t X τ R t τ
Robotics Research Labo-ratory
50
Let ( ) and ( ) be random processes.
( ) ( )
is cross covariance between ( ) and ( ).
( ) and ( ) are uncorrelated
if
. ., ( ) ( ) 0
A
T
X Y
T T
T
X Y
X t Y t
E X t m t Y τ m τ
X t Y τ
X t Y τ
E X t Y τ E X t E Y τ
i e E X t m t Y τ m τ
1 1
1 1
1 1
random process ( ) is stationary in the strict sense
if , ,
, ,
for all , , , , , and all for .
. ., independent of .
m m
m m
m m
X t
P X t x X t x
P X t τ x X t τ x
x x t t m τ
i e t
Robotics Research Labo-ratory
51
1
If ( ) is stationary, then ( , ) ( ( ) ) is indepent of
( , )and ( , ) is also
If the following two things hold,
is constant.
,
independent of
e
n
.
d peT
x
nx
xn
E
X t F x t P
X t
X t x t
F x tf x t t
x
m
E R t τ
x
X t X τ
ds on only .
. ., ,
,then ( ) is wide sense stationary (or weakly stationary)
t τ
i e R t τ R t τ
X t
Robotics Research Labo-ratory
52
A random process X(t) is Gaussian process
if for any t1, …, tm, and any m, the random vector
X(t1) … X(tm) have the Gaussian distribution.
A Gaussian process is completely characterized
by its mean
and its autocorrelation
If Gaussian process X(t) is w.s.s, then it is strictly stationary.
Assume that X(t) is wide sense stationary
Let
then is Fourier
transform of
. It is called a Spectral Density Matrix.
1
2jωτS ω R τ e dτ
π
( ) ( )E X t m t
( ) ( ) ( , )TE X t X τ R t τ
( ) ( ) ( ) ,TR τ E X t X t τ τ where - < <
( )R τ
Robotics Research Labo-ratory
53
Remark:
A random process X(t) is a Markov process
if
for all t1<t2<···<tm, all m, all x1,…, xm
A random processes X(t) is independent
if the random vectors X(t1) ··· X(tm)
are mutually independent for all t1<t2<···<tm and all m.
, ,1 1 1
1
1
1m
m
m m
m m m
m
P X t x X t x X t x
X tP X t xx
Note: Andrei Andreevich Markov (1856 – 1922)
*( ) ( ) ( ) ( )TS ω S ω S ω S ω
Robotics Research Labo-ratory
54
ex) Consider a scalar random process X(t), t 0 defined from
where X(0) is zero mean Gaussian random variable with .
0 0 t
X(t)
2σ
( )( )
1
1
dX tX t
dt t
1( ) (0) (solution)
11
( ,0) state transition funtion1
X t Xt
Φ tt
Robotics Research Labo-ratory
55
1 2
11
2
2 22
( ), 0 is a Gaussian process.
Let
1( ) ( )
1
( ) is a Markov process.
1 1( ) (0) (0) 0
1 1
1 1 1( ) ( ) (0) (0)
1 1 ( 1)( 1)
1( )
( 1)
m
mm m
m
X t t
t t t
tX t X t
t
X t
E X t E X E Xt t
E X t X τ E X X σt τ t τ
E X t σt
not w.s.s.
Robotics Research Labo-ratory
56
The density function X(t) is
A random process w(t), t 0 is a white process
if it is zero mean with the property that w(t1) and w(t2)
are independent for all t1 t2 and
where Q(t1) is intensity
( )
( , ) .
2 2
21
21
2
t xσt
f x t eπσ
1 2 1 2 1 1 2( , ) ( ) ( ) ( ) ( )wR t t E w t w t Q t δ t t
Robotics Research Labo-ratory
57
Remarks:
i) If Q(t) is constant, i.e. Q(t) = Q then w(t) is w.s.s.
and the spectral density is
ii) A white process is not a mathematical rigorous
random process.
iii) A sample function for a white noise process can be
thought as composed of superposition of large number of independent pulse of brief duration with random amplitude.
iv) If the amplitude of the pulse is Gaussian, the w(t) is
a Gaussian white noise.
v) A white noise is a ‘derivative’ of a Wiener process
(Brown motion)
( ) wS ω Q ω
Robotics Research Labo-ratory
58
ex)
Similarly,
Since {v(k)} is a white process, {X(k)} is a random process.
X(0) should be specified.
It is assumed that
white process
Wiener process
( )( ) ( ) ( )
dX tA t X t w t
dt
0 00( ) ( ) ( ) ( ) ( )
t t
t tX t X t A τ X τ dτ w τ dτ
( 1) ( ) ( )
( ) ( )
X k X k v k
y k CX k
Φ
( )
( ( ) )( ( ) )
( ) ( )
0
0 0 0
1
0
0 0 T
T
E X m
E X m X m R
E v k v k R
Robotics Research Labo-ratory
59
0
1 0
Mean: ( 1) Φ ( ), (0)
Define ( ) ( ( ) ( ))( ( ) ( ))
Variance: ( 1) Φ ( )Φ , (0)
Autocorrelation:
( , ) ( ( ) ( )
x x x
Tx x x
Tx x x
Tx
m k m k m m
P k E X k m k X k m k
P k P k R P R
R k τ k E X k τ X k
0
Φ ( ( ) ( )
If (0) 0, ( , ) Φ ( )
For the output,
( ) ( ) ( )
( , ) ( ) ( ) ( , )
( , ) ( ) ( )
τ T
Tx x
y x
T T Ty x
Tyx
E X k X k
E X m R k τ k P k
m k E CX k Cm k
R k τ k E CX k τ X k C CR k τ k C
R k τ k E CX k τ X k
( , ) xCR k τ k
Robotics Research Labo-ratory
60
k
j j
yj
uj
Ty
u
y k h k j u j h j u k j
m k E y k E h j u k j
h j m k j
R k τ k E y k τ y k
h i R k
0
0
0
Consider I/O model.
( ) ( ) ( ) ( ) ( )
( ) ( ) ( ) ( )
( ) ( )
( , ) ( ) ( )
( ) ( T
i j
yu uj
τ j i k h j
R k τ k h j R k τ j k
0 0
0
, ) ( )
Similarly,
( , ) ( ) ( , )
Robotics Research Labo-ratory
61
-
-
-
By the definition of a spectral density function
1 ( ) ( )
2
In a discrete-time system
1 ( ) ( )
2
From the above definition
1 ( ) ( )
2
jωτ
jnω
n
jnωy y
n
S ω R τ e dτπ
S ω R n eπ
S ω R n eπ
- 0 0
0 0 0
1 ( ) ( ) ( )
2
1 ( ) ( ) ( )
2
jnω Tu
n k l
jkω jmω jlω Tu
k m l
e h k R n l k h lπ
e h k e R m e h lπ
Robotics Research Labo-ratory
62
Remarks:
A stable linear time-invariant discrete-time system
has a pulse transfer function H(z).
Suppose that the input u(k) is w.s.s. with a spectral
density matrix .
Then the output y(k) is w.s.s. and the spectral density
of the output y(k) is
In a scalar case,
jω T jωy uS ω H e S ω H e
h(k-j)u(j) y(k)
jω T jωy uS ω H e S ω H e
jωy uS ω H e S ω
2
uS ω
Robotics Research Labo-ratory
63
T
T
u
jω jωx
x k ax k u k
y k x k e k
E u k E u k u k r
E e k E e k e k r
H zz a
rS ω
πr r
S ω H e H eπ π
1
2
1
1 1
ex) ( 1) ( ) ( )
( ) ( ) ( )
( ) 0, ( ) ( )
( ) 0, ( ) ( )
1 ( )
( )2
( ) ( ) ( ) =2 2 (
y
y
a a ω
rS ω r
π a a ω
y t
S ω
2
12 2
1 2 cos )
1 ( )
2 1 2 cos
Suppose ( ) is a wide sense stationary random process
with spectral density ( ).
Robotics Research Labo-ratory
65
ex)
12 1
2 11 2 2
1
2 1
1
2 2 21 2 1 2 1 2
2
21 2
12
11
2
2
(1 ) ( (1 ) )( (1 ) )where
2
1(1
2
jω
jω
Y
z e
z e
rS ω r
π z a z a
r r a r a z z
π z a z a
λ z b z bπ z a z a
r r a r r a r r ab
ar
λ r r
2 2 21 2 1 2) ( (1 ) )( (1 ) )a r r a r r a
( )( )λ z b z b r r a r a z z 2 1 2 11 2 2 Note: 1
Robotics Research Labo-ratory
66
white noisewith intensity I
Note: Norbert Wiener (1894 – 1964) Wiener filter for stationary I/O case in 1949
“Everything” can be generated by filtering white noise .
L.T.Iw(k) y(k)
white processwith intensity I
colored noise
z bG z λ
z az b
Y z λ W zz a
( )
( ) ( )
Robotics Research Labo-ratory
67
z zF z
π z z z z
1
1 2 2
ex)
We want to generate a stochastic signal with the spectral density
1 0.3125 0.125( )
2 2.25 1.5( ) 0.5( )
Then the desired noise properties a
zH z
z z2
re obtained by filtering white noise
through the filter
0.5 0.25
0.5
Reference: "Probabilty and Random Processes", W.B. Davenport,Jr., McGraw-Hill
"Probability, Random Variables, and Stochastic Processes", A. Papoulis,
McGraw-Hill
"Computer-Controlled Systems - Theory and Design", K.J. Astrom and
B. Wittenmark, Prentice-Hall
Robotics Research Labo-ratory
68
LQ + Kalman Filter
( ~ state feedback + observer by pole placement)
LQG(Linear Quadratic Gaussian) problem
- Partially informed states
1
0
0
12
( 1) ( ) ( ) ( )
( ) ( ) ( )
(0)
(0)
( ) ( ) positive semi-definte
( ) ( )
( ) ( ) postive definte
w
v
x k Φx k Γu k Γw k
y k Hx k v k
E x m
Var x R
E w k w k R
E w k v k R
E v k v k R
Robotics Research Labo-ratory
69
12
1
0
1
If 0, ( ) and ( ) are independent.
1 1
2 2
Assumptions:
, , , and may be time-invariant deterministic.
( ) 0
( ) 0
( ) ( ) 0
( ) ( ) 0 if
N
k
R w k v k
J E x N Sx N x k Qx k u k Ru k
Φ Γ Γ H
E w k
E v k
E w i v j
E w i w j i j
0
( ) ( ) 0 if
( (0) ) ( ) 0
( ) ( ) v
E v i v j i j
E x m y k
E v k v k R
Robotics Research Labo-ratory
70
Given y(0), y(1), … y(k), determine the optimal estimate
such that an n n positive definite matrix
i.e., minimum variance of error
Remarks:
i) P(k) is minimum
*P(k) is minimum where is an arbi-
trary vector
ii) P(k) is minimum
x̂ k x k x k1 1 1 (estimate = true - error)
Problem Formulation
J E x k x k trace P k
P k E x k x k1 1 1 is minimum
Robotics Research Labo-ratory
71
Let the prediction-type Kalman filter have the form.
- Predictor type, One-step-ahead estimator
-
where L(k) is time-varying
y(k) is a measured output
is an output from the model.
Define as a reconstruction error.
ˆ ˆ( ) (ˆ( ) ( )) ( ) ( )1 Φx k Γux k y kk L k Hx k
ˆ( )Hx k
ˆx x x
ˆ( ) ( ) ( ) ( ) ( ) ( )
( ) ( ) ( ) ( )
( ) ( ) ( ) ( ) ( )
1
1
1
x k Φx k Γu k Γw k Φ L k H x k
Γu k L k Hx k v k
Φ L k H x k L k v k Γw k
Robotics Research Labo-ratory
72
0
1
1
0
( 1) ( ) ( )
(0)
( 1) ( 1) ( 1)
(( - ( ) ) ( ) ( ) ( ) ( )))
( ( )( - ( ) ) ( )
ˆIf (0) , ( ) 0 0
( ) ( ))
Defi
E x k Φ L k H E x k
E x m
P k E x k x k
E Φ L k H x k Γw k L k v k
x k Φ L k H
E x m E x k
w Γ
k
k v k L k
1
ne ( ) ( )
( ) ( ) ( ) ( )
Φ L k H N k
L k v k Γw k e k
Robotics Research Labo-ratory
73
where
*
( ) ( )
P k E N k x k e k x k N k e k
E N k x k x k N k N k E x k e k
E e k x k N k E e k e k
1
0
0
E x k e k E x e k 0 0 0
1 1
1
v w
P k Φ L k H P k Φ L k H
L k R L k Γ R Γ
P E x x P 00 0 0
Robotics Research Labo-ratory
74
minimize P k 1 matrix
scalar α P k α 1
*( )
1 1
1
1
1
independent term of
w
v
v
v v
α P k α α ΦP k Φ Γ R Γ L k HP k Φ
ΦP k H L k L k R HP k H L k α
α L k α
α L k ΦP k H R HP k H
R HP k H L k ΦP k H R HP k H α
Robotics Research Labo-ratory
75
ˆ ˆ ˆ
ˆ( ) ( )
,
1
1
0
1
1 1
If 0
then
1
0 0
0
1
v
w v
v
x k Φx k Γu k L
L k ΦP k H R HP k H
P k ΦP k Φ Γ R Γ ΦP k H R
L k ΦP k H R HP
HP k H HP k Φ
k y k Hx k
x E x
k H
P P
Note: Kalman and Bucy filter for time-varying state space in 1960
Robotics Research Labo-ratory
76
Remarks:
i)
ii) a priori information are
iii) due to system dynamics
due to disturbance
w(k)
last term due to newly measured information
iv) P(k) does not depend on the observation. Thus the
gain can be precomputed in forward time and
stored.
v) steady-state Kalman filter – all constants
, , , .0 and 0w vR R P m
ΦP k Φ
1
1 1w vP ΦPΦ Γ R Γ ΦPH R HPH HPΦ
1
vL ΦPH R HPH
( ) is time-varying.L k
1 1wΓ R Γ
Robotics Research Labo-ratory
77
** *
ˆ( ) ( ) ( )( ( ) ( ))
( ) ( ) () ( ) )( 1 1
Another Form of Kalman Filter - Filter type (p. 389~391)
At the measurement time (measurement update)
~ Recursive Least Squar e
where
v v
x k x k L k y k Hx k
L k P k H R M k H HM k H R
*
* *( ) ( ) ( ) ( ( ) ) ( )
ˆ(
ˆ( ) ( ) ( ), ( )
( ) ( ) ( ) )
)
( ) (
0
1
1
1 0
1
Between measurement (time update)
and
Notes:
0
1 ,
is the ac
0 0 0w
v
x k Φx k Γu k E x m
M k ΦP k Φ Γ R Γ M E x x R
P k M k M k H HM k H R HM k
x k
ˆ. ~ ( | )
ˆ( ) . ~ ( | )
( ) ~ ( | ) ( ) ~ ( | )
tual state estimate at
is the predicted state estimate at the sampling instant 1
Also, and 1
k x k k
x k k x k k
P k P k k M k P k k
Robotics Research Labo-ratory
78
) ( ) ( )
( ) ( )
( )
( ) .
, ,
ˆ ˆ ˆ( ) ( ) ( ) ( )
2 2
1
where
0 2
0 0 5
1 0 1
1
w
ex x k x k
y k x k v k
E v k R k σ
E x
P
Φ R H
x k x k L k y k
( )
( )( )
( )
( )( ) , ( ) .
( )
2
2
2
1 0 0 5 decrease with time
x k
P kL k
σ P k
σ P kP k P
σ P k
Robotics Research Labo-ratory
79
0 100 200 300 400 500 6000
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4
0.45
0.5
k
p(k)
L(k)
0 100 200 300 400 500 600-2.5
-2.4
-2.3
-2.2
-2.1
-2
-1.9
-1.8
-1.7
k
x__hat
E[v(k)2] = 1
E[v(k)2] = 0.5
Robotics Research Labo-ratory
80
time
transient
1
2
0
L = 0.01
L = 0.05
L(k):optimal gain
Steady-state
x
where 1σ
Robotics Research Labo-ratory
81
colored noise
( ) ( ) ( )1
1
1
where is a white noise
is a colored noise.
x k Φ x k w k
y k H x k n k
w k
n k
Frequency Domain Properties of Kalman Filter
( ) ( ) ( )
.
2
2
1
where and are white noises
z k Φ z k v k
n k H z k e k
v k e k
Robotics Research Labo-ratory
82
( )
( ) ( ) ( )
( ) ( ) ( )
( )
( )
( ), ( ) ( ) .
1
2
1 2
1
2
1 2
1
1
01
01
where and are uncorrelated white noises
x k Φ x k w k
z k Φ z k v k
y k H x k H z k e k
Φx k x k w k
Φz k z k v k
x ky k H H e k
z k
w k v k e k
Robotics Research Labo-ratory
83
ˆ ˆ( ) ( )
ˆ ˆ( ) ( ) ( )ˆ ˆ( ) ( )
1 11 2
2 2
The steady-state Kalman filter for is given by
01
01
x
Φ Lx k x ky k H x k H z k
Φ Lz k z k
ˆ ˆ( ) ( )( )
ˆ ˆ( ) ( )
ˆ ˆ
1 1 1 1 2 1
2 1 2 2 2 2
1
1 1 1 2 1
2 1 2 2 2 2
1= +
1
Pulse-transfer function from to and
0
Φ L H L H Lx k x ky k
L H Φ L H Lz k z k
y x z
zI Φ L H L H LH z I
L H zI Φ L H L
Robotics Research Labo-ratory
84
Remarks:
i) It gives an idea how the Kalman filter attenuates dif-
ferent frequency.
ii) Kalman filter has zeros at the poles of the noise
model. (notch filter)
Robotics Research Labo-ratory
85
Smoothing: To estimate the Wednesday temperature based on temperature measurements from Monday, Tuesday and Thursday.
Filtering: To estimate the Wednesday temperature based on temperature measurements from Monday, Tuesday and Wednesday.
Prediction: To estimate the Wednesday temperature based on temperature based on temperature measurements from Sunday, Monday and Tuesday.
Robotics Research Labo-ratory
86
-1
1 12 2 00
1
0
Minimize ( ) ( ) 2 ( ) ( ) ( ) ( ) ( ) ( )
subject to ( 1) ( ) ( ) ( )
where (0) 0, (0) (0) ,
NT T T
k
T Tw
E x k Q x k x k Q u k u k Q u k x N Q x N
x k Φx k Γu k Γ w k
E x E x x R E ww R
The control law ( ) ( ) ( ) gives the minimum.u k K k x k
Stochastic LQ Control Problem
Robotics Research Labo-ratory
87
1
0
Given the linear stochastic difference equatiom
( 1) ( ) ( ) ( )
( ) ( ) ( )
where (0) 0, (0) (0) ,
( ) ( )
( ) (
T
x k Φx k Γu k Γ w k
y k Cx k v k
E x E x x R
w k w kE
v k v k
12
12
-1
1 2 00
,)
ˆ find a linear control law ( ) ( ) ( ) that minimizes
( ) ( ) ( ) ( ) ( ) ( )
Tw
v
NT T
k
R R
R R
u k K k x k
E x k Q x k u k Q u k x N Q x N
LQG Control Problem
Robotics Research Labo-ratory
88
For ( 1) ( ) ( ) ( )
ˆThe state feedback control law ( ) ( ) ( ) is independent of ( ).
It is a unique admissible control stategy that minimizes
x k Φx k Γu k w k
u k K k x k w k
The Ideas of Separation (Separation Theorem)
the cost function.
The Kalman filter minimizes ( ) ( ) ( ) .
ˆAs a result, ( ) is reconstructed.
ˆThis makes it possible to use the control law ( ) ( ) ( )
with the dynamics ( 1) ( )
TP k E x k x k
x k
u k K k x k
x k Φx k
(
( ) ( ) and
ˆ ( 1) ( ) ( ( ) ( )) ( ) ( ) ( ) ( )
The term ( ) ( ) is viewed as a part of the
) ( ) ( )
noise.
ΓK k
Γu k w k
x k Φx k Γ K k x k w k Φx k ΓK k x k x k w
ΓK k x k
k
Robotics Research Labo-ratory
89
( ) ( ) ( ) ( )
( ) ( ) ( )
( ) ( )
( ) ( )
1
12
12
Given the linear stochastic difference equation
1
where
find a l
Tw
v
x k Φx k Γu k Γ w k
y k Cx k v k
R Rw k w kE
R Rv k v k
ˆ( ) ( ) ( )
( ) ( ) ( ) ( )
ˆ ˆ ˆ ˆ( ) ( ) ( ) ( ) ( ) ( )
1 20
inear control law that minimizes
where x(k+1)=
T T
k
u k K k x k
E x k Q x k u k Q u k
u k x k Φx k Γu k y k HxK L k
Stationary LQG Control Problem
Robotics Research Labo-ratory
90
2
1
1 1
1
1
1
2
LQ:
Kalman Filter:
wher
e
T T
T T T T Tw
T T
T
v
T
P ΦPΦ Γ R Γ
S Φ SΦ Φ SΓ Q Γ SΓ Γ S
ΦPH
Φ Q
K Q Γ SΓ Γ S
R H
Φ
HPH PΦ
1
1 2
1
1
1 1
or : LQ
Kalman Filt
er:
T Tv
T T T T
T
v
T T
w
S Φ SΦ Q K Q Γ
L ΦPH R HPH
P ΦPΦ Γ R Γ L R H
S
L
K
PH
Γ