Optimal Control And Estimation - Princeton University

Optimal Control and EstimationMAE 546, Princeton UniversityRobert Stengel, 20181Copyright 2018 by Robert Stengel. All rights reserved. For educational use only.http://www.princeton.edu/ stengel/MAE546.htmlhttp://www.princeton.edu/ stengel/OptConEst.htmlPreliminaries!Tuesday and Thursday, 3-4:20 pm!Room 306, Friend Center! ! Reference! ! GRADING!–!–!–!–!Class participation: 15%!5-min quizzes: 10%!Homework assignments: 35%!Final Paper: 40%!–! R. Stengel, Optimal Control and Estimation, Dover, 1994!–! Various journal papers and book chapters! ! Resources!–! Blackboard ! ! ! Course Home Page, Syllabus, and Links! ! www.princeton.edu/ stengel/MAE546.html!–! Engineering Library, including Databases and e-Journals! ! ! ! Assignments will be submitted using the notation andsymbols of this course!!2

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

Typical OptimizationProblems ! Minimize the probable error in an estimate of thedynamic state of a system ! Maximize the probability of making a correctdecision ! Minimize the time or energy required to achieve anobjective ! Minimize the regulation error in a controlledsystem ! Estimation! ! Control!5Dynamic SystemsDynamic Process: Current state depends onprior statex dynamic stateu inputw exogenous disturbancep parametert or k time or event indexObservation Process: Measurement maycontain error or be incompletey output (error-free)z measurementn measurement errorAll of these quantities are vectors6

Mathematical Modelsof Dynamic SystemsDynamic Process: Current state depends onprior statex dynamic stateu inputw exogenous disturbancep parametert time indexObservation Process: Measurement maycontain error or be incompletey output (error-free)z measurementn measurement errorContinuous-time dynamic process:Vector Ordinary Differential Equationdx(t)x! (t) f[x(t), u(t), w(t), p(t),t]dtt time, sOutput Transformationy(t) h[x(t),u(t)]Measurement with Errorz(t) y(t) n(t)7Example: LateralAutomobile Dynamics!Constant forward (axial) velocity, uNo rigid-body rolling motionState Vector!" x #% " Side velocity, m/s' ' Yaw angle rate, rad/s' Lateral position, m ! '' Yaw angle, rad& #vry%'''''&Parameter Vector!#%%%%%p %%%%%% mI zz!Y !v!Y !" steer !N !v!N !" steer & #( %( %( %( %( %( %( %( %( %( %( %' 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!u ! steer Steering angle, rad" vwindw # froad% "%Crosswind, m/s' ''& # Side force on front wheel, N '&Output and Measurement Vectors!" y % " Lateral position, m %y ' ' # ! '& # Yaw angle, rad '&" ymeasured % " y error %' z ' # ! measured '& # ! error '&8

Lateral AutomobileDynamics ExampleDynamic Process!" x! ( t ) #v! ( t )r! ( t )y! ( t )!! ( t )Observation Process!"Y ( x, u, w )% m' ' N ( x, u, w )' I yy' ' u sin ! v cos!'& #r%''''''''&x1 %'x2 'x3 ''x4 '&" " y % " y1 % " 0 0 1 0 % ' y ' ' # ! '& # y2 '& # 0 0 0 1 & #" z1 % " y1 n1' z # z2 '& # y2 n2%''&Discrete-Time Models ofDynamic SystemsDynamic Process: Current state depends onprior statex dynamic stateu inputw exogenous disturbancep parametert time indexObservation Process: Measurement maycontain error or be incompletey output (error-free)z measurementn measurement errorDiscrete-time dynamic process:Vector Ordinary Difference Equationx k 1 fk [x k ,u k ,w k ,p k , k]k time index, -(t k 1 ! t k ) Output Transformationy k h k [x k ,u k ]time interval, sMeasurement with Errorz k y k nk10

Approximate Discrete-Time LateralAutomobile Dynamics Example!Approximate Dynamic Process!(Rectangular Integration)!" x k 1 #vk 1rk 1yk 1! k 1"Yk ( x k ,u k ,w k ) % m' N k ( x k ,u k ,w k )' (' I yy' '& uk sin! k vk cos! k rk#%'''' ( t k 1 ) t k )''''&Observation Process!" ykyk # ! k% " y1k' '& y2 k#%''&" " 0 0 1 0 % ' # 0 0 0 1 & #" z1kzk z2 k#x1k %'x2 k ''x3k 'x4 k ''&%''&" y1 n1kk y2 k n2 k#%''&11Dynamic System Model Types ! NTV ! NTI ! LTV ! LTIdx(t) f [ x(t), u(t), w(t), p(t),t ]dtdx(t) f [ x(t), u(t), w(t)]dtdx(t) F(t)x(t) G(t)u(t) L(t)w(t)dtdx(t) F x(t) G u(t) L w(t)dt12

Controllability andObservability !Controllability: State can be broughtfrom an arbitrary initial condition tozero in finite time by the use ofcontrolObservability: Initial state can bederived from measurements over afinite time intervalSubsets of the system may beeither, both, or neitherEffects of Stability !Blocks subject to feedback control? ! ! !–! Stabilizability–! Detectability13Introduction to Optimization!14

Optimization Implies Choice ! ! ! ! !Choice of best strategyChoice of best design parametersChoice of best control historyChoice of best estimateOptimization is provided byselection of best control variable(s)15Tradeoff BetweenTwo Cost FactorsMinimum-Cost Cruising Speed ! NominalTradeoff ! Fuel CostDoubled–! Fuel cost proportional tovelocity-squared–! Cost of time inverselyproportional to velocity ! Time CostDoubled16

DesirableCharacteristics ofa Cost Function, J ! Scalar ! Clearly defined (preferably unique)maximum or minimum–! Local–! Global ! Preferably positive-definite (i.e., always apositive number)17Criteria for Optimization ! Names for criteria–!–!–!–!–!MinimumFigure of meritPerformance indexUtility functionValue functionCost function, JJ kx 2 ! Optimal cost function J* ! Optimal control u* ! Different criteria lead to differentoptimal solutionsMaximumJ ke! x2 ! Types of Optimality Criteria–! Absolute–! Regulatory–! Feasible18

!MinimumCost Functions with TwoControl Parameters ! 3-D plot of equal-costcontours (iso-contours) !Maximum ! 2-D plot of equal-costcontours (iso-contours)19Real-World TopographyLocal vs. globalmaxima/minima20

Cost Functions withEquality Constraints ! Must stay on the trail ! Equality constraint mayallow control dimension tobe reducedc ( u1 ,u2 ) 0 ! u2 fcn ( u1 )J ( u1 ,u2 ) J !"u1 , fcn ( u1 ) # J ' ( u1 )21Example: Minimize Concentrations ofBacteria, Infected Cells, and Drug Usage ! ! ! ! !x1 Concentration of apathogen, which displaysantigenx2 Concentration ofplasma cells, which arecarriers and producers ofantibodiesx3 Concentration ofantibodies, whichrecognize antigen and killpathogenx4 Relative characteristicof a damaged organ [0 healthy, 1 dead]What is a reasonable cost function to minimize?!22

Optimal Estimate of the State,x, Given Uncertainty23Optimal 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 estimatesbased on the dynamic model alone24

Typical Problems in OptimalControl and Estimation!25Minimize anAbsolute Criterion ! Achieve a specific objective–! Minimum time–! Minimum fuel–! Minimum financial cost ! to achieve a goal ! What is thecontrol variable?26

Optimal System RegulationFind feedback control gains that minimize trackingerror in presence of random disturbances27Feasible Control Logic ! Find feedbackcontrolstructure thatguaranteesstability (i.e.,that keeps !xfrom diverging)http://www.youtube.com/watch?v 8HDDzKxNMEY28

Cost Functions withInequality Constraints ! Must stay to theleft of the trail ! Must stay to theright of the trail29Static vs. DynamicOptimization ! Static–! Optimal state, x*, and control, u*, are fixed,i.e., they do not change over time ! J* J(x*, 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 cases30

Deterministic vs.Stochastic Optimization ! Deterministic–! System model, parameters, initial conditions, anddisturbances are known without error–! Optimal control operates on the system with certainty ! J* J(x*, u*) ! Stochastic–! Uncertainty in ! ! ! ! !system modelparametersinitial conditionsdisturbancesresulting cost function–! Optimal control minimizes theexpected value of the cost: ! Optimal cost E{J[x*, u*]} ! Cost function is a scalar, real numberin both cases31Example: Pursuit-Evasion: #Competitive OptimizationProblem ! !Pursuer s goal: minimize final miss distanceEvader s goal: maximize final miss distance ! Minimax (saddle-point) cost function ! Optimal control laws for pursuer and evader! u P (t) ! C P (t) C PE (t) ! x̂ P (t) & '#&#&u(t) ##" u E (t) &%#" C EP (t) C E (t) &% #" x̂ E (t) &%Example of a differential game, Isaacs (1965),Bryson & Ho (1969)32

Example: SimultaneousLocation and Mapping (SLAM)Durrant- Whyte et al !Build or update a local map withinan unknown environment–! Stochastic map, defined by meanand covariance–! SLAM Algorithm State estimationwith extended Kalman filter–! Landmark and terrain tracking33Next Time:!Minimization of Static CostFunctions!!Reading:!"Optimal Control and Estimation(OCE): Chapter 1, Section 2.1!34

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

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!! x1 #&#x # x2 & ; y ### x &#" 3 %"3x14x13-D Vector ! Transpose: interchange rows andcolumnsxT ! x1"x2x3 # 371x3Matrices and Transpose ! Matrix:–! Usually bold capital or capital: F or F–! Dimension (m x n) ! Transpose:!#A ####"adbelmgh4x3c &f &k &&n &%–! Interchange rows and columns! a d#AT # b e# c f"abcdgl &h m &k n &%3x438 &&&&%

Trace of a Square MatrixnTrace of A ! aiii 1! a b#A # d e# g h"c &f & ; Tr ( A ) a e ii &%39Sums and Multiplication by a Scalar ! Operands must be conformable ! Conformable vectors and matrices are added term by term! (a c)! c ! a x #& ; x z #& ; z #db# (b d )"%"%"! a1A ##" a3 &&%! (a b )! b1 b2 a2 11& ; B #& ; A B ## ( a3 b3 )a4 &#" b3 b4 &%%"( a2 b2 )( a4 b4 ) &&% ! Multiplication of vector by scalar is–! associative–! commutative–! distributive! ax1 &#ax xa # ax2 &# ax &3%"axT ! ax1 ax2"ax3 # 40