applications in industry - university of...
TRANSCRIPT
1
Applications in Industry – (Extended) Kalman Filter
© 2016 School of Information Technology and Electrical Engineering at The University of Queensland
TexPoint fonts used in EMF.
Read the TexPoint manual before you delete this box.: AAAAA
http://elec3004.com
Lecture Schedule: Week Date Lecture Title
29-Feb Introduction
3-Mar Systems Overview
7-Mar Systems as Maps & Signals as Vectors
10-Mar Data Acquisition & Sampling
14-Mar Sampling Theory
17-Mar Antialiasing Filters
21-Mar Discrete System Analysis
24-Mar Convolution Review
28-Mar
31-Mar
4-Apr Frequency Response & Filter Analysis
7-Apr Filters
11-Apr Digital Filters
14-Apr Digital Filters
18-Apr Digital Windows
21-Apr FFT
25-Apr Holiday
28-Apr Introduction to Feedback Control
3-May Holiday
5-May Feedback Control & Regulation
9-May Servoregulation/PID
12-May Introduction to (Digital) Control
16-May Digital Control Design & State-Space
19-May Observability, Controllability & Stability of Digital Systems
23-MayDigital Control Systems: Shaping the
Dynamic Response & Estimation26-May Applications in Industry
30-May System Identification & Information Theory
2-Jun Summary and Course Review
Holiday
1
13
7
8
9
10
11
12
6
2
3
4
5
26 May 2016 - ELEC 3004: Systems 2
2
Announcements
PS 3 Peer Review Competition
• The PS 3 Review with the
highest Likert Score
• Deadline for reviews:
June 3 (11:59 pm)
• Good reviews discussed
June 2nd Last Lecture
• Reward: 3004¢
ELEC 3004
“Review Lab”
(“Lab 5”):
• Redo any aspects of any
of the labs
• Review course
• Review 2015 Final exam
(which we will do
on June 2nd also)
!
26 May 2016 - ELEC 3004: Systems 3
G. Franklin,
J. Powell,
M. Workman
Digital Control
of Dynamic Systems
1990
TJ216.F72 1990
[Available as
UQ Ebook]
Follow Along Reading:
B. P. Lathi
Signal processing
and linear systems
1998
TK5102.9.L38 1998
State-space
• FPW
– Chapter 6 - Design of Digital
Control Systems Using State-Space
Methods
• Lathi Ch. 2 (?)
– § 2.7-6 Time Constant and Rate of Information Transmission
• Information Theory!
Today
26 May 2016 - ELEC 3004: Systems 4
3
• Nothing beyond this point is on the exam.
(except for the exam review )
• Do not pay attention.
• Do not attempt to learn.
</assessable>
WARNING: Not assessable
26 May 2016 - ELEC 3004: Systems 5
Example 0:
26 May 2016 - ELEC 3004: Systems 6
4
TF 2 SS – Control Canonical Form)
26 May 2016 - ELEC 3004: Systems 7
Control Canonical Form as a Block Diagram
26 May 2016 - ELEC 3004: Systems 8
5
• CCF is not the only way to tf2ss
• Partial-fraction expansion of the system
System poles appear as diagonals of Am
• Two issues: – The elements of matrix maybe complex if the poles are complex
– It is non-diagonal with repeated poles
Modal Form
26 May 2016 - ELEC 3004: Systems 9
Modal Form
26 May 2016 - ELEC 3004: Systems 10
6
Modal Form Block Diagram
26 May 2016 - ELEC 3004: Systems 11
• Given: 𝑌 𝑠
𝑈 𝑠=
25.04𝑠+5.008
𝑠3+5.03247𝑠2+25.1026𝑠+5.008
Get a state space representation of this system
• Matlab:
• Answer:
Matlab’s tf2ss
26 May 2016 - ELEC 3004: Systems 12
7
Example 1:
Inverted Pendulum
26 May 2016 - ELEC 3004: Systems 13
Digital Control
Wikipedia,
Cart and pole
26 May 2016 - ELEC 3004: Systems 14
8
Inverted Pendulum
26 May 2016 - ELEC 3004: Systems 15
• The equations of motion of an inverted pendulum (under a
small angle approximation) may be linearized as:
𝜃 = 𝜔
𝜔 = 𝜃 = 𝑄2𝜃 + 𝑃𝑢
Where:
𝑄2 =𝑀 +𝑚
𝑀𝑙𝑔
𝑃 =1
𝑀𝑙.
If we further assume unity Ml (𝑀𝑙 ≈ 1), then 𝑃 ≈ 1
Inverted Pendulum – Equations of Motion
26 May 2016 - ELEC 3004: Systems 16
9
• We then select a state-vector as:
𝒙 =𝜃𝜔
, hence 𝒙 = 𝜃
𝜔 =
𝜔𝜔
• Hence giving a state-space model as:
𝐴 =0 1𝑄2 0
,𝐵 =01
• The resolvent of which is:
Φ 𝑠 = 𝑠𝐼 − 𝐴 −1 =𝑠 −1
−𝑄2 𝑠
−1
=1
𝑠2 − 𝑄2
𝑠 1𝑄2 𝑠
• And a state-transition matrix as:
Φ 𝑡 =cosh 𝑄𝑡
sinh 𝑄𝑡
𝑄𝑄 𝑠𝑖𝑛ℎ 𝑄𝑡 cosh𝑄𝑡
Inverted Pendulum –State Space
26 May 2016 - ELEC 3004: Systems 17
Cart & Pole in State-Space With Obstacles?
Swing-up is a little more than stabilization…
See also: METR4202 – Tutorial 11: http://robotics.itee.uq.edu.au/~metr4202/tpl/t11-Week11-pendulum.pdf
26 May 2016 - ELEC 3004: Systems 18
10
Cart & Pole in State-Space Swing-up is a little more than stabilization…
26 May 2016 - ELEC 3004: Systems 19
Example 2:
Command Shaping
26 May 2016 - ELEC 3004: Systems 20
11
Experiments: Scanning Over Obstacle
26 May 2016 - ELEC 3004: Systems 21
Command Shaping
26 May 2016 - ELEC 3004: Systems 22
12
Robust Control: Command Shaping for Vibration Reduction
Command
ShappingRegulator
Integrated
Planner
Controller
Error PlantΣ+
–
SensorTunning
26 May 2016 - ELEC 3004: Systems 23
Command Shaping
Original velocity profile
Input shaper
Command-shaped velocity profile
Ve
locity
Ve
locity
Time
Time
Time*
26 May 2016 - ELEC 3004: Systems 24
13
Command Shaping
• Zero Vibration (ZV)
• Zero Vibration and Derivative (ZVD)
20
11
1
di
i
TK
K
Kt
A
21
eK
ddi
i
TT
K
K
K
K
Kt
A
20
)1()1(
2
)1(
12
2
22
26 May 2016 - ELEC 3004: Systems 25
Experiments: Command Shaping
26 May 2016 - ELEC 3004: Systems 26
14
Estimation:
Yet another way
to beat the noise
26 May 2016 - ELEC 3004: Systems 27
Along multiple dimensions
26 May 2016 - ELEC 3004: Systems 28
15
State Space • We collect our set of uncertain variables into a vector …
x = [x1, x2,…, xN]T
• The set of values that x might take on is termed the state space
• There is a single true value for x,
but it is unknown
26 May 2016 - ELEC 3004: Systems 29
State Space Dynamics
26 May 2016 - ELEC 3004: Systems 30
16
Measured versus True • Measurement errors are inevitable
• So, add Noise to State... – State Dynamics becomes:
• Can represent this as a “Normal” Distribution
26 May 2016 - ELEC 3004: Systems 31
Recovering The Truth • Numerous methods
• Termed “Estimation” because we are trying to estimate the
truth from the signal
• A strategy discovered by Gauss
• Least Squares in Matrix Representation
26 May 2016 - ELEC 3004: Systems 32
17
Recovering the Truth: Terminology
26 May 2016 - ELEC 3004: Systems 33
General Problem…
26 May 2016 - ELEC 3004: Systems 34
18
Duals and Dual Terminology
26 May 2016 - ELEC 3004: Systems 35
Estimation Process in Pictures
26 May 2016 - ELEC 3004: Systems 36
19
Kalman Filter Process
26 May 2016 - ELEC 3004: Systems 37
KF Process in Equations
26 May 2016 - ELEC 3004: Systems 38
20
KF Considerations
26 May 2016 - ELEC 3004: Systems 39
Ex: Kinematic KF: Tracking • Consider a System with Constant Acceleration
26 May 2016 - ELEC 3004: Systems 40
21
In Summary
• KF: – The true state (x) is separate from the measured (z)
– Lets you combine prior controls knowledge with
measurements to filter signals and find the truth
– It regulates the covariance (P) • As P is the scatter between z and x
• So, if P 0, then z x (measurements truth)
• EKF: – Takes a Taylor series approximation to get a local “F” (and
“G” and “H”)
26 May 2016 - ELEC 3004: Systems 41
Estimation:
“Bayesian Perspective”
26 May 2016 - ELEC 3004: Systems 42
22
• (Optimal) estimation of the (hidden) state of a
linear dynamic process of which we obtain noisy (partial)
measurements
• Example: radar tracking of an airplane.
What is the state of an airplane given noisy radar
measurements of the airplane’s position?
Kalman Filtering
ELEC 3004: Systems 26 May 2016 - 43
Model • Discrete time steps, continuous state-space
• (Hidden) state: xt , measurement: yt
• Airplane example:
• Position, speed and acceleration
tt
t
t
t
t x
x
x
x~,
yx
26 May 2016 - ELEC 3004: Systems 44
23
Dynamics and Observation model • Linear dynamics model describes relation between the state
and the next state, and the observation:
• Airplane example (if process has time-step ):
),(~
),(~
,
,1
RNV
QNW
C
A
tt
tt
tt
tt
t
t
0v
0w
vx
wx
y
x
001,
100
10
1 2
21
CA
26 May 2016 - ELEC 3004: Systems 45
• Let X0 be a normal distribution
of the initial state x0
• Then, every Xt is a normal distribution
of hidden state xt. Recursive definition:
• And every Yt is a normal distribution of observation yt.
Definition:
• Goal of filtering: compute conditional distribution
Normal distributions
ELEC 3004: Systems 26 May 2016 - 46
ttt WAXX 1
ttt VCXY
ttt YYX yy ,,| 00
24
• Because Xt’s and Yt’s are normal
distributions,
is also a normal distribution
• Normal distribution is fully
specified by mean and covariance
• We denote:
Problem reduces to computing xt|t and Pt|t
Normal distribution
ELEC 3004: Systems 26 May 2016 - 47
stst
sstsst
sstst
PN
YYXYYXN
YYXX
||
0000
00|
,ˆ
,,|Var,,,|E
,,|
x
yyyy
yy
ttt YYX yy ,,| 00
• Kalman filtering algorithm: repeat… – Time update:
from Xt|t, compute a priori distribution Xt+1|t
– Measurement update:
from Xt+1|t (and given yt+1), compute
a posteriori distribution Xt+1|t+1
Recursive update of state
ELEC 3004: Systems 26 May 2016 - 48
X0 X1 X2 X3 X4 X5
Y1 Y2 Y3 Y4 Y5
…
25
Time update • From Xt|t, compute a priori distribution Xt+1|t:
• So:
QAAPAN
WAXAWXAN
WAXWAXN
WAXX
T
tttt
t
T
ttttt
tttttt
ttttt
||
||
||
||1
,ˆ
VarVar,EE
Var,E
x
QAAPP
A
T
tttt
tttt
||1
||1ˆˆ xx
26 May 2016 - ELEC 3004: Systems 49
From Xt+1|t (and given yt+1), compute Xt+1|t+1.
1. Compute a priori distribution of the observation
Yt+1|t from Xt+1|t:
Measurement update
ELEC 3004: Systems 26 May 2016 - 50
RCCPCN
VCXCVXCN
VCXVCXN
VCXY
T
tttt
t
T
ttttt
tttttt
ttttt
|1|1
1|11|1
1|11|1
1|1|1
,ˆ
VarVar,EE
Var,E
x
26
2. Look at joint distribution of Xt+1|t and Yt+1|t:
where
Measurement update (cont’d)
ELEC 3004: Systems 26 May 2016 - 51
RCCPCP
CPP
CN
YXY
YXX
Y
XNYX
T
tttt
T
tttt
tt
tt
tttttt
tttttt
tt
tt
tttt
|1|1
|1|1
|1
|1
|1|1|1
|1|1|1
|1
|1
|1|1
,ˆ
ˆ
Var,Cov
,CovVar,
E
E,
x
x
tt
tt
ttttttt
tttttttt
CP
XC
XVXXC
XVCXXY
|1
|1
|11|1|1
|11|1|11
Var
,Cov,Cov
,Cov,Cov
• Recall that if
then
3. Compute Xt+1|t +1 = (Xt+1|t|Yt+1|t = yt+1):
Measurement update (cont’d)
ELEC 3004: Systems 26 May 2016 - 52
2221
1211
2
1
21 ,,
NZZ
21
1
22121122
1
22121221 ,| zz NZZ
tt
T
tt
T
tttt
ttt
T
tt
T
tttt
ttttttt
CPRCCPCPP
CRCCPCPN
YXX
|1
1
|1|1|1
|11
1
|1|1|1
1|1|11|1
,ˆˆ
|
xyx
y
27
This can also (often) be written in terms of the
Kalman gain matrix:
Measurement update (cont’d):
ELEC 3004: Systems 26 May 2016 - 53
ttttttt
tttttttt
T
tt
T
ttt
CPKPP
CK
RCCPCPK
|11|11|1
|111|11|1
1
|1|11
ˆˆˆ
xyxx
• Choose distribution of initial state by picking x0 and P0
• Start with measurement update given measurement y0
• Choice for Q and R (identity) – small Q: dynamics “trusted” more
– small R: measurements “trusted” more
Initialization
ELEC 3004: Systems 26 May 2016 - 54
28
I. Model:
II. Algorithm: Repeat… – Time update:
– Measurement update:
(Bayesian) Kalman Filter Summary
ELEC 3004: Systems 26 May 2016 - 55
QAAPP
A
T
tttt
tttt
||1
||1ˆˆ xx
ttttttt
tttttttt
T
tt
T
ttt
CPKPP
CK
RCCPCPK
|11|11|1
|111|11|1
1
|1|11
ˆˆˆ
xyxx
),(~
),(~
,
,1
RNV
QNW
C
A
tt
tt
tt
tt
t
t
0v
0w
vx
wx
y
x
Take Aways:
• Kalman filter can be used in real time
• Use xt|t’s as optimal estimate of state at time t, and use Pt|t as a
measure of uncertainty.
Extensions:
• Dynamic process with known control input
• Non-linear dynamic process
• Kalman smoothing: compute optimal estimate of state xt
given all data y1, …, yT,
with T > t (not real-time).
• Automatic parameter (Q and R) fitting using EM-algorithm
(Bayesian) Kalman Filter Summary [II]
ELEC 3004: Systems 26 May 2016 - 56
29
• Information Theory & More!
• Review: – Chapter 6 of FPW
– Chapter 13 of Lathi
• Deeper Pondering??
Next Time…
26 May 2016 - ELEC 3004: Systems 57
Final Exam Review
• June 10, 2016
• From: 2-4
• In: 45-204 (???)
• Some Review Notes
(from Course Textbooks) http://robotics.itee.uq.edu.au/~elec3004/tutes.html
26 May 2016 - ELEC 3004: Systems 58