Optimal Control and Estimation MAE 546, Princeton University Robert Stengel, 2017 Copyright 2017 by Robert Stengel. All rights reserved. For educational use only. http://www.princeton.edu/~stengel/mae546.html http://www.princeton.edu/~stengel/optconest.html 1 Tuesday and Thursday, 3-4:20 pm Room 306, Friend Center Preliminaries Reference R. Stengel, Optimal Control and Estimation, Dover, 1994 Various journal papers and book chapters Resources Blackboard https://blackboard.princeton.edu/webapps/login Course Home Page, Syllabus, and Links www.princeton.edu/~stengel/mae546.html Engineering Library, including Databases and e-journals http://library.princeton.edu/catalogs/articles.php GRADING, tentative Class participation: 35 Mid-Term Paper: 15 Final Paper: 50 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 Mid-Term Paper due 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
Seminar Format Introduction Interactive Presentation and Discussion of Syllabus Topic Course Slides Assigned reading Additional visuals Will attempt to adhere to Syllabus Schedule After first few weeks, discussion led by students Discussion of recent journal papers Mid-Term Paper, Final Paper 5 Seminar Grading Rubric from CMU, 2000) 6
Seminar Grading Rubric from CMU, 2000) 7 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 8
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 9 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) 10
p = Example: Lateral Automobile Dynamics m I zz Y v Y steer N v N steer Constant forward axial) velocity, u No rigid-body rolling motion State Vector v Side velocity, m/s r x = Yaw angle rate, rad/s = y Lateral position, m Yaw angle, rad Parameter Vector u = steer = Steering angle, rad w = 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 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 11 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 = 0 0 1 0 0 0 0 1 x 1 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] t k+1 t k k = time index, - ) = time interval, s Output Transformation Measurement with Error y k = h k [x k,u k ] z k = y k + n k 13 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 = y k k = 0 0 1 0 0 0 0 1 z k = = y 1k y 2k z 1k z 2k + n 1k + n 2k y 1k y 2k x 1k k x 3k x 4k 14
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 15 Controllability and Observability are Fundamental Characteristics of a Dynamic System 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? 16
Introduction to Optimization 17 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) 18
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 19 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) 20
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 21 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) 22
Real-World Topography Local vs. global maxima/minima 23 Cost Functions with Equality Constraints Must stay on the trail c u 1,u 2 ) = 0 u 2 = fcn u 1 ) then ) = J u 1, fcn u 1 ) J u 1,u 2 ) = J u 1 Equality constraint may allow control dimension to be reduced 24
Example: Minimize Concentrations of Bacteria, Infected Cells, and Drug Usage x 1 = 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? 25 Optimal Estimate of the State, x, Given Uncertainty 26
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 27 Typical Problems in Optimal Control and Estimation 28
Minimize an Absolute Criterion Achieve a specific objective Minimum time Minimum fuel Minimum financial cost to achieve a goal What is the control variable? 29 Optimal System Regulation Find feedback control gains that minimize tracking error in presence of random disturbances 30
Feasible Control Logic Find feedback control structure that guarantees stability i.e., that keeps x from diverging) http://www.youtube.com/watch?v=8hddzkxnmey 31 Cost Functions with Inequality Constraints Must stay to the left of the trail Must stay to the right of the trail 32
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 33 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 34
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 t) PE C EP t) C E t) ˆx P t) ˆx E t) Example of a differential game, Isaacs 1965), Bryson Ho 1969) 35 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 36
SLAM Senior Thesis Research Quan Zhou, 15 Turtlebot Kinect 1 SLAM Navigation Results 37 Next Time: Minimization of Static Cost Functions Reading: Optimal Control and Estimation OCE): Chapter 1, Section 2.1 38
Supplemental Material 39 Math Review Scalars and Vectors Matrices, Transpose, and Trace Sums and Multiplication Vector Products Matrix Products Derivatives, Integrals, and Identity Matrix Matrix Inverse 40
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 x 1 x = x 1 x 3 3-D Vector ; y = a b c d 3 x 1 4 x 1 Transpose: interchange rows and columns x T = x 1 x 3 1 x 3 41 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 42
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 43 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 ax 1 ax = xa = a ax 3 ax T = ax 1 a ax 3 44
Vector Products Inner dot) product of vectors produces a scalar x T x = x x = x 1 x 3 1 m)m 1) = 11) x 1 x 3 = 1 + 2 + 3 ) Outer product of vectors produces a matrix xx T = x x = x 1 x 3 m 1)1 m) = m m) x 1 x 3 = x 1 2 x 1 2 x 1 x 1 x 3 x 3 x 3 x 1 x 3 x 3 2 45 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 1 x 3 = ax 1 + b + cx 3 dx 1 + e + fx 3 gx 1 + h + kx 3 lx 1 + 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) 46
Inner product x T x = 1 2 3 Examples 1 2 3 = 1+ 4 + 9) = 14 = length) 2 y = v x v y v z Rotation of expression for velocity vector through pitch angle = 2 cos 0 sin 0 1 0 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 1 1 2 3 4 a c b d = a + 2c 3a + 4c b + 2d 3b + 4d 47 Vector Transformation Example y = Ax = 2 4 6 3 5 7 4 1 8 9 6 3 n 1) = n m)m 1) x 1 x 3 = 2x 1 + 4 + 6x 3 ) 3x 1 5 + 7x 3 ) 4x 1 + + 8x 3 ) ) 9x 1 6 3x 3 = y 1 y 2 y 3 y 4 48
Derivatives and Integrals of Vectors Derivatives and integrals of vectors are vectors of derivatives and integrals dx dt = dx 1 dt d dt dx 3 dt x dt = x 1 dt dt x 3 dt 49 Matrix Identity and Inverse Identity matrix: no change when it multiplies a conformable vector or matrix I 3 = 1 0 0 0 1 0 0 0 1 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 0 1 0 sin 0 cos cos 0 sin 0 1 0 sin 0 cos = 1 0 0 0 1 0 0 0 1 cos 0 sin 0 1 0 sin 0 cos cos 0 sin 0 1 0 sin 0 cos 1 50
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 51 Matrix Inverse Example Transformation = Ax 1 x y z 2 = cos 0 sin 0 1 0 sin 0 cos x y z 1 Inverse Transformation x 1 = A 1 x y z 1 = cos 0 sin 0 1 0 sin 0 cos x y z 2 52