Documentation

## Swing-up Control of a Pendulum Using Nonlinear Model Predictive Control

This example uses nonlinear model predictive controller object and block to achieve swing-up and balancing control of an inverted pendulum on a cart.

### Product Requirement

This example requires Optimization Toolbox™ software to provide the default nonlinear programming solver for nonlinear MPC to compute optimal control moves at each control interval.

```if ~mpcchecktoolboxinstalled('optim') disp('Optimization Toolbox is required to run this example.') return end ```

### Pendulum/Cart Assembly

The plant for this example is a pendulum/cart assembly, where z is the cart position and theta is the pendulum angle. The manipulated variable for this system is a variable force F acting on the cart. The range of the force is between `-100` and `100`. An impulsive disturbance dF can push the pendulum as well. ### Control Objectives

Assume the following initial conditions for the pendulum/cart assembly:

• The cart is stationary at z = `0`.

• The pendulum is in a downward equilibrium position where theta = `-pi`.

The control objectives are:

• Swing-up control: Initially swing the pendulum up to an inverted equilibrium position where z = `0` and theta = `0`.

• Cart position reference tracking: Move the cart to a new position with a step setpoint change, keeping the pendulum inverted.

• Pendulum balancing: When an impulse disturbance of magnitude of `2` is applied to the inverted pendulum, keep the pendulum balanced, and return the cart to its original position.

The downward equilibrium position is stabl,e and the inverted equilibrium position is unstable, which makes swing-up control more challenging for a single linear controller, which nonlinear MPC handles easily.

### Control Structure

In this example, nonlinear MPC has the following I/O configuration:

• One manipulated variable: Variable force (F)

• Two measured outputs: Cart position (z) and pendulum angle (theta)

Two other states, cart velocity (zdot) and pendulum angular velocity (thetadot) are not measurable.

While the setpoint of the cart position, z, can vary, the setpoint of the pendulum angle, theta, is always `0` (inverted equilibrium position).

### Create Nonlinear MPC Controller

First, create a nonlinear MPC controller with the proper dimensions using an `nlmpc` object. In this example, the prediction model has `4` states, `2` outputs, and `1` input (MV).

```nx = 4; ny = 2; nu = 1; nlobj = nlmpc(nx, ny, nu); ```
```In standard cost function, zero weights are applied by default to one or more OVs because there are fewer MVs than OVs. ```

The prediction model has sample time of `0.1` seconds, which is the same as the controller sample time.

```Ts = 0.1; nlobj.Ts = Ts; ```

Set the prediction horizon to `10`, which is long enough to capture major dynamics in the plant but not so long that it hurts computational efficiency.

```nlobj.PredictionHorizon = 10; ```

Set the control horizon to `5`, which is long enough to give the controller enough degrees of freedom to handle the unstable mode without introducing excessive decision variables.

```nlobj.ControlHorizon = 5; ```

### Specify Nonlinear Plant Model

The major benefit of nonlinear model predictive control is that it uses a nonlinear dynamic model to predict plant behavior in the future across a wide range of operating conditions.

This nonlinear model is usually a first principle model consisting of a set of differential and algebraic equations (DAEs). In this example, a discrete-time cart and pendulum system is defined in the `pendulumDT0` function. It integrates continuous-time model, `pendulumCT0`, between control intervals using a multistep forward Euler method. The same function is used by the nonlinear state estimator as well.

```nlobj.Model.StateFcn = "pendulumDT0"; ```

To use a discrete-time model, set the `Model.IsContinuousTime` property of the controller to `false`.

```nlobj.Model.IsContinuousTime = false; ```

The prediction model uses an optional parameter, `Ts`, to represent the sample time. Using this parameter means that, if you change the prediction sample time during the design, you do not have to modify the `pendulumDT0` file.

```nlobj.Model.NumberOfParameters = 1; ```

The two plant outputs are the first and third state in the model, the cart position and pendulum angle, respectively. Specify the corresponding output function using an anonymous function.

```nlobj.Model.OutputFcn = @(x,u,Ts) [x(1); x(3)]; ```

It is best practice to provide analytical Jacobian functions whenever possible, since they significantly improve the simulation speed. In this example, provide a Jacobian for the output function.

```nlobj.Jacobian.OutputFcn = @(x,u,Ts) [1 0 0 0; 0 0 1 0]; ```

Since you do not provide Jacobian for the state function, the nonlinear MPC controller estimates the state function Jacobian during optimization using numerical perturbation. Doing so slows down simulation to some degree.

### Define Cost and Constraints

Like linear MPC, nonlinear MPC solves a constrained optimization problem at each control interval. However, since the plant model is nonlinear, nonlinear MPC converts the optimal control problem into a nonlinear optimization problem with a nonlinear cost function and nonlinear constraints.

The cost function used in this example is the same standard cost function used by linear MPC, where output reference tracking and manipulated variable move suppression are enforced. Therefore, specify standard MPC tuning weights.

```nlobj.Weights.OutputVariables = [3 3]; nlobj.Weights.ManipulatedVariablesRate = 0.1; ```

The cart position is limited to the range `-10` to `10`.

```nlobj.OV(1).Min = -10; nlobj.OV(1).Max = 10; ```

The force has a range between `-100` and `100`.

```nlobj.MV.Min = -100; nlobj.MV.Max = 100; ```

### Validating Nonlinear MPC

After designing nonlinear MPC controller object, it is best practice to check all the functions you specified for the prediction model, custom cost, and custom constraints, as well as their Jacobians, using the `validateFcns` command. This command detects any dimensional and numerical inconsistencies in these functions.

```x0 = [0.1;0.2;-pi/2;0.3]; u0 = 0.4; validateFcns(nlobj, x0, u0, [], {Ts}); ```
```Model.StateFcn is OK. Model.OutputFcn is OK. Jacobian.OutputFcn is OK. Analysis of user-provided model, cost, and constraint functions complete. ```

### State Estimation

In this example, only two plant states (cart position and pendulum angle) are measurable. Therefore, you estimate the four plant states using an extended Kalman filter. Its state transition function is defined in `pendulumStateFcn.m` and its measurement function is defined in `pendulumMeasurementFcn.m`.

```EKF = extendedKalmanFilter(@pendulumStateFcn, @pendulumMeasurementFcn); ```

### Closed-Loop Simulation in MATLAB®

Specify the initial conditions for simulations by setting the initial plant state and output values. Also, specify the initial state of the extended Kalman filter.

The initial conditions of the simulation are:

• The cart is stationary at z = 0.

• The pendulum is in a downward equilibrium position, theta = `-pi`.

```x = [0;0;-pi;0]; y = [x(1);x(3)]; EKF.State = x; ```

`mv` is the optimal control move computed at any control interval. Initialize `mv` to zero, since the force applied to the cart is zero at the beginning.

```mv = 0; ```

In the first stage of the simulation, the pendulum swings up from a downward equilibrium position to an inverted equilibrium position. The state references for this stage are all zero.

```yref1 = [0 0]; ```

At a time of `10` seconds, the cart moves from position `0` to `5`. Set the state references for this position.

```yref2 = [5 0]; ```

Using the `nlmpcmove` command, compute optimal control moves at each control interval. This function constructs a nonlinear programming problem and solves it using the `fmincon` function from the Optimization Toolbox.

Specify the prediction model parameter using an `nlmpcmoveopt` object, and pass this object to `nlmpcmove`.

```nloptions = nlmpcmoveopt; nloptions.Parameters = {Ts}; ```

Run the simulation for `20` seconds.

```Duration = 20; hbar = waitbar(0,'Simulation Progress'); xHistory = x; for ct = 1:(20/Ts) % Set references if ct*Ts<10 yref = yref1; else yref = yref2; end % Correct previous prediction using current measurement xk = correct(EKF, y); % Compute optimal control moves [mv,nloptions,info] = nlmpcmove(nlobj,xk,mv,yref,[],nloptions); % Predict prediction model states for the next iteration predict(EKF, [mv; Ts]); % Implement first optimal control move and update plant states. x = pendulumDT0(x,mv,Ts); % Generate sensor data with some white noise y = x([1 3]) + randn(2,1)*0.01; % Save plant states for display. xHistory = [xHistory x]; %#ok<*AGROW> waitbar(ct*Ts/20,hbar); end close(hbar); ```

Plot the closed-loop response.

```figure subplot(2,2,1) plot(0:Ts:Duration,xHistory(1,:)) xlabel('time') ylabel('z') title('cart position') subplot(2,2,2) plot(0:Ts:Duration,xHistory(2,:)) xlabel('time') ylabel('zdot') title('cart velocity') subplot(2,2,3) plot(0:Ts:Duration,xHistory(3,:)) xlabel('time') ylabel('theta') title('pendulum angle') subplot(2,2,4) plot(0:Ts:Duration,xHistory(4,:)) xlabel('time') ylabel('thetadot') title('pendulum velocity') ``` The pendulum angle plot shows that the pendulum successfully swings up in two seconds. During the swing-up process, the cart is displaced with a peak deviation of `-1`, and returned to its original position around a time of `2` seconds.

The cart position plot shows that the cart successfully moves to z = `5` in two seconds. While the cart moves, the pendulum is displaced with a peak deviation of `1` radian (`57` degrees) and returned to an inverted equilibrium position around a time of `12` seconds.

### Closed-Loop Simulation in Simulink

This example also requires Simulink® software to simulate nonlinear MPC control of a pendulum model in Simulink.

```if ~mpcchecktoolboxinstalled('simulink') disp('Simulink is required to run this example.') return end ```

Validate the nonlinear MPC controller with a closed-loop simulation in Simulink.

Open the Simulink model.

```mdl = 'mpc_pendcartNMPC'; open_system(mdl) ``` In this model, the Nonlinear MPC Controller block is configured to use the previously designed controller, `nlobj`.

To use optional parameters in the prediction model, the model has a Simulink Bus block connected to the `params` input port of the Nonlinear MPC Controller block. To configure this bus block to use the `Ts` parameter, create a Bus object in the MATLAB® workspace and configure the Bus Creator block to use this object. To do so, use the `createParameterBus` function. In this example, name the Bus object `'myBusObject'`.

```createParameterBus(nlobj,[mdl '/Nonlinear MPC Controller'],'myBusObject',{Ts}); ```
```A Simulink Bus object "myBusObject" created in the MATLAB Workspace, and Bus Creator block "mpc_pendcartNMPC/Nonlinear MPC Controller" is configured to use it. ```

Run the simulation for `30` seconds.

```open_system([mdl '/Scope']) sim(mdl) ```  The nonlinear simulation in Simulink produces identical swing-up and cart position tracking results compared to the MATLAB simulation. Also, a push (impulse disturbance `dF`) is applied to the inverted pendulum at a time of `20` seconds. The nonlinear MPC controller successfully rejects the disturbance and returns the cart to z = `5` and the pendulum to an inverted equilibrium position.

Neither the `nlmpc` object nor Nonlinear MPC Controller block support code generation.

### Conclusion

This example illustrates a general workflow to design and simulate nonlinear MPC in MATLAB and Simulink using an `nlmpc` object and Nonlinear MPC Controller block, respectively. Depending on the specific nonlinear plant characteristics and control requirements, the implementation details can vary significantly. The key design challenges include:

• Choosing proper horizons, bounds and weights

• Designing a nonlinear state estimator

• Designing custom nonlinear cost function and constraint function

• Selecting solver options or choosing a custom NLP solver

You can use the functions and Simulink model in this example as templates for other nonlinear MPC design and simulation tasks.

```bdclose(mdl) ```

## Related Topics

#### Implementing an Adaptive Cruise Controller with Simulink

Download technical paper