MR3038. EI-AD24-153.

Embedded optimization techniques

for multivariable control strategies improvement

“Implementation of linear discrete MPC for a Double

Pendulum on a Cart”

Research advisors:
Carlos Gustavo Sotelo Molina
David Alejandro Sotelo Molina

Victor Bobadilla Hernández - A01770261

December the 2nd 2024

Introduction

Model Predictive Control is one of the most powerful control strategies today, due to its ability to work
with both linear and nonlinear MIMO and SISO systems, but especially for its ability to set constraints on
the control inputs, making it a powerful technique that is highly robust to disturbances and able to operate
near the limits of a system. Despite its advantages, MPC is computationally demanding because it needs
to solve an optimization problem at each step, making it difficult to implement on less robust hardware or
simplifying the model to reduce complexity.

Compared to other control methods, MPC´s ability to predict the behaviour of a plant makes it robust to
disturbances, since it's not necessarily reactive. However one of the issues it faces is the high
computational burden due to the optimization of a cost function at every time step and for plants with cost
functions that aren't strictly convex, the iteration steps to find the optimal solution may take too long for
MPC to be applied in systems with fast-changing dynamics, research in this area has brought upon many
techniques such as convex optimization techniques[], Newton's method to approximate a gradient[], Fast
gradient methods[] and even the use of artificial intelligence to compute the optimal control without
calculating the cost function[].

In this paper, we will compare MPC to Gradient Descent methods using the double pendulum on a cart.
This model is good to test due to its chaotic behaviour, nonlinearity, and fast dynamics. This paper aims
to compare different methods of gradient descent and their performance. We will use an LQR as a control
benchmark.

Modelling the plant

We will model the system dynamics using the Lagrangian to obtain the system's equations of motion.
First, we model the kinematics of our system, characterizing the position of both pendulums and the cart,
as shown in Figure (). For our model we are assuming that the majority of the mass is at the tips of the
pendulum rods, therefore the centre of gravity of each pendulum is at the furthest it can be, this makes our
calculations easier and in a real life prototype having the centre of gravity further from the pivot increases
our moment of inertia, reducing the energy required to maintain equilibrium since a greater moment of
inertia means that the torque applied by gravity will accelerate the pendulum less and therefore our
control effort can be reduced.

1
Kinematic model

C=x

p1 x =x +l 1 sinθ 1 ṗ1 x = ẋ +l 1 θ̇ 1 cos θ1

p1 y =l 1 cos θ1 ṗ1 y =−l 1 θ̇1 sin θ 1

p2 x =x +l 1 sin θ1+ l 2 sin θ2 ṗ2 x = ẋ +l 1 θ̇1 cos θ1 +l 2 θ̇2 cos θ2

p2 y =l 1 cos θ1 +l 2 cos θ2 ṗ2 y =−l 1 θ̇1 sin θ 1−l 2 θ̇2 sin θ2

We have now defined our system's kinematics. To derive the equations of motion, we will use the
Lagrangian since it´s easier to derive the dynamics using energies rather than forces.

L=T −U

Where T and U are the system's total kinetic and potential energy respectively, we will find the kinetic
and potential energies of the cart and both pendulums individually and then add them together.

2
Kinetic Energy

Cart

1 2
T C= mC V C
2

Pendulum 1

1 2
T P 1= m 1 V 1 V 1=
2
√ ṗ + ṗ 1 y 2
1x
2

1 2 2
T P 1= m 1 ( ṗ1 x + ṗ1 y )
2

1
T P 1= m 1 ¿
2

1 2 2 2 2 2 2 2
T P 1= m 1 [ ẋ +2 ẋ l 1 θ̇1 cos θ1 +l 1 θ̇1 cos θ1 +l 1 θ̇1 sin θ 1]
2

1
T P 1= m1 ¿)]
2

1
T P 1= m 1 ¿]
2

Pendulum 2

1
2
2
T P 2= m 2 V 2 V 2= √ ṗ 2x
2
+ ṗ 2 y 2

1 2 2
T P 2= m 2 ( ṗ2 x + ṗ2 y )
2

1
T P 2= m 2 ¿
2

1 2 2 2 2 2 2 2 2 2 2 2 2
T P 2= m 2 [ ẋ +l 1 θ̇ 1 cos θ 1+l 2 θ̇ 2 cos θ 2+2 ( ẋ l 1 θ̇1 cos θ1 + ẋ l2 θ̇2 cos θ2 +l 1 θ̇ 1 cos θ 1 l 2 θ̇ 2 cos θ2 ) +l 1 θ̇1 sin θ 1+ l2 θ̇2 s
2

3
 1
T P 2 = m 2 ¿]
2

1
T P 2= m2 ¿]
2

Potential energy

Pendulum 1 & 2

U P 1=m1 g l 1 cos θ1

U P 2=m2 g ( l 1 cos θ1 +l 2 cos θ2 )

U = U P 1+ U P 2 = m1 g l 1 cos θ1 +m2 g ( l 1 cos θ1 +l 2 cos θ 2 )

L=T −U

L=T C +T P 1 +T P 2−U P 1−U P 2

1 2 1 1
L= mC ẋ + m1 [ ẋ +2 ẋ l 1 θ̇1 cos θ1 +l 1 θ̇1 ] + m2 [ ẋ +l 1 θ̇1 +l 2 θ̇ 2 +2 l 1 l 2 θ̇ 1 θ̇ 2 cos ( θ 1+ θ2 ) +2 ẋ ( l 1 θ̇ 1 cos θ1+
2 2 2 2 2 2 2 2
2 2 2

1 2 1 2 2 1 2 2 1 2 2
L= (m C + m1 +m 2 ) ẋ +m1 ẋ l1 θ̇1 cos θ1 + m1 l 1 θ̇ 1 + m 2 l 1 θ̇1 + m2 l 2 θ̇2 +m2 l 1 l 2 θ̇1 θ̇2 cos ( θ1 +θ2 ) +m 2 ẋ l 1 θ̇
2 2 2 2

1 2 1 2 2 1 2 2
L= ( mC +m 1 +m2 ) ẋ +m 1 ẋ l 1 θ̇1 cos θ1 + ( m 1 +m2 ) l 1 θ̇1 + m2 l 2 θ̇2 +m2 l 1 l2 θ̇1 θ̇2 cos ( θ1 +θ2 ) +m2 ẋ l 1 θ̇ 1 cos θ 1+ m2 ẋ l
2 2 2

To obtain our equations of motion, we will need to derive the Euler-Lagrange equations first, by taking
the partial derivatives of the Lagrangian with respect to the generalized coordinates and generalized
velocities. Our double pendulum has 3 degrees of freedom since it can move along the x axis, and rotate
in θ1and θ2 , therefore we will define our generalized coordinates as:

4
 []
x
q= θ 1
θ2

Therefore, our generalized velocities are:

[]
ẋ
q̇= θ˙1
θ˙2

These definitions are useful since a good practice for defining state space coordinates is to choose
variables that store energy, and since the potential energy of the system is dependent on the generalized
coordinates (except for the x position) and the kinetic energy of the system is dependent on the
generalized velocities, q and q̇ will also be defining our state space.

The expression () is the Euler-Lagrange equation where F gen is our generalized force, in our case it is 0
for both pendulums equations of motion since we are not accounting for friction in this model, however
for the equation of motion of the cart our F gen is equal to u which is our control input, which would be the
torque that accelerates the cart.

Let us remember the expression for the Euler-Lagrange equation as follows:

∂ L d ∂L
=
∂ q dt ∂ q̇( )
+ F gen

Where F gen is a generalized force, in our case it is 0 for both pendulums since we are not accounting for
friction in this model, however for the equation of motion of the cart F gen is equal to u which is the force
that accelerates the cart linearly.

1 2 1 2 2 1 2 2
L= ( mC +m 1 +m2 ) ẋ +m 1 ẋ l 1 θ̇1 cos θ1 + ( m 1 +m2 ) l 1 θ̇1 + m2 l 2 θ̇2 +m2 l 1 l2 θ̇1 θ̇2 cos ( θ1 +θ2 ) +m2 ẋ l 1 θ̇ 1 cos θ
2 2 2

Cart

∂L
=0
∂x

∂L
=( mC + m1 +m2 ) ẋ+ m1 l 1 θ̇1 cos θ1 +m 2 l1 θ̇1 cos θ1 +m2 l 2 θ̇2 cos θ2
∂ ẋ

( )
d ∂L
dt ∂ ẋ
=( mC + m1 +m2 ) ẍ+ ( m1 +m2 ) l 1 ( θ̇ 1 cos θ 1 ) +m2 l 2 θ̇ 2 cos θ2

5
 ( )
d ∂L
dt ∂ ẋ
=( mC + m1 +m2 ) ẍ+ ( m1 +m2 ) l 1 ( θ̈1 cos θ1−θ̇12 sin θ1)+ m2 l 2 ( θ̈2 cos θ2−θ̇ 22 sin θ2 )

∂ L d ∂L
−
∂ x dt ∂ ẋ ( )
=u

( mC +m1 +m2 ) ẍ + ( m1 +m2 ) l 1 cos θ1 θ̈1 +m2 l2 cos θ2 θ̈2=u+ ( m1+m2 ) l 1 sin θ1 θ̇12 +m2 l 2 sinθ 2 θ̇ 22
Pendulum 1

1 2 1 2 2 1 2 2
L= ( mC +m1 +m2 ) ẋ +m1 ẋ l 1 θ̇1 cos θ1 + ( m1 +m2 ) l 1 θ̇1 + m2 l 2 θ̇2 +m2 l 1 l2 θ̇1 θ̇2 cos ( θ1 +θ2 ) +m2 ẋ l 1 θ̇ 1 cos θ
2 2 2

∂L d ∂L
−
( )
∂ θ1 dt ∂ θ̇ 1
=0

∂L
=−( m1+ m2) l 1 ẋ θ̇1 sinθ 1−m 2 l 1 l 2 θ̇ 1 θ̇2 sin ( θ1 +θ2 ) −(m1 +m 2) g l1 sin θ1
∂ θ1

∂L 2
=m1 l 1 ( ẋ cos θ1 ) + ( m1 +m2 ) l 1 θ̇1 +m2 l 1 l 2 θ̇2 cos ( θ 1+θ 2 ) +m2 l 1 ( ẋ cos θ 1)
∂ θ̇1

d ∂L
( )
dt ∂ θ̇1
2
=(m1 +m2)l 1( ẍ cos θ 1− ẋ θ̇1 sin θ1 )+ ( m1+ m2 ) l 1 θ̈1 +m2 l 1 l 2 ( θ̈2 cos ( θ1 +θ2 ) −θ̇2 sin ( θ1+ θ2 ) ( θ̇1 + θ̇ 2))

−m1 l 1 ẋ θ̇1 sin θ1 −m2 l 1 l 2 θ̇1 θ̇ 2 sin ( θ1 +θ 2) −m2 l 1 ẋ θ̇1 sin θ1 + ( m1+ m2) g l 1 sin θ1 −( m1+ m2 ) l 1 ( ẍ cos θ 1− ẋ θ̇ 1 sin θ1 ) −( m1+

−m1 l 1 ẋ θ̇1 sin θ1 −m2 l 1 l 2 θ̇1 θ̇ 2 sin ( θ1 +θ 2) −m2 l 1 ẋ θ̇1 sin θ1 + ( m1+ m2) g l 1 sin θ1 −( m1+ m2 ) l 1 ẍ cos θ1 + ( m1 +m2 ) l1 ẋ θ̇ 1 sin

( m1 +m2 ) l1 cos θ1 ẍ+ ( m1+ m2 ) l1 θ¨1 +m2 l1 l2 cos ( θ1 +θ2 ) θ̈2=−m2 l 1 l 2 θ̇ 1 θ̇2 sin ( θ1 +θ2 ) −m2 l1 l2 θ̇2 sin ( θ1+ θ2 ) + ( m1 +m2 ) l1 ẋ
2 2

( m1 +m2 ) l1 cos θ1 ẍ+ ( m1+ m2 ) l1 θ¨1 +m2 l1 l2 cos ( θ1 +θ2 ) θ̈2=−m2 l 1 l 2 θ̇ 1 θ̇2 sin ( θ1 +θ2 ) −m2 l1 l2 θ̇2 sin ( θ1+ θ2 ) + ( m1 +m2 ) l1 ẋ
2 2

( m1 +m2 ) l1 cos θ1 ẍ+ ( m1+ m2 ) l1 θ¨1 +m2 l1 l2 cos ( θ1 +θ2 ) θ̈2=−2 m2 l1 l2 θ̇1 θ̇2 sin ( θ1+ θ2 )−m2 l1 l2 θ˙2 sin ( θ 1+θ 2 )+ ( m1+ m2 ) g
2 2

Pendulum 2

∂L d ∂L
−
( )
∂ θ2 dt ∂ θ̇ 2
=0

∂L
=−m2 l 1 l 2 θ̇1 θ̇ 2 sin ( θ 1+θ 2 )−m2 ẋ l 2 θ̇ 2 sin θ2 +m 2 g l 2 sinθ 2
∂ θ2

6
 ∂L 2
=m2 l 2 θ̇ 2+m2 l 1 l 2 ¿ ¿
∂ θ̇2

( )
d ∂L
dt ∂ θ̇2
=m2 l 2 θ¨2 +m2 l1 l 2 ¿
2

2
−m2 l 1 l 2 θ̇1 θ̇2 sin ( θ1 +θ2 )−m2 ẋ l 2 θ̇ 2 sin θ 2+ m2 g l 2 sin θ2−m2 l 2 θ̈2−m2 l 1 l 2 ¿ 0

2
−m2 l 1 l 2 θ̇1 θ̇2 sin ( θ1 +θ2 )−m2 ẋ l 2 θ̇ 2 sin θ 2+ m2 g l 2 sin θ2−m2 l 2 θ̈2−m2 l 1 l 2 ¿ 0

Nonlinear State Space

Taking the equations of motion, we can represent the nonlinear state space matrix form of the system as
shown below.

[ ][ ] [ ][ ][]
a 4 θ˙1+ a5 θ̇2
2 2
a 1 a2 a 3 ẍ 0 1
b1 b2 b 3 θ̈1 = −b4 θ̇2
2 + +
−b5 0 u
d 1 d2 d 3 θ̈2 d 4 θ˙1
2 −d 5 0

[ ][ ][]
ẍ f1 1
M θ̈1 = f 2 + 0 u
θ̈2 f3 0

Table 1. Nonlinear Matrix Coefficients.

a 1=( m1+ m2+ mc ) b 1=( m1+ m2) l 1 cos θ1 d 1=m2 l 2 cos θ 2
a 2=(m1 +m2)l 1 cos θ1 b 2= ( m 1+ m 2) l 1
2
d 2=m2 l 1 l 2 cos(θ 2+θ 1)
a 3=m2 l 2 cos θ2 b 3=m2 l 1 l 2 cos (θ2 +θ1 ) d 3=m2 l 2
2

a 4=(m1+ m2)l1 sin θ1 b 4=m2 l 1 l 2 sin (θ2 +θ1 ) d 4 =m2 l 1 l 2 sin(θ 2+ θ1)
a 5=m2 l 2 sin θ2 b 5=(m1 +m2) gl 1 sin θ1 d 5=m2 gl 2 sin θ2

Simulation of dynamics

We define our systems dynamics in a function as in Figure (). We introduce an element of damping
proportional to the velocities of the cart and pendulums. This function returns the matrices M and f (X, u)
and to obtain our dynamics we take M −1 and multiply it by F . M is symmetric so its invertible.

7
 []
ẍ
−1
θ̈1 =M F
θ̈2

To numerically integrate our dynamics, we will use a 4 th order Runge-Kutta to reduce the number of steps
and avoid numerical errors that Simpson’s rule or Riemann sums tend to generate as shown in Figure ().

Validation of model

To validate the model dynamics, we simulate our pendulum with no control inputs and no energy
dissipation. If the sum of potential and kinetic energies is constant, then we can prove that the pendulum´s
dynamics make sense, as shown in Figure ().

8
As shown before, the pendulum energy remains constant which is a good indication, however, another
way we can prove that the model is valid is by using different initial conditions that are close to one
another. Since a double pendulum is a chaotic system, small changes to the initial conditions will yield
very different trajectories even if they are close. We can do this using a phase diagram of the system and
since we have a state space of 6 variables we can pair the generalized coordinates with the generalized
velocities. We then program different initial conditions and plot them.

In Figure () we plot 3 different initial conditions with a difference of 10−8 degrees with respect to each
other. As we can see, the trajectories do diverge enough to be noticeable.

9
Although this is a fast way to check, it is not necessarily mathematically rigorous. Therefore, we will also
use Lyapunov exponents to check for chaos in our dynamics. If the Lyapunov exponent is positive, it
confirms chaos, which will further validate the model.

“Calculate here Lyapunov exponents”

Linearization

Our system is highly nonlinear, and our control will be a linear MPC and LQR, therefore we need to
linearize the dynamics around an operating point. To do this we first analyse the equilibrium
configurations of the system.

Table 2. Equilibrium configurations of the Double Pendulum

Equilibrium Configuration Pendulum 1 Pendulum 2 Stability
1 Down Down Stable
2 Down Up Unstable
3 Up Down Unstable
4 Up Up Unstable

We will focus on Equilibrium position 4 but in practice we can control all 4 positions if the configuration
of the system stays within close range of the operating point. To do this we will compute the Jacobian
matrix of our nonlinear function.

Ẋ =f ( X , u )

Let us remember that the Jacobian tells us the sensitivity of a system of functions to “nudges” in its
variables, and the computation is just to take the gradient of these functions around an operating point.

[ ]
∂f1 ∂f1
⋯

[ ]
∇f1 ∂ z1 ∂ zn
J= ⋮ = ⋮ ⋱ ⋮
∇fm ∂fm ∂fm
⋯
∂ z1 ∂ zn

Once we compute the Jacobian matrix, we analyse it at an operating point Z 0.

10
 Δf
=J ( Z 0 )
ΔZ

We can define the Jacobian as the change in f with respect to the change in the state vector Z then solve
for F to obtain the linearized function around the operating point.

F−F 0
=J ( Z 0)
Z−Z 0

F=F 0+ J ( Z 0 ) Δ Z

We will do the same for our system since we need the state space representation to be in the form:

Ẋ =AX + Bu

We will take the Jacobian with respect to the state vectors and control inputs. These will be our A and B
matrices respectively, but since this is a linearization our state space matrices will be in the form
Ẋ =A ∆ X + B ∆ u where ∆ X is the difference between the state variables at the equilibrium position and
the state vector, the same goes for ∆ u , therefore:

∂F
A=
∂X

∂F
B=
∂u
¿ ¿
Ẋ =A ( X−X )+ B(u−u )
¿ ¿
Where X and u are the operating points of the linearization and the updated state vector . Using
MATLAB´s symbolic library and calculating the Jacobian with respect to the state vector and the control
input respectively as shown in Figure (), we obtain the linearized state transition matrix and control input
matrix.

11
Control

Controllability

A system is controllable if we can transition our states to anywhere in state space in a finite amount of
time, a simple way to evaluate this is to compute the controllability matrix:
2 n−1
C=[B AB A B … A B]

Where n is the number of columns in the state transition matrix A. If the matrix C is full rank, then this
means that the system is controllable. Using MATLAB´s ctrb (A, B) function and getting its rank
determines that the system is indeed controllable as shown in Figure ().

LQR

To evaluate the MPC´s performance an LQR control is implemented as a benchmark, and since the
system has already been linearized, we can solve the Algebraic Ricatti Equation and obtain the full state
feedback gain K.

12
 Show graphs and results

MPC

Basic Structure

Model Predictive Control uses a model of the plant to make predictions of the evolution of the system and
compute the optimal control solving an optimization problem at each time step. For this paper we focus
on a linear discrete MPC and its structure is shown in Figure ().

Discrete Time MPC

We will use a Zero Order Hold to convert the linear dynamics

from continuous to discrete as shown in Figure ().

13
In practice we also need to discretize the A and B matrices. Since we are solving for a linear system, the
solution will be of the form:
t
X ( t 0 ) +∫ e
¿ A (t −t 0 ) A ( t−τ )
X ( t )=X +e Bu ( τ ) dτ
t0

With this expression we can discretize the dynamics:

A ( t−t 0)
Ad =e
t
Bd =∫ e
A ( t −τ )
Bu ( τ ) dτ
t0

Since the control input u is constant from the interval t 0 <t< t s , the control input matrix can be solved as:

t
Bd =u ( τ )∫ (e ¿¿ A ( t−τ ) ¿ dτ)B ¿ ¿
t0

We remember that since this is a linearization we take ∆ X k and not the state vector itself and since the
operating point for the control input matrix is 0 (because we want the control input to be as close to 0
when the pendulum is upright). Since the MPC methodology requires predictions, we can express these
predictions as follows:
¿
X k+1= X + Ad ∆ X k + Bd uk
¿
X k+2= X + Ad ∆ X k +1+ B d u k+1
¿
X k+3= X + A d ∆ X k +2+ B d u k+2

We do this for k up to k + N where N is the prediction horizon. For nonlinear fast dynamical systems, a
prediction horizon between 15 to 30 is considered good []. Generalizing the latter expressions we obtain
the following:
¿
X k+1= X + Ad ∆ X k + Bd uk

14
 X k+2= X ¿ + Ad 2 ( X k − X ¿ ) + A d B d uk +B d uk+ 1

X k+3= X + A d ( X k − X ) + A d B d uk + A d Bd uk +1+ B d uk+2

¿ 3 ¿ 2

⋮
¿ N N −1
X k+ N = X + Ad ∆ X k + Ad Bd uk +⋯+ Bd u k+ N−1

[] [] [ ][ ]
X k+1 Ad Bd 0 ⋯ 0 uk
2 2
X k+2 =X ¿ + Ad ∆ X + Ad Bd Bd ⋯ 0 uk+ 1
k
⋮ ⋮ ⋮ ⋮ ⋱ ⋮ ⋮
N
X k +N Ad N−1 N−1
A d B d A d Bd ⋯ Bd uk +N −1

~
Since we want our controller to track a given reference Y , we can multiply our concatenated matrix X (k)
by the matrix C for each value in the matrix as follows.

[][]
Y k +1 X k +1
Y k +2 =C X k +2
⋮ ⋮
Y k+ N X k+ N

To simplify programming, we can use a selector matrix to represent the accumulation of the control
inputs to the system which we denote below as ψ i.

( m ,N )
Πi = [ 0 0 ⋯ I ⋯ 0 0]

[ ]
Bd 0 ⋯ 0
2
~
X ( k)=X ¿ + ϕ ∆ X k + A d Bd B d ⋯ 0 ~
u (k)
⋮ ⋮ ⋱ ⋮
N−1 N −1
A d Bd Ad Bd ⋯ Bd

[ ]
( nu , N )
Π1
( nu , N )
X (k +i)¿ X +ϕ i ∆ X k + [ A B ⋯ AB B ] Π 2 u~(k)
¿ i

⋮
Π (i )
n ,N u

The expression below will be used for predicting the future states of the system.

X (k +i)¿ X +ϕ i ∆ X k +ψ i ~u(k)
¿

Cost function

Let us remember that the cost function is a weighted sum of the differences between the reference and the

15
output of the plant, as well as the control effort minus the desired control. Since we want to minimize the
control effort, the desired control will be 0. Also, the reference will be static, therefore we will treat it as a
constant.
N (n y , N ) (nu , N)
~
J=∑ ¿ ∏ Y −ref ∨¿2Q + ¿ ∏ ~u (k )∨¿ 2R ¿ ¿
i=1 i i

N (nx , N ) (nu , N )
~
J=∑ ¿ C ∏ X (k )−ref ∨¿ 2Q +¿ ∏ ~u(k )∨¿2R ¿ ¿
i=1 i i

N (nu , N )

J=∑ ¿ C( X ¿ + ϕi ∆ X k +ψ i ~
u(k))−ref ∨¿2Q +¿ ∏ ~u(k )∨¿2R ¿ ¿
i=1 i

N (nu , N)

J=∑ ¿ C X + C ϕ i ∆ X k +C ψ i ~
¿
u(k)¿−ref ∨¿2Q + ¿ ∏ ~u (k )∨¿ 2R ¿
i=1 i

¿
Since X and ref are constant during the calculation of the cost function we can take the difference
between them and call it a new variable e , this will reduce complexity in calculations.

N (nu , N )

J=∑ ¿ C ϕ i ∆ X k +C ψ i ~
u ( k ) +e∨¿ 2Q +¿ ∏ ~u(k)∨¿2R ¿ ¿
i=1 i

J i=∆ X k ϕi C QC ϕ i ∆ X k + ~
u ( k ) ψ i C QC ψ i ~
u ( k ) + e Qe+2 ∆ X k ϕ i C Q C ψ i ~u ( k ) +2 ∆ X k ϕ i C Qe+2 ~
T T T T T T T T T T T T T T
u (k ) ψ

Now that we have expanded the cost function, we can group the terms to get a quadratic form of the
expression as follows:

1 T
J i= u Hu+[]
2

The latter is useful if we want to use QP solvers for constrained optimization.

J i =~
u ( k )T H ~
u ( k ) + [ F1 ∆ X k + F 2 ] ~
T
u (k )
N
H=2 ∑ [ ψ iT C T QC ψ i+ Π (ni , N)T R Π (n
i
,N )
u
] u

i=1

N
T
F 1=2 ∑ ❑ [ 2ψ iT C T Qe ] ~u (k )
i=1

To obtain the optimal control vector, we need to take the gradient of the cost function and equate it to 0, since when
the gradient is 0, we have reached a peak or a valley, and if our Hessian matrix H is positive semidefinite we have a
convex function and we have a global minima.

16
 ∂J
∇ J u= ~ = ~
T
u ( k ) H + [ F 1 ∆ X k + F2 ] =0
T

∂u ( k )

Now we can take the transpose of both sides and solve for ~
u ( k ). Since we are assuming H is symmetric, H=H T
and invertible.

∇ J u=( ~
T T T
u ( k ) H + F =0)

∇ J u=H ~u ( k ) + F=0

~ −1
u ( k )=−H F

For strictly convex cost functions the expression −H −1 F gives the optimal control trajectory at each time step,
however this is not the control yet, remembering that the methodology of MPC is taking the first value of these
optimal control trajectory as follows.

~
u ( k )=−H−1 F
(nu , N )

uopt = ∏ ~
u (k )
1

Having all the latter expressions we can implement them in a MATLAB simulation as follows:

Continuous to discrete dynamics

Main control loop

The main control loop uses the LinMPC( ) function which offers different optimizers for the control input. To
simulate the discretization, we use a condition that is applied when the modulus between the current time step and
the sampling time equals 0 and in the beginning of the simulation.

17
LinMPC

This function is a selection function that uses different optimization methods of the cost function, it uses the HFQ
function which calculates the H and F matrices to obtain the optimal analytical control, but these only for convex
cost functions.

HFQ

This function receives the discrete A and B matrices which we call Phi and Gamma, the current state X,
the penalization matrices Q and R and the prediction horizon Np.

18
Selector Matrix PI

PSI

Matrix ψ i is implemented in the function PSI, it takes the previous ψ i value, the current selection matrix, the
discrete transition matrix and discrete control matrix, as well as the current iteration “i”. the z value works to
concatenate the cumulative control inputs.

Gradient Descent function GD

This function implements a normal gradient descent, it takes

Nesterov gradient descent

19
Momentum gradient descent

Results

20
Articulos científicos

Nombre del Implementación Casos de Descripci

Referencia Año de publicación
Articulo embebida estudio ón

“Quickly 2019
Finding
Recursively
Feasible
[1]
Solutions for
MPC with
Discrete
Variables”

“Embedded 2013
Online
Optimization
[2] for Model
Predictive
Control at
Megahertz”

“Embedded 2017
Model
Predictive
[5] Control for
Energy-
Efficient
Buildings”

“Embedded 2019
Model
Predictive
[6]
Control for
Autonomous
Vehicles”

“Embedded 2018
Model
[7] Predictive
Control for
Robotics”

21
References

1. Inserta tu texto aquí

2. Inserta tu texto aquí
3. Inserta tu texto aquí Inserta tu texto aquí
4. r
5. r
6. r
7. r
8. r
9. r
10. r

22