Lecture 9. Introduction to Kalman Filtering. Linear Quadratic Gaussian Control (LQG) G. Hovland 2004

Size: px

Start display at page:

Download "Lecture 9. Introduction to Kalman Filtering. Linear Quadratic Gaussian Control (LQG) G. Hovland 2004"

Candice Ferguson
5 years ago
Views:

1 MER42 Advanced Control Lecture 9 Introduction to Kalman Filtering Linear Quadratic Gaussian Control (LQG) G. Hovland 24 Announcement No tutorials on hursday 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 lab this week or early next week. You can arrange individual consultations (by ) leading up to the final exam on Nov. 16.

2 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 Quadratic Regulator

3 LQR Scalar Case See next slide Matlab Script for Minimising J(k) Divide by b

LQR Controller Gain k 2 General form Summary LQR Controller Keep the proof for the LQR controller in mind he proof for the (steady-state) optimal observer is similar

4 LQR Controller Gain k 2 General form Summary LQR Controller Keep the proof for the LQR controller in mind he proof for the (steady-state) optimal observer is similar Optimal state feedback gain K R SA 1 + B A Matrices A,B in linear state-space model S S SBR Riccati solution 1 B S + Q Riccati equation Penalty Matrices Controller uning Parameters

5 State-feedback design using observers A) Unforced observer B) Forced observer Details of forced observer In Lecture 7 we designed the observer by pole-placement. he Kalman filter is the optimal observer. Introduction to the Kalman Filter Kalman filter invented in 196 by R. E. Kalman It is the optimal estimator under a few assumptions. First derived as a discrete filter. his is how it is thought of by many (signal processing, etc.). Kalman and Busy extended the filter to the continuous version in his is the version commonly used in control systems engineering, and the filter discussed in this presentation.

6 Starting Point: he State-Space Model Recall a linear system in state-space form:. Know input vector u x Ax + Bu and output vector y, y Cx as well as the matrices A, B, and C perfectly. 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 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) he controller and observer can be designed independently!

7 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: J K R [x(t) Qx(t) + u(t) Ru(t)]dt 1 SA + A S SBR S + Q Now, how do we choose the observer gain L? B S 1 B State-Space Model with Noise In practice, there are other unknown inputs:. Disturbances on states x Ax + Bu + d y Cx + n Noise in sensors here is no obvious best way to choose L: x x x X(s) Single output system helps visualisation error in estimated state Want L small (si A) -1 (si A) -1 L D(s) + N(s) 1 + C (si A) -1 L 1 + C (si A) -1 L Want L big

8 Assumptions about the Noise d(t) and n(t) are stationary, zero mean, Gaussian white noise processes E[d(t)] E[n(t)] E[d(t)d(τ) ] Q δ(t-τ) E[n(t)n(τ) ] R δ (t-τ) Initial state has mean and covariance x E[x()] Q x E[(x() x )(x() x ) )] Covariances Q and R }Constant spectral densities delta function 1 Assume d(t), n(t), and x() are mutually uncorrelated E[d(t)n(t) ] E[x()d(t) ] E[x()n(t) ] Minimisation of Mean-Square Error he Kalman filter generates the linear estimate of the plant that minimises the mean square estimation error: J E[(x(t) x(t)) (x(t) x(t))] he optimal linear estimator can be found by setting the differential of the mean square error, with respect to the estimator parameters, to. Results in a necessary condition: E[(x(t) x(t))y(t) ] his results in enough equations to find the optimal gain L (using the definition of optimal above).

9 Kalman Filter Equations Optimal Gain Error Covariance Matrix L(t) S e (t)c R -1. where S e (t) E[(x(t) x(t))(x(t) x(t)) ] S e (t) S e (t)a + AS e (t) + Q - S e (t)c R -1 C S e (t) Ricatti Equation S e (t) can be computed off-line with numerical integration (just need S e () S ), therefore, L(t) can be computed off-line. S e (t) gives a constant measure of the error in the filter! Note: he optimal L has very similar structure to the LQR controller gain K. B replaced by C and A is transposed he 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 S e C R -1 Sub-optimal Gain where S e E[(x(t) x(t))(x(t) x(t)) ] S e A + AS e + Q - S e C R -1 C S e Error Covariance Matrix Algebraic Ricatti Equation For solution to A.R.E., we can integrate S e (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

10 Proof of Steady-State Kalman Filter Equations Formulate Kalman Filter as an LQR problem x& xˆ & z x xˆ z& Az + LCz z& Ax + B u Axˆ + Bu + L( y yˆ ) z A + z C L State estimation error z LQR Analogy: C acts as B L acts as K A matrix transposed Comparison LQR and Kalman Solutions K R 1 B S LQR SA + A S SBR 1 B S + Q L L S S R e e C A R CS + 1 e AS e R S 1 e CS C e R 1 CS Note: Q and R are penalty matrices in LQR Q and R are noise covariances in Kalman Filter e Steady-State Kalman Filter + Q LQR Analogy: C acts as B L acts as K A matrix transposed

11 Kalman Filter: Performance Index J Optimality Criterion: Same as LQR where zx and y-y ^ u J ( x xˆ) ( y z Qz + ( y yˆ ) e yˆ ) Q( x xˆ) + ( y S ( y yˆ ) dt R( y Can be shown yˆ) dt yˆ ) R( y yˆ ) dt he Kalman filter minimises the squared prediction error Summary so Far: Kalman Filter Produces best possible state estimate under the following conditions: Gaussian process noise with covariance Q Gaussian measurement noise with covariance R Perfect knowledge of linear state-space model he state prediction error x-x ^ is optimal, ie. x-x ^ contains only zero mean white noise with smallest possible covariance.

12 What if the Noise is not White? Create a coloured noise filter, and add it to the system. Coloured noise filter: coloured noise output. x f (t) A f x f (t) + B f w(t) d(t) C f x f (t) white noise input Add it to original system, for a new system:. x(t) + + x. A C x(t) f B u(t) w(t) f (t) A x f f (t) y(t) [ C ] x(t) + v(t) x f (t) B f 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. Advantage: We can control multivariable systems without measuring the entire state vector x. Disadvantage: he LQG controller does not have the 6 o phase margin and the gain margins of the LQR controller.

13 LQG Controller: Schematics Kalman Filter Gain Normal LQR design Useful Matlab Functions lqe: kalman: estim: lqg: lqgreg: Returns stationary Kalman estimator gain L Returns entire stationary Kalman estimator General estimator design (pole-placement or Kalman Filter) LQG controller (LQR + Kalman filter) Designs LQG controller given an estimator

LQG Control: Vertical-Plane Aircraft Dynamics Example from: Hung and MacFarlane, "Multivariable feedback: A quasi-classical approach", Lecture Notes in Control and Information Sciences, Vol. 4, 1982.

14 LQG Control: Vertical-Plane Aircraft Dynamics Example from: Hung and MacFarlane, "Multivariable feedback: A quasi-classical approach", Lecture Notes in Control and Information Sciences, Vol. 4, x 1 : x 2 : x 3 : x 4 : x 5 : altitude in (m) forward speed (m/s) pitch angle (deg) pitch rate (deg/s) vertical speed (m/s) u 1 : spoiler angle (deg/1) u 2 : forward acceleration (m/s 2 ) u 3 : elevator angle (deg) Aircraft: Linearised State-Space Model x& Ax + Bu + w y Cx + v Linearised model at an operating point (for example at cruising altitude and speed) A B C x 4 and x 5 are not measured. Kalman filter will be used to estimate these states.

15 Simulink Model Process Noise w Covariance Q Measurement Noise v Covariance R x& y Ax Cx + Bu + + v w Matrix Gains Uncontrolled Response: u x& y Ax + Bu + Cx + v w y 1 y 2 x [ 1 1 ] y 3 Q diag R Random walk when noise is integrated. Noise Covariance Q (9,9,16,1,4)

16 Uncontrolled Response: Sensor Noise x& Ax + Bu + y Cx + v w y 1 Sensor Noise Covariance R y 2 y 3 x [ 1 1 ] Q R diag(.1,.4,.9) Kalman Filter: Simulink Implementation Process: Hardware Kalman Filter: Software

17 ^ Actual (y) vs Filtered (y) Measurements x ˆ x Filtered y ^ 3 ^ Actual (y) vs Filtered (y) Measurements Initial y ^ 1 Filter ransient x ˆ

18 ^ Actual States (x) vs Filtered States (x) x 1 and x ^ 1 x 4 and x 5 are not measured. x ^ 4 x^ 5 x 2 and x ^ 2 ^ x 3 and x 3 Hence, in practice it will not be possible to verify these two estimates. 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 you get satisfactory estimates. he Kalman Filter is only optimal for linear state-space models. As a control engineer you will meet the term "he Extended Kalman Filter". his filter is used on nonlinear models by frequent linearisation around the operating state estimate. he Extended Kalman Filter is not optimal.

19 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. When simulation works satisfactorily, replace the simulated model with physical plant / process. Aircraft: LQR assuming known states x

20 LQR Control: Faster Altitude Response LQG Control Architecture

21 ^ Actual (y) vs Filtered (y) Outputs Controls to zero, x ^ Actual (x) vs Filtered (x) States

22 Summary LQG Controller General control method for multivariable input, multivariable output systems. Optimal for linear systems with Gaussian process and measurement noise. Overcomes the limitation of full state vector knowledge. In this lecture, the regulation problem has been presented (final x) he Kalman filter works regardless of regulation or tracking. he 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: MER42 Lecturer: G. Hovland utor: J. Bray Anonymous Feedback - your feedback will only be public after exams. Use black/blue pen or HB pencil. Only one answer per question. he purpose of the feedback is to identify what teaching material is of good quality and what needs improvement in 25. Please also evaluate the CIEC lecture and workshop.

Course Outline. Higher Order Poles: Example. Higher Order Poles. Amme 3500 : System Dynamics & Control. State Space Design. 1 G(s) = s(s + 2)(s +10)

Course Outline. Higher Order Poles: Example. Higher Order Poles. Amme 3500 : System Dynamics & Control. State Space Design. 1 G(s) = s(s + 2)(s +10) Amme 35 : System Dynamics Control State Space Design Course Outline Week Date Content Assignment Notes 1 1 Mar Introduction 2 8 Mar Frequency Domain Modelling 3 15 Mar Transient Performance and the s-plane