METR4200 – Advanced Control
Lecture 9
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
Introduction to Kalman Filtering
Linear Quadratic Gaussian Control
(LQG)
G. Hovland 2004
Announcement
No tutorials on Thursday mornings 8-9am
I will be present in all practical sessions from now until
the end of the semester. PA for 2 hours and 15 minutes
in each of the sessions PB, PC, PD and PE.
Physical setups for the inverted pendulum will be in the
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
lab this week or early next week.
You can arrange individual consultations (by email)
leading up to the final exam on Nov. 16.
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
Review State-Space Control
Poles can be placed anywhere,
but be careful with input saturation
We have two ways of
designing K: pole-placement
in phase-variable form or
optimal LQR control.
In both cases we assume
knowledge of all states.
Review LQR Control from last week
Linear
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
Quadratic
Regulator
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
LQR Scalar Case
See next slide
Matlab Script for Minimising J(k)
Divide by b
LQR Controller Gain k
2
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
General form
Summary LQR Controller
Keep the proof for the LQR controller in mind
The proof for the (steady-state) optimal observer is
similar
Optimal state feedback gain
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
K = R −1BT S
Riccati solution
Riccati
equation
0 = SA + AT S − SBR −1BT S + Q
Matrices A,B in
linear state-space
model
Penalty Matrices
Controller Tuning Parameters
State-feedback design using observers
B) Forced observer
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
A) Unforced observer
Details of
forced observer
In Lecture 7 we
designed the observer
by pole-placement.
The Kalman filter is
the optimal observer.
Introduction to the Kalman Filter
• Kalman filter invented in 1960 by R. E. Kalman
• It is the “optimal” estimator under a few
assumptions.
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
• First derived as a discrete filter. This is how it is
thought of by many (signal processing, etc.).
• Kalman and Busy extended the filter to the
continuous version in 1961. This is the version
commonly used in control systems engineering, and
the filter discussed in this presentation.
Starting Point: The State-Space Model
Recall a linear system in state-space form:
.
Know input vector “u”
and output vector “y”,
as well as the matrices
A, B, and C perfectly.
x = Ax + Bu
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
y = Cx
System of first-order O.D.E.’s
Controller and Observer Design
Controllers can be designed with full state knowledge:
.x = Ax + Bu
y = Cx
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
u = r - Kx
}
.x = (A – BK)x + Br
y = Cx
System poles can be
placed anywhere.
Observers can be designed to estimate the full state from
the output:
.
x = Ax + Bu + L(y – Cx)
The controller and observer can be designed
independently!
How to Choose Gains K and L?
• Controller gain K can be chosen by direct pole placement.
• For systems of high order, this loses physical meaning.
• K can be chosen in an “optimal” way, using the linear
quadratic regulator (LQR) method, by minimising:
∫
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
J=
∞
[x(t)TQx(t) + u(t)TRu(t)]dt
0
K = R −1BT S
0 = SA + A T S − SBR −1BT S + Q
• Now, how do we choose the observer gain L?
State-Space Model with Noise
In practice, there are other unknown inputs:
.x = Ax + Bu + d
y = Cx + n
Disturbances on states
Noise in sensors
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
There is no obvious best way to choose L:
∆x ≡ x – x
Want L small
error in estimated state
(sI – A)-1L
(sI – A)-1
N(s)
D(s) +
∆X(s) =
1 + C (sI – A)-1L
1 + C (sI – A)-1L
Single output system
helps visualisation
Want L big
Assumptions about the Noise
• d(t) and n(t) are stationary, zero mean, Gaussian
white noise processes
Covariances Q and R
E[d(t)] = E[n(t)] = 0
E[d(t)d(τ)T] = Q δ(t-τ)
E[n(t)n(τ)T]
Constant spectral densities
}
= R δ (t-τ)
delta function
1
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
• Initial state has mean and covariance
x0 = E[x(0)]
0
Qx0 = E[(x(0) – x0)(x(0) – x0)T)]
• Assume d(t), n(t), and x(0) are mutually uncorrelated
E[d(t)n(t)T] = E[x(0)d(t)T] = E[x(0)n(t)T] = 0
Minimisation of Mean-Square Error
• The Kalman filter generates the linear estimate of the
plant that minimises the mean square estimation error:
J = E[(x(t) – x(t))T(x(t) – x(t))]
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
• The optimal linear estimator can be found by setting the
differential of the mean square error, with respect to the
estimator parameters, to 0.
• Results in a necessary condition:
E[(x(t) – x(t))y(t)T] = 0
• This results in enough equations to find the optimal gain L
(using the definition of “optimal” above).
Kalman Filter Equations
Optimal Gain
Error Covariance Matrix
• L(t) = Se(t)CTR-1
.
where Se(t) = E[(x(t) – x(t))(x(t) – x(t))T]
Ricatti Equation
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
• Se(t) = Se(t)AT + ASe(t) + Q - Se(t)CTR-1C Se(t)
• Se(t) can be computed off-line with numerical integration (just
need Se(0) = S0), therefore, L(t) can be computed off-line.
• Se(t) gives a constant measure of the error in the filter!
Note: The optimal L has very similar structure to the LQR
controller gain K. B replaced by CT and A is transposed
The Steady-State Kalman Filter
• What if we are going to run the system for a long time, or
we don’t have a good estimate of the error in the initial state?
• L = SeCTR-1
Sub-optimal Gain
Error Covariance Matrix
where Se = E[(x(t) – x(t))(x(t) – x(t))T]
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
• 0 = SeAT + ASe + Q - SeCTR-1C Se
Algebraic Ricatti Equation
• For solution to A.R.E., we can integrate Se(t) forward in time
until a steady-state is reached, OR we can use the Hamiltonian
matrix (better conditioned numerically).
• Suboptimal solution approaches optimal as time goes on.
• Requires (A,C) is detectable and (A,Q) is stabilisable
Proof of Steady-State Kalman Filter Equations
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
Formulate Kalman Filter as an LQR problem
x& = Ax + B u
xˆ& = Axˆ + Bu + L( y − yˆ )
z = x − xˆ
z& = Az + LCz
State estimation error z
z& T = zT A T + zT CT LT
LQR Analogy:
CT acts as B
LT acts as K
A matrix transposed
Comparison LQR and Kalman Solutions
LQR
K = R −1BT S
0 = SA + A T S − SBR −1BT S + Q
Steady-State
Kalman Filter
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
L = S eCT R −1
LT = R −T CS e = R −1CS e
T
0 = S e A T + AS e − S eCT R −1CS e + Q
Note: Q and R are penalty matrices in LQR
Q and R are noise covariances in Kalman Filter
LQR Analogy:
CT acts as B
LT acts as K
A matrix transposed
Kalman Filter: Performance Index J
Optimality Criterion:
Same as LQR where
^=u
z=x and y-y
∞
J = ∫ zT Qz + ( y − yˆ )T R ( y − yˆ )dt
0
∞
= ∫ ( x − xˆ )T Q(x − xˆ ) + ( y − yˆ )T R ( y − yˆ )dt
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
0
Can be shown
∞
= ∫ ( y − yˆ )T S e ( y − yˆ )dt
0
The Kalman filter minimises
the squared prediction error
Summary so Far: Kalman Filter
Produces best possible state estimate under the
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
following conditions:
Gaussian process noise with covariance Q
Gaussian measurement noise with covariance R
Perfect knowledge of linear state-space model
The state prediction error x-x^ is optimal, ie. x-x^ contains only
zero mean white noise with smallest possible covariance.
What if the Noise is not White?
• Create a coloured noise filter, and add it to the system.
• Coloured noise filter:
.x (t) = A x (t) + B w(t)
f
coloured noise
output
f f
white noise input
f
d(t) = Cfxf(t)
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
• Add it to original system, for a new system:
.
x(t)
.
x (t)
=
f
y(t)
=
A Cf
x(t)
0 Af
xf(t)
[C 0]
x(t)
+
B
0
u(t) +
+ v(t)
xf(t)
LQG Controller
If a controller is designed using the LQR method,
and the observer is designed using the Kalman
filter, the resulting system is referred to as Linear
Quadratic Gaussian (LQG) control.
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
Advantage:
We can control multivariable systems
without measuring the entire state vector x.
Disadvantage:
The LQG controller does not have the 60o
phase margin and the gain margins of the
LQR controller.
0
Bf
w(t)
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
LQG Controller: Schematics
Kalman Filter
Gain
Normal LQR design
Useful Matlab Functions
lqe:
kalman:
estim:
Returns stationary Kalman estimator gain L
Returns entire stationary Kalman estimator
General estimator design (pole-placement or
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
Kalman Filter)
lqg:
lqgreg:
LQG controller (LQR + Kalman filter)
Designs LQG controller given an estimator
LQG Control: Vertical-Plane Aircraft Dynamics
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
Example from: Hung and MacFarlane, "Multivariable feedback:
A quasi-classical approach", Lecture Notes in Control and
Information Sciences, Vol. 40, 1982.
x1:
x2:
x3:
x4:
x5:
altitude in (m)
forward speed (m/s)
pitch angle (deg)
pitch rate (deg/s)
vertical speed (m/s)
u1:
u2:
u3:
spoiler angle (deg/10)
forward acceleration (m/s2)
elevator angle (deg)
www.boeing.com/commercial/7e7
Aircraft: Linearised State-Space Model
Linearised model at an operating
point (for example at cruising altitude
and speed)
x& = Ax + Bu + w
y = Cx + v
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
A=
B=
0

0

0

0


0
0
1.132
− 0.0538 − 0.1712
−1
0.0705
0
0
0
0.0485
0
0
1
− 0.8556
− 0.2909
0
1.0532
 0

− 0.12

 0

4.419


1.575
0
0
1
0
0
0
0 − 1.665
0 − 0.0732












0

− 1.013 
− 0.6859
C=
1

0


0
x4 and x5 are
not measured.
Kalman filter
will be used
to estimate
these states.
0 0 0 0
1 0 0 0
0 10

0 
Simulink Model
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
Process Noise w
Covariance Q
Measurement Noise v
Covariance R
x& = Ax + Bu + w
y = Cx + v
Matrix Gains
Uncontrolled Response: u=0
x& = Ax + Bu + w
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
y = Cx + v
y1
Random walk
when noise is
integrated.
Noise
Covariance Q
y2
x 0 = [10 0 − 1 0 0]
T
Q = diag (9,9,16,1,4)
y3
R=0
Uncontrolled Response: Sensor Noise
x& = Ax + Bu + w
y = Cx + v
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
y1
Sensor Noise
Covariance R
y2
x 0 = [10 0 − 1 0 0]
T
y3
Q=0
R = diag (0.01,0.04,0.09)
Kalman Filter: Simulink Implementation
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
Process: Hardware
Kalman Filter: Software
^
Actual (y) vs Filtered (y)
Measurements
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
xˆ 0 = x 0
Filtered
^y
3
^
Actual (y) vs Filtered (y)
Measurements
Initial y^1 Filter Transient
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
xˆ 0 = 0
^
Actual States (x) vs Filtered States (x)
x1 and x^ 1
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
x4 and x5
are not
measured.
x2 and x^ 2
^x
4
x3 and x^3
Hence, in
practice it
will not be
possible to
verify these
two estimates.
x^5
Summary: Kalman Filter Example
If you know the noise covariances Q and R, no tuning of
the Kalman Filter is required.
In practice, it is easy to estimate the noise covariance R,
but Q can be difficult to estimate.
Hence, fix R to the correct covariance and "tune" Q until
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
you get satisfactory estimates.
The Kalman Filter is only optimal for linear state-space
models. As a control engineer you will meet the term
"The Extended Kalman Filter". This filter is used on
nonlinear models by frequent linearisation around the
operating state estimate.
The Extended Kalman Filter is not optimal.
LQG Control
Separation Principle
Design Observer (Kalman Filter) and Controller (LQR)
separately.
Simulate total system with process noise w and sensor
noise v to verify your design.
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
When simulation works satisfactorily, replace the
simulated model with physical plant / process.
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
Aircraft: LQR assuming known states x
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
LQR Control: Faster Altitude Response
LQG Control Architecture
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
^
Actual (y) vs Filtered (y)
Outputs
Controls
to zero,
x=0
^
Actual (x) vs Filtered (x)
States
Summary LQG Controller
General control method for multivariable input,
multivariable output systems.
Optimal for linear systems with Gaussian process and
measurement noise.
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
Overcomes the limitation of full state vector knowledge.
In this lecture, the regulation problem has been
presented (final x=0)
The Kalman filter works regardless of regulation or
tracking.
The LQG controller can be extended to reference
tracking exactly the same way as the LQR controller
(see lecture notes 8).
Course Evaluation
3 Evaluation Forms
Course:
Lecturer:
Tutor:
METR4200
G. Hovland
J. Bray
METR4200 – ADVANCED CONTROL SEMESTER 2, 2004
Anonymous Feedback - your feedback will only be
public after exams. Use black/blue pen or HB pencil.
Only one answer per question.
The purpose of the feedback is to identify what teaching
material is of good quality and what needs improvement
in 2005.
Please also evaluate the CITECT lecture and workshop.

Download Report