# Swing-Up Control of a Pendulum on a Cart using Multistage Nonlinear MPC with Nonlinear Grey-Box Model

This example uses a multistage nonlinear model predictive controller to swing-up and balance an inverted pendulum on a cart. The nonlinear plant that you use to design the MPC controller is a grey-box model in which three parameters are known and the other two have been estimated from input-output data.

#### Modeling and Estimating the Pendulum/Cart Assembly

The nonlinear plant is the pendulum and cart assembly system as seen below.

The plant model consists of four states which are the cart position, cart velocity, pendulum angle and pendulum angular velocity. The control variable is the force (*F*) with an impulsive disturbance *dF* acting on the cart. The measured outputs are the cart position and pendulum angle.

This model is also used (and illustrated in more detail) in the example Swing-Up Control of Pendulum Using Nonlinear Model Predictive Control. For a more thorough explanation of the conventions and how to derive the underlying system equations, see Derive Equations of Motion and Simulate Cart-Pole System (Symbolic Math Toolbox).

To estimate the model, load the available input-output data from the MAT-file. The data consists of a uniformly sampled data set with 4001 samples in the variables u and y with a sample time of 0.01 seconds. The input (u) is the force applied on the cart which is a periodic sinusoidal signal and the output (y) is a vector containing the cart position and pendulum angle.

`load("pendulum_cart_data.mat")`

Create an `iddata`

(System Identification Toolbox) object to represent and store the data. For more information, see Representing Time- and Frequency-Domain Data Using iddata Objects (System Identification Toolbox).

`z = iddata(y, u, 0.01, Name="Pendulum Cart Data");`

Assign names and units to the stored signals.

z.InputName = {"Angle"}; z.InputUnit = {"rad"}; z.OutputName = {"x" ... % Position of the Cart "theta" ... % Angle of the Pendulum }; z.OutputUnit = {"m" "rad"}; z.Tstart = 0; z.TimeUnit = "s";

The system has 5 parameters, the mass of the cart (`mCart`

), mass of the pendulum (`mPend`

), gravitational acceleration (`g`

), length of the pendulum (`L`

) and the damping coefficient (`Kd`

). Of these five parameters, three of the parameters (`mPend`

, `g`

and `L`

) are assumed to be known. To create the `idnlgrey`

(System Identification Toolbox) object, first, organize the parameter details into a structure. The structure contains parameter names, units, initial values, limits, and a boolean variable indicating whether the true parameter value is known or not.

Parameters = struct("Name", ... {"Mass of the Cart" ... % mCart "Mass of the Pendulum" ... % mPend "Gravitational Accerleration" ... % g "Length of the Pendulum" ... % L "Damping Coefficient"}, ... % Kd "Unit", ... {"Kg", "Kg", "m/s^2", "m", "Ns/m"}, ... "Value", ... {0.4, 1, 9.81, 0.5, 6}, ... "Minimum", {-Inf -Inf -Inf -Inf -Inf}, ... "Maximum", {Inf Inf Inf Inf Inf},... % Ignore constraints. "Fixed", ... {false true true true false}); % mCart and Kd are unknown.

The system dynamics are described in the file `swingup_m.m`

.

`type("swingup_m.m");`

function [dxdt, y] = swingup_m( ... t, x, u, mCart, mPend, g, L, Kd, varargin) %% Continuous-time nonlinear dynamic model % of a pendulum on a cart % % 4 states (x): % cart position (z) % cart velocity (z_dot): positive right % angle (theta): 0 at upright position % angular velocity (theta_dot): % anti-clockwise positive % % 1 inputs: (u) % force (F): when positive, % force pushes cart to right % % Copyright 2024 The MathWorks, Inc. %% Obtain x, u and y % x z_dot = x(2); theta = x(3); th_dot = x(4); % u F = u; %% Compute dxdt dxdt = [ z_dot;... (F - Kd*z_dot - mPend*L*th_dot^2*sin(theta) ... + mPend*g*sin(theta)*cos(theta))/ ... (mCart + mPend*sin(theta)^2); ... th_dot; ... ((F - Kd*z_dot - mPend*L*th_dot^2*sin(theta))* ... cos(theta)/(mCart + mPend) + g*sin(theta))/ ... (L - mPend*L*cos(theta)^2/(mCart + mPend)) ]; % y y = [x(1);x(3)];

Use the model file, along with the order, parameters, and initial states data to create an `idnlgrey`

object describing the system.

FileName = "swingup_m"; % ODE File Order = [2 1 4]; % Model orders [ny nu nx] InitialStates = [0; 0; -pi; 0]; % Initial states values Ts = 0; % Continuous-time system nlgr = idnlgrey(FileName, Order, Parameters, InitialStates, Ts);

The system described by `nlgr`

has four states, one input, and two outputs. The model also contains 5 parameters, 3 of which are fixed and the other 2 need to be estimated.

nlgr

nlgr = Continuous-time nonlinear grey-box model defined by 'swingup_m' (MATLAB file): dx/dt = F(t, x(t), u(t), p1, ..., p5) y(t) = H(t, x(t), u(t), p1, ..., p5) + e(t) with 1 input(s), 4 state(s), 2 output(s), and 2 free parameter(s) (out of 5). Status: Created by direct construction or transformation. Not estimated.

To estimate the unknown system parameters, first, create an `nlgreyestOptions`

object to display the estimation progress and to set the maximum number of iterations.

```
opt = nlgreyestOptions(Display="on");
opt.SearchOptions.MaxIterations = 5;
```

Then, use `nlgreyest`

(System Identification Toolbox) to estimate the unknown parameters.

nlgr = nlgreyest(z, nlgr, opt);

To evaluate the quality of the estimated model, simulate the estimated model and compare the measured and simulated output.

compare(z, nlgr);

#### Generate State Function and its Jacobian from Nonlinear Grey-Box

The multistage nonlinear MPC controller requires state and Jacobian functions defined as MATLAB® function files. The nonlinear grey-box model provides the `generateMATLABFunction`

command, which you can use to generate these function files from the estimated `idnlgrey`

object. If you name the state function as `stateFcnPend`

then the software names the corresponding Jacobian function as `stateFcnPendJacobian`

.

`generateMATLABFunction(nlgr, "stateFcnPend");`

The software creates the files `stateFcnPend`

and `stateFcnPendJacobian`

in your current folder to store the state function and its Jacobian, respectively. These functions utilize the estimated parameters and invoke the corresponding ODE file.

#### Design Multistage Nonlinear MPC Using the Provided Functions

In this example, the plant has four states and one input. Set the prediction horizon `h`

to `10`

and sample time `Ts`

to `0.1`

s.

Create the multistage nonlinear MPC controller.

Ts = 0.1; h = 10; msobj = nlmpcMultistage(h,4,1); msobj.Ts = Ts;

Specify the state and Jacobian functions obtained from `generateMATLABFunction`

.

msobj.Model.StateFcn = "stateFcnPend"; msobj.Model.StateJacFcn = "stateFcnPendJacobian"; msobj.Model.IsContinuousTime = true;

Specify the constraints on the cart position (first state) and the force (the manipulated variable).

msobj.ManipulatedVariables.Min = -100; msobj.ManipulatedVariables.Max = 100; msobj.States(1).Min = -10; msobj.States(1).Max = 10; msobj.UseMVRate = true;

For this example, the cost function and its Jacobians for all stages are specified in the files `myCostSwingup.m`

and `myCostSwingupGradientFcn.m`

. In both functions, the last input argument `p`

is the parameter vector containing the reference signal for the cart position and the pendulum angle.

`type("myCostSwingup.m")`

function cost = myCostSwingup(stage,x,u,dmv,p) Wmv = 0.01; output = [x(1) x(3)]; ref = [p(1) p(2)]; if stage == 1 cost = Wmv*(dmv^2); elseif stage == 11 cost = (output-ref)*(output-ref)'; else cost = (output-ref)*(output-ref)' + Wmv*(dmv^2); end end

`type("myCostSwingupGradientFcn.m")`

function [Gx, Gmv, Gdmv] = myCostSwingupGradientFcn(stage,x,u,dmv,p) Wmv = 0.01; Gmv = zeros(1,1); if stage == 1 Gx = zeros(4,1); Gdmv = 2*Wmv*dmv; elseif stage == 11 Gx = [2*(x(1)-p(1)); 0; 2*(x(3)-p(2)); 0]; Gdmv = zeros(1,1); else Gx = [2*(x(1)-p(1)); 0; 2*(x(3)-p(2)); 0]; Gdmv = 2*Wmv*dmv; end

Set the cost function, Jacobian, and parameter length for each stage.

for ct=1:h+1 msobj.Stages(ct).CostFcn = "myCostSwingup"; msobj.Stages(ct).CostJacFcn = "myCostSwingupGradientFcn"; msobj.Stages(ct).ParameterLength = 2; end

Validate the state and cost functions along with their Jacobians.

simdata = getSimulationData(msobj); simdata.StageParameter = repmat([0;-pi], h+1, 1); validateFcns(msobj, [0.1 0.2 -pi/2 0.3], 0.4, simdata)

Model.StateFcn is OK. Model.StateJacFcn is OK. "CostFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11] are OK. "CostJacFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11] are OK. Analysis of user-provided model, cost, and constraint functions complete.

#### Closed-Loop Simulation in Simulink

Validate the performance of the controller by simulating it in closed-loop with the original nonlinear model.

Open the Simulink® model of the closed-loop system.

```
mdl = "swingup_pendulum_multistageNMPC";
open_system(mdl)
```

In this model, the Multistage MPC block is configured to use the `msobj`

object in the workspace.

Run the simulation for 30 seconds.

```
open_system(mdl+'/Scope')
sim(mdl);
```

As seen from the simulation, the multistage nonlinear MPC controller designed from the estimated grey-box model can successfully balance the pendulum in the inverted equilibrium, track the reference position, and reject disturbances.

## See Also

### Functions

### Objects

### Blocks

## Related Examples

- Derive Equations of Motion and Simulate Cart-Pole System (Symbolic Math Toolbox)
- Control House Heating Using Nonlinear MPC with Neural State-Space Prediction Model
- Landing a Vehicle Using Multistage Nonlinear MPC