efficient numerical methods for moving horizon...
TRANSCRIPT
Katholieke Universiteit Leuven
Efficient Numerical Methods for Moving Horizon Estimation
Niels Haverbeke
Doctoral presentation – public defense
Promotor
Prof Dr. Ir. Bart De Moor
Co-promotor
Prof Dr. Moritz Diehl
Chairman
Prof. Dr. Ir. Yves Willems
Jury
Prof. Dr. Ir. Johan Suykens Prof. Dr. Ir. Jan Willems Prof. Dr. Ir. Wim Michiels Prof. Dr. Michel Kinnaert (U.L.B.) Prof. Dr. Michel Verhaegen (T.U.Delft) Prof. Dr. Ir. Lieven Vandenberghe (U.C.L.A.)
Niels Haverbeke – Public defense, 16th September 2011
Overview
Motivation Closed loop control scheme
Principle and situation of MHE
Driven by applications
2
1
3
Optimization problem and KKT system
Huber penalty MHE
4
7
Joint estimation of piecewise changing inputs 8 Convex and nonlinear MHE
Structure exploiting
MHE algorithms
Nonlinear MHE and MPC 9
Interior point methods 5
Active-set methods 6
CONCLUSIONS
Niels Haverbeke – Public defense, 16th September 2011
MOTIVATION
MOTIVATION
Niels Haverbeke – Public defense, 16th September 2011
Driven by applications
Recursive techniques, e.g. Kalman filter
Advanced dynamic optimization techniques, e.g. parameter estimation, MPC
Applied to fast systems
Applied to slow systems
Dynamic optimization for fast real-time systems
MOTIVATION
Niels Haverbeke – Public defense, 16th September 2011
States
Angle of attack Pitch rate
Pitch angle Roll rate
Roll angle
Controls
Elevators Ailerons Rudders
Outputs
Pitch angle Path angle
Example
MOTIVATION
Niels Haverbeke – Public defense, 16th September 2011
Past Future
Estimate the states and model from past data
States
Angle of attack Pitch rate
Pitch angle Roll rate
Roll angle
Controls
Elevators Ailerons Rudders
Outputs
Pitch angle Path angle
Example
MOTIVATION
Niels Haverbeke – Public defense, 16th September 2011
Past Future
Estimate the states and model from past data
States
Angle of attack Pitch rate
Pitch angle Roll rate
Roll angle
Controls
Elevators Ailerons Rudders
Outputs
Pitch angle Path angle
Example
MOTIVATION
Niels Haverbeke – Public defense, 16th September 2011
t
t
t
FILTERING:
SMOOTHING:
PREDICTION:
= span of available measurements
TIME
Filtering, smoothing and prediction
Recursive estimation Batch estimation
Window of one time step
Typically online state estimation
Large window
Typically offline optimization
Parameter fitting Kalman filter and extensions
MOTIVATION
Niels Haverbeke – Public defense, 16th September 2011
MHE principle
Dynamic model
Constraints
Subject to
MOTIVATION
Niels Haverbeke – Public defense, 16th September 2011
The role of constraints
What can go wrong?
nonlinear model may give rise to multiple optima
Contours of (rescaled) true conditional probability density
EKF tries to fit
MHE retains dominant characteristics: multiple optima
* Source: Haseltine and Rawlings, 2004
MOTIVATION
Niels Haverbeke – Public defense, 16th September 2011
Waste water treatment process
Fifth order system
-0,5
0
0,5
1
1,5
2
2,5
3
3,5
equal. Tank Tank 1 Tank 2 Tank 3 Waste
MHE
Kalman
True losses
Mean losses Mean squared errors
0
1
2
3
4
5
6
equal. Tank
Tank 1 Tank 2 Tank 3 Waste
MOTIVATION
Niels Haverbeke – Public defense, 16th September 2011
The closed loop control scheme
Controller
Process
Sensor
Estimator
MOTIVATION
Niels Haverbeke – Public defense, 16th September 2011
The closed loop control scheme
MOTIVATION
MHE MPC
Free initial state
Positive semidefinite Hessian
Changing arrival cost
Control dimension ≈ state dimension
Few active constraints
Niels Haverbeke – Public defense, 16th September 2011
STRUCTURE EXPLOITING MHE ALGORITHMS
STRUCTURE EXPLOITING MHE ALGORITHMS
Niels Haverbeke – Public defense, 16th September 2011
The MHE optimization problem
Linear MHE: a quadratic (sub)problem
Writing down the optimality conditions (KKT system), and
Ordering the block rows,
… yields a highly structured linear system of equations
which can be solved with Riccati and vector recursions
STRUCTURE EXPLOITING MHE ALGORITHMS
Niels Haverbeke – Public defense, 16th September 2011
A highly structured KKT system
1. A priori information
Every time step represents one block in the KKT matrix
Information is translated in three steps
2. Model forwarding
3. Measurement updating
STRUCTURE EXPLOITING MHE ALGORITHMS
Niels Haverbeke – Public defense, 16th September 2011
Decomposing the KKT system
= .
M L U
LU decomposition yields the normal Riccati recursion
FORWARD VECTOR SOLVE
RICCATI MATRIX RECURSION
BACKWARD VECTOR SOLVE
Updated initial condition
Final state covariance
STRUCTURE EXPLOITING MHE ALGORITHMS
Niels Haverbeke – Preliminary defense, 1 July 2011
Decomposing the KKT system
LDLT decomposition yields the square-root Riccati recursion
=
M L D
. .
LT
Measurement update and time forwarding via Q-less QR factorizations
Fully exploits symmetry
Yields increased numerical stability
STRUCTURE EXPLOITING MHE ALGORITHMS
Niels Haverbeke – Public defense, 16th September 2011
Riccati based MHE
Computation times for 5th order systems
0 20 40 60 80
Time step
Growing horizon Moving horizon
0 20 40 60 80
Time step
Square-root Riccati
Normal Riccati
x 10-4
0
2
4
6
8
2
4
6
8
10
10
x 10-5
N=10
N=5
N=2
N=1
STRUCTURE EXPLOITING MHE ALGORITHMS
Niels Haverbeke – Public defense, 16th September 2011
Riccati based MHE
Accuracy
Normal
Square-root
10-4
10-2
10-1
10-3
100
2P PError
n=m=p=2 n=m=p=5 n=m=p=10
STRUCTURE EXPLOITING MHE ALGORITHMS
Niels Haverbeke – Public defense, 16th September 2011
Structured QR factorization
10-16
10-14
10-13
Average error 2
QR A
10-15
0 10 20 30 40 50 10-6
Dimension n
10-5
10-4
10-3
0 10 20 30 40 50
Dimension n
A =
Computation time (sec)
Givens
Householder
Structured Givens
Structured Householder
STRUCTURE EXPLOITING MHE ALGORITHMS
10-2
Niels Haverbeke – Public defense, 16th September 2011
Interior-point MHE
Primal barrier method
Newton method
With
STRUCTURE EXPLOITING MHE ALGORITHMS
Niels Haverbeke – Public defense, 16th September 2011
Interior-point MHE
With and
Modified Riccati recursion
100
102
104
Condition number (Mk)
Condition number (Dk) 101
103
100
102
104
101
103
2
iteration
4 6 8 10 12 14 2
iteration
4 6 8 10 12 14
1 / √κ
Normal update
Square-root update
STRUCTURE EXPLOITING MHE ALGORITHMS
Niels Haverbeke – Public defense, 16th September 2011
Interior-point MHE
Computation times
Finite number of iterations with decreasing
Example - second order system
10 iterations yield satisfying results in this case
True state
MHE 10 iterations
MHE converged
STRUCTURE EXPLOITING MHE ALGORITHMS
Niels Haverbeke – Public defense, 16th September 2011
Interior-point MHE
Hot starting
STRUCTURE EXPLOITING MHE ALGORITHMS
Niels Haverbeke – Public defense, 16th September 2011
Interior-point MHE
Hot starting
A good initialization is necessary for fast convergence
Hot starting with the previous solution or the proposed strategy
Yields convergence improvement for first iterations
STRUCTURE EXPLOITING MHE ALGORITHMS
Niels Haverbeke – Public defense, 16th September 2011
A Schur-complement active-set method
Method outline
Compute unconstrained MHE solution
Add to working set
Check constraints
Solve QP in projected space
Fast Riccati based computation
Violated constraints
Searching for such that
Define
Update
Non-negativity constrained QP
Remove inactive constraints and terminate
STRUCTURE EXPLOITING MHE ALGORITHMS
Niels Haverbeke – Public defense, 16th September 2011
A Schur-complement active-set method
Gradient projection method for non-negativity constrained QP
x1
x2
1. Cauchy calculation step
2. Projected Newton step
Without projected Newton step
With projected Newton step
Convergence
STRUCTURE EXPLOITING MHE ALGORITHMS
Principle
Niels Haverbeke – Public defense, 16th September 2011
A Schur-complement active-set method
1. Use semidefinite Cholesky factorization of M
2. Set
3. Keep adding non-positive constraints to working set
4. Delete rows and colums of (new) working set constraints
5. Continue until all components non-negative
Gradient projection method for non-negativity constrained QP
Projected Newton step
Between outer active-set iterations: Cholesky block downdating (constraints added)
Upon termination: Cholesky block updating (constraints removed)
STRUCTURE EXPLOITING MHE ALGORITHMS
Niels Haverbeke – Public defense, 16th September 2011
A Schur-complement active-set method
Convergence
Time step
0
10
20
30
40
0 100 200 300 400 500
1
2
3
4
# final working set constraints
# active constraints
# active-set iterations
STRUCTURE EXPLOITING MHE ALGORITHMS
Computational burden
uMHE asetMHE Total
Riccati 1 1
Fsolve 1 1
Partial Fsolve
nA nA
Bsolve 1 nit 1+nit
Red. QP
nit
nit
Niels Haverbeke – Public defense, 16th September 2011
A Schur-complement active-set method
0 100 200 300 400 500
Time step
0
0.5
1
1.5
x 10-3
Interior-point method 10 iterations
Schur-complement active-set method
Computation time
STRUCTURE EXPLOITING MHE ALGORITHMS
Niels Haverbeke – Public defense, 16th September 2011
CONVEX AND NONLINEAR MHE
CONVEX AND NONLINEAR MHE
Niels Haverbeke – Public defense, 16th September 2011
Huber penalty MHE
-M M 0
The Huber penalty
Preserves LS performance
Increases robustness to outliers
Univariate QP
Multivariate SOCP
CONVEX AND NONLINEAR MHE
Niels Haverbeke – Public defense, 16th September 2011
Huber penalty MHE
Computation
time (sec)
Componentwise univariate Huber
Multivariate Huber
Univariate vs multivariate
CONVEX AND NONLINEAR MHE
n x 2 variables
1 x n+1 variables
Niels Haverbeke – Public defense, 16th September 2011
Huber penalty MHE
Output
disturbance
0
0,01
0,02
0,03
0,04
0,05
0,06
0,07
L2 L1 Huber Huber + L2
Simulation
Prediction
CONVEX AND NONLINEAR MHE
Batch estimation
Mean squared error
(50 repetitions)
500 time steps
50 time steps
2,5 %
contaminated
with N(0,5)
Niels Haverbeke – Public defense, 16th September 2011
Joint estimation with piecewise inputs
CONVEX AND NONLINEAR MHE
F16 example – linearized longitudinal model
4 states: velocity, angle-of-attack, pitch angle, pitch rate
2 outputs: pitch angle, flight path angle
1 input: elevator deflection
Input: Elevator deflection (deg) State and output - Pitch angle (rad)
Time step Time step
Niels Haverbeke – Public defense, 16th September 2011
Joint estimation with piecewise inputs
Joint MHE: quality of input estimates
L1 MHE
Cardinality MHE with polishing step
CONVEX AND NONLINEAR MHE
L2 MHE
Low output disturbance level R=0.001 High output disturbance level R=0.01
Niels Haverbeke – Public defense, 16th September 2011
Nonlinear MHE
CONVEX AND NONLINEAR MHE
Nonlinear MHE =
Structured NLP
Structured QP with inequality constraints
SQP Gauss Newton with globalization
Active-set or interior-point method
Linear algebra
Structured equality constrained QP
Structured KKT system
Niels Haverbeke – Public defense, 16th September 2011
Nonlinear MHE
Estimation and control of glycemia in critically-ill patients
Controlled variable: glycemia level (G)
Known input: carbohydrate calories flow (FG)
Unkown input: medication (FM)
Manipulated variable: exogenous insulin (FI)
Regulate glycemia to normoglycemic range (80-110 mg/dl)
CONVEX AND NONLINEAR MHE
Niels Haverbeke – Public defense, 16th September 2011
Nonlinear MHE
Estimation and control of glycemia in critically-ill patients
CONVEX AND NONLINEAR MHE
Niels Haverbeke – Public defense, 16th September 2011
CONCLUSIONS
CONCLUSIONS
Niels Haverbeke – Public defense, 16th September 2011
KKT conditions reveal symmetry and structure
Block diagonal structure is preserved in interior-point methods
Decomposition yields Riccati methods
Proposed and demonstrated square-root Riccati method using QR factorizations
Proposed and demonstrated modified square-root Riccati method
Block diagonal structure is NOT preserved in active-set methods
Proposed and demonstrated a dedicated Schur-complement active-set method
Huber penalty increases robustness to outliers
Demonstrated Huber penalty MHE
Joint input estimation with piecewise inputs has finite number of break points
Proposed and demonstrated cardinality MHE yielding a sequence of L1-type MHE
Nonlinear MHE can be solved by SQP Gauss-Newton method
Demonstrated NMHE on a biomedical application
Conclusions
CONCLUSIONS
Niels Haverbeke – Public defense, 16th September 2011
Future research
Applications
Automotive
Power electronics
Algorithms
Ultra-fast nonlinear MHE: fast simulation
Distributed MHE
Adaptive control: interaction between MHE and MPC
CONCLUSIONS
Intensive Care Unit
Niels Haverbeke – Preliminary defense, 1 July 2011
Battery
Auxiliaries
DC/DC converter
Charger
Infrastructure
DC/DC converter
Inv
MG
Inv
MG
ECU
Torque vectoring control
Look ahead active suspension
Battery parameter estimation and battery management
Power converter control and fast charging power control
Future research
CONCLUSIONS
Niels Haverbeke – Public defense, 16th September 2011
Promotor
Acknowledgements
Co-promotor
Bart De Moor
Moritz Diehl
Co-authors
Tom Van Herpe, Greet Van den Berghe, Toni Barjas-Blanco, Steven Gillijns, Bert Pluymers, Marcello Espinoza, Joachim Ferreau, Patrick Willems, Jean Berlamont, Po-Kuan Chiang
Financial support
IWT, FWO-Vlaanderen
CONCLUSIONS