[PPT] - Mario Zanon Numerical methods for NMPC Thanks to S. Gros, M. PowerPoint Presentation

SLIDE 1

Mario Zanon Numerical methods for NMPC Thanks to

S. Gros, M. Vukov, M. Diehl

and the whole KU Leuven group for some of the material and their precious help

ESAT – Katholieke Universiteit Leuven

SLIDE 2

Outline

2 / 47

1

From Linear Feedback to NMPC

2

Nonlinear Programming and SQP

3

From Continuous Time to Discrete Time

4

Moving Horizon Estimation

5

Practical NMPC

6

Tutorial

SLIDE 3

From Linear Feedback to NMPC

3 / 47

1

From Linear Feedback to NMPC

2

Nonlinear Programming and SQP

3

From Continuous Time to Discrete Time

4

Moving Horizon Estimation

5

Practical NMPC

6

Tutorial

SLIDE 4

From Linear Feedback to NMPC

4 / 47

Linear system sk+1 = Ask + Buk

SLIDE 5

From Linear Feedback to NMPC

4 / 47

Linear system sk+1 = Ask + Buk Linear feedback uk = −Ksk sk+1 = (A − BK)sk = AKsk

SLIDE 6

From Linear Feedback to NMPC

4 / 47

Linear system sk+1 = Ask + Buk Linear feedback uk = −Ksk sk+1 = (A − BK)sk = AKsk stable if max

|λ(AK)|
≤ 1

SLIDE 7

From Linear Feedback to NMPC

4 / 47

Linear system sk+1 = Ask + Buk Linear feedback uk = −Ksk sk+1 = (A − BK)sk = AKsk stable if max

|λ(AK)|
≤ 1

How to choose K?

SLIDE 8

From Linear Feedback to NMPC

4 / 47

Linear system sk+1 = Ask + Buk Linear feedback uk = −Ksk sk+1 = (A − BK)sk = AKsk stable if max

|λ(AK)|
≤ 1

How to choose K?

What about LQR? min

s,u

1 2

∞

k=0

sk2

Q + uk2 R

s.t. s0 = ¯ x sk+1 = Ask + Buk, k ≥ 0 lim

k→∞ sk = 0

SLIDE 9

From Linear Feedback to NMPC

4 / 47

Linear system sk+1 = Ask + Buk Linear feedback uk = −Ksk sk+1 = (A − BK)sk = AKsk stable if max

|λ(AK)|
≤ 1

How to choose K?

What about LQR? min

s,u

1 2

∞

k=0

sk2

Q + uk2 R

s.t. s0 = ¯ x sk+1 = Ask + Buk, k ≥ 0 lim

k→∞ sk = 0

Equivalent to solving the DARE (discrete algebraic Riccati equation) P = Q + AT PA − AT PBK K = (R + BT PB)−1BT PA

SLIDE 10

From Linear Feedback to NMPC

5 / 47

Note the equivalence Horizon: ∞

min

s,u

1 2

∞

k=0

sk2

Q + uk2 R

s.t. s0 = ¯ x sk+1 = Ask + Buk, k ≥ 0 lim

k→∞ sk = 0

⇔

SLIDE 11

From Linear Feedback to NMPC

5 / 47

Note the equivalence Horizon: ∞

min

s,u

1 2

∞

k=0

sk2

Q + uk2 R

s.t. s0 = ¯ x sk+1 = Ask + Buk, k ≥ 0 lim

k→∞ sk = 0

⇔

Horizon: N

min

s,u

1 2

N

k=0

sk2

Q + uk2 R + 1

2 sN2

P

s.t. s0 = ¯ x sk+1 = Ask + Buk, k = 0, . . . , N − 1 with N ≥ 1 and P from the DARE.

SLIDE 12

From Linear Feedback to NMPC

5 / 47

Note the equivalence Horizon: ∞

min

s,u

1 2

∞

k=0

sk2

Q + uk2 R

s.t. s0 = ¯ x sk+1 = Ask + Buk, k ≥ 0 lim

k→∞ sk = 0

⇔

Horizon: N

min

s,u

1 2

N

k=0

sk2

Q + uk2 R + 1

2 sN2

P

s.t. s0 = ¯ x sk+1 = Ask + Buk, k = 0, . . . , N − 1 with N ≥ 1 and P from the DARE.

The term 1

2sN2 P is called cost to go

SLIDE 13

From Linear Feedback to NMPC

5 / 47

Note the equivalence Horizon: ∞

min

s,u

1 2

∞

k=0

sk2

Q + uk2 R

s.t. s0 = ¯ x sk+1 = Ask + Buk, k ≥ 0 lim

k→∞ sk = 0

⇔

Horizon: N

min

s,u

1 2

N

k=0

sk2

Q + uk2 R + 1

2 sN2

P

s.t. s0 = ¯ x sk+1 = Ask + Buk, k = 0, . . . , N − 1 with N ≥ 1 and P from the DARE.

The term 1

2sN2 P is called cost to go

If we don’t want to solve the DARE Choose P large enough Solve the finite horizon problem: Quadratic Program (QP)

SLIDE 14

From Linear Feedback to NMPC

6 / 47

At each sampling instant i, solve the QP

min

u,s

1 2

N−1

k=0

sk2

Q + uk2 R + 1

2 sN2

P

s.t. s0 = ˆ xi sk+1 = A sk + B uk

SLIDE 15

From Linear Feedback to NMPC

6 / 47

At each sampling instant i, solve the QP

min

u,s

1 2

N−1

k=0

sk2

Q + uk2 R + 1

2 sN2

P

s.t. s0 = ˆ xi sk+1 = A sk + B uk ⇔ min

w

1 2 wT Fw + f T w s.t. Gw + g = 0

SLIDE 16

From Linear Feedback to NMPC

6 / 47

At each sampling instant i, solve the QP

min

u,s

1 2

N−1

k=0

sk2

Q + uk2 R + 1

2 sN2

P

s.t. s0 = ˆ xi sk+1 = A sk + B uk ⇔ min

w

1 2 wT Fw + f T w s.t. Gw + g = 0 Lagrangian Function L(w, λ, µ) = 1 2 wT Fw + f T w − λT (Gw + g)

SLIDE 17

From Linear Feedback to NMPC

6 / 47

At each sampling instant i, solve the QP

min

u,s

1 2

N−1

k=0

sk2

Q + uk2 R + 1

2 sN2

P

s.t. s0 = ˆ xi sk+1 = A sk + B uk ⇔ min

w

1 2 wT Fw + f T w s.t. Gw + g = 0 Lagrangian Function L(w, λ, µ) = 1 2 wT Fw + f T w − λT (Gw + g) First order necessary condition (FONC) ∇L(w, λ, µ) = 0 ⇒

Fw + f − G T λ = 0

Gw + g = 0

SLIDE 18

From Linear Feedback to NMPC

6 / 47

At each sampling instant i, solve the QP

min

u,s

1 2

N−1

k=0

sk2

Q + uk2 R + 1

2 sN2

P

s.t. s0 = ˆ xi sk+1 = A sk + B uk ⇔ min

w

1 2 wT Fw + f T w s.t. Gw + g = 0 Lagrangian Function L(w, λ, µ) = 1 2 wT Fw + f T w − λT (Gw + g) First order necessary condition (FONC) ∇L(w, λ, µ) = 0 ⇒

Fw + f − G T λ = 0

Gw + g = 0 Solve a linear system:

F

G T G w −λ

= −

f g

SLIDE 19

From Linear Feedback to NMPC

7 / 47

Treating Constrained Systems min

u,s

1 2

N−1

k=0

sk2

Q + uk2 R + 1

2sN2

P

s.t. s0 = ˆ xi sk+1 = A sk + B uk LQR: unconstrained

SLIDE 20

From Linear Feedback to NMPC

7 / 47

Treating Constrained Systems min

u,s

1 2

N−1

k=0

sk2

Q + uk2 R + 1

2sN2

P

s.t. s0 = ˆ xi sk+1 = A sk + B uk C sk + D uk + c ≥ 0 LQR: unconstrained MPC: state and input constraints

SLIDE 21

From Linear Feedback to NMPC

7 / 47

Treating Constrained Systems min

u,s

1 2

N−1

k=0

sk2

Q + uk2 R + 1

2sN2

P

s.t. s0 = ˆ xi sk+1 = A sk + B uk C sk + D uk + c ≥ 0 LQR: unconstrained MPC: state and input constraints sN2

P only approximates the cost

to go

SLIDE 22

From Linear Feedback to NMPC

7 / 47

Treating Constrained Systems min

u,s

1 2

N−1

k=0

sk2

Q + uk2 R + 1

2sN2

P

s.t. s0 = ˆ xi sk+1 = A sk + B uk C sk + D uk + c ≥ 0 LQR: unconstrained MPC: state and input constraints sN2

P only approximates the cost

to go Handle explicitly: Actuator limitations, e.g. saturation of an input signal State constraints, e.g. concentration of a reactant Mixed state-input constraints MPC yields a nonlinear control law!

SLIDE 23

From Linear Feedback to NMPC

8 / 47

At each sampling instant i, solve the QP

min

u,s P

k=0

sk − xref 2

Q + P−1

k=0

uk − uref 2

R

s.t. s0 = ˆ xi sk+1 = A sk + B uk C sk + D uk + c ≥ 0 ⇔ min

w

1 2 wT Fw + f T w s.t. Gw + g = 0 Hw + h ≥ 0 Lagrangian Function L(w, λ, µ) = 1 2 wT Fw + f T w − λT (Gw + g) − µT (Hw + h) First order necessary condition (FONC): the KKT conditions ∇L(w, λ, µ) = 0 ⇒          Fw + f − G T λ − HT µ = 0 Gw + g = 0 Hw + h ≥ 0 µ ≥ 0 µi(Hiw + hi) = 0

SLIDE 24

From Linear Feedback to NMPC

9 / 47

Solving the KKT conditions Fw + f − G Tλ − HTµ = 0 Gw + g = 0 Hw + h ≥ 0 µ ≥ 0 µi(Hiw + hi) = 0

SLIDE 25

From Linear Feedback to NMPC

9 / 47

Solving the KKT conditions Fw + f − G Tλ − HTµ = 0 Gw + g = 0 Hw + h ≥ 0 µ ≥ 0 µi(Hiw + hi) = 0 The Active Set method Let A be the set of active constraints Fw + f − G Tλ − HTµ = 0 Gw + g = 0 HAw + hA = 0 µ¯

A = 0

Guess A Solve the AS-KKT system Update A

SLIDE 26

From Linear Feedback to NMPC

9 / 47

Solving the KKT conditions Fw + f − G Tλ − HTµ = 0 Gw + g = 0 Hw + h ≥ 0 µ ≥ 0 µi(Hiw + hi) = 0 The Active Set method Let A be the set of active constraints Fw + f − G Tλ − HTµ = 0 Gw + g = 0 HAw + hA = 0 µ¯

A = 0

Guess A Solve the AS-KKT system Update A The Interior Point method Fw + f − G Tλ − HTµ = 0 Gw + g = 0 Hw + h + s = 0 µTs = τ µ ≥ 0 s ≥ 0 Choose τ “big” Solve the IP-KKT system Perform linesearch update τ

SLIDE 27

From Linear Feedback to NMPC

10 / 47

QP solution “QP is almost a technology”, S. Boyd

SLIDE 28

From Linear Feedback to NMPC

10 / 47

QP solution “QP is almost a technology”, S. Boyd

Convex QP: No inequalities: solve a linear system

SLIDE 29

From Linear Feedback to NMPC

10 / 47

QP solution “QP is almost a technology”, S. Boyd

Convex QP: No inequalities: solve a linear system Inequalities: interior point or active set method Active set: algorithm

guess active constr. solve linear system add/remove constr.

SLIDE 30

From Linear Feedback to NMPC

10 / 47

QP solution “QP is almost a technology”, S. Boyd

Convex QP: No inequalities: solve a linear system Inequalities: interior point or active set method Active set: algorithm

guess active constr. solve linear system add/remove constr.

properties

can be warm started extremely fast with good initial guess

SLIDE 31

From Linear Feedback to NMPC

10 / 47

QP solution “QP is almost a technology”, S. Boyd

Convex QP: No inequalities: solve a linear system Inequalities: interior point or active set method Active set: algorithm

guess active constr. solve linear system add/remove constr.

properties

can be warm started extremely fast with good initial guess

Nonconvex QP: NP-hard problem

SLIDE 32

From Linear Feedback to NMPC

10 / 47

QP solution “QP is almost a technology”, S. Boyd

Convex QP: No inequalities: solve a linear system Inequalities: interior point or active set method Active set: algorithm

guess active constr. solve linear system add/remove constr.

properties

can be warm started extremely fast with good initial guess

Nonconvex QP: NP-hard problem Many reliable QP solvers available: qpOASES FORCES quadprog many others

SLIDE 33

From Linear Feedback to NMPC

11 / 47

Complexity of Active set method vs. IP

Active set: qpOASES

[H.J. Ferreau, KU Leuven]

vs. IP: Forces

[A. Domahidi, EPFL] Very fast for few AS changes !! # inputs << # states Need for condensing Limited to QP Cubic in P

10 15 20 25 30 35 40 45 50 20 40 60 80 100 120 140 160 Horizon length Maximum execution times per one RTI [ms] NMPC comparison for M = 3 (nx = 21) NMPC with condensing NMPC with FORCES

Speed is consistent # inputs irrelevant No need for condensing Extension to convex programming Linear in P

SLIDE 34

From Linear Feedback to NMPC

12 / 47

Linear system?

Linear MPC at time i

min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R

s.t. sk+1 = A sk + B uk C sk + D uk ≥ 0, s0 = ˆ xi

SLIDE 35

From Linear Feedback to NMPC

12 / 47

Linear system?

Linear MPC at time i

min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R

s.t. sk+1 = A sk + B uk C sk + D uk ≥ 0, s0 = ˆ xi

1

Linear dynamics

2

Linear path constraints

3

Solve a QP at each iteration

4

Extremely fast for small to medium scale problems

SLIDE 36

From Linear Feedback to NMPC

12 / 47

Linear system?

Linear MPC at time i

min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R

s.t. sk+1 = A sk + B uk C sk + D uk ≥ 0, s0 = ˆ xi

1

Linear dynamics

2

Linear path constraints

3

Solve a QP at each iteration

4

Extremely fast for small to medium scale problems Nonlinear system? Linearize at xref , uref , use linear MPC

SLIDE 37

From Linear Feedback to NMPC

12 / 47

Linear system?

Linear MPC at time i

min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R

s.t. sk+1 = A sk + B uk C sk + D uk ≥ 0, s0 = ˆ xi

1

Linear dynamics

2

Linear path constraints

3

Solve a QP at each iteration

4

Extremely fast for small to medium scale problems Nonlinear system? Linearize at xref , uref , use linear MPC

r...

Nonlinear MPC at time i

min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi

SLIDE 38

From Linear Feedback to NMPC

12 / 47

Linear system?

Linear MPC at time i

min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R

s.t. sk+1 = A sk + B uk C sk + D uk ≥ 0, s0 = ˆ xi

1

Linear dynamics

2

Linear path constraints

3

Solve a QP at each iteration

4

Extremely fast for small to medium scale problems Nonlinear system? Linearize at xref , uref , use linear MPC

r...

Nonlinear MPC at time i

min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi

Problem is non-convex, use NLP solver

SLIDE 39

Nonlinear Programming and SQP

13 / 47

1

From Linear Feedback to NMPC

2

Nonlinear Programming and SQP

3

From Continuous Time to Discrete Time

4

Moving Horizon Estimation

5

Practical NMPC

6

Tutorial

SLIDE 40

Nonlinear Programming and SQP

14 / 47

Newton’s Method

Problem: Find the zeros of F(w) Newton’s Method: Linearize and iteratively solve F(wk) + ∇F(wk)T pk = 0

SLIDE 41

Nonlinear Programming and SQP

14 / 47

Newton’s Method

Problem: Find the zeros of F(w) Newton’s Method: Linearize and iteratively solve F(wk) + ∇F(wk)T pk = 0

Unconstrained Optimization

Problem: minw f (w) First order necessary conditions (FONC): ∇f (w) = 0 Find the zeros of FONC: Iteratively solve ∇f (wk) + ∇2f (wk)pk = 0

SLIDE 42

Nonlinear Programming and SQP

15 / 47

Nonlinear Programming Problem (NLP)

minimize

w

f (w) subject to g(w) = 0 h(w) ≥ 0 (1)

SLIDE 43

Nonlinear Programming and SQP

15 / 47

Nonlinear Programming Problem (NLP)

minimize

w

f (w) subject to g(w) = 0 h(w) ≥ 0 (1)

Newton Type Algorithm

Given an initial guess w0, keep iterating:

SLIDE 44

Nonlinear Programming and SQP

15 / 47

Nonlinear Programming Problem (NLP)

minimize

w

f (w) subject to g(w) = 0 h(w) ≥ 0 (1)

Newton Type Algorithm

Given an initial guess w0, keep iterating:

1

determine a (descent) direction pk

SLIDE 45

Nonlinear Programming and SQP

15 / 47

Nonlinear Programming Problem (NLP)

minimize

w

f (w) subject to g(w) = 0 h(w) ≥ 0 (1)

Newton Type Algorithm

Given an initial guess w0, keep iterating:

1

determine a (descent) direction pk

2

determine a step length αk

SLIDE 46

Nonlinear Programming and SQP

15 / 47

Nonlinear Programming Problem (NLP)

minimize

w

f (w) subject to g(w) = 0 h(w) ≥ 0 (1)

Newton Type Algorithm

Given an initial guess w0, keep iterating:

1

determine a (descent) direction pk

2

determine a step length αk

3

compute the step: wk+1 = wk + αkpk

SLIDE 47

Nonlinear Programming and SQP

15 / 47

Nonlinear Programming Problem (NLP)

minimize

w

f (w) subject to g(w) = 0 h(w) ≥ 0 (1)

Newton Type Algorithm

Given an initial guess w0, keep iterating:

1

determine a (descent) direction pk

2

determine a step length αk

3

compute the step: wk+1 = wk + αkpk

4

check for convergence and return the solution

SLIDE 48

Nonlinear Programming and SQP

15 / 47

Nonlinear Programming Problem (NLP)

minimize

w

f (w) subject to g(w) = 0 h(w) ≥ 0 (1)

Lagrangian Function

L(w, λ, µ) = f (w) − λT g(w) − µT h(w)

SLIDE 49

Nonlinear Programming and SQP

15 / 47

Nonlinear Programming Problem (NLP)

minimize

w

f (w) subject to g(w) = 0 h(w) ≥ 0 (1)

Lagrangian Function

L(w, λ, µ) = f (w) − λT g(w) − µT h(w)

1st Order Necessary Conditions: the KKT system

∇wL(w∗, λ∗, µ∗)= ∇f (w∗) − ∇g(w∗)λ∗ − ∇h(w∗)µ∗= 0 ∇λL(w∗, λ∗, µ∗)= g(w∗)= 0 ∇µL(w∗, λ∗, µ∗)= h(w∗)≥ 0 µ∗≥ 0 µ∗

i hi(w∗)= 0

SLIDE 50

Nonlinear Programming and SQP

16 / 47

Without Inequalities With Inequalities

SLIDE 51

Nonlinear Programming and SQP

16 / 47

Without Inequalities

Linearize the KKT system ∇2

wL

∇g ∇gT ∆wk λk+1

= −

∇f g

Solve the linear system

With Inequalities

SLIDE 52

Nonlinear Programming and SQP

16 / 47

Without Inequalities

Linearize the KKT system ∇2

wL

∇g ∇gT ∆wk λk+1

= −

∇f g

Solve the linear system

Corresponding QP minimize

∆wk

1 2 ∆wT

k ∇2 wL ∆wk + ∇f T ∆wk

subject to g + ∇gT ∆wk = 0

With Inequalities

SLIDE 53

Nonlinear Programming and SQP

16 / 47

Without Inequalities

Linearize the KKT system ∇2

wL

∇g ∇gT ∆wk λk+1

= −

∇f g

Solve the linear system

Corresponding QP minimize

∆wk

1 2 ∆wT

k ∇2 wL ∆wk + ∇f T ∆wk

subject to g + ∇gT ∆wk = 0

With Inequalities

The last 3 KKT conditions are nonsmooth.

SLIDE 54

Nonlinear Programming and SQP

16 / 47

Without Inequalities

Linearize the KKT system ∇2

wL

∇g ∇gT ∆wk λk+1

= −

∇f g

Solve the linear system

Corresponding QP minimize

∆wk

1 2 ∆wT

k ∇2 wL ∆wk + ∇f T ∆wk

subject to g + ∇gT ∆wk = 0

With Inequalities

The last 3 KKT conditions are nonsmooth. At each iteration solve the QP minimize

∆wk

1 2 ∆wT

k ∇2 wL ∆wk + ∇f T ∆wk

subject to g + ∇gT ∆wk = 0 h + ∇hT ∆wk ≥ 0

SLIDE 55

Nonlinear Programming and SQP

17 / 47

SQP method in a nutshell

NMPC at time i

min

w

f (w) s.t. g(w) h (w) ≥ 0 Iterative procedure:

SLIDE 56

Nonlinear Programming and SQP

17 / 47

SQP method in a nutshell Quadratic Problem Approximation

NMPC at time i

min

w

f (w) s.t. g(w) h (w) ≥ 0

QP (for a given s, u)

min

∆w

1 2 ∆w ∆w + ∆w s.t. + ∆w = 0 + ∆w ≥ 0,

Iterative procedure:

1

Given current guess wk, λk, µk

SLIDE 57

Nonlinear Programming and SQP

17 / 47

SQP method in a nutshell Quadratic Problem Approximation

NMPC at time i

min

w

f (w) s.t. g(w) h (w) ≥ 0

QP (for a given s, u)

min

∆w

1 2 ∆w B(wk) ∆w + ∇f (wk)T ∆w s.t. g(wk) + ∇g(wk)T ∆w = 0 h(wk) + ∇h(wk)T ∆w ≥ 0,

Iterative procedure:

1

Given current guess wk, λk, µk

2

Linearize at wk, λk, µk: need 2nd order derivatives for B(wk)

SLIDE 58

Nonlinear Programming and SQP

17 / 47

SQP method in a nutshell Quadratic Problem Approximation

NMPC at time i

min

w

f (w) s.t. g(w) h (w) ≥ 0

QP (for a given s, u)

min

∆w

1 2 ∆w B(wk) ∆w + ∇f (wk)T ∆w s.t. g(wk) + ∇g(wk)T ∆w = 0 h(wk) + ∇h(wk)T ∆w ≥ 0,

Iterative procedure:

1

Given current guess wk, λk, µk

2

Linearize at wk, λk, µk: need 2nd order derivatives for B(wk)

3

Make sure Hessian B(wk) ≻ 0: avoid negative curvature

SLIDE 59

Nonlinear Programming and SQP

17 / 47

SQP method in a nutshell Quadratic Problem Approximation

NMPC at time i

min

w

f (w) s.t. g(w) h (w) ≥ 0

QP (for a given s, u)

min

∆w

1 2 ∆w B(wk) ∆w + ∇f (wk)T ∆w s.t. g(wk) + ∇g(wk)T ∆w = 0 h(wk) + ∇h(wk)T ∆w ≥ 0,

Iterative procedure:

1

Given current guess wk, λk, µk

2

Linearize at wk, λk, µk: need 2nd order derivatives for B(wk)

3

Make sure Hessian B(wk) ≻ 0: avoid negative curvature

4

Solve QP

SLIDE 60

Nonlinear Programming and SQP

17 / 47

SQP method in a nutshell Quadratic Problem Approximation

NMPC at time i

min

w

f (w) s.t. g(w) h (w) ≥ 0

QP (for a given s, u)

min

∆w

1 2 ∆w B(wk) ∆w + ∇f (wk)T ∆w s.t. g(wk) + ∇g(wk)T ∆w = 0 h(wk) + ∇h(wk)T ∆w ≥ 0,

Iterative procedure:

1

Given current guess wk, λk, µk

2

Linearize at wk, λk, µk: need 2nd order derivatives for B(wk)

3

Make sure Hessian B(wk) ≻ 0: avoid negative curvature

4

Solve QP

5

Globalization (e.g. line-search): ensure descent, stepsize α ∈ (0, 1]

SLIDE 61

Nonlinear Programming and SQP

17 / 47

SQP method in a nutshell Quadratic Problem Approximation

NMPC at time i

min

w

f (w) s.t. g(w) h (w) ≥ 0

QP (for a given s, u)

min

∆w

1 2 ∆w B(wk) ∆w + ∇f (wk)T ∆w s.t. g(wk) + ∇g(wk)T ∆w = 0 h(wk) + ∇h(wk)T ∆w ≥ 0,

Iterative procedure:

1

Given current guess wk, λk, µk

2

Linearize at wk, λk, µk: need 2nd order derivatives for B(wk)

3

Make sure Hessian B(wk) ≻ 0: avoid negative curvature

4

Solve QP

5

Globalization (e.g. line-search): ensure descent, stepsize α ∈ (0, 1]

6

Update   wk+1 λk+1 µk+1   =   wk λk µk   + α   ∆w ∆λ ∆µ   and iterate

SLIDE 62

Nonlinear Programming and SQP

18 / 47

The Generalized Gauss-Newton Method [Bock1983]

Specific Structure of f (w)

minimize

w

1 2 F(w)2

2

subject to g(w) = 0 h(w) ≥ 0 (2)

SLIDE 63

Nonlinear Programming and SQP

18 / 47

The Generalized Gauss-Newton Method [Bock1983]

Specific Structure of f (w)

minimize

w

1 2 F(w)2

2

subject to g(w) = 0 h(w) ≥ 0 (2)

Gauss-Newton Hessian Approximation

Linearize inside the norm to obtain minimize

∆w

1 2 F(wk) + J(wk) ∆w 2

2

subject to g(wk) + ∇g(wk)T ∆w = 0 h(wk) + ∇h(wk)T ∆w ≥ 0 where J(wk) = ∇F(wk)T .

SLIDE 64

Nonlinear Programming and SQP

19 / 47

Why Does it perform well?

Exact Hessian: ∇2

wL = ∇2f

−

λi∇2gi −
µi∇2hi

= JT J +

Fi∇2Fi −
λi∇2gi −
µi∇2hi

Gauss-Newton Hessian: ∇2

wL ≈ JT J

No need for 2nd order derivatives Lagrange multipliers

When Does it perform well?

F small: good fit ∇2Fi small: residuals F nearly linear λ and µ small: true when F small

SLIDE 65

Nonlinear Programming and SQP

20 / 47

Wide Range of Applications

System identification: min

p

y(p) − y 2

S

Model Predictive Control: min

x,u

x − xr 2

Q + u − ur 2 R

Moving Horizon Estimation: min

x,u

y(x, u) − y 2

S

SLIDE 66

From Continuous Time to Discrete Time

21 / 47

1

From Linear Feedback to NMPC

2

Nonlinear Programming and SQP

3

From Continuous Time to Discrete Time

4

Moving Horizon Estimation

5

Practical NMPC

6

Tutorial

SLIDE 67

From Continuous Time to Discrete Time

22 / 47

Linear system

Continuous time:

˙ x(t) = Acx(t) + Bcu(t)

Discrete time:

sk+1 = A sk + B uk

Discretization over a time interval t ∈ [tk, tk+1], input u(t) = uk

A = eAc(tk+1−tk), B = tk+1

tk

eAcτBcdτ

SLIDE 68

From Continuous Time to Discrete Time

22 / 47

Linear system Nonlinear system

Continuous time:

˙ x(t) = Acx(t) + Bcu(t) ˙ x(t) = fc (x(t), u(t))

Discrete time:

sk+1 = A sk + B uk sk+1 = f (sk, uk)

Discretization over a time interval t ∈ [tk, tk+1], input u(t) = uk

A = eAc(tk+1−tk), B = tk+1

tk

eAcτBcdτ sk+1 = f (sk, uk) Integration of function fc can be complex, possibly implicit (algorithm) !!

SLIDE 69

From Continuous Time to Discrete Time

23 / 47

How to Discretize the System?

SLIDE 70

From Continuous Time to Discrete Time

23 / 47

How to Discretize the System?

Single Shooting: From x(t0) integrate the system on the whole horizon → continuous trajectory

0.05 0.1 0.15 0.2 0.25 0.3 0.045 0.05 0.055 0.06 0.065

s0 = x(0) s1 = x(Ts) s2 = x(2Ts) t x

SLIDE 71

From Continuous Time to Discrete Time

23 / 47

How to Discretize the System?

Single Shooting: From x(t0) integrate the system on the whole horizon → continuous trajectory Multiple Shooting: From x(tk) integrate the system on each interval separately → discontinuous trajectory

0.05 0.1 0.15 0.2 0.25 0.3 0.045 0.05 0.055 0.06 0.065

s0 = x0(0) f(s0, u0) = x0(Ts) s1 = x1(0) f(s1, u1) = x1(Ts) s2 = x2(0) t x

SLIDE 72

From Continuous Time to Discrete Time

23 / 47

How to Discretize the System?

Single Shooting: From x(t0) integrate the system on the whole horizon → continuous trajectory Multiple Shooting: From x(tk) integrate the system on each interval separately → discontinuous trajectory

0.05 0.1 0.15 0.2 0.25 0.3 0.045 0.05 0.055 0.06 0.065

s0 = x0(0) s1 = x1(0) s2 = x2(0) s1-f(s0, u0) s2-f(s1, u1) t x

SLIDE 73

From Continuous Time to Discrete Time

24 / 47

Multiple Shooting vs Single Shooting

Better: unstable systems

SLIDE 74

From Continuous Time to Discrete Time

24 / 47

Multiple Shooting vs Single Shooting

Better: unstable systems Better: initialization of states at intermediate nodes

SLIDE 75

From Continuous Time to Discrete Time

24 / 47

Multiple Shooting vs Single Shooting

Better: unstable systems Better: initialization of states at intermediate nodes Warning: leads to bigger QP/NLP

S. Shooting:

nx + (N − 1)nu opt. vars (x0, u0, u1, . . . , uN−1)

M. Shooting: Nnx + (N − 1)nu opt. vars (x0, u0, x1, u1, . . . , xN)

SLIDE 76

From Continuous Time to Discrete Time

24 / 47

Multiple Shooting vs Single Shooting

Better: unstable systems Better: initialization of states at intermediate nodes Warning: leads to bigger QP/NLP

S. Shooting:

nx + (N − 1)nu opt. vars (x0, u0, u1, . . . , uN−1)

M. Shooting: Nnx + (N − 1)nu opt. vars (x0, u0, x1, u1, . . . , xN)

Good news!: after integration, all xk, k = 1, . . . , N can be eliminated → Condensing: reduce to the size of Single Shooting (to be continued...)

SLIDE 77

From Continuous Time to Discrete Time

24 / 47

Multiple Shooting vs Single Shooting

Better: unstable systems Better: initialization of states at intermediate nodes Warning: leads to bigger QP/NLP

S. Shooting:

nx + (N − 1)nu opt. vars (x0, u0, u1, . . . , uN−1)

M. Shooting: Nnx + (N − 1)nu opt. vars (x0, u0, x1, u1, . . . , xN)

Good news!: after integration, all xk, k = 1, . . . , N can be eliminated → Condensing: reduce to the size of Single Shooting (to be continued...) Continuity conditions:

S. Shooting:

imposed by the integration

M. Shooting:

imposed by the QP/NLP

SLIDE 78

From Continuous Time to Discrete Time

25 / 47

Let’s get a closer look at the problem

SLIDE 79

From Continuous Time to Discrete Time

25 / 47

Let’s get a closer look at the problem

QP (for a given s, u)

min

∆u,∆s

1 2

∆s

∆u

B
∆s

∆u

+ JT
∆s

∆u

s.t.

∆sk+1 = f + ∂f ∂s ∆sk + ∂f ∂u ∆uk , h + ∂h ∂s ∆sk + ∂h ∂u ∆uk ≥ 0, s0 = ˆ xi

Linearize f : evaluate integrator

∂f ∂s , ∂f ∂u : differentiate integrator

h: evaluate nonlinear function

∂h ∂s , ∂h ∂u : differentiate nonlinear function

B = diag(Q, . . . , Q, R . . . , R) + λ ∂2f

∂w2 + µ ∂2h ∂w2 ,

w = s u

J = 2 w Tdiag(Q, . . . , Q, R . . . , R)

SLIDE 80

From Continuous Time to Discrete Time

25 / 47

Let’s get a closer look at the problem

QP (for a given s, u)

min

∆u,∆s

1 2

∆s

∆u

B
∆s

∆u

+ JT
∆s

∆u

s.t.

∆sk+1 = f + ∂f ∂s ∆sk + ∂f ∂u ∆uk , h + ∂h ∂s ∆sk + ∂h ∂u ∆uk ≥ 0, s0 = ˆ xi

Ensure B ≻ 0 Exact Hessian: add curvature to the negative directions Quadratic convergence

SLIDE 81

From Continuous Time to Discrete Time

25 / 47

Let’s get a closer look at the problem

QP (for a given s, u)

min

∆u,∆s

1 2

∆s

∆u

B
∆s

∆u

+ JT
∆s

∆u

s.t.

∆sk+1 = f + ∂f ∂s ∆sk + ∂f ∂u ∆uk , h + ∂h ∂s ∆sk + ∂h ∂u ∆uk ≥ 0, s0 = ˆ xi

Ensure B ≻ 0 Exact Hessian: add curvature to the negative directions Quadratic convergence BFGS update: Bk+1 = Bk + Bk σσT Bk

σT Bk σ

+ γγT

σT γ

Superlinear convergence

SLIDE 82

From Continuous Time to Discrete Time

25 / 47

Let’s get a closer look at the problem

QP (for a given s, u)

min

∆u,∆s

1 2

∆s

∆u

B
∆s

∆u

+ JT
∆s

∆u

s.t.

∆sk+1 = f + ∂f ∂s ∆sk + ∂f ∂u ∆uk , h + ∂h ∂s ∆sk + ∂h ∂u ∆uk ≥ 0, s0 = ˆ xi

Ensure B ≻ 0 Exact Hessian: add curvature to the negative directions Quadratic convergence BFGS update: Bk+1 = Bk + Bk σσT Bk

σT Bk σ

+ γγT

σT γ

Superlinear convergence Gauss-Newton approximation: B ≈ JTJ (for linear MPC it is exact!) Linear convergence

SLIDE 83

From Continuous Time to Discrete Time

25 / 47

Let’s get a closer look at the problem

QP (for a given s, u)

min

∆u,∆s

1 2

∆s

∆u

B
∆s

∆u

+ JT
∆s

∆u

s.t.

∆sk+1 = f + ∂f ∂s ∆sk + ∂f ∂u ∆uk , h + ∂h ∂s ∆sk + ∂h ∂u ∆uk ≥ 0, s0 = ˆ xi

Iterate to convergence All previous steps are repeated until convergence! Computations can become very long Cannot apply the control instantaneously

SLIDE 84

The Real Time Iteration Scheme

26 / 47

Can we be faster?

What about:

SLIDE 85

The Real Time Iteration Scheme

26 / 47

Can we be faster?

What about: 1 Newton step → no need to iterate

SLIDE 86

The Real Time Iteration Scheme

26 / 47

Can we be faster?

What about: 1 Newton step Initial value embedding: s0 = ˆ xi as a constraint → no need to iterate → faster convergence, clever computations

SLIDE 87

The Real Time Iteration Scheme

26 / 47

Can we be faster?

What about: 1 Newton step Initial value embedding: s0 = ˆ xi as a constraint No globalization → no need to iterate → faster convergence, clever computations → need to enforce s0 = ˆ xi

SLIDE 88

The Real Time Iteration Scheme

26 / 47

Can we be faster?

What about: 1 Newton step Initial value embedding: s0 = ˆ xi as a constraint No globalization Gauss-Newton Hessian approximation → no need to iterate → faster convergence, clever computations → need to enforce s0 = ˆ xi → only 1st order derivatives, Hessian B ≻ 0

SLIDE 89

The Real Time Iteration Scheme

26 / 47

Can we be faster?

What about: 1 Newton step Initial value embedding: s0 = ˆ xi as a constraint No globalization Gauss-Newton Hessian approximation → no need to iterate → faster convergence, clever computations → need to enforce s0 = ˆ xi → only 1st order derivatives, Hessian B ≻ 0 Result:

SLIDE 90

The Real Time Iteration Scheme

26 / 47

Can we be faster?

What about: 1 Newton step Initial value embedding: s0 = ˆ xi as a constraint No globalization Gauss-Newton Hessian approximation → no need to iterate → faster convergence, clever computations → need to enforce s0 = ˆ xi → only 1st order derivatives, Hessian B ≻ 0 Result: Converge while the system evolves next SQP iteration takes place on the new problem ˆ xi+1

SLIDE 91

The Real Time Iteration Scheme

26 / 47

Can we be faster?

What about: 1 Newton step Initial value embedding: s0 = ˆ xi as a constraint No globalization Gauss-Newton Hessian approximation → no need to iterate → faster convergence, clever computations → need to enforce s0 = ˆ xi → only 1st order derivatives, Hessian B ≻ 0 Result: Converge while the system evolves next SQP iteration takes place on the new problem ˆ xi+1 Need to have a good initial guess better to shift

SLIDE 92

The Real Time Iteration Scheme

26 / 47

Can we be faster?

What about: 1 Newton step Initial value embedding: s0 = ˆ xi as a constraint No globalization Gauss-Newton Hessian approximation → no need to iterate → faster convergence, clever computations → need to enforce s0 = ˆ xi → only 1st order derivatives, Hessian B ≻ 0 Result: Converge while the system evolves next SQP iteration takes place on the new problem ˆ xi+1 Need to have a good initial guess better to shift

Under some (mild) conditions, the SQP solution is closely tracked

SLIDE 93

The Real Time Iteration Scheme

27 / 47

Standard SQP

NMPC at time i

min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi Iterative procedure (at each time i):

1

Given current guess s, u

2

Linearize at s, u

3

Make sure Hessian B ≻ 0

4

Solve QP

5

Globalization (e.g. line-search)

6

Update and iterate

SLIDE 94

The Real Time Iteration Scheme

27 / 47

Standard SQP Real Time Iterations

NMPC at time i

min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi

RTI at time i

min

∆u,∆s

1 2

∆s

∆u

JT J
∆s

∆u

+ JT
∆s

∆u

s.t.

∆sk+1 = f + ∂f ∂s ∆sk + ∂f ∂u ∆uk h + ∂h ∂s ∆sk + ∂h ∂u ∆uk ≥ 0, s0 = ˆ xi

Iterative procedure (at each time i):

1

Given current guess s, u

2

Linearize at s, u

3

Make sure Hessian B ≻ 0

4

Solve QP

5

Globalization (e.g. line-search)

6

Update and iterate

SLIDE 95

The Real Time Iteration Scheme

27 / 47

Standard SQP Real Time Iterations

NMPC at time i

min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi

RTI at time i

min

∆u,∆s

1 2

∆s

∆u

JT J
∆s

∆u

+ JT
∆s

∆u

s.t.

∆sk+1 = f + ∂f ∂s ∆sk + ∂f ∂u ∆uk h + ∂h ∂s ∆sk + ∂h ∂u ∆uk ≥ 0, s0 = ˆ xi

Iterative procedure (at each time i):

1

Given current guess s, u

2

Linearize at s, u

3

Make sure Hessian B ≻ 0

4

Solve QP

5

Globalization (e.g. line-search)

6

Update and iterate Preparation Phase Without knowing ˆ xi Linearize ( Gauss-Newton ⇒ B ≻ 0) Prepare the QP

SLIDE 96

The Real Time Iteration Scheme

27 / 47

Standard SQP Real Time Iterations

NMPC at time i

min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi

RTI at time i

min

∆u,∆s

1 2

∆s

∆u

JT J
∆s

∆u

+ JT
∆s

∆u

s.t.

∆sk+1 = f + ∂f ∂s ∆sk + ∂f ∂u ∆uk h + ∂h ∂s ∆sk + ∂h ∂u ∆uk ≥ 0, s0 = ˆ xi

Iterative procedure (at each time i):

1

Given current guess s, u

2

Linearize at s, u

3

Make sure Hessian B ≻ 0

4

Solve QP

5

Globalization (e.g. line-search)

6

Update and iterate Preparation Phase Without knowing ˆ xi Linearize ( Gauss-Newton ⇒ B ≻ 0) Prepare the QP Feedback Phase: Solve QP once ˆ xi available → same latency as linear MPC

SLIDE 97

The Real Time Iteration Scheme

28 / 47

Linear MPC at time i

min

u,s N

k=0

sk − xref 2

Q + uk − uref 2 R

s.t. sk+1 = Aksk + Bkuk Cksk + Dkuk ≥ 0, s0 = ˆ xi

RTI at time i

min

u,s N

k=0

sk − xref 2

Q + uk − uref 2 R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi

SLIDE 98

The Real Time Iteration Scheme

28 / 47

Linear MPC at time i

min

u,s N

k=0

sk − xref 2

Q + uk − uref 2 R

s.t. sk+1 = Aksk + Bkuk Cksk + Dkuk ≥ 0, s0 = ˆ xi

RTI at time i

min

u,s N

k=0

sk − xref 2

Q + uk − uref 2 R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi

At each time i:

1

Solve the QP At each time i:

1

Solve the QP

SLIDE 99

The Real Time Iteration Scheme

28 / 47

Linear MPC at time i

min

u,s N

k=0

sk − xref 2

Q + uk − uref 2 R

s.t. sk+1 = Aksk + Bkuk Cksk + Dkuk ≥ 0, s0 = ˆ xi

RTI at time i

min

u,s N

k=0

sk − xref 2

Q + uk − uref 2 R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi

At each time i:

1

Solve the QP At each time i:

1

Solve the QP

2

Compute the new linearization

f the constraints

3

Prepare the new QP

SLIDE 100

The Real Time Iteration Scheme

28 / 47

Linear MPC at time i

min

u,s N

k=0

sk − xref 2

Q + uk − uref 2 R

s.t. sk+1 = Aksk + Bkuk Cksk + Dkuk ≥ 0, s0 = ˆ xi

RTI at time i

min

u,s N

k=0

sk − xref 2

Q + uk − uref 2 R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi

At each time i:

1

Solve the QP At each time i:

1

Solve the QP

2

Compute the new linearization

f the constraints

3

Prepare the new QP

RTI differs from linear MPC in the sense that the constraints are re-linearized at each time instant on the current trajectory rather than only once on the reference trajectory

SLIDE 101

Moving Horizon Estimation

29 / 47

1

From Linear Feedback to NMPC

2

Nonlinear Programming and SQP

3

From Continuous Time to Discrete Time

4

Moving Horizon Estimation

5

Practical NMPC

6

Tutorial

SLIDE 102

Moving Horizon Estimation

30 / 47

Need for an observer Full state measurement not possible Sensor noise Fuse sensor information and model prediction Online parameter estimation

SLIDE 103

Moving Horizon Estimation

30 / 47

Need for an observer Full state measurement not possible Sensor noise Fuse sensor information and model prediction Online parameter estimation Available data Previous estimate ˆ x−E and related uncertainty SE Measurement model y(sk) Measurements ¯ yk

SLIDE 104

Moving Horizon Estimation

30 / 47

Need for an observer Full state measurement not possible Sensor noise Fuse sensor information and model prediction Online parameter estimation Available data Previous estimate ˆ x−E and related uncertainty SE Measurement model y(sk) Measurements ¯ yk MHE problem Different nature from MPC: estimate vs control Similar formulation to MPC: use the same algorithms!

SLIDE 105

Moving Horizon Estimation

31 / 47

Looking back in the past... ...find the system trajectory that best fits the measurements

0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0.05 0.1 0.15 0.2 t y 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 −20 −10 10 t u

min

u,s −E

k=0

y(sk) − ¯ yk2

Q + −E

k=−1

uk − ¯ uk2

R

→ mismatch meas. model - meas. s.t. sk+1 = f (sk, uk) → model of the system

SLIDE 106

Moving Horizon Estimation

31 / 47

Looking back in the past... ...find the system trajectory that best fits the measurements

0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0.05 0.1 0.15 0.2 t y 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 −20 −10 10 t u

min

u,s −E

k=0

y(sk) − ¯ yk2

Q + −E

k=−1

uk − ¯ uk2

R

→ mismatch meas. model - meas. s.t. sk+1 = f (sk, uk) → model of the system

SLIDE 107

Moving Horizon Estimation

31 / 47

Looking back in the past... ...find the system trajectory that best fits the measurements

0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0.05 0.1 0.15 0.2 t y 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 −20 −10 10 t u

min

u,s −E

k=0

y(sk) − ¯ yk2

Q + −E

k=−1

uk − ¯ uk2

R

→ mismatch meas. model - meas. s.t. sk+1 = f (sk, uk) → model of the system

SLIDE 108

Moving Horizon Estimation

31 / 47

Looking back in the past... ...find the system trajectory that best fits the measurements

0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0.05 0.1 0.15 0.2 t y 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 −20 −10 10 t u

min

u,s −E

k=0

y(sk) − ¯ yk2

Q + −E

k=−1

uk − ¯ uk2

R

→ mismatch meas. model - meas. s.t. sk+1 = f (sk, uk) → model of the system

SLIDE 109

Moving Horizon Estimation

31 / 47

Looking back in the past... ...find the system trajectory that best fits the measurements

0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0.05 0.1 0.15 0.2 t y 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 −20 −10 10 t u

min

u,s −E

k=0

y(sk) − ¯ yk2

Q + −E

k=−1

uk − ¯ uk2

R

→ mismatch meas. model - meas. s.t. sk+1 = f (sk, uk) → model of the system

SLIDE 110

Moving Horizon Estimation

32 / 47

MHE and the Kalman filter Weights selection

SLIDE 111

Moving Horizon Estimation

32 / 47

MHE and the Kalman filter Similar: Kalman filter extension

Nonlinear system Constraints

Weights selection

SLIDE 112

Moving Horizon Estimation

32 / 47

MHE and the Kalman filter Similar: Kalman filter extension

Nonlinear system Constraints

Different: MHE is deterministic

No specific noise distribution assumption

Weights selection

SLIDE 113

Moving Horizon Estimation

32 / 47

MHE and the Kalman filter Similar: Kalman filter extension

Nonlinear system Constraints

Different: MHE is deterministic

No specific noise distribution assumption

Weights selection Probabilistic insight is valuable

Use the inverse of the covariance matrix (measurement reliability) Arrival cost s−E − ˆ s−E 2

SE : Kalman update of SE and ˆ

s−E

SLIDE 114

Moving Horizon Estimation

33 / 47

MHE-MPC Scheme

Plant Sensors IMU, GPS, encoders, force sensors NMPC (RTI) MHE (RTI)

SLIDE 115

Moving Horizon Estimation

34 / 47

MHE ↓

0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0.05 0.1 0.15 0.2 t y 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 −20 −10 10 t u

ˆ x

↓ NMPC

0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 1.1 1.2 1.3 1.4 1.5 −0.2 −0.1 0.1 0.2 x [m], v [m/s] 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 1.1 1.2 1.3 1.4 1.5 −1.2 −0.8 −0.4 0.4 0.8 1.2 time [s] u [m/s2] x v

SLIDE 116

Moving Horizon Estimation

35 / 47

The RTI workflow for NMPC

Initial Value Embedding QP solving: qpOASES / CVXGEN Integration and sensitivity generation Condensing Preparation Step (setup of the QP) Feedback Step

SLIDE 117

Moving Horizon Estimation

36 / 47

The RTI workflow for MHE

Last Meas. Embedding QP solving: qpOASES / CVXGEN Integration and sensitivity generation Condensing Preparation Step (setup of the QP) Feedback Step yN

xN Cov(xN)

SLIDE 118

Moving Horizon Estimation

37 / 47

Real Time Iteration Time Line for MHE & NMPC

Data acquisition

NMPC Feedback Step NMPC Preparation Step One control/estimation interval time k k + 1 MHE Feedback Step MHE Preparation Step

SLIDE 119

Moving Horizon Estimation

37 / 47

Real Time Iteration Time Line for MHE & NMPC

Data acquisition

NMPC Feedback Step

NMPC Preparation Step One control/estimation interval x0 is ready u0

pt

is ready time k k + 1

MHE Feedback Step

MHE Preparation Step

SLIDE 120

Moving Horizon Estimation

37 / 47

Real Time Iteration Time Line for MHE & NMPC

Data acquisition NMPC Feedback Step

NMPC Preparation Step

One control/estimation interval x0 is ready u0

pt

is ready time k k + 1 MHE Feedback Step

MHE Preparation Step

SLIDE 121

Moving Horizon Estimation

37 / 47

Real Time Iteration Time Line for MHE & NMPC

Data acquisition NMPC Feedback Step

NMPC Preparation Step

One control/estimation interval x0 is ready u0

pt

is ready time k k + 1 MHE Feedback Step

MHE Preparation Step

SLIDE 122

Moving Horizon Estimation

37 / 47

Real Time Iteration Time Line for MHE & NMPC

Data acquisition NMPC Feedback Step NMPC Preparation Step One control/estimation interval x0 is ready u0

pt

is ready time k k + 1 MHE Feedback Step MHE Preparation Step

Yes, both preparation phases can be executed in parallel

SLIDE 123

Moving Horizon Estimation

38 / 47

Code Generation

RTI at time i

min

u,s N

k=0

sk − xref 2

Q + uk − uref 2 R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi Properties: Fixed problem dimensions Specific structure

SLIDE 124

Moving Horizon Estimation

38 / 47

Code Generation

RTI at time i

min

u,s N

k=0

sk − xref 2

Q + uk − uref 2 R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi Properties: Fixed problem dimensions Specific structure Export tailored C code Exploit the structure and minimize number of operations No dynamic memory allocation Minimize branching in the exported code Optimized linear algebra routines

SLIDE 125

Moving Horizon Estimation

38 / 47

Code Generation

RTI at time i

min

u,s N

k=0

sk − xref 2

Q + uk − uref 2 R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi Properties: Fixed problem dimensions Specific structure Export tailored C code Exploit the structure and minimize number of operations No dynamic memory allocation Minimize branching in the exported code Optimized linear algebra routines

ACADO Toolkit Multiple shooting Real time iterations Code generation

SLIDE 126

Practical NMPC

39 / 47

1

From Linear Feedback to NMPC

2

Nonlinear Programming and SQP

3

From Continuous Time to Discrete Time

4

Moving Horizon Estimation

5

Practical NMPC

6

Tutorial

SLIDE 127

Practical NMPC

40 / 47

OK but... how to have a reliable implementation?

SLIDE 128

Practical NMPC

41 / 47

Importance of the Initial Guess

RTI (single Newton step): 1st order correction → [s, u] − [s∗, u∗] = o(e2

guess)

Guess: shift the solution at the previous step

0.5 1 1.5 2 2.5 3 3.5 4 4.5 −1 −0.5 0.5 1

a b e x

0.5 1 1.5 2 2.5 3 3.5 4 4.5 −1 −0.5 0.5 1

u T c d f

Guess error eguess small if...

1

{ui−1, si−1} close to {ui−1∗, si−1∗}

2

si

0 close to ˆ

xi

SLIDE 129

Practical NMPC

41 / 47

Importance of the Initial Guess

RTI (single Newton step): 1st order correction → [s, u] − [s∗, u∗] = o(e2

guess)

Guess: shift the solution at the previous step

0.5 1 1.5 2 2.5 3 3.5 4 4.5 −1 −0.5 0.5 1

b e x

0.5 1 1.5 2 2.5 3 3.5 4 4.5 −1 −0.5 0.5 1

u T d f

Guess error eguess small if...

1

{ui−1, si−1} close to {ui−1∗, si−1∗}

2

si

0 close to ˆ

xi

SLIDE 130

Practical NMPC

41 / 47

Importance of the Initial Guess

RTI (single Newton step): 1st order correction → [s, u] − [s∗, u∗] = o(e2

guess)

Guess: shift the solution at the previous step

0.5 1 1.5 2 2.5 3 3.5 4 4.5 −1 −0.5 0.5 1

b e x

0.5 1 1.5 2 2.5 3 3.5 4 4.5 −1 −0.5 0.5 1

u T d f g

Guess error eguess small if...

1

{ui−1, si−1} close to {ui−1∗, si−1∗}

2

si

0 close to ˆ

xi

SLIDE 131

Practical NMPC

41 / 47

Importance of the Initial Guess

RTI (single Newton step): 1st order correction → [s, u] − [s∗, u∗] = o(e2

guess)

Guess: shift the solution at the previous step

0.5 1 1.5 2 2.5 3 3.5 4 4.5 −1 −0.5 0.5 1

b e x

0.5 1 1.5 2 2.5 3 3.5 4 4.5 −1 −0.5 0.5 1

u T d f g

Guess error eguess small if...

1

{ui−1, si−1} close to {ui−1∗, si−1∗}

2

si

0 close to ˆ

xi

SLIDE 132

Practical NMPC

41 / 47

Importance of the Initial Guess

RTI (single Newton step): 1st order correction → [s, u] − [s∗, u∗] = o(e2

guess)

Guess: shift the solution at the previous step

0.5 1 1.5 2 2.5 3 3.5 4 4.5 −1 −0.5 0.5 1

a b h e x

0.5 1 1.5 2 2.5 3 3.5 4 4.5 −1 −0.5 0.5 1

u T d i f g

Guess error eguess small if...

1

{ui−1, si−1} close to {ui−1∗, si−1∗}

2

si

0 close to ˆ

xi

SLIDE 133

Practical NMPC

41 / 47

Importance of the Initial Guess

RTI (single Newton step): 1st order correction → [s, u] − [s∗, u∗] = o(e2

guess)

Guess: shift the solution at the previous step

0.5 1 1.5 2 2.5 3 3.5 4 4.5 −1 −0.5 0.5 1

a b h e x

0.5 1 1.5 2 2.5 3 3.5 4 4.5 −1 −0.5 0.5 1

u T d i f g

Guess error eguess small if...

1

{ui−1, si−1} close to {ui−1∗, si−1∗}

2

si

0 close to ˆ

xi Key for algorithmic reliability

1

Sample fast enough

2

Warm start

SLIDE 134

Practical NMPC

42 / 47

Weight Selection

Actuators Attitude Velocities Positions

Uref U ˙ ω, ω, R ˙ x, ˙ y, ˙ z x, y, z

Track pref = [xref, yref, zref]

SLIDE 135

Practical NMPC

42 / 47

Weight Selection

Actuators Attitude Velocities Positions

Uref U ˙ ω, ω, R ˙ x, ˙ y, ˙ z x, y, z

Track pref = [xref, yref, zref]

Temptation to use: N

k=0 p − pref2 2

SLIDE 136

Practical NMPC

42 / 47

Weight Selection

Actuators Attitude Velocities Positions

Uref U ˙ ω, ω, R ˙ x, ˙ y, ˙ z x, y, z

Track pref = [xref, yref, zref]

Temptation to use: N

k=0 p − pref2 2

Non-uniqueness of the solution

SLIDE 137

Practical NMPC

42 / 47

Weight Selection

Actuators Attitude Velocities Positions

Uref U ˙ ω, ω, R ˙ x, ˙ y, ˙ z x, y, z

Track pref = [xref, yref, zref]

Temptation to use: N

k=0 p − pref2 2

Non-uniqueness of the solution Always weight the controls:

N−1

k=0

u − uref2

R

SLIDE 138

Practical NMPC

42 / 47

Weight Selection

Actuators Attitude Velocities Positions

Uref U ˙ ω, ω, R ˙ x, ˙ y, ˙ z x, y, z

Track pref = [xref, yref, zref]

Temptation to use: N

k=0 p − pref2 2 + N−1 k=0 u − uref2 R

SLIDE 139

Practical NMPC

42 / 47

Weight Selection

Actuators Attitude Velocities Positions

Uref U ˙ ω, ω, R ˙ x, ˙ y, ˙ z x, y, z

Track pref = [xref, yref, zref]

Temptation to use: N

k=0 p − pref2 2 + N−1 k=0 u − uref2 R

−5 5 −5 5 5 10 15 20 25 30 35

Positions P, Inputs U Others Problem

Directions of “bad curvature”

SLIDE 140

Practical NMPC

42 / 47

Weight Selection

Actuators Attitude Velocities Positions

Uref U ˙ ω, ω, R ˙ x, ˙ y, ˙ z x, y, z

Track pref = [xref, yref, zref]

Temptation to use: N

k=0 p − pref2 2 + N−1 k=0 u − uref2 R

Weight the full dynamic chain:

N

k=0

s − sref2

Q + N−1

k=0

u − uref2

R

Avoid rank deficiency of Q, R

−5 5 −5 5 5 10 15 20 25 30 35

Positions P, Inputs U Others Problem

Directions of “bad curvature”

SLIDE 141

Practical NMPC

43 / 47

Handling Infeasibility

SLIDE 142

Practical NMPC

43 / 47

Handling Infeasibility

Path constraints might become infeasible min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi

SLIDE 143

Practical NMPC

43 / 47

Handling Infeasibility

Path constraints might become infeasible min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi ˆ xi imposed by the (perturbed) system

SLIDE 144

Practical NMPC

43 / 47

Handling Infeasibility

Path constraints might become infeasible min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi ˆ xi imposed by the (perturbed) system perturbations might make h (sk, uk) ≥ 0 infeasible

SLIDE 145

Practical NMPC

43 / 47

Handling Infeasibility

Path constraints might become infeasible min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi ˆ xi imposed by the (perturbed) system perturbations might make h (sk, uk) ≥ 0 infeasible limited controllability of h (sk, uk) at low k

SLIDE 146

Practical NMPC

43 / 47

Handling Infeasibility

Path constraints might become infeasible min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi ˆ xi imposed by the (perturbed) system perturbations might make h (sk, uk) ≥ 0 infeasible limited controllability of h (sk, uk) at low k Slack Variables Reformulation min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R + W Tv

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ −v, v ≥ 0 s0 = ˆ xi

SLIDE 147

Practical NMPC

43 / 47

Handling Infeasibility

Path constraints might become infeasible min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi ˆ xi imposed by the (perturbed) system perturbations might make h (sk, uk) ≥ 0 infeasible limited controllability of h (sk, uk) at low k Slack Variables Reformulation min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R + W Tv

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ −v, v ≥ 0 s0 = ˆ xi No effect for h (sk, uk) ≥ 0

SLIDE 148

Practical NMPC

43 / 47

Handling Infeasibility

Path constraints might become infeasible min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi ˆ xi imposed by the (perturbed) system perturbations might make h (sk, uk) ≥ 0 infeasible limited controllability of h (sk, uk) at low k Slack Variables Reformulation min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R + W Tv

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ −v, v ≥ 0 s0 = ˆ xi No effect for h (sk, uk) ≥ 0 Strong penalty for h (sk, uk) ≤ 0 → choose W large enough

SLIDE 149

Practical NMPC

43 / 47

Handling Infeasibility

Path constraints might become infeasible min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ 0, s0 = ˆ xi ˆ xi imposed by the (perturbed) system perturbations might make h (sk, uk) ≥ 0 infeasible limited controllability of h (sk, uk) at low k Slack Variables Reformulation min

u,s N

k=0

sk − xref 2

Q + N−1

k=0

uk − uref 2

R + W Tv

s.t. sk+1 = f (sk, uk) h (sk, uk) ≥ −v, v ≥ 0 s0 = ˆ xi

10 20 30 40 50 60 0.8 0.85 0.9 0.95 1 1.05 1.1 1.15 1.2

Time (s) CL (−)

NMPC Reference S1

SLIDE 150

Practical NMPC

44 / 47

Real Time Considerations

Real-time feasible: tcomp ≤ Ts = TP/N Integrators QP Solution

SLIDE 151

Practical NMPC

44 / 47

Real Time Considerations

Real-time feasible: tcomp ≤ Ts = TP/N Integrators Preparation phase dominated by integration with sensitivities QP Solution

SLIDE 152

Practical NMPC

44 / 47

Real Time Considerations

Real-time feasible: tcomp ≤ Ts = TP/N Integrators Preparation phase dominated by integration with sensitivities Full accuracy rarely needed

→ imperfect model → perturbed system

QP Solution

SLIDE 153

Practical NMPC

44 / 47

Real Time Considerations

Real-time feasible: tcomp ≤ Ts = TP/N Integrators Preparation phase dominated by integration with sensitivities Full accuracy rarely needed

→ imperfect model → perturbed system

Save computations

→ fix the integration grid → use tailored integration

schemes QP Solution

SLIDE 154

Practical NMPC

44 / 47

Real Time Considerations

Real-time feasible: tcomp ≤ Ts = TP/N Integrators Preparation phase dominated by integration with sensitivities Full accuracy rarely needed

→ imperfect model → perturbed system

Save computations

→ fix the integration grid → use tailored integration

schemes Implicit, A-stable integrators (stiff systems) QP Solution

SLIDE 155

Practical NMPC

44 / 47

Real Time Considerations

Real-time feasible: tcomp ≤ Ts = TP/N Integrators Preparation phase dominated by integration with sensitivities Full accuracy rarely needed

→ imperfect model → perturbed system

Save computations

→ fix the integration grid → use tailored integration

schemes Implicit, A-stable integrators (stiff systems) Few grid points QP Solution

SLIDE 156

Practical NMPC

44 / 47

Real Time Considerations

Real-time feasible: tcomp ≤ Ts = TP/N Integrators Preparation phase dominated by integration with sensitivities Full accuracy rarely needed

→ imperfect model → perturbed system

Save computations

→ fix the integration grid → use tailored integration

schemes Implicit, A-stable integrators (stiff systems) Few grid points Fixed number of iterations QP Solution

SLIDE 157

Practical NMPC

44 / 47

Real Time Considerations

Real-time feasible: tcomp ≤ Ts = TP/N Integrators Preparation phase dominated by integration with sensitivities Full accuracy rarely needed

→ imperfect model → perturbed system

Save computations

→ fix the integration grid → use tailored integration

schemes Implicit, A-stable integrators (stiff systems) Few grid points Fixed number of iterations QP Solution As N increases: Longer condensing

SLIDE 158

Practical NMPC

44 / 47

Real Time Considerations

Real-time feasible: tcomp ≤ Ts = TP/N Integrators Preparation phase dominated by integration with sensitivities Full accuracy rarely needed

→ imperfect model → perturbed system

Save computations

→ fix the integration grid → use tailored integration

schemes Implicit, A-stable integrators (stiff systems) Few grid points Fixed number of iterations QP Solution As N increases: Longer condensing Longer QP solution

SLIDE 159

Practical NMPC

44 / 47

Real Time Considerations

Real-time feasible: tcomp ≤ Ts = TP/N Integrators Preparation phase dominated by integration with sensitivities Full accuracy rarely needed

→ imperfect model → perturbed system

Save computations

→ fix the integration grid → use tailored integration

schemes Implicit, A-stable integrators (stiff systems) Few grid points Fixed number of iterations QP Solution As N increases: Longer condensing Longer QP solution N long enough but not too long

SLIDE 160

Practical NMPC

44 / 47

Real Time Considerations

Real-time feasible: tcomp ≤ Ts = TP/N Integrators Preparation phase dominated by integration with sensitivities Full accuracy rarely needed

→ imperfect model → perturbed system

Save computations

→ fix the integration grid → use tailored integration

schemes Implicit, A-stable integrators (stiff systems) Few grid points Fixed number of iterations QP Solution As N increases: Longer condensing Longer QP solution N long enough but not too long LQR terminal cost QN

SLIDE 161

Tutorial

45 / 47

1

From Linear Feedback to NMPC

2

Nonlinear Programming and SQP

3

From Continuous Time to Discrete Time

4

Moving Horizon Estimation

5

Practical NMPC

6

Tutorial

SLIDE 162

Tutorial

46 / 47

Let’s control an overhead crane!

Model equations ˙ xC = vC ˙ vC = aC = − 1 τ1 vC + a1 τ1 uC ˙ xL = vL ˙ vL = aL = − 1 τ2 vL + a2 τ2 uL ˙ θ = ω ˙ ω = 1 xL (−g sin(θ) − aC cos(θ) − 2vLω − cω mxL ) ˙ uC = uR

C

˙ uL = uR

L

SLIDE 163

47 / 47