nonlinear model predictive control - research unit dyscodysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf ·...
TRANSCRIPT
![Page 1: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/1.jpg)
Nonlinear Model Predictive Control
Mario Zanon, Alberto Bemporad
Thanks toS. Gros, M. Vukov, J. Frasch, M. Diehl
for some of the material and their precious help
IMT Lucca
![Page 2: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/2.jpg)
2 / 28
Course: Numerical Methods for Optimal Control
Direct Nonlinear Optimal Control
May 25-29, 2020
20 hours lectures
10 hours supervised assignments
![Page 3: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/3.jpg)
Numerical Methods - From Linear Feedback to MPC 3 / 28
Linear system
sk+1 = Ask + Buk
Linear feedback uk = −Ksksk+1 = (A− BK)sk = AK sk
stable if max(|λ(AK )|
)≤ 1
How to choose K?
What about LQR?
mins,u
1
2
∞∑k=0
‖sk‖2Q + ‖uk‖2
R
s.t. s0 = x
sk+1 = Ask + Buk , k ≥ 0
limk→∞
sk = 0
Equivalent to solving the DARE(discrete algebraic Riccati equation)
P = Q + ATPA− ATPBK
K = (R + BTPB)−1BTPA
![Page 4: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/4.jpg)
Numerical Methods - From Linear Feedback to MPC 3 / 28
Linear system
sk+1 = Ask + Buk
Linear feedback uk = −Ksksk+1 = (A− BK)sk = AK sk
stable if max(|λ(AK )|
)≤ 1
How to choose K?
What about LQR?
mins,u
1
2
∞∑k=0
‖sk‖2Q + ‖uk‖2
R
s.t. s0 = x
sk+1 = Ask + Buk , k ≥ 0
limk→∞
sk = 0
Equivalent to solving the DARE(discrete algebraic Riccati equation)
P = Q + ATPA− ATPBK
K = (R + BTPB)−1BTPA
![Page 5: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/5.jpg)
Numerical Methods - From Linear Feedback to MPC 3 / 28
Linear system
sk+1 = Ask + Buk
Linear feedback uk = −Ksksk+1 = (A− BK)sk = AK sk
stable if max(|λ(AK )|
)≤ 1
How to choose K?
What about LQR?
mins,u
1
2
∞∑k=0
‖sk‖2Q + ‖uk‖2
R
s.t. s0 = x
sk+1 = Ask + Buk , k ≥ 0
limk→∞
sk = 0
Equivalent to solving the DARE(discrete algebraic Riccati equation)
P = Q + ATPA− ATPBK
K = (R + BTPB)−1BTPA
![Page 6: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/6.jpg)
Numerical Methods - From Linear Feedback to MPC 3 / 28
Linear system
sk+1 = Ask + Buk
Linear feedback uk = −Ksksk+1 = (A− BK)sk = AK sk
stable if max(|λ(AK )|
)≤ 1
How to choose K?
What about LQR?
mins,u
1
2
∞∑k=0
‖sk‖2Q + ‖uk‖2
R
s.t. s0 = x
sk+1 = Ask + Buk , k ≥ 0
limk→∞
sk = 0
Equivalent to solving the DARE(discrete algebraic Riccati equation)
P = Q + ATPA− ATPBK
K = (R + BTPB)−1BTPA
![Page 7: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/7.jpg)
Numerical Methods - From Linear Feedback to MPC 3 / 28
Linear system
sk+1 = Ask + Buk
Linear feedback uk = −Ksksk+1 = (A− BK)sk = AK sk
stable if max(|λ(AK )|
)≤ 1
How to choose K?
What about LQR?
mins,u
1
2
∞∑k=0
‖sk‖2Q + ‖uk‖2
R
s.t. s0 = x
sk+1 = Ask + Buk , k ≥ 0
limk→∞
sk = 0
Equivalent to solving the DARE(discrete algebraic Riccati equation)
P = Q + ATPA− ATPBK
K = (R + BTPB)−1BTPA
![Page 8: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/8.jpg)
Numerical Methods - From Linear Feedback to MPC 3 / 28
Linear system
sk+1 = Ask + Buk
Linear feedback uk = −Ksksk+1 = (A− BK)sk = AK sk
stable if max(|λ(AK )|
)≤ 1
How to choose K?
What about LQR?
mins,u
1
2
∞∑k=0
‖sk‖2Q + ‖uk‖2
R
s.t. s0 = x
sk+1 = Ask + Buk , k ≥ 0
limk→∞
sk = 0
Equivalent to solving the DARE(discrete algebraic Riccati equation)
P = Q + ATPA− ATPBK
K = (R + BTPB)−1BTPA
![Page 9: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/9.jpg)
Numerical Methods - From Linear Feedback to MPC 4 / 28
Note the equivalence
Horizon: ∞
mins,u
1
2
∞∑k=0
‖sk‖2Q + ‖uk‖2
R
s.t. s0 = x
sk+1 = Ask + Buk , k ≥ 0
limk→∞
sk = 0
⇔
Horizon: N
mins,u
1
2
N∑k=0
‖sk‖2Q + ‖uk‖2
R +1
2‖sN‖2
P
s.t. s0 = x
sk+1 = Ask + Buk , k = 0, . . . ,N − 1
with N ≥ 1 and P from the DARE.
The term 12‖sN‖2
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)
![Page 10: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/10.jpg)
Numerical Methods - From Linear Feedback to MPC 4 / 28
Note the equivalence
Horizon: ∞
mins,u
1
2
∞∑k=0
‖sk‖2Q + ‖uk‖2
R
s.t. s0 = x
sk+1 = Ask + Buk , k ≥ 0
limk→∞
sk = 0
⇔
Horizon: N
mins,u
1
2
N∑k=0
‖sk‖2Q + ‖uk‖2
R +1
2‖sN‖2
P
s.t. s0 = x
sk+1 = Ask + Buk , k = 0, . . . ,N − 1
with N ≥ 1 and P from the DARE.
The term 12‖sN‖2
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)
![Page 11: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/11.jpg)
Numerical Methods - From Linear Feedback to MPC 4 / 28
Note the equivalence
Horizon: ∞
mins,u
1
2
∞∑k=0
‖sk‖2Q + ‖uk‖2
R
s.t. s0 = x
sk+1 = Ask + Buk , k ≥ 0
limk→∞
sk = 0
⇔
Horizon: N
mins,u
1
2
N∑k=0
‖sk‖2Q + ‖uk‖2
R +1
2‖sN‖2
P
s.t. s0 = x
sk+1 = Ask + Buk , k = 0, . . . ,N − 1
with N ≥ 1 and P from the DARE.
The term 12‖sN‖2
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)
![Page 12: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/12.jpg)
Numerical Methods - From Linear Feedback to MPC 5 / 28
At each sampling instant i , solve the QP
minu,s
1
2
N−1∑k=0
‖sk‖2Q + ‖uk‖2
R +1
2‖sN‖2
P
s.t. s0 = xi
sk+1 = A sk + B uk
⇔
minw
1
2wTFw + f Tw
s.t. Gw + g = 0
Lagrangian Function
L(w , λ) =1
2wTFw + f Tw − λT (Gw + g)
First order necessary condition (FONC)
∇L(w , λ) = 0 ⇒{
Fw + f − GTλ = 0Gw + g = 0
Solve a linear system: [F GT
G 0
] [w−λ
]= −
[fg
]
![Page 13: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/13.jpg)
Numerical Methods - From Linear Feedback to MPC 5 / 28
At each sampling instant i , solve the QP
minu,s
1
2
N−1∑k=0
‖sk‖2Q + ‖uk‖2
R +1
2‖sN‖2
P
s.t. s0 = xi
sk+1 = A sk + B uk
⇔
minw
1
2wTFw + f Tw
s.t. Gw + g = 0
Lagrangian Function
L(w , λ) =1
2wTFw + f Tw − λT (Gw + g)
First order necessary condition (FONC)
∇L(w , λ) = 0 ⇒{
Fw + f − GTλ = 0Gw + g = 0
Solve a linear system: [F GT
G 0
] [w−λ
]= −
[fg
]
![Page 14: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/14.jpg)
Numerical Methods - From Linear Feedback to MPC 5 / 28
At each sampling instant i , solve the QP
minu,s
1
2
N−1∑k=0
‖sk‖2Q + ‖uk‖2
R +1
2‖sN‖2
P
s.t. s0 = xi
sk+1 = A sk + B uk
⇔
minw
1
2wTFw + f Tw
s.t. Gw + g = 0
Lagrangian Function
L(w , λ) =1
2wTFw + f Tw − λT (Gw + g)
First order necessary condition (FONC)
∇L(w , λ) = 0 ⇒{
Fw + f − GTλ = 0Gw + g = 0
Solve a linear system: [F GT
G 0
] [w−λ
]= −
[fg
]
![Page 15: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/15.jpg)
Numerical Methods - From Linear Feedback to MPC 5 / 28
At each sampling instant i , solve the QP
minu,s
1
2
N−1∑k=0
‖sk‖2Q + ‖uk‖2
R +1
2‖sN‖2
P
s.t. s0 = xi
sk+1 = A sk + B uk
⇔
minw
1
2wTFw + f Tw
s.t. Gw + g = 0
Lagrangian Function
L(w , λ) =1
2wTFw + f Tw − λT (Gw + g)
First order necessary condition (FONC)
∇L(w , λ) = 0 ⇒{
Fw + f − GTλ = 0Gw + g = 0
Solve a linear system: [F GT
G 0
] [w−λ
]= −
[fg
]
![Page 16: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/16.jpg)
Numerical Methods - From Linear Feedback to MPC 5 / 28
At each sampling instant i , solve the QP
minu,s
1
2
N−1∑k=0
‖sk‖2Q + ‖uk‖2
R +1
2‖sN‖2
P
s.t. s0 = xi
sk+1 = A sk + B uk
⇔
minw
1
2wTFw + f Tw
s.t. Gw + g = 0
Lagrangian Function
L(w , λ) =1
2wTFw + f Tw − λT (Gw + g)
First order necessary condition (FONC)
∇L(w , λ) = 0 ⇒{
Fw + f − GTλ = 0Gw + g = 0
Solve a linear system: [F GT
G 0
] [w−λ
]= −
[fg
]
![Page 17: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/17.jpg)
Numerical Methods - From Linear Feedback to MPC 6 / 28
Treating Constrained Systems
minu,s
1
2
N−1∑k=0
‖sk‖2Q + ‖uk‖2
R +1
2‖sN‖2
P
s.t. s0 = xi
sk+1 = A sk + B uk
C sk + D uk + c ≥ 0
LQR: unconstrained
MPC: state and input constraints
‖sN‖2P 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!
![Page 18: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/18.jpg)
Numerical Methods - From Linear Feedback to MPC 6 / 28
Treating Constrained Systems
minu,s
1
2
N−1∑k=0
‖sk‖2Q + ‖uk‖2
R +1
2‖sN‖2
P
s.t. s0 = xi
sk+1 = A sk + B uk
C sk + D uk + c ≥ 0
LQR: unconstrained
MPC: state and input constraints
‖sN‖2P 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!
![Page 19: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/19.jpg)
Numerical Methods - From Linear Feedback to MPC 6 / 28
Treating Constrained Systems
minu,s
1
2
N−1∑k=0
‖sk‖2Q + ‖uk‖2
R +1
2‖sN‖2
P
s.t. s0 = xi
sk+1 = A sk + B uk
C sk + D uk + c ≥ 0
LQR: unconstrained
MPC: state and input constraints
‖sN‖2P 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!
![Page 20: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/20.jpg)
Numerical Methods - From Linear Feedback to MPC 6 / 28
Treating Constrained Systems
minu,s
1
2
N−1∑k=0
‖sk‖2Q + ‖uk‖2
R +1
2‖sN‖2
P
s.t. s0 = xi
sk+1 = A sk + B uk
C sk + D uk + c ≥ 0
LQR: unconstrained
MPC: state and input constraints
‖sN‖2P 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!
![Page 21: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/21.jpg)
Numerical Methods - From Linear Feedback to MPC 7 / 28
At each sampling instant i , solve the QP
minu,s
1
2
N−1∑k=0
‖sk‖2Q + ‖uk‖2
R +1
2‖sN‖2
P
s.t. s0 = xi
sk+1 = A sk + B uk
C sk + D uk + c ≥ 0
⇔minw
1
2wTFw + f Tw
s.t. Gw + g = 0
Hw + h ≥ 0
Lagrangian Function
L(w , λ, µ) =1
2wTFw + f Tw − λT (Gw + g)− µT (Hw + h)
First order necessary condition (FONC): the KKT conditions
∇L(w , λ, µ) = 0 ⇒
Fw + f − GTλ− HTµ = 0Gw + g = 0Hw + h ≥ 0µ ≥ 0µi (Hw + h)i = 0
![Page 22: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/22.jpg)
Numerical Methods - From Linear Feedback to MPC 7 / 28
At each sampling instant i , solve the QP
minu,s
1
2
N−1∑k=0
‖sk‖2Q + ‖uk‖2
R +1
2‖sN‖2
P
s.t. s0 = xi
sk+1 = A sk + B uk
C sk + D uk + c ≥ 0
⇔minw
1
2wTFw + f Tw
s.t. Gw + g = 0
Hw + h ≥ 0
Lagrangian Function
L(w , λ, µ) =1
2wTFw + f Tw − λT (Gw + g)− µT (Hw + h)
First order necessary condition (FONC): the KKT conditions
∇L(w , λ, µ) = 0 ⇒
Fw + f − GTλ− HTµ = 0Gw + g = 0Hw + h ≥ 0µ ≥ 0µi (Hw + h)i = 0
![Page 23: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/23.jpg)
Numerical Methods - From Linear Feedback to MPC 7 / 28
At each sampling instant i , solve the QP
minu,s
1
2
N−1∑k=0
‖sk‖2Q + ‖uk‖2
R +1
2‖sN‖2
P
s.t. s0 = xi
sk+1 = A sk + B uk
C sk + D uk + c ≥ 0
⇔minw
1
2wTFw + f Tw
s.t. Gw + g = 0
Hw + h ≥ 0
Lagrangian Function
L(w , λ, µ) =1
2wTFw + f Tw − λT (Gw + g)− µT (Hw + h)
First order necessary condition (FONC): the KKT conditions
∇L(w , λ, µ) = 0 ⇒
Fw + f − GTλ− HTµ = 0Gw + g = 0Hw + h ≥ 0µ ≥ 0µi (Hw + h)i = 0
![Page 24: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/24.jpg)
Numerical Methods - From Linear Feedback to MPC 8 / 28
Solving the KKT conditions
Fw + f − GTλ− HTµ = 0Gw + g = 0Hw + h ≥ 0µ ≥ 0µi (Hw + h)i = 0
The Active Set methodLet A be the set of active constraints
Fw + f − GTλ− HTµ = 0Gw + g = 0HAw + hA = 0µA = 0
Guess ASolve the AS-KKT system
Update A
The Interior Point method
Fw + f − GTλ− HTµ = 0Gw + g = 0Hw + h + s = 0µi si = τµ ≥ 0s ≥ 0
Choose τ “big”
Solve the IP-KKT system
Perform linesearch
update τ
![Page 25: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/25.jpg)
Numerical Methods - From Linear Feedback to MPC 8 / 28
Solving the KKT conditions
Fw + f − GTλ− HTµ = 0Gw + g = 0Hw + h ≥ 0µ ≥ 0µi (Hw + h)i = 0
The Active Set methodLet A be the set of active constraints
Fw + f − GTλ− HTµ = 0Gw + g = 0HAw + hA = 0µA = 0
Guess ASolve the AS-KKT system
Update A
The Interior Point method
Fw + f − GTλ− HTµ = 0Gw + g = 0Hw + h + s = 0µi si = τµ ≥ 0s ≥ 0
Choose τ “big”
Solve the IP-KKT system
Perform linesearch
update τ
![Page 26: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/26.jpg)
Numerical Methods - From Linear Feedback to MPC 8 / 28
Solving the KKT conditions
Fw + f − GTλ− HTµ = 0Gw + g = 0Hw + h ≥ 0µ ≥ 0µi (Hw + h)i = 0
The Active Set methodLet A be the set of active constraints
Fw + f − GTλ− HTµ = 0Gw + g = 0HAw + hA = 0µA = 0
Guess ASolve the AS-KKT system
Update A
The Interior Point method
Fw + f − GTλ− HTµ = 0Gw + g = 0Hw + h + s = 0µi si = τµ ≥ 0s ≥ 0
Choose τ “big”
Solve the IP-KKT system
Perform linesearch
update τ
![Page 27: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/27.jpg)
Numerical Methods - From Linear Feedback to MPC 9 / 28
QP solvers for MPC
Convex QP:
No inequalities: solve a linear system
Inequalities: interior point or active set method
Nonconvex QP: NP-hard problem
Classes of QP solvers:
Active-set
Interior-point
First-order methods (difficult to use for nonconvex problems)
Many reliable QP solvers available:
qpOASES, qpDUNES
FORCES, HPMPC / HPIPM
ODYSQP
many others
Condensing
Eliminate states (cost N2)
Solve dense QP
Sparse linear algebra
Exploit the qp structure
![Page 28: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/28.jpg)
Numerical Methods - From Linear Feedback to MPC 9 / 28
QP solvers for MPC
Convex QP:
No inequalities: solve a linear system
Inequalities: interior point or active set method
Nonconvex QP: NP-hard problem
Classes of QP solvers:
Active-set
Interior-point
First-order methods (difficult to use for nonconvex problems)
Many reliable QP solvers available:
qpOASES, qpDUNES
FORCES, HPMPC / HPIPM
ODYSQP
many others
Condensing
Eliminate states (cost N2)
Solve dense QP
Sparse linear algebra
Exploit the qp structure
![Page 29: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/29.jpg)
Numerical Methods - From Linear Feedback to MPC 9 / 28
QP solvers for MPC
Convex QP:
No inequalities: solve a linear system
Inequalities: interior point or active set method
Nonconvex QP: NP-hard problem
Classes of QP solvers:
Active-set
Interior-point
First-order methods (difficult to use for nonconvex problems)
Many reliable QP solvers available:
qpOASES, qpDUNES
FORCES, HPMPC / HPIPM
ODYSQP
many others
Condensing
Eliminate states (cost N2)
Solve dense QP
Sparse linear algebra
Exploit the qp structure
![Page 30: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/30.jpg)
Numerical Methods - From Linear Feedback to MPC 9 / 28
QP solvers for MPC
Convex QP:
No inequalities: solve a linear system
Inequalities: interior point or active set method
Nonconvex QP: NP-hard problem
Classes of QP solvers:
Active-set
Interior-point
First-order methods (difficult to use for nonconvex problems)
Many reliable QP solvers available:
qpOASES, qpDUNES
FORCES, HPMPC / HPIPM
ODYSQP
many others
Condensing
Eliminate states (cost N2)
Solve dense QP
Sparse linear algebra
Exploit the qp structure
![Page 31: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/31.jpg)
Numerical Methods - From Linear Feedback to MPC 9 / 28
QP solvers for MPC
Convex QP:
No inequalities: solve a linear system
Inequalities: interior point or active set method
Nonconvex QP: NP-hard problem
Classes of QP solvers:
Active-set
Interior-point
First-order methods (difficult to use for nonconvex problems)
Many reliable QP solvers available:
qpOASES, qpDUNES
FORCES, HPMPC / HPIPM
ODYSQP
many others
Condensing
Eliminate states (cost N2)
Solve dense QP
Sparse linear algebra
Exploit the qp structure
![Page 32: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/32.jpg)
Numerical Methods - From Linear Feedback to MPC 9 / 28
QP solvers for MPC
Convex QP:
No inequalities: solve a linear system
Inequalities: interior point or active set method
Nonconvex QP: NP-hard problem
Classes of QP solvers:
Active-set
Interior-point
First-order methods (difficult to use for nonconvex problems)
Many reliable QP solvers available:
qpOASES, qpDUNES
FORCES, HPMPC / HPIPM
ODYSQP
many others
Condensing
Eliminate states (cost N2)
Solve dense QP
Sparse linear algebra
Exploit the qp structure
![Page 33: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/33.jpg)
Numerical Methods - From Linear Feedback to MPC 9 / 28
QP solvers for MPC
Convex QP:
No inequalities: solve a linear system
Inequalities: interior point or active set method
Nonconvex QP: NP-hard problem
Classes of QP solvers:
Active-set
Interior-point
First-order methods (difficult to use for nonconvex problems)
Many reliable QP solvers available:
qpOASES, qpDUNES
FORCES, HPMPC / HPIPM
ODYSQP
many others
Condensing
Eliminate states (cost N2)
Solve dense QP
Sparse linear algebra
Exploit the qp structure
![Page 34: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/34.jpg)
Numerical Methods - From MPC to NMPC 10 / 28
Linear system?
Linear MPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
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 tomedium scale problems
Nonlinear system?
Linearize at xref , uref , uselinear MPC
or...
Nonlinear MPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk )
h (sk , uk ) ≥ 0,
s0 = xi
Problem is non-convex,use NLP solver
![Page 35: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/35.jpg)
Numerical Methods - From MPC to NMPC 10 / 28
Linear system?
Linear MPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
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 tomedium scale problems
Nonlinear system?
Linearize at xref , uref , uselinear MPC
or...
Nonlinear MPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk )
h (sk , uk ) ≥ 0,
s0 = xi
Problem is non-convex,use NLP solver
![Page 36: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/36.jpg)
Numerical Methods - From MPC to NMPC 10 / 28
Linear system?
Linear MPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
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 tomedium scale problems
Nonlinear system?
Linearize at xref , uref , uselinear MPC
or...
Nonlinear MPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk )
h (sk , uk ) ≥ 0,
s0 = xi
Problem is non-convex,use NLP solver
![Page 37: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/37.jpg)
Numerical Methods - From MPC to NMPC 10 / 28
Linear system?
Linear MPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
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 tomedium scale problems
Nonlinear system?
Linearize at xref , uref , uselinear MPC
or...
Nonlinear MPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk )
h (sk , uk ) ≥ 0,
s0 = xi
Problem is non-convex,use NLP solver
![Page 38: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/38.jpg)
Numerical Methods - From MPC to NMPC 10 / 28
Linear system?
Linear MPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
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 tomedium scale problems
Nonlinear system?
Linearize at xref , uref , uselinear MPC
or...
Nonlinear MPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk )
h (sk , uk ) ≥ 0,
s0 = xi
Problem is non-convex,use NLP solver
![Page 39: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/39.jpg)
Numerical Methods - From MPC to NMPC 11 / 28
SQP for NMPC in a nutshell
Quadratic Problem Approximation
NMPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ 0,
s0 = xi
QP (for a given s, u)
min∆u,∆s
1
2
[∆s ∆u
] [∆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: need 2nd order derivatives for B
3 Make sure Hessian B � 0: avoid negative curvature
4 Solve QP
5 Globalization (e.g. line-search): ensure descent, stepsize α ∈ (0, 1]
6 Update
[s+
u+
]=
[su
]+ α
[∆s∆u
]and iterate
![Page 40: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/40.jpg)
Numerical Methods - From MPC to NMPC 11 / 28
SQP for NMPC in a nutshell Quadratic Problem Approximation
NMPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ 0,
s0 = xi
QP (for a given s, u)
min∆u,∆s
1
2
[∆s ∆u
] [∆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: need 2nd order derivatives for B
3 Make sure Hessian B � 0: avoid negative curvature
4 Solve QP
5 Globalization (e.g. line-search): ensure descent, stepsize α ∈ (0, 1]
6 Update
[s+
u+
]=
[su
]+ α
[∆s∆u
]and iterate
![Page 41: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/41.jpg)
Numerical Methods - From MPC to NMPC 11 / 28
SQP for NMPC in a nutshell Quadratic Problem Approximation
NMPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ 0,
s0 = xi
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
Iterative procedure (at each time i):
1 Given current guess s, u
2 Linearize at s, u: need 2nd order derivatives for B
3 Make sure Hessian B � 0: avoid negative curvature
4 Solve QP
5 Globalization (e.g. line-search): ensure descent, stepsize α ∈ (0, 1]
6 Update
[s+
u+
]=
[su
]+ α
[∆s∆u
]and iterate
![Page 42: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/42.jpg)
Numerical Methods - From MPC to NMPC 11 / 28
SQP for NMPC in a nutshell Quadratic Problem Approximation
NMPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ 0,
s0 = xi
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
Iterative procedure (at each time i):
1 Given current guess s, u
2 Linearize at s, u: need 2nd order derivatives for B
3 Make sure Hessian B � 0: avoid negative curvature
4 Solve QP
5 Globalization (e.g. line-search): ensure descent, stepsize α ∈ (0, 1]
6 Update
[s+
u+
]=
[su
]+ α
[∆s∆u
]and iterate
![Page 43: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/43.jpg)
Numerical Methods - From MPC to NMPC 11 / 28
SQP for NMPC in a nutshell Quadratic Problem Approximation
NMPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ 0,
s0 = xi
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
Iterative procedure (at each time i):
1 Given current guess s, u
2 Linearize at s, u: need 2nd order derivatives for B
3 Make sure Hessian B � 0: avoid negative curvature
4 Solve QP
5 Globalization (e.g. line-search): ensure descent, stepsize α ∈ (0, 1]
6 Update
[s+
u+
]=
[su
]+ α
[∆s∆u
]and iterate
![Page 44: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/44.jpg)
Numerical Methods - From MPC to NMPC 11 / 28
SQP for NMPC in a nutshell Quadratic Problem Approximation
NMPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ 0,
s0 = xi
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
Iterative procedure (at each time i):
1 Given current guess s, u
2 Linearize at s, u: need 2nd order derivatives for B
3 Make sure Hessian B � 0: avoid negative curvature
4 Solve QP
5 Globalization (e.g. line-search): ensure descent, stepsize α ∈ (0, 1]
6 Update
[s+
u+
]=
[su
]+ α
[∆s∆u
]and iterate
![Page 45: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/45.jpg)
Numerical Methods - From MPC to NMPC 11 / 28
SQP for NMPC in a nutshell Quadratic Problem Approximation
NMPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ 0,
s0 = xi
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
Iterative procedure (at each time i):
1 Given current guess s, u
2 Linearize at s, u: need 2nd order derivatives for B
3 Make sure Hessian B � 0: avoid negative curvature
4 Solve QP
5 Globalization (e.g. line-search): ensure descent, stepsize α ∈ (0, 1]
6 Update
[s+
u+
]=
[su
]+ α
[∆s∆u
]and iterate
![Page 46: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/46.jpg)
Numerical Methods - From Continuous Time to Discrete Time 12 / 28
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τ
Integration of function fc can becomplex, possibly iterative implicit
(algorithm) !!
![Page 47: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/47.jpg)
Numerical Methods - From Continuous Time to Discrete Time 12 / 28
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τ
Integration of function fc can becomplex, possibly iterative implicit
(algorithm) !!
![Page 48: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/48.jpg)
Numerical Methods - From Continuous Time to Discrete Time 13 / 28
Integration (with sensitivities)
Consider x = fc(x , u)
Discretize with explicit Euler:
x1 = hfc(x0, u) + x0, A1 =dx1
dx0, B1 =
dx1
du
x2 = hfc(x1, u) + x1, A2 =dx2
dx0, B2 =
dx2
du
Sensitivities wrt states:
A1 =∂x1
∂x0= (I + h∇x fc(x0, u))
A2 =∂x2
∂x1
∂x1
∂x0= (I + h∇x fc(x1, u))A1
For the controls it’s a bit trickier:
B1 =∂x1
∂u= h∇ufc(x0, u)
B2 =∂x2
∂u+∂x2
∂x1
∂x1
∂u= h∇ufc(x1, u) + (I + h∇fc(x1, u))B1
![Page 49: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/49.jpg)
Numerical Methods - From Continuous Time to Discrete Time 13 / 28
Integration (with sensitivities)
Consider x = fc(x , u)
Discretize with explicit Euler:
x1 = hfc(x0, u) + x0, A1 =dx1
dx0, B1 =
dx1
du
x2 = hfc(x1, u) + x1, A2 =dx2
dx0, B2 =
dx2
du
Sensitivities wrt states:
A1 =∂x1
∂x0= (I + h∇x fc(x0, u))
A2 =∂x2
∂x1
∂x1
∂x0= (I + h∇x fc(x1, u))A1
For the controls it’s a bit trickier:
B1 =∂x1
∂u= h∇ufc(x0, u)
B2 =∂x2
∂u+∂x2
∂x1
∂x1
∂u= h∇ufc(x1, u) + (I + h∇fc(x1, u))B1
![Page 50: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/50.jpg)
Numerical Methods - From Continuous Time to Discrete Time 13 / 28
Integration (with sensitivities)
Consider x = fc(x , u)
Discretize with explicit Euler:
x1 = hfc(x0, u) + x0, A1 =dx1
dx0, B1 =
dx1
du
x2 = hfc(x1, u) + x1, A2 =dx2
dx0, B2 =
dx2
du
Sensitivities wrt states:
A1 =∂x1
∂x0= (I + h∇x fc(x0, u))
A2 =∂x2
∂x1
∂x1
∂x0= (I + h∇x fc(x1, u))A1
For the controls it’s a bit trickier:
B1 =∂x1
∂u= h∇ufc(x0, u)
B2 =∂x2
∂u+∂x2
∂x1
∂x1
∂u= h∇ufc(x1, u) + (I + h∇fc(x1, u))B1
![Page 51: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/51.jpg)
Numerical Methods - From Continuous Time to Discrete Time 13 / 28
Integration (with sensitivities)
Consider x = fc(x , u)
Discretize with explicit Euler:
x1 = hfc(x0, u) + x0, A1 =dx1
dx0, B1 =
dx1
du
x2 = hfc(x1, u) + x1, A2 =dx2
dx0, B2 =
dx2
du
Sensitivities wrt states:
A1 =∂x1
∂x0= (I + h∇x fc(x0, u))
A2 =∂x2
∂x1
∂x1
∂x0= (I + h∇x fc(x1, u))A1
For the controls it’s a bit trickier:
B1 =∂x1
∂u= h∇ufc(x0, u)
B2 =∂x2
∂u+∂x2
∂x1
∂x1
∂u= h∇ufc(x1, u) + (I + h∇fc(x1, u))B1
![Page 52: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/52.jpg)
Numerical Methods - From Continuous Time to Discrete Time 13 / 28
Integration (with sensitivities)
Consider x = fc(x , u)
Discretize with explicit Euler:
x1 = hfc(x0, u) + x0, A1 =dx1
dx0, B1 =
dx1
du
x2 = hfc(x1, u) + x1, A2 =dx2
dx0, B2 =
dx2
du
Sensitivities wrt states:
A1 =∂x1
∂x0= (I + h∇x fc(x0, u))
A2 =∂x2
∂x1
∂x1
∂x0= (I + h∇x fc(x1, u))A1
For the controls it’s a bit trickier:
B1 =∂x1
∂u= h∇ufc(x0, u)
B2 =∂x2
∂u+∂x2
∂x1
∂x1
∂u= h∇ufc(x1, u) + (I + h∇fc(x1, u))B1
![Page 53: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/53.jpg)
Numerical Methods - From Continuous Time to Discrete Time 13 / 28
Integration (with sensitivities)
Consider x = fc(x , u)
Discretize with explicit Euler:
x1 = hfc(x0, u) + x0, A1 =dx1
dx0, B1 =
dx1
du
x2 = hfc(x1, u) + x1, A2 =dx2
dx0, B2 =
dx2
du
Sensitivities wrt states:
A1 =∂x1
∂x0= (I + h∇x fc(x0, u))
A2 =∂x2
∂x1
∂x1
∂x0= (I + h∇x fc(x1, u))A1
For the controls it’s a bit trickier:
B1 =∂x1
∂u= h∇ufc(x0, u)
B2 =∂x2
∂u+∂x2
∂x1
∂x1
∂u= h∇ufc(x1, u) + (I + h∇fc(x1, u))B1
![Page 54: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/54.jpg)
Numerical Methods - From Continuous Time to Discrete Time 13 / 28
Integration (with sensitivities)
Consider x = fc(x , u)
Discretize with explicit Euler:
x1 = hfc(x0, u) + x0, A1 =dx1
dx0, B1 =
dx1
du
x2 = hfc(x1, u) + x1, A2 =dx2
dx0, B2 =
dx2
du
Sensitivities wrt states:
A1 =∂x1
∂x0= (I + h∇x fc(x0, u))
A2 =∂x2
∂x1
∂x1
∂x0= (I + h∇x fc(x1, u))A1
For the controls it’s a bit trickier:
B1 =∂x1
∂u= h∇ufc(x0, u)
B2 =∂x2
∂u+∂x2
∂x1
∂x1
∂u= h∇ufc(x1, u) + (I + h∇fc(x1, u))B1
![Page 55: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/55.jpg)
Numerical Methods - From Continuous Time to Discrete Time 13 / 28
Integration (with sensitivities)
Consider x = fc(x , u)
Discretize with explicit Euler:
x1 = hfc(x0, u) + x0, A1 =dx1
dx0, B1 =
dx1
du
x2 = hfc(x1, u) + x1, A2 =dx2
dx0, B2 =
dx2
du
Sensitivities wrt states:
A1 =∂x1
∂x0= (I + h∇x fc(x0, u))
A2 =∂x2
∂x1
∂x1
∂x0= (I + h∇x fc(x1, u))A1
For the controls it’s a bit trickier:
B1 =∂x1
∂u= h∇ufc(x0, u)
B2 =∂x2
∂u+∂x2
∂x1
∂x1
∂u= h∇ufc(x1, u) + (I + h∇fc(x1, u))B1
![Page 56: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/56.jpg)
Numerical Methods - From Continuous Time to Discrete Time 13 / 28
Integration (with sensitivities)
Consider x = fc(x , u)
Discretize with explicit Euler:
x1 = hfc(x0, u) + x0, A1 =dx1
dx0, B1 =
dx1
du
x2 = hfc(x1, u) + x1, A2 =dx2
dx0, B2 =
dx2
du
Sensitivities wrt states:
A1 =∂x1
∂x0= (I + h∇x fc(x0, u))
A2 =∂x2
∂x1
∂x1
∂x0= (I + h∇x fc(x1, u))A1
For the controls it’s a bit trickier:
B1 =∂x1
∂u= h∇ufc(x0, u)
B2 =∂x2
∂u+∂x2
∂x1
∂x1
∂u= h∇ufc(x1, u) + (I + h∇fc(x1, u))B1
![Page 57: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/57.jpg)
Numerical Methods - From Continuous Time to Discrete Time 13 / 28
Integration (with sensitivities)
Consider x = fc(x , u)
Discretize with explicit Euler:
x1 = hfc(x0, u) + x0, A1 =dx1
dx0, B1 =
dx1
du
x2 = hfc(x1, u) + x1, A2 =dx2
dx0, B2 =
dx2
du
Sensitivities wrt states:
A1 =∂x1
∂x0= (I + h∇x fc(x0, u))
A2 =∂x2
∂x1
∂x1
∂x0= (I + h∇x fc(x1, u))A1
For the controls it’s a bit trickier:
B1 =∂x1
∂u= h∇ufc(x0, u)
B2 =∂x2
∂u+∂x2
∂x1
∂x1
∂u= h∇ufc(x1, u) + (I + h∇fc(x1, u))B1
![Page 58: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/58.jpg)
Numerical Methods - From Continuous Time to Discrete Time 13 / 28
Integration (with sensitivities)
There are many numerical schemes:
Explicit Euler is usually not the most efficient method! Inaccuracy: O(h)
Explicit Runge-Kutta of order 4 is rather successful. Inaccuracy: O(h4)
k1 = fc(x , u) k2 = fc
(x +
h
2k1, u
)k3 = fc
(x +
h
2k2, u
)k4 = fc(x + hk3, u)
x+ = x +h
6(k1 + 2k2 + 2k3 + k4)
Implicit schemes have desirable properties (stiff systems)
Simplest example (implicit Euler): x+ = x + hfc(x+, u)
Collocation = Implicit Runge-Kutta
Exponential integrators, e.g.
x+ = Ax + Bu, A = eh∇x fc(x,u), B =
∫ h
0
eτ∇x fc(x,u)∇ufc(x , u)dτ
![Page 59: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/59.jpg)
Numerical Methods - From Continuous Time to Discrete Time 13 / 28
Integration (with sensitivities)
There are many numerical schemes:
Explicit Euler is usually not the most efficient method! Inaccuracy: O(h)
Explicit Runge-Kutta of order 4 is rather successful. Inaccuracy: O(h4)
k1 = fc(x , u) k2 = fc
(x +
h
2k1, u
)k3 = fc
(x +
h
2k2, u
)k4 = fc(x + hk3, u)
x+ = x +h
6(k1 + 2k2 + 2k3 + k4)
Implicit schemes have desirable properties (stiff systems)
Simplest example (implicit Euler): x+ = x + hfc(x+, u)
Collocation = Implicit Runge-Kutta
Exponential integrators, e.g.
x+ = Ax + Bu, A = eh∇x fc(x,u), B =
∫ h
0
eτ∇x fc(x,u)∇ufc(x , u)dτ
![Page 60: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/60.jpg)
Numerical Methods - From Continuous Time to Discrete Time 13 / 28
Integration (with sensitivities)
There are many numerical schemes:
Explicit Euler is usually not the most efficient method! Inaccuracy: O(h)
Explicit Runge-Kutta of order 4 is rather successful. Inaccuracy: O(h4)
k1 = fc(x , u) k2 = fc
(x +
h
2k1, u
)k3 = fc
(x +
h
2k2, u
)k4 = fc(x + hk3, u)
x+ = x +h
6(k1 + 2k2 + 2k3 + k4)
Implicit schemes have desirable properties (stiff systems)
Simplest example (implicit Euler): x+ = x + hfc(x+, u)
Collocation = Implicit Runge-Kutta
Exponential integrators, e.g.
x+ = Ax + Bu, A = eh∇x fc(x,u), B =
∫ h
0
eτ∇x fc(x,u)∇ufc(x , u)dτ
![Page 61: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/61.jpg)
Numerical Methods - From Continuous Time to Discrete Time 13 / 28
Integration (with sensitivities)
There are many numerical schemes:
Explicit Euler is usually not the most efficient method! Inaccuracy: O(h)
Explicit Runge-Kutta of order 4 is rather successful. Inaccuracy: O(h4)
k1 = fc(x , u) k2 = fc
(x +
h
2k1, u
)k3 = fc
(x +
h
2k2, u
)k4 = fc(x + hk3, u)
x+ = x +h
6(k1 + 2k2 + 2k3 + k4)
Implicit schemes have desirable properties (stiff systems)
Simplest example (implicit Euler): x+ = x + hfc(x+, u)
Collocation = Implicit Runge-Kutta
Exponential integrators, e.g.
x+ = Ax + Bu, A = eh∇x fc(x,u), B =
∫ h
0
eτ∇x fc(x,u)∇ufc(x , u)dτ
![Page 62: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/62.jpg)
Numerical Methods - From Continuous Time to Discrete Time 13 / 28
Integration (with sensitivities)
There are many numerical schemes:
Explicit Euler is usually not the most efficient method! Inaccuracy: O(h)
Explicit Runge-Kutta of order 4 is rather successful. Inaccuracy: O(h4)
k1 = fc(x , u) k2 = fc
(x +
h
2k1, u
)k3 = fc
(x +
h
2k2, u
)k4 = fc(x + hk3, u)
x+ = x +h
6(k1 + 2k2 + 2k3 + k4)
Implicit schemes have desirable properties (stiff systems)
Simplest example (implicit Euler): x+ = x + hfc(x+, u)
Collocation = Implicit Runge-Kutta
Exponential integrators, e.g.
x+ = Ax + Bu, A = eh∇x fc(x,u), B =
∫ h
0
eτ∇x fc(x,u)∇ufc(x , u)dτ
![Page 63: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/63.jpg)
Numerical Methods - From Continuous Time to Discrete Time 13 / 28
Integration (with sensitivities)
There are many numerical schemes:
Explicit Euler is usually not the most efficient method! Inaccuracy: O(h)
Explicit Runge-Kutta of order 4 is rather successful. Inaccuracy: O(h4)
k1 = fc(x , u) k2 = fc
(x +
h
2k1, u
)k3 = fc
(x +
h
2k2, u
)k4 = fc(x + hk3, u)
x+ = x +h
6(k1 + 2k2 + 2k3 + k4)
Implicit schemes have desirable properties (stiff systems)
Simplest example (implicit Euler): x+ = x + hfc(x+, u)
Collocation = Implicit Runge-Kutta
Exponential integrators, e.g.
x+ = Ax + Bu, A = eh∇x fc(x,u), B =
∫ h
0
eτ∇x fc(x,u)∇ufc(x , u)dτ
![Page 64: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/64.jpg)
Numerical Methods - From Continuous Time to Discrete Time 14 / 28
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
![Page 65: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/65.jpg)
Numerical Methods - From Continuous Time to Discrete Time 14 / 28
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 0.05 0.1 0.15 0.2 0.25 0.30.045
0.05
0.055
0.06
0.065
s0 = x(0)
s1 = x(Ts)
s2 = x(2Ts)
t
x
![Page 66: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/66.jpg)
Numerical Methods - From Continuous Time to Discrete Time 14 / 28
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 0.05 0.1 0.15 0.2 0.25 0.30.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
![Page 67: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/67.jpg)
Numerical Methods - From Continuous Time to Discrete Time 14 / 28
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 0.05 0.1 0.15 0.2 0.25 0.30.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
![Page 68: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/68.jpg)
Numerical Methods - From Continuous Time to Discrete Time 15 / 28
Single Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 0
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 69: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/69.jpg)
Numerical Methods - From Continuous Time to Discrete Time 15 / 28
Single Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 1
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 70: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/70.jpg)
Numerical Methods - From Continuous Time to Discrete Time 15 / 28
Single Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 71: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/71.jpg)
Numerical Methods - From Continuous Time to Discrete Time 15 / 28
Single Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 3
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 72: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/72.jpg)
Numerical Methods - From Continuous Time to Discrete Time 15 / 28
Single Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 73: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/73.jpg)
Numerical Methods - From Continuous Time to Discrete Time 15 / 28
Single Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 5
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 74: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/74.jpg)
Numerical Methods - From Continuous Time to Discrete Time 15 / 28
Single Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 6
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 75: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/75.jpg)
Numerical Methods - From Continuous Time to Discrete Time 15 / 28
Single Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 7
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 76: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/76.jpg)
Numerical Methods - From Continuous Time to Discrete Time 15 / 28
Single Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 8
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 77: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/77.jpg)
Numerical Methods - From Continuous Time to Discrete Time 15 / 28
Single Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 9
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 78: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/78.jpg)
Numerical Methods - From Continuous Time to Discrete Time 15 / 28
Single Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 10
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 79: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/79.jpg)
Numerical Methods - From Continuous Time to Discrete Time 15 / 28
Single Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 20
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 80: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/80.jpg)
Numerical Methods - From Continuous Time to Discrete Time 15 / 28
Single Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 30
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 81: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/81.jpg)
Numerical Methods - From Continuous Time to Discrete Time 15 / 28
Single Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 40
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 82: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/82.jpg)
Numerical Methods - From Continuous Time to Discrete Time 15 / 28
Single Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 50
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 83: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/83.jpg)
Numerical Methods - From Continuous Time to Discrete Time 15 / 28
Single Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 60
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 84: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/84.jpg)
Numerical Methods - From Continuous Time to Discrete Time 15 / 28
Single Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 70
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 85: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/85.jpg)
Numerical Methods - From Continuous Time to Discrete Time 15 / 28
Single Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 80
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 86: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/86.jpg)
Numerical Methods - From Continuous Time to Discrete Time 15 / 28
Single Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 84
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 87: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/87.jpg)
Numerical Methods - From Continuous Time to Discrete Time 16 / 28
Multiple Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 0
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 88: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/88.jpg)
Numerical Methods - From Continuous Time to Discrete Time 16 / 28
Multiple Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 1
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 89: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/89.jpg)
Numerical Methods - From Continuous Time to Discrete Time 16 / 28
Multiple Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 90: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/90.jpg)
Numerical Methods - From Continuous Time to Discrete Time 16 / 28
Multiple Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 3
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 91: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/91.jpg)
Numerical Methods - From Continuous Time to Discrete Time 16 / 28
Multiple Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 92: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/92.jpg)
Numerical Methods - From Continuous Time to Discrete Time 16 / 28
Multiple Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 5
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 93: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/93.jpg)
Numerical Methods - From Continuous Time to Discrete Time 16 / 28
Multiple Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 6
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 94: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/94.jpg)
Numerical Methods - From Continuous Time to Discrete Time 16 / 28
Multiple Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 7
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 95: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/95.jpg)
Numerical Methods - From Continuous Time to Discrete Time 16 / 28
Multiple Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 8
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 96: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/96.jpg)
Numerical Methods - From Continuous Time to Discrete Time 16 / 28
Multiple Shooting
minu(·)
∫ 2
t0
u(t)2 dt
s.t. x(0) =[
0 0 0 0]>
x(2) =[
0 π 0 0]>
x(t) = F (x(t), u(t)), t ∈ [0, 2]
−20 ≤ u(t) ≤ 20, t ∈ [0, 2]
[z
θ
]=
[v
ω
]=
ml sin(θ)ω2+mg cos(θ) sin(θ)+u
M+m−m(cos(θ))2
− (ml cos(θ) sin(θ)ω2+(M+m)g sin(θ)+u cos(θ))
L(M+m−m(cos(θ))2)
θ
M
m
l
u
z
y
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-25
-20
-15
-10
-5
0
5
10
15
20
25SQP iter: 9
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5
0
0.5
1
1.5
2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-6
-4
-2
0
2
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-2
-1
0
1
2
3
4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-4
-2
0
2
4
6
8
![Page 97: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/97.jpg)
Numerical Methods - From Continuous Time to Discrete Time 17 / 28
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 (using thedynamic constraints)
→ Efficient Sparse Linear Algebra can be very effective, especially forlong horizons
Continuity conditions:
S. Shooting: imposed by the integrationM. Shooting: imposed by the QP/NLP
![Page 98: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/98.jpg)
Numerical Methods - From Continuous Time to Discrete Time 17 / 28
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 (using thedynamic constraints)
→ Efficient Sparse Linear Algebra can be very effective, especially forlong horizons
Continuity conditions:
S. Shooting: imposed by the integrationM. Shooting: imposed by the QP/NLP
![Page 99: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/99.jpg)
Numerical Methods - From Continuous Time to Discrete Time 17 / 28
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 (using thedynamic constraints)
→ Efficient Sparse Linear Algebra can be very effective, especially forlong horizons
Continuity conditions:
S. Shooting: imposed by the integrationM. Shooting: imposed by the QP/NLP
![Page 100: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/100.jpg)
Numerical Methods - From Continuous Time to Discrete Time 17 / 28
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 (using thedynamic constraints)
→ Efficient Sparse Linear Algebra can be very effective, especially forlong horizons
Continuity conditions:
S. Shooting: imposed by the integrationM. Shooting: imposed by the QP/NLP
![Page 101: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/101.jpg)
Numerical Methods - From Continuous Time to Discrete Time 17 / 28
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 (using thedynamic constraints)
→ Efficient Sparse Linear Algebra can be very effective, especially forlong horizons
Continuity conditions:
S. Shooting: imposed by the integrationM. Shooting: imposed by the QP/NLP
![Page 102: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/102.jpg)
Numerical Methods - From Continuous Time to Discrete Time 18 / 28
Let’s get a closer look at SQP
QP (for a given s, u)
min∆u,∆s
1
2
[∆s ∆u
] [ ∆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
![Page 103: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/103.jpg)
Numerical Methods - From Continuous Time to Discrete Time 18 / 28
Let’s get a closer look at SQP
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 =
[su
]J = 2wTdiag(Q, . . . ,Q,R . . . ,R)
![Page 104: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/104.jpg)
Numerical Methods - From Continuous Time to Discrete Time 18 / 28
Let’s get a closer look at SQP
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 directionsQuadratic convergence
BFGS update: Bk+1 = Bk + BkσσTBk
σTBkσ+ γγT
σTγ
Superlinear convergence
Gauss-Newton approximation: B ≈ JTJ (for linear MPC it is exact!)Linear convergence
![Page 105: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/105.jpg)
Numerical Methods - From Continuous Time to Discrete Time 18 / 28
Let’s get a closer look at SQP
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 directionsQuadratic convergence
BFGS update: Bk+1 = Bk + BkσσTBk
σTBkσ+ γγT
σTγ
Superlinear convergence
Gauss-Newton approximation: B ≈ JTJ (for linear MPC it is exact!)Linear convergence
![Page 106: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/106.jpg)
Numerical Methods - From Continuous Time to Discrete Time 18 / 28
Let’s get a closer look at SQP
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 directionsQuadratic convergence
BFGS update: Bk+1 = Bk + BkσσTBk
σTBkσ+ γγT
σTγ
Superlinear convergence
Gauss-Newton approximation: B ≈ JTJ (for linear MPC it is exact!)Linear convergence
![Page 107: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/107.jpg)
Numerical Methods - From Continuous Time to Discrete Time 18 / 28
Let’s get a closer look at SQP
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
![Page 108: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/108.jpg)
Numerical Methods - The Real Time Iteration Scheme 19 / 28
Can we exploit the MPC structure to be faster?
What about:
1 Newton step
Initial value embedding:s0 = xi as a constraint
No globalization
Gauss-Newton Hessianapproximation
→ no need to iterate
→ faster convergence, clevercomputations
→ need to enforce s0 = xi
→ only 1st order derivatives,Hessian B � 0
Result:
Converge while the system evolvesnext SQP iteration takes place on the new problem xi+1
Need to have a good initial guessbetter to shift (to be continued...)
We are essentially doing path-followingwith a generalized tangential predictor
Under some (mild) conditions, the SQP solution is closely tracked
![Page 109: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/109.jpg)
Numerical Methods - The Real Time Iteration Scheme 19 / 28
Can we exploit the MPC structure to be faster?
What about:
1 Newton step
Initial value embedding:s0 = xi as a constraint
No globalization
Gauss-Newton Hessianapproximation
→ no need to iterate
→ faster convergence, clevercomputations
→ need to enforce s0 = xi
→ only 1st order derivatives,Hessian B � 0
Result:
Converge while the system evolvesnext SQP iteration takes place on the new problem xi+1
Need to have a good initial guessbetter to shift (to be continued...)
We are essentially doing path-followingwith a generalized tangential predictor
Under some (mild) conditions, the SQP solution is closely tracked
![Page 110: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/110.jpg)
Numerical Methods - The Real Time Iteration Scheme 19 / 28
Can we exploit the MPC structure to be faster?
What about:
1 Newton step
Initial value embedding:s0 = xi as a constraint
No globalization
Gauss-Newton Hessianapproximation
→ no need to iterate
→ faster convergence, clevercomputations
→ need to enforce s0 = xi
→ only 1st order derivatives,Hessian B � 0
Result:
Converge while the system evolvesnext SQP iteration takes place on the new problem xi+1
Need to have a good initial guessbetter to shift (to be continued...)
We are essentially doing path-followingwith a generalized tangential predictor
Under some (mild) conditions, the SQP solution is closely tracked
![Page 111: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/111.jpg)
Numerical Methods - The Real Time Iteration Scheme 19 / 28
Can we exploit the MPC structure to be faster?
What about:
1 Newton step
Initial value embedding:s0 = xi as a constraint
No globalization
Gauss-Newton Hessianapproximation
→ no need to iterate
→ faster convergence, clevercomputations
→ need to enforce s0 = xi
→ only 1st order derivatives,Hessian B � 0
Result:
Converge while the system evolvesnext SQP iteration takes place on the new problem xi+1
Need to have a good initial guessbetter to shift (to be continued...)
We are essentially doing path-followingwith a generalized tangential predictor
Under some (mild) conditions, the SQP solution is closely tracked
![Page 112: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/112.jpg)
Numerical Methods - The Real Time Iteration Scheme 19 / 28
Can we exploit the MPC structure to be faster?
What about:
1 Newton step
Initial value embedding:s0 = xi as a constraint
No globalization
Gauss-Newton Hessianapproximation
→ no need to iterate
→ faster convergence, clevercomputations
→ need to enforce s0 = xi
→ only 1st order derivatives,Hessian B � 0
Result:
Converge while the system evolvesnext SQP iteration takes place on the new problem xi+1
Need to have a good initial guessbetter to shift (to be continued...)
We are essentially doing path-followingwith a generalized tangential predictor
Under some (mild) conditions, the SQP solution is closely tracked
![Page 113: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/113.jpg)
Numerical Methods - The Real Time Iteration Scheme 19 / 28
Can we exploit the MPC structure to be faster?
What about:
1 Newton step
Initial value embedding:s0 = xi as a constraint
No globalization
Gauss-Newton Hessianapproximation
→ no need to iterate
→ faster convergence, clevercomputations
→ need to enforce s0 = xi
→ only 1st order derivatives,Hessian B � 0
Result:
Converge while the system evolvesnext SQP iteration takes place on the new problem xi+1
Need to have a good initial guessbetter to shift (to be continued...)
We are essentially doing path-followingwith a generalized tangential predictor
Under some (mild) conditions, the SQP solution is closely tracked
![Page 114: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/114.jpg)
Numerical Methods - The Real Time Iteration Scheme 19 / 28
Can we exploit the MPC structure to be faster?
What about:
1 Newton step
Initial value embedding:s0 = xi as a constraint
No globalization
Gauss-Newton Hessianapproximation
→ no need to iterate
→ faster convergence, clevercomputations
→ need to enforce s0 = xi
→ only 1st order derivatives,Hessian B � 0
Result:
Converge while the system evolvesnext SQP iteration takes place on the new problem xi+1
Need to have a good initial guessbetter to shift (to be continued...)
We are essentially doing path-followingwith a generalized tangential predictor
Under some (mild) conditions, the SQP solution is closely tracked
![Page 115: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/115.jpg)
Numerical Methods - The Real Time Iteration Scheme 19 / 28
Can we exploit the MPC structure to be faster?
What about:
1 Newton step
Initial value embedding:s0 = xi as a constraint
No globalization
Gauss-Newton Hessianapproximation
→ no need to iterate
→ faster convergence, clevercomputations
→ need to enforce s0 = xi
→ only 1st order derivatives,Hessian B � 0
Result:
Converge while the system evolvesnext SQP iteration takes place on the new problem xi+1
Need to have a good initial guessbetter to shift (to be continued...)
We are essentially doing path-followingwith a generalized tangential predictor
Under some (mild) conditions, the SQP solution is closely tracked
![Page 116: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/116.jpg)
Numerical Methods - The Real Time Iteration Scheme 19 / 28
Can we exploit the MPC structure to be faster?
What about:
1 Newton step
Initial value embedding:s0 = xi as a constraint
No globalization
Gauss-Newton Hessianapproximation
→ no need to iterate
→ faster convergence, clevercomputations
→ need to enforce s0 = xi
→ only 1st order derivatives,Hessian B � 0
Result:
Converge while the system evolvesnext SQP iteration takes place on the new problem xi+1
Need to have a good initial guessbetter to shift (to be continued...)
We are essentially doing path-followingwith a generalized tangential predictor
Under some (mild) conditions, the SQP solution is closely tracked
![Page 117: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/117.jpg)
Numerical Methods - The Real Time Iteration Scheme 19 / 28
Can we exploit the MPC structure to be faster?
What about:
1 Newton step
Initial value embedding:s0 = xi as a constraint
No globalization
Gauss-Newton Hessianapproximation
→ no need to iterate
→ faster convergence, clevercomputations
→ need to enforce s0 = xi
→ only 1st order derivatives,Hessian B � 0
Result:
Converge while the system evolvesnext SQP iteration takes place on the new problem xi+1
Need to have a good initial guessbetter to shift (to be continued...)
We are essentially doing path-followingwith a generalized tangential predictor
Under some (mild) conditions, the SQP solution is closely tracked
![Page 118: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/118.jpg)
Numerical Methods - The Real Time Iteration Scheme 20 / 28
Standard SQP
Real Time Iterations
NMPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
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 PhaseWithout knowing xi
Linearize
(Gauss-Newton ⇒ B � 0)
Prepare the QP
Feedback Phase:
Solve QP once xi available→ same latency as linear MPC
![Page 119: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/119.jpg)
Numerical Methods - The Real Time Iteration Scheme 20 / 28
Standard SQP Real Time Iterations
NMPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
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 PhaseWithout knowing xi
Linearize
(Gauss-Newton ⇒ B � 0)
Prepare the QP
Feedback Phase:
Solve QP once xi available→ same latency as linear MPC
![Page 120: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/120.jpg)
Numerical Methods - The Real Time Iteration Scheme 20 / 28
Standard SQP Real Time Iterations
NMPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
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 PhaseWithout knowing xi
Linearize
(Gauss-Newton ⇒ B � 0)
Prepare the QP
Feedback Phase:
Solve QP once xi available→ same latency as linear MPC
![Page 121: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/121.jpg)
Numerical Methods - The Real Time Iteration Scheme 20 / 28
Standard SQP Real Time Iterations
NMPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
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 PhaseWithout knowing xi
Linearize
(Gauss-Newton ⇒ B � 0)
Prepare the QP
Feedback Phase:
Solve QP once xi available→ same latency as linear MPC
![Page 122: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/122.jpg)
Numerical Methods - The Real Time Iteration Scheme 21 / 28
Linear MPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q + ‖uk − uref ‖2
R
s.t. sk+1 = Aksk + Bkuk
Cksk + Dkuk ≥ 0,
s0 = xi
RTI at time i
minu,s
N∑k=0
‖sk − xref ‖2Q + ‖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
2 Wait
At each time i :
1 Solve the QP
2 Compute the new linearizationof the constraints
3 Prepare the new QP
RTI differs from linear MPC in the sense that the constraintsare re-linearized at each time instant on the current
trajectory rather than only once on the reference trajectory
![Page 123: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/123.jpg)
Numerical Methods - The Real Time Iteration Scheme 21 / 28
Linear MPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q + ‖uk − uref ‖2
R
s.t. sk+1 = Aksk + Bkuk
Cksk + Dkuk ≥ 0,
s0 = xi
RTI at time i
minu,s
N∑k=0
‖sk − xref ‖2Q + ‖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
2 Wait
At each time i :
1 Solve the QP
2 Compute the new linearizationof the constraints
3 Prepare the new QP
RTI differs from linear MPC in the sense that the constraintsare re-linearized at each time instant on the current
trajectory rather than only once on the reference trajectory
![Page 124: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/124.jpg)
Numerical Methods - The Real Time Iteration Scheme 21 / 28
Linear MPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q + ‖uk − uref ‖2
R
s.t. sk+1 = Aksk + Bkuk
Cksk + Dkuk ≥ 0,
s0 = xi
RTI at time i
minu,s
N∑k=0
‖sk − xref ‖2Q + ‖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
2 Wait
At each time i :
1 Solve the QP
2 Compute the new linearizationof the constraints
3 Prepare the new QP
RTI differs from linear MPC in the sense that the constraintsare re-linearized at each time instant on the current
trajectory rather than only once on the reference trajectory
![Page 125: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/125.jpg)
Numerical Methods - The Real Time Iteration Scheme 21 / 28
Linear MPC at time i
minu,s
N∑k=0
‖sk − xref ‖2Q + ‖uk − uref ‖2
R
s.t. sk+1 = Aksk + Bkuk
Cksk + Dkuk ≥ 0,
s0 = xi
RTI at time i
minu,s
N∑k=0
‖sk − xref ‖2Q + ‖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
2 Wait
At each time i :
1 Solve the QP
2 Compute the new linearizationof the constraints
3 Prepare the new QP
RTI differs from linear MPC in the sense that the constraintsare re-linearized at each time instant on the current
trajectory rather than only once on the reference trajectory
![Page 126: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/126.jpg)
Numerical Methods - The Real Time Iteration Scheme 22 / 28
Embedded Solver
RTI at time i
minu,s
N∑k=0
‖sk − xref ‖2Q + ‖uk − uref ‖2
R
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ 0,
s0 = xi
Properties:
Fixed problem dimensions
Specific structure
Tailored C code
Exploit the structure andminimize number of operations
No dynamic memory allocation
Minimize branching in theexported code
Optimized linear algebra routines
Tailored fixed-step RK integrators
ACADO / ACADOS
Multiple shooting
Real time iterations
![Page 127: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/127.jpg)
Numerical Methods - The Real Time Iteration Scheme 22 / 28
Embedded Solver
RTI at time i
minu,s
N∑k=0
‖sk − xref ‖2Q + ‖uk − uref ‖2
R
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ 0,
s0 = xi
Properties:
Fixed problem dimensions
Specific structure
Tailored C code
Exploit the structure andminimize number of operations
No dynamic memory allocation
Minimize branching in theexported code
Optimized linear algebra routines
Tailored fixed-step RK integrators
ACADO / ACADOS
Multiple shooting
Real time iterations
![Page 128: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/128.jpg)
Numerical Methods - The Real Time Iteration Scheme 22 / 28
Embedded Solver
RTI at time i
minu,s
N∑k=0
‖sk − xref ‖2Q + ‖uk − uref ‖2
R
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ 0,
s0 = xi
Properties:
Fixed problem dimensions
Specific structure
Tailored C code
Exploit the structure andminimize number of operations
No dynamic memory allocation
Minimize branching in theexported code
Optimized linear algebra routines
Tailored fixed-step RK integrators
ACADO / ACADOS
Multiple shooting
Real time iterations
![Page 129: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/129.jpg)
Numerical Methods - The Real Time Iteration Scheme 23 / 28
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 0.5 1 1.5 2 2.5 3 3.5 4 4.5−1
−0.5
0
0.5
1a b
e
x
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5−1
−0.5
0
0.5
1
u
T
c d
f
Guess error eguess small if...
1 {ui−1, s i−1} close to {ui−1∗ , s i−1∗}2 s i0 close to x i
Key for algorithmic reliability
1 Sample fast enough
2 Warm start
![Page 130: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/130.jpg)
Numerical Methods - The Real Time Iteration Scheme 23 / 28
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 0.5 1 1.5 2 2.5 3 3.5 4 4.5−1
−0.5
0
0.5
1b
e
x
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5−1
−0.5
0
0.5
1
u
T
d
f
Guess error eguess small if...
1 {ui−1, s i−1} close to {ui−1∗ , s i−1∗}2 s i0 close to x i
Key for algorithmic reliability
1 Sample fast enough
2 Warm start
![Page 131: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/131.jpg)
Numerical Methods - The Real Time Iteration Scheme 23 / 28
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 0.5 1 1.5 2 2.5 3 3.5 4 4.5−1
−0.5
0
0.5
1b
e
x
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5−1
−0.5
0
0.5
1
u
T
d
f g
Guess error eguess small if...
1 {ui−1, s i−1} close to {ui−1∗ , s i−1∗}2 s i0 close to x i
Key for algorithmic reliability
1 Sample fast enough
2 Warm start
![Page 132: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/132.jpg)
Numerical Methods - The Real Time Iteration Scheme 23 / 28
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 0.5 1 1.5 2 2.5 3 3.5 4 4.5−1
−0.5
0
0.5
1b
e
x
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5−1
−0.5
0
0.5
1
u
T
d
f g
Guess error eguess small if...
1 {ui−1, s i−1} close to {ui−1∗ , s i−1∗}2 s i0 close to x i
Key for algorithmic reliability
1 Sample fast enough
2 Warm start
![Page 133: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/133.jpg)
Numerical Methods - The Real Time Iteration Scheme 23 / 28
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 0.5 1 1.5 2 2.5 3 3.5 4 4.5−1
−0.5
0
0.5
1a b
he
x
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5−1
−0.5
0
0.5
1
u
Td
i
f g
Guess error eguess small if...
1 {ui−1, s i−1} close to {ui−1∗ , s i−1∗}2 s i0 close to x i
Key for algorithmic reliability
1 Sample fast enough
2 Warm start
![Page 134: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/134.jpg)
Numerical Methods - The Real Time Iteration Scheme 23 / 28
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 0.5 1 1.5 2 2.5 3 3.5 4 4.5−1
−0.5
0
0.5
1a b
he
x
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5−1
−0.5
0
0.5
1
u
Td
i
f g
Guess error eguess small if...
1 {ui−1, s i−1} close to {ui−1∗ , s i−1∗}2 s i0 close to x i
Key for algorithmic reliability
1 Sample fast enough
2 Warm start
![Page 135: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/135.jpg)
Numerical Methods - The Real Time Iteration Scheme 24 / 28
Tuning: Trial-and-error, experience-based
Positive-definite cost
not necessary, but recommendedhelps convergence
For pure setpoint tracking1 use a scaled identity Wi,i = σ−2
i , where σi = range of state i2 simulate and adjust the weights as Wi,i = ωiσ
−2i
3 iterate until performance is satisfactory
If you have a local linear controller you wish to imitate
controller matching for feedback K [Zanon,Bemporad, w.i.p.]
minα,β,W ,P
γβ − α
s.t. P = Q + A>PA− (S> + A>PB)K
(R + A>PB)K = S + B>PA
βI �W � αI
where W =
Q S>
S R
If there is a clear “economic” objective
automatic tuning [Zanon, Gros, Diehl, JPC2016]
![Page 136: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/136.jpg)
Numerical Methods - The Real Time Iteration Scheme 24 / 28
Tuning: Trial-and-error, experience-based
Positive-definite cost
not necessary, but recommendedhelps convergence
For pure setpoint tracking1 use a scaled identity Wi,i = σ−2
i , where σi = range of state i2 simulate and adjust the weights as Wi,i = ωiσ
−2i
3 iterate until performance is satisfactory
If you have a local linear controller you wish to imitate
controller matching for feedback K [Zanon,Bemporad, w.i.p.]
minα,β,W ,P
γβ − α
s.t. P = Q + A>PA− (S> + A>PB)K
(R + A>PB)K = S + B>PA
βI �W � αI
where W =
Q S>
S R
If there is a clear “economic” objective
automatic tuning [Zanon, Gros, Diehl, JPC2016]
![Page 137: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/137.jpg)
Numerical Methods - The Real Time Iteration Scheme 24 / 28
Tuning: Trial-and-error, experience-based
Positive-definite cost
not necessary, but recommendedhelps convergence
For pure setpoint tracking1 use a scaled identity Wi,i = σ−2
i , where σi = range of state i2 simulate and adjust the weights as Wi,i = ωiσ
−2i
3 iterate until performance is satisfactory
If you have a local linear controller you wish to imitate
controller matching for feedback K [Zanon,Bemporad, w.i.p.]
minα,β,W ,P
γβ − α
s.t. P = Q + A>PA− (S> + A>PB)K
(R + A>PB)K = S + B>PA
βI �W � αI
where W =
Q S>
S R
If there is a clear “economic” objective
automatic tuning [Zanon, Gros, Diehl, JPC2016]
![Page 138: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/138.jpg)
Numerical Methods - The Real Time Iteration Scheme 24 / 28
Tuning: Trial-and-error, experience-based
Positive-definite cost
not necessary, but recommendedhelps convergence
For pure setpoint tracking1 use a scaled identity Wi,i = σ−2
i , where σi = range of state i2 simulate and adjust the weights as Wi,i = ωiσ
−2i
3 iterate until performance is satisfactory
If you have a local linear controller you wish to imitate
controller matching for feedback K [Zanon,Bemporad, w.i.p.]
minα,β,W ,P
γβ − α
s.t. P = Q + A>PA− (S> + A>PB)K
(R + A>PB)K = S + B>PA
βI �W � αI
where W =
Q S>
S R
If there is a clear “economic” objective
automatic tuning [Zanon, Gros, Diehl, JPC2016]
![Page 139: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/139.jpg)
Numerical Methods - The Real Time Iteration Scheme 24 / 28
Tuning: Trial-and-error, experience-based
Positive-definite cost
not necessary, but recommendedhelps convergence
For pure setpoint tracking1 use a scaled identity Wi,i = σ−2
i , where σi = range of state i2 simulate and adjust the weights as Wi,i = ωiσ
−2i
3 iterate until performance is satisfactory
If you have a local linear controller you wish to imitate
controller matching for feedback K [Zanon,Bemporad, w.i.p.]
minα,β,W ,P
γβ − α
s.t. P = Q + A>PA− (S> + A>PB)K
(R + A>PB)K = S + B>PA
βI �W � αI
where W =
Q S>
S R
If there is a clear “economic” objective
automatic tuning [Zanon, Gros, Diehl, JPC2016]
![Page 140: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/140.jpg)
Numerical Methods - The Real Time Iteration Scheme 25 / 28
Handling Infeasibility
Path constraints might become infeasible
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ 0,
s0 = xi
xi imposed by the (perturbed)system
perturbations might makeh (sk , uk) ≥ 0 infeasible
limited controllability of h (sk , uk)at low k
Slack Variables Reformulation
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R + W T v
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ −v , v ≥ 0
s0 = xi
![Page 141: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/141.jpg)
Numerical Methods - The Real Time Iteration Scheme 25 / 28
Handling Infeasibility
Path constraints might become infeasible
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ 0,
s0 = xi
xi imposed by the (perturbed)system
perturbations might makeh (sk , uk) ≥ 0 infeasible
limited controllability of h (sk , uk)at low k
Slack Variables Reformulation
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R + W T v
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ −v , v ≥ 0
s0 = xi
![Page 142: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/142.jpg)
Numerical Methods - The Real Time Iteration Scheme 25 / 28
Handling Infeasibility
Path constraints might become infeasible
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ 0,
s0 = xi
xi imposed by the (perturbed)system
perturbations might makeh (sk , uk) ≥ 0 infeasible
limited controllability of h (sk , uk)at low k
Slack Variables Reformulation
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R + W T v
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ −v , v ≥ 0
s0 = xi
![Page 143: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/143.jpg)
Numerical Methods - The Real Time Iteration Scheme 25 / 28
Handling Infeasibility
Path constraints might become infeasible
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ 0,
s0 = xi
xi imposed by the (perturbed)system
perturbations might makeh (sk , uk) ≥ 0 infeasible
limited controllability of h (sk , uk)at low k
Slack Variables Reformulation
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R + W T v
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ −v , v ≥ 0
s0 = xi
![Page 144: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/144.jpg)
Numerical Methods - The Real Time Iteration Scheme 25 / 28
Handling Infeasibility
Path constraints might become infeasible
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ 0,
s0 = xi
xi imposed by the (perturbed)system
perturbations might makeh (sk , uk) ≥ 0 infeasible
limited controllability of h (sk , uk)at low k
Slack Variables Reformulation
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R + W T v
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ −v , v ≥ 0
s0 = xi
![Page 145: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/145.jpg)
Numerical Methods - The Real Time Iteration Scheme 25 / 28
Handling Infeasibility
Path constraints might become infeasible
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ 0,
s0 = xi
xi imposed by the (perturbed)system
perturbations might makeh (sk , uk) ≥ 0 infeasible
limited controllability of h (sk , uk)at low k
Slack Variables Reformulation
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R + W T v
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ −v , v ≥ 0
s0 = xi
![Page 146: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/146.jpg)
Numerical Methods - The Real Time Iteration Scheme 25 / 28
Handling Infeasibility
Path constraints might become infeasible
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ 0,
s0 = xi
xi imposed by the (perturbed)system
perturbations might makeh (sk , uk) ≥ 0 infeasible
limited controllability of h (sk , uk)at low k
Slack Variables Reformulation
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R + W T v
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
![Page 147: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/147.jpg)
Numerical Methods - The Real Time Iteration Scheme 25 / 28
Handling Infeasibility
Path constraints might become infeasible
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ 0,
s0 = xi
xi imposed by the (perturbed)system
perturbations might makeh (sk , uk) ≥ 0 infeasible
limited controllability of h (sk , uk)at low k
Slack Variables Reformulation
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R + W T v
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
![Page 148: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/148.jpg)
Numerical Methods - The Real Time Iteration Scheme 25 / 28
Handling Infeasibility
Path constraints might become infeasible
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R
s.t. sk+1 = f (sk , uk)
h (sk , uk) ≥ 0,
s0 = xi
xi imposed by the (perturbed)system
perturbations might makeh (sk , uk) ≥ 0 infeasible
limited controllability of h (sk , uk)at low k
Slack Variables Reformulation
minu,s
N∑k=0
‖sk − xref ‖2Q +
N−1∑k=0
‖uk − uref ‖2R + W T v
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
![Page 149: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/149.jpg)
RTI Illustration 26 / 28
Consider a simple (discrete-time) problem
minu,x
N∑k=0
‖xk‖2 + 20N−1∑k=0
‖uk‖2
s.t. x0 = xi ,
xk+1 = 0.9xk +
[sin([
0 1]xk)
uk + u3k
],
|uk | < 0.5, k = 0, . . . ,N − 1,
![Page 150: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/150.jpg)
RTI Illustration 26 / 28
Consider a simple (discrete-time) problem
Initialise everything at the reference
November 10, 2015 International Journal of Control RTI˙LTV˙Seb˙3
0 5 10 15 200
5
10
0 5 10 15 20
0
2
4
0 5 10 15 20
-0.4
-0.2
0
0.2
x 1x 2
u
Time
Linear MPC1st (full) SQP stepConverged SQP
Figure 1. Comparison between the open-loop predicted trajectories obtained via linear MPC (circles), via performing a single iteration of Al-gorithm 1 using a full step (α = 1) and using x = xref, u = uref as an initial guess (crosses), and via running Algorithm 1 to full convergence(squares).
linearisation (7) used in QPMPC in (1). Additionally, Jk evaluated at x = xref,u = uref is zero. It immediatelyfollows that the solution of QPMPC given by (1) is identical to the solution of QPNMPC given by (13).
If the SQP Algorithm 1 is fed with the reference trajectory as an initial guess, i.e. is run as:
xi, ui = SQP!
xi, xrefi , uref
i ,xrefi , uref
i
"(17)
then the first Newton direction (∆x, ∆u) computed in Algorithm 1, line 2 is identical to the solution of thelinear MPC problem computed via (3). This observation is illustrated in Figure 1 for the simple problem:
minu,x
N
∑k=0∥xk∥2 +20
N−1
∑k=0∥uk∥2 (18a)
s.t. x0 = xi, (18b)
xk+1 = 0.9xk +
#sin
$%0 1
&xk
'
uk +u3k
(, (18c)
|uk| < 0.5, k = 0, . . . ,N−1, (18d)
with state xk ∈ R2, input uk ∈ R, and N = 19. The optimal trajectories obtained via linear MPC (1) arereported using circles (◦). The trajectories obtained from performing a single iteration of Algorithm 1,using a full step (α = 1) and x = xref, u = uref as the initial guess are displayed using plus signs (+). Thetrajectories resulting from running Algorithm 1 to full convergence are displayed using squares (!).
3.2 Warm-started SQP for NMPC
Algorithm 1 requires the initial guess xguess, uguess as an input. Selecting an adequate initial guess is crucialfor obtaining a fast and reliable convergence of the SQP iterations. First, a good initial guess allows for
6
![Page 151: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/151.jpg)
RTI Illustration 26 / 28
Consider a simple (discrete-time) problem
Shift from previous solution, no noise
November 10, 2015 International Journal of Control RTI˙LTV˙Seb˙3
2 4 6 8 10 12 14 16 18 20
0
2
4
2 4 6 8 10 12 14 16 18 20
0
2
4
2 4 6 8 10 12 14 16 18 20-1
-0.5
0
x 1x 2
u
Time
Solution xi−1
Solution xi−1
Guess xguessi from (21)
Guess xguessi from (21)
Solution xi
Solution xi
Solution ui−1Guess uguess
i from (21)Solution ui
(a) In the absence of disturbance and model error, the guess ob-tained via shifting the previous solution is typically an excellent ap-proximation of the current solution. In this graph, the guess for timei and the corresponding solution are indistinguishable.
2 4 6 8 10 12 14 16 18 20
0
2
4
2 4 6 8 10 12 14 16 18 20
0
2
4
2 4 6 8 10 12 14 16 18 20-1
-0.5
0
x 1x 2
u
Time
Solution xi−1
Solution xi−1
Guess xguessi from (21)
Guess xguessi from (21)
Solution xi
Solution xi
Solution ui−1Guess uguess
i from (21)Solution ui
(b) In the presence of disturbances, xi diverges significantly fromthe predicted one xi−1,1, and therefore the guess xguess
i obtained viashifting needs a correction.
Figure 2. Illustration of the shifting procedure.
These observations provide an important intuitive justification of the Real-Time Iteration approach, de-scribed in Section 4. Before presenting it in detail, we first review the real-time dilemma.
3.3 The real-time dilemma
Upon obtaining a new state estimation xi, the SQP procedure can be started. The real-time dilemma stemsfrom the fact that while the SQP iterations are performed, the physical system evolves, and the informationused to compute the state estimation xi becomes outdated.
Clearly, this problem can be partially addressed by using a prediction of what the state of the system willbe at the time the SQP algorithm will be completed, as opposed to directly using the current state estima-tion. However, even when using such a prediction approach, since updating the control policy requires thecompletion of the SQP algorithm, the SQP procedure introduces a potentially large delay between gatheringmeasurements from the physical system and delivering the corresponding required control action.
It follows that even if the SQP computational time is accounted for via a state prediction, the SQPalgorithm is nonetheless based on outdated system information. We illustrate this key issue in Fig. 3(a).
The key idea of the RTI procedure detailed in Section 4, and first presented in [12] is to consistentlyincorporate the latest information on the system evolution in the iterations computing the NMPC solutions.
The real-time dilemma then consists in choosing between applying an exact solution computed usingoutdated information versus applying an approximate solution computed using the most up-to-date infor-mation.Summary of the section
• When SQP is deployed on NMPC, and the reference trajectory is used as an initial guess, the firststep of a full step Gauss-Newton SQP delivers the same control solution as linear MPC (Lemma 1).
• In the context of NMPC, the SQP iteration can be efficiently warm-started by shifting the solutionobtained at the previous time instant (Sec. 3.2). In the presence of reasonably small disturbances, theSQP algorithm then needs only a couple of full Newton steps to reach full convergence.
• When running SQP to full convergence, only the first iteration is using up-to-date estimation of thesystem state xi. Subsequent iterations are still performed based on xi, while the system state evolves,making xi outdated (Sec. 3.3).
8
![Page 152: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/152.jpg)
RTI Illustration 26 / 28
Consider a simple (discrete-time) problem
Shift from previous solution, process noise
November 10, 2015 International Journal of Control RTI˙LTV˙Seb˙3
2 4 6 8 10 12 14 16 18 20
0
2
4
2 4 6 8 10 12 14 16 18 20
0
2
4
2 4 6 8 10 12 14 16 18 20-1
-0.5
0
x 1x 2
u
Time
Solution xi−1
Solution xi−1
Guess xguessi from (21)
Guess xguessi from (21)
Solution xi
Solution xi
Solution ui−1Guess uguess
i from (21)Solution ui
(a) In the absence of disturbance and model error, the guess ob-tained via shifting the previous solution is typically an excellent ap-proximation of the current solution. In this graph, the guess for timei and the corresponding solution are indistinguishable.
2 4 6 8 10 12 14 16 18 20
0
2
4
2 4 6 8 10 12 14 16 18 20
0
2
4
2 4 6 8 10 12 14 16 18 20-1
-0.5
0
x 1x 2
u
Time
Solution xi−1
Solution xi−1
Guess xguessi from (21)
Guess xguessi from (21)
Solution xi
Solution xi
Solution ui−1Guess uguess
i from (21)Solution ui
(b) In the presence of disturbances, xi diverges significantly fromthe predicted one xi−1,1, and therefore the guess xguess
i obtained viashifting needs a correction.
Figure 2. Illustration of the shifting procedure.
These observations provide an important intuitive justification of the Real-Time Iteration approach, de-scribed in Section 4. Before presenting it in detail, we first review the real-time dilemma.
3.3 The real-time dilemma
Upon obtaining a new state estimation xi, the SQP procedure can be started. The real-time dilemma stemsfrom the fact that while the SQP iterations are performed, the physical system evolves, and the informationused to compute the state estimation xi becomes outdated.
Clearly, this problem can be partially addressed by using a prediction of what the state of the system willbe at the time the SQP algorithm will be completed, as opposed to directly using the current state estima-tion. However, even when using such a prediction approach, since updating the control policy requires thecompletion of the SQP algorithm, the SQP procedure introduces a potentially large delay between gatheringmeasurements from the physical system and delivering the corresponding required control action.
It follows that even if the SQP computational time is accounted for via a state prediction, the SQPalgorithm is nonetheless based on outdated system information. We illustrate this key issue in Fig. 3(a).
The key idea of the RTI procedure detailed in Section 4, and first presented in [12] is to consistentlyincorporate the latest information on the system evolution in the iterations computing the NMPC solutions.
The real-time dilemma then consists in choosing between applying an exact solution computed usingoutdated information versus applying an approximate solution computed using the most up-to-date infor-mation.Summary of the section
• When SQP is deployed on NMPC, and the reference trajectory is used as an initial guess, the firststep of a full step Gauss-Newton SQP delivers the same control solution as linear MPC (Lemma 1).
• In the context of NMPC, the SQP iteration can be efficiently warm-started by shifting the solutionobtained at the previous time instant (Sec. 3.2). In the presence of reasonably small disturbances, theSQP algorithm then needs only a couple of full Newton steps to reach full convergence.
• When running SQP to full convergence, only the first iteration is using up-to-date estimation of thesystem state xi. Subsequent iterations are still performed based on xi, while the system state evolves,making xi outdated (Sec. 3.3).
8
![Page 153: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/153.jpg)
RTI Illustration 26 / 28
Consider a simple (discrete-time) problem
RTI vs SQP
November 10, 2015 International Journal of Control RTI˙LTV˙Seb˙3
2 4 6 8 10 12 14 16 18 20
0
2
4
2 4 6 8 10 12 14 16 18 20
0
2
4
2 4 6 8 10 12 14 16 18 20-1
-0.5
0
Single step at time i
Single step at time i
Single step at time i
x 1x 2
u
Time
Solution xi−1
Solution xi−1
Guess xguessi from (21)
Guess xguessi from (21)
Solution xi
Solution xi
Solution ui−1Guess uguess
i from (21)
Solution ui
Figure 4. Illustration of the single full Newton step approach with state noise.
Figure 4, where a single full Newton step strategy is compared to a fully converged SQP method.Algorithm 2 computes the RTI feedback control policy. However, as we will present in the next sec-
tion, the genuine RTI algorithm divides the computations in two phases so as to achieve shorter feedbacklatencies.
4.2 The RTI algorithm: preparation-feedback split
The RTI algorithm is an improved version of Algorithm 2, where its feedback time is reduced. The im-provement is using the fact that steps 1 and 2 of Algorithm 2 do not require the knowledge of the stateestimation xi, and can therefore be performed before the state estimation xi becomes available.
The RTI scheme (see Algorithm 3) thus proposes to split the operation between:
• a preparation phase, performing the computations involved in the steps 1 and 2 of Algorithm 2 priorto obtaining the new state estimation xi.
• a feedback phase, performing the computations involved in steps 3 and 4 upon obtaining the lateststate estimation xi.
Note that usually the Gauss-Newton Hessian approximation [3], i.e. Hk = Wk , is used because (a) it doesnot require the computation of second order derivatives and (b) it always delivers a positive (semi)definiteHessian approximation. For a detailed overview on the RTI scheme, including a proof of nominal stability,we refer to [9, 10, 13].
It is important to remark that:
• The delay introduced by the feedback time can be accommodated as in linear MPC, by including acorresponding prediction in the state estimation.
• The overall sampling time ti− ti−1 that can be achieved by the RTI scheme is limited by the total timespent in solving both the feedback phase and the preparation phase.
• The time required to perform the feedback phase is practically the same as the time required to solvethe linear MPC problem.
• The sampling time that can be achieved via RTI-based NMPC increases from standard linear MPCby the time required for the preparation phase.
• It is typically desirable that the feedback time is only a fraction of the overall sampling time. Because
10
![Page 154: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/154.jpg)
RTI Illustration 26 / 28
Consider a simple (discrete-time) problem
RTI vs linear MPC
November 10, 2015 International Journal of Control RTI˙LTV˙Seb˙3
2 4 6 8 10 12 14 16 18 20-5
0
5
10
2 4 6 8 10 12 14 16 18 20-2
0
2
4
2 4 6 8 10 12 14 16 18 20
-0.4-0.2
00.2
Converged
x 1x 2
u
Time
Linear MPC
Linear MPC
Linear MPC
RTI
RTI
RTI
Converged
Converged
Figure 5. Illustration of the RTI solution vs. the linear MPC solutions at the discrete time instant i = 2, with state disturbances.
0 5 10 15 20 25 30
0
1
2
3
4
0 5 10 15 20 25 30
0
1
2
3
4
0 5 10 15 20 25 30
-0.4
-0.2
0
0.2
x 1x 2
u
Time
Linear MPCRTIConverged
(a) No state disturbances.
0 5 10 15 20 25 30-1
0
1
2
3
4
0 5 10 15 20 25 30-1
0
1
2
3
4
0 5 10 15 20 25 30
-0.4
-0.2
0
0.2
Converged
x 1x 2
u
Time
Linear MPCRTI
(b) State disturbances of covariance 0.1.
Figure 6. Illustration of the RTI solution vs. the linear MPC solutions in closed-loop simulations, with and without state disturbances.
Equation (ODE) of the form:
x(t) = F (x(t),u(t)) . (23)
In this section, we will present a family of numerical methods for simulation and sensitivity generation.It is important to stress that the well-known matrix exponential can also be considered as such a method
13
![Page 155: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/155.jpg)
RTI Illustration 26 / 28
Consider a simple (discrete-time) problem
Closed-loop: RTI, linear MPC and SQP, no noise
November 10, 2015 International Journal of Control RTI˙LTV˙Seb˙3
2 4 6 8 10 12 14 16 18 20-5
0
5
10
2 4 6 8 10 12 14 16 18 20-2
0
2
4
2 4 6 8 10 12 14 16 18 20
-0.4-0.2
00.2
Converged
x 1x 2
u
Time
Linear MPC
Linear MPC
Linear MPC
RTI
RTI
RTI
Converged
Converged
Figure 5. Illustration of the RTI solution vs. the linear MPC solutions at the discrete time instant i = 2, with state disturbances.
0 5 10 15 20 25 30
0
1
2
3
4
0 5 10 15 20 25 30
0
1
2
3
4
0 5 10 15 20 25 30
-0.4
-0.2
0
0.2
x 1x 2
u
Time
Linear MPCRTIConverged
(a) No state disturbances.
0 5 10 15 20 25 30-1
0
1
2
3
4
0 5 10 15 20 25 30-1
0
1
2
3
4
0 5 10 15 20 25 30
-0.4
-0.2
0
0.2
Converged
x 1x 2
uTime
Linear MPCRTI
(b) State disturbances of covariance 0.1.
Figure 6. Illustration of the RTI solution vs. the linear MPC solutions in closed-loop simulations, with and without state disturbances.
Equation (ODE) of the form:
x(t) = F (x(t),u(t)) . (23)
In this section, we will present a family of numerical methods for simulation and sensitivity generation.It is important to stress that the well-known matrix exponential can also be considered as such a method
13
![Page 156: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/156.jpg)
RTI Illustration 27 / 28
Consider a simple (continuous-time) problem
Pendulum on a cart:
w =ml sin(θ)θ2 + mg cos(θ) sin(θ) + u
M + m −m(cos(θ))2,
θ = −ml cos(θ) sin(θ)θ2 + u cos(θ) + (M + m)g sin(θ)
l(M + m −m(cos(θ))2),
with M = 1 kg, m = 0.1 kg, l = 0.5 m, g = 9.81 m/s2.
θ
M
m
l
F
w
y
Prediction horizon: 2 s
Stage cost matrices:
Q = diag([
10 10 0.1 0.1]),
R = 0.01
![Page 157: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/157.jpg)
RTI Illustration 27 / 28
Consider a simple (continuous-time) problem
Sampling time (s)
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-1
0
10.0250.050.10.20.4
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-2
0
2
4
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-40-20
02040
![Page 158: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/158.jpg)
RTI Illustration 27 / 28
Consider a simple (continuous-time) problem
Prediction horizon (s)
![Page 159: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/159.jpg)
RTI Illustration 27 / 28
Consider a simple (continuous-time) problem
Integrator accuracy (steps of explicit Euler)
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-1
0
1201052
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-2
0
2
4
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-40-20
02040
![Page 160: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/160.jpg)
RTI Illustration 27 / 28
Consider a simple (continuous-time) problem
Shift
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-1
0
1RTI with shiftRTI without shiftconverged NMPC
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-2
0
2
4
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-100
0
100
![Page 161: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/161.jpg)
RTI Illustration 27 / 28
Consider a simple (continuous-time) problem
Reference trajectory
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-1
0
1step at 0 sstep at 1 sstep at 2 s
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-2
0
2
4
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-100
0
100
![Page 162: Nonlinear Model Predictive Control - Research Unit DYSCOdysco.imtlucca.it/mpc-cdc19/pdf/2a.pdf · Nonlinear Model Predictive Control Mario Zanon, Alberto Bemporad Thanks to S. Gros,](https://reader033.vdocuments.us/reader033/viewer/2022052310/5f057d6a7e708231d413381c/html5/thumbnails/162.jpg)
28 / 28
Thank you for your attention!