Optimal Control and Estimation MAE 546, Princeton University Robert Stengel, Preliminaries!

Transcription

1 Optimal Control and Estimation MAE 546, Princeton University Robert Stengel, 2018 Copyright 2018 by Robert Stengel. All rights reserved. For educational use only Reference Preliminaries R. Stengel, Optimal Control and Estimation, Dover, 1994 Various journal papers and book chapters Resources Blackboard Course Home Page, Syllabus, and Links Engineering Library, including Databases and e-journals Assignments will be submitted using the notation and symbols of this course Tuesday and Thursday, 3-4:20 pm Room 306, Friend Center GRADING Class participation: 15 5-min quizzes: 10 Homework assignments: 35 Final Paper: 40 2

2 Syllabus - 1 Week Tuesday Thursday ==== ======= ======== 1 Overview and Preliminaries Minimization of Static Cost Functions 2 Principles for Optimal Control Principles for Optimal Control of Dynamic Systems Part 2 3 Path Constraints and Minimum-Time and -Fuel Numerical Optimization Optimization 4 Linear-Quadratic LQ) Control Dynamic System Stability 5 Linear-Quadratic Regulators Cost Functions and Controller Structures 6 LQ Control System Design Modal Properties of LQ Systems MID-TERM BREAK 3 Syllabus - 2 Week Tuesday Thursday ==== ======= ======== 7 Spectral Properties of LQ Systems Singular-Value Analysis 8 Probability and Statistics Least-Squares Estimation for Static Systems 9 Propagation of Uncertainty Kalman Filter in Dynamic Systems 10 Kalman-Bucy Filter Nonlinear State Estimation 11 Nonlinear State Estimation Adaptive State Estimation 12 Stochastic Optimal Control Linear-Quadratic-Gaussian Control READING PERIOD Final Paper due on Deans Date 4

3 Typical Optimization Problems Minimize the probable error in an estimate of the dynamic state of a system Maximize the probability of making a correct decision Minimize the time or energy required to achieve an objective Minimize the regulation error in a controlled system Estimation Control 5 Dynamic Systems Dynamic Process: Current state depends on prior state x = dynamic state u = input w = exogenous disturbance p = parameter t or k = time or event index Observation Process: Measurement may contain error or be incomplete y = output error-free) z = measurement n = measurement error All of these quantities are vectors 6

4 Mathematical Models of Dynamic Systems Dynamic Process: Current state depends on prior state x = dynamic state u = input w = exogenous disturbance p = parameter t = time index Observation Process: Measurement may contain error or be incomplete y = output error-free) z = measurement n = measurement error Continuous-time dynamic process: Vector Ordinary Differential Equation xt) = dxt) dt = f[xt),ut),wt),pt),t] t = time, s Output Transformation yt) = h[xt),ut)] Measurement with Error zt) = yt) + nt) 7 Example: Lateral Automobile Dynamics Constant forward axial) velocity, u No rigid-body rolling motion State Vector p = m I zz Y v Y steer N v N steer v Side velocity, m/s r x = Yaw angle rate, rad/s = y Lateral position, m Yaw angle, rad Parameter Vector mass, kg Lateral moment of inertia, N-m Side force sensitivity to side velocity, N/m/s) Side force sensitivity to steering angle, N/rad = Yawing moment sensitivity to side velocity, N-m/m/s) Yawing moment sensitivity to steering angle, N-m/rad u = steer = Steering angle, rad w = Control and Disturbance Vectors v wind f road = Crosswind, m/s Side force on front wheel, N Output and Measurement Vectors y y = = Lateral position, m Yaw angle, rad z = y measured measured = y + error + error 8

5 Lateral Automobile Dynamics Example Dynamic Process Observation Process x t) = ) ) ) ) v t r t y t t = Y x,u,w) m N x,u,w) I yy u sin + vcos r y = z = y z 1 z 2 = y 1 y 2 = y 1 + n 1 y 2 + n 2 = x 3 x 4 Discrete-Time Models of Dynamic Systems Dynamic Process: Current state depends on prior state x = dynamic state u = input w = exogenous disturbance p = parameter t = time index Discrete-time dynamic process: Vector Ordinary Difference Equation Observation Process: Measurement may contain error or be incomplete y = output error-free) z = measurement n = measurement error x k+1 = f k [x k,u k,w k,p k,k] k = time index, - ) = time interval, s t k+1 t k Output Transformation Measurement with Error y k = h k [x k,u k ] z k = y k + n k 10

6 Approximate Discrete-Time Lateral Automobile Dynamics Example x k+1 = Approximate Dynamic Process Rectangular Integration) v k+1 r k+1 y k+1 k+1 ) Y k x k,u k,w k m N k x k,u k,w k ) I yy u k sin k + v k cos k r k t k+1 ) t k ) Observation Process y k = = z k = = y k k = y 1k y 2k z 1k z 2k y 1k y 2k + n 1k + n 2k k k x 3k x 4k 11 Dynamic System Model Types NTV NTI LTV LTI dxt) dt dxt) dt = f [ xt),ut),wt),pt),t ] dxt) dt = f [ xt),ut),wt) ] = Ft)xt) + Gt)ut) + Lt)wt) dxt) = Fxt) + Gut) + Lwt) dt 12

7 Controllability and Observability Controllability: State can be brought from an arbitrary initial condition to zero in finite time by the use of control Observability: Initial state can be derived from measurements over a finite time interval Subsets of the system may be either, both, or neither Effects of Stability Stabilizability Detectability Blocks subject to feedback control? 13 Introduction to Optimization 14

8 Optimization Implies Choice Choice of best strategy Choice of best design parameters Choice of best control history Choice of best estimate Optimization is provided by selection of best control variables) 15 Tradeoff Between Two Cost Factors Minimum-Cost Cruising Speed Nominal Tradeoff Fuel Cost Doubled Fuel cost proportional to velocity-squared Cost of time inversely proportional to velocity Time Cost Doubled 16

9 Desirable Characteristics of a Cost Function, J Scalar Clearly defined preferably unique) maximum or minimum Local Global Preferably positive-definite i.e., always a positive number) 17 Criteria for Optimization Names for criteria Figure of merit Performance index Utility function Value function Cost function, J Optimal cost function = J* Optimal control = u* Minimum J = k Different criteria lead to different optimal solutions Maximum J = ke x2 Types of Optimality Criteria Absolute Regulatory Feasible 18

10 Minimum Cost Functions with Two Control Parameters Maximum 3-D plot of equal-cost contours iso-contours) 2-D plot of equal-cost contours iso-contours) 19 Real-World Topography Local vs. global maxima/minima 20

11 Must stay on the trail Cost Functions with Equality Constraints c u 1,u 2 ) = 0 u 2 = fcn u 1 ) Equality constraint may allow control dimension to be reduced ) = J u 1, fcn u 1 ) J u 1,u 2 ) = J u 1 21 Example: Minimize Concentrations of Bacteria, Infected Cells, and Drug Usage = Concentration of a pathogen, which displays antigen = Concentration of plasma cells, which are carriers and producers of antibodies x 3 = Concentration of antibodies, which recognize antigen and kill pathogen x 4 = Relative characteristic of a damaged organ [0 = healthy, 1 = dead] What is a reasonable cost function to minimize? 22

12 Optimal Estimate of the State, x, Given Uncertainty 23 Optimal State Estimation 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 24

13 Typical Problems in Optimal Control and Estimation 25 Minimize an Absolute Criterion Achieve a specific objective Minimum time Minimum fuel Minimum financial cost to achieve a goal What is the control variable? 26

14 Optimal System Regulation Find feedback control gains that minimize tracking error in presence of random disturbances 27 Feasible Control Logic Find feedback control structure that guarantees stability i.e., that keeps x from diverging) 28

15 Cost Functions with Inequality Constraints Must stay to the left of the trail Must stay to the right of the trail 29 Static vs. Dynamic Optimization Static Optimal state, x*, and control, u*, are fixed, i.e., they do not change over time J* = Jx*, u*) Functional minimization or maximization) Parameter optimization Dynamic Optimal state and control vary over time J* = J[x*t), u*t)] Optimal trajectory Optimal feedback strategy Optimized cost function, J*, is a scalar, real number in both cases 30

16 Deterministic vs. Stochastic Optimization Deterministic System model, parameters, initial conditions, and disturbances are known without error Optimal control operates on the system with certainty J* = Jx*, u*) Stochastic Uncertainty in system model parameters initial conditions disturbances resulting cost function Optimal control minimizes the expected value of the cost: Optimal cost = E{J[x*, u*]} Cost function is a scalar, real number in both cases 31 Example: Pursuit-Evasion: Competitive Optimization Problem Pursuer s goal: minimize final miss distance Evader s goal: maximize final miss distance Minimax saddle-point) cost function Optimal control laws for pursuer and evader ut) = u P t) u E t) = C P t) C PEt) C EP t) C E t) ˆx P t) ˆx E t) Example of a differential game, Isaacs 1965), Bryson Ho 1969) 32

17 Example: Simultaneous Location and Mapping SLAM) Durrant- Whyte et al Build or update a local map within an unknown environment Stochastic map, defined by mean and covariance SLAM Algorithm = State estimation with extended Kalman filter Landmark and terrain tracking 33 Next Time: Minimization of Static Cost Functions Reading: Optimal Control and Estimation OCE): Chapter 1, Section

18 Supplemental Material 35 Math Review Scalars and Vectors Matrices, Transpose, and Trace Sums and Multiplication Vector Products Matrix Products Derivatives, Integrals, and Identity Matrix Matrix Inverse 36

19 Scalars and Vectors Scalar: usually lower case: a, b, c,, x, y, z Vector: usually bold or with underbar: x or x Ordered set Column of scalars Dimension = n Transpose: interchange rows and columns x T = x 3 x = x 3 3-D Vector ; y = a b c d x 3 37 Matrices and Transpose Matrix: Usually bold capital or capital: F or F Dimension = m x n) Transpose: Interchange rows and columns A = a b c d e f g h k l m n 4 x 3 A T = a d g l b e h m c f k n 3 x 4 38

20 Trace of a Square Matrix Trace of A = n i=1 a ii A = a b c d e f g h i ; Tr A) = a + e + i 39 Sums and Multiplication by a Scalar Operands must be conformable Conformable vectors and matrices are added term by term x = a b ; z = c d ; x + z = a + c) b + d) A = a 1 a 2 a 3 a 4 ; B = b 1 b 2 b 3 b 4 ; A + B = a 1 + b 1 ) a 2 + b 2 ) a 3 + b 3 ) a 4 + b 4 ) Multiplication of vector by scalar is associative commutative distributive a ax = xa = a ax 3 ax T = a a ax 3 40

21 Vector Products Inner dot) product of vectors produces a scalar x T x = x x = x 3 1 m)m 1) = 11) x 3 = ) Outer product of vectors produces a matrix xx T = x x = x 3 m 1)1 m) = m m) x 3 = 2 2 x 3 x 3 x 3 x 3 x Matrix Products Matrix-vector product transforms one vector into another y = Ax = a b c d e f g h k l m n n 1) = n m)m 1) x 3 = a + b + cx 3 d + e + fx 3 g + h + kx 3 l + m + nx 3 Matrix-matrix product produces a new matrix a 1 a 2 b 1 b 2 A = ; B = ; AB = a 3 a 4 b 3 b 4 a 1 b 1 + a 2 b 3 ) a 1 b 2 + a 2 b 4 ) a 3 b 1 + a 4 b 3 ) a 3 b 2 + a 4 b 4 ) n m) = n l)l m) 42

22 Inner product x T x = Examples = ) = 14 = length) 2 y = v x v y v z Rotation of expression for velocity vector through pitch angle = 2 cos 0 sin sin 0 cos Matrix product v x v y v z v x1 cos + v z1 sin = v y1 v x1 cos + v z1 sin a c b d = a + 2c 3a + 4c b + 2d 3b + 4d 43 Vector Transformation Example y = Ax = n 1) = n m)m 1) x 3 = x 3 ) x 3 ) x 3 ) ) 9 6 3x 3 = y 1 y 2 y 3 y 4 44

23 Derivatives and Integrals of Vectors Derivatives and integrals of vectors are vectors of derivatives and integrals dx dt = d dt d dt dx 3 dt x dt = dt dt x 3 dt 45 Matrix Identity and Inverse Identity matrix: no change when it multiplies a conformable vector or matrix I 3 = y = Iy A non-singular square matrix multiplied by its inverse forms an identity matrix AA 1 = A 1 A = I AA 1 = = cos 0 sin sin 0 cos cos 0 sin sin 0 cos = cos 0 sin sin 0 cos cos 0 sin sin 0 cos 1 46

24 A non-singular square matrix multiplied by its inverse forms an identity matrix Matrix Inverse AA 1 = A 1 A = I The inverse allows a reverse transformation of vectors of equal dimension y = Ax; x = A 1 y; dimx) = dimy) = n 1); dima) = n n) ) [ A] 1 = Adj A A = Adj A) det A n n) 1 1) Cofactors are signed minors of A = CT det A ; C = matrix of cofactors ij th minor of A is the determinant of A with the i th row and j th column removed 47 Matrix Inverse Example Transformation = A x y z 2 = cos 0 sin sin 0 cos x y z 1 Inverse Transformation = A 1 x y z 1 = cos 0 sin sin 0 cos x y z 2 48