How to add disturbance to system to check robustness of my MPC controller

I'm a newcomer to MATLAB and Control Theory, and I'm currently working on my university project, which involves implementing Model Predictive Control (MPC) for a 2-wheel self-balancing robot. To assess the controller's robustness, I'm aiming to introduce an input impulse disturbance. However, I'm encountering difficulties in doing so. I'm including my code here kindly suggest me fix, thank you.

clear; close all; clc;

%% Inverted Pendulum Model Parameters

M = 0.5;

m = 0.2;

b = 0.1;

I = 0.006;

g = 9.8;

l = 0.3;

p = I*(M+m)+M*m*l^2; %denominator for the A and B matrices

A = [0 1 0 0;

0 -(I+m*l^2)*b/p (m^2*g*l^2)/p 0;

0 0 0 1;

0 -(m*l*b)/p m*g*l*(M+m)/p 0];

B = [ 0;

(I+m*l^2)/p;

0;

m*l/p];

C = [1 0 0 0;

0 0 1 0];

D = [0;

0];

%% Discretize the system

Ts = 0.1; % Sampling time [s]

sys = ss(A, B, C, D);

sys_d = c2d(sys, Ts, 'zoh'); % Discretized system

Ad = sys_d.a;

Bd = sys_d.b;

Cd = sys_d.c;

%% Design MPC Controller

N = 20; % Prediction horizon

Nu = 5; % Control horizon

Q = diag([10, 1, 100, 1]); % State weighting matrix

R = 0.1; % Control input weighting

% Create MPC controller object

mpcobj = mpc(sys_d, Ts);

mpcobj.PredictionHorizon = N;

mpcobj.ControlHorizon = Nu;

mpcobj.Weights.OutputVariables = [1000, 100];

mpcobj.Weights.ManipulatedVariables = R;

mpcobj.Weights.ManipulatedVariablesRate = 0.1;

% Constraints

mpcobj.MV(1).Min = -10; % Force min [N]

mpcobj.MV(1).Max = 10; % Force max [N]

%% Simulation

Nsim = 100; % Number of simulation steps

x0 = [0.2; 0; 0.1; 0]; % Initial state

% Preallocate arrays for simulation results

nx = size(Ad, 1);

nu = size(Bd, 2);

ny = size(Cd, 1);

x = zeros(nx, Nsim);

y = zeros(ny, Nsim);

u = zeros(nu, Nsim);

x(:,1) = x0;

y(:,1) = Cd * x0;

% Create initial MPC state

mpcstate = mpcstate(mpcobj, x(:,1));

% Main simulation loop

for k = 1:Nsim-1

% Measure the current output

ymeas = y(:,k);

% Compute the control action using the MPC controller

[u(:,k), mpc_info] = mpcmove(mpcobj, mpcstate, ymeas, []);

% Apply the control action to the system and obtain the next state

x(:,k+1) = Ad * x(:,k) + Bd * u(:,k);

% Calculate the output

y(:,k+1) = Cd * x(:,k+1);

% Update MPC state

mpcstate.Plant = x(:,k+1);

end

% Plot the results

figure(1)

subplot(3,1,1);

t = (0:Nsim-1) * Ts;

figure(1);

plot(t, y(1,:), 'LineWidth', 2);

ylabel('Cart Position (m)');

title('Inverted Pendulum MPC Control');

grid on;

subplot(3,1,2);

plot(t, y(2,:), 'LineWidth', 2);

ylabel('Pendulum Angle (rad)');

grid on;

subplot(3,1,3);

plot(t, u(:,1:Nsim), 'LineWidth', 2); % Corrected dimensions of u

xlabel('Time (s)');

ylabel('Force (N)');

grid on;

Answer 1

[Sam Chak]

Hi @Ashutosh

I seldom employ MPC due to the challenges associated with deterministically controlling the controller's behavior from the Applied Mathematician's point of view. Nevertheless, I believe you can try describing the expected unmeasured input disturbance using setmpcsignals() and mpcobj.Model.Disturbance.

In the pendulum case, where the input disturbance is assumed to enter the system through the same channel as the control input, denoted as 'u' or the "Manipulated Variable" in MPC terminology, you will observe an additional column introduced in the state-space representation.

%% Inverted Pendulum
% Parameters
M   = 0.5;
m   = 0.2;
b   = 0.1;
I   = 0.006;
g   = 9.8;
l   = 0.3;
p   = I*(M + m) + M*m*l^2; % denominator for the A and B matrices

% State-space representation
A   = [0      1              0           0;
       0 -(I+m*l^2)*b/p  (m^2*g*l^2)/p   0;
       0      0              0           1;
       0 -(m*l*b)/p       m*g*l*(M+m)/p  0];
B   = [          0           0;     % Added one column for the input disturbance
       (I+m*l^2)/p (I+m*l^2)/p;
                 0           0;
             m*l/p       m*l/p];
C   = [1 0 0 0;
       0 0 1 0];
D   =  0*C*B;
sys =  ss(A, B, C, D)

sys =
 
  A = 
            x1       x2       x3       x4
   x1        0        1        0        0
   x2        0  -0.1818    2.673        0
   x3        0        0        0        1
   x4        0  -0.4545    31.18        0
 
  B = 
          u1     u2
   x1      0      0
   x2  1.818  1.818
   x3      0      0
   x4  4.545  4.545
 
  C = 
       x1  x2  x3  x4
   y1   1   0   0   0
   y2   0   0   1   0
 
  D = 
       u1  u2
   y1   0   0
   y2   0   0
 
Continuous-time state-space model.

%% Discretize the system
Ts     = 0.1;                    % Sampling time [s]
sys_d  = c2d(sys, Ts, 'zoh');    % Discretized system
Ad     = sys_d.a;
Bd     = sys_d.b;
Cd     = sys_d.c;

%% Design MPC Controller
N      = 20;                    % Prediction horizon
Nu     = 5;                     % Control horizon
Q      = diag([10, 1, 100, 1]); % State weighting matrix
R      = 0.1;                   % Control input weighting

% Create MPC controller object
sys_d  = setmpcsignals(sys_d, MV=[1], UD=[2]);
mpcobj = mpc(sys_d, Ts);

-->"PredictionHorizon" is empty. Assuming default 10.
-->"ControlHorizon" is empty. Assuming default 2.
-->"Weights.ManipulatedVariables" is empty. Assuming default 0.00000.
-->"Weights.ManipulatedVariablesRate" is empty. Assuming default 0.10000.
-->"Weights.OutputVariables" is empty. Assuming default 1.00000.
   for output(s) y1 and zero weight for output(s) y2 

mpcobj.PredictionHorizon                = N;
mpcobj.ControlHorizon                   = Nu;
mpcobj.Weights.OutputVariables          = [1000, 100];
mpcobj.Weights.ManipulatedVariables     = R;
mpcobj.Weights.ManipulatedVariablesRate = 0.1;

% Constraints
mpcobj.MV(1).Min = -10; % Force min [N]
mpcobj.MV(1).Max = 10; % Force max [N]

% Input Disturbance
mpcobj.Model.Disturbance = tf(1)    %  Laplace transform of Dirac delta function is 1

 
MPC object (created on 13-Oct-2023 16:36:45):
---------------------------------------------
Sampling time:      0.1 (seconds)
Prediction Horizon: 20
Control Horizon:    5

Plant Model:        
                                      --------------
      1  manipulated variable(s)   -->|  4 states  |
                                      |            |-->  2 measured output(s)
      0  measured disturbance(s)   -->|  2 inputs  |
                                      |            |-->  0 unmeasured output(s)
      1  unmeasured disturbance(s) -->|  2 outputs |
                                      --------------
Indices:
  (input vector)    Manipulated variables: [1 ]
                  Unmeasured disturbances: [2 ]
  (output vector)        Measured outputs: [1 2 ]

Disturbance and Noise Models:
        Output disturbance model: default (type "getoutdist(mpcobj)" for details)
         Input disturbance model: user specified (type "getindist(mpcobj)" for more details)
         Measurement noise model: default (unity gain after scaling)

Weights:
        ManipulatedVariables: 0.1000
    ManipulatedVariablesRate: 0.1000
             OutputVariables: [1000 100]
                         ECR: 100000

State Estimation:  Default Kalman Filter (type "getEstimator(mpcobj)" for details)

Constraints:
 -10 <= MV1 <= 10, MV1/rate is unconstrained, MO1 is unconstrained
                                              MO2 is unconstrained

Use built-in "active-set" QP solver with MaxIterations of 120.

indist = tf(getindist(mpcobj))

indist =
 
  From input "Noise#1" to output "UD1":
  1
 
Static gain.

%% Simulation
Nsim = 100; % Number of simulation steps
x0   = [0.2; 0; 0.1; 0]; % Initial state

% Preallocate arrays for simulation results
nx     = size(Ad, 1);
nu     = size(Bd, 2);
ny     = size(Cd, 1);
x      = zeros(nx, Nsim);
y      = zeros(ny, Nsim);
u      = zeros(nu, Nsim);
x(:,1) = x0;
y(:,1) = Cd*x0;

% Create initial MPC state
mpcstate = mpcstate(mpcobj, x(:,1));

   Assuming no disturbance added to measured output #1.
-->Assuming output disturbance added to measured output #2 is integrated white noise.
-->"Model.Noise" is empty. Assuming white noise on each measured output.

%% Main simulation loop
for k = 1:Nsim-1
    % Measure the current output
    ymeas = y(:,k);
    
    % Compute the control action using the MPC controller
    [u(:,k), mpc_info] = mpcmove(mpcobj, mpcstate, ymeas, []);
    
    % Apply the control action to the system and obtain the next state
    x(:,k+1) = Ad*x(:,k) + Bd*u(:,k);
    
    % Calculate the output
    y(:,k+1) = Cd*x(:,k+1);
    
    % Update MPC state
    mpcstate.Plant = x(:,k+1);
end

%% Plot the results
figure(1)

subplot(3,1,1);
t = (0:Nsim-1)*Ts;
plot(t, y(1,:), 'LineWidth', 2);
ylabel('Cart Position (m)');
title('Inverted Pendulum MPC Control');
grid on;

subplot(3,1,2);
plot(t, y(2,:), 'LineWidth', 2);
ylabel('Pendulum Angle (rad)');
grid on;

subplot(3,1,3);
plot(t, u(:,1:Nsim), 'LineWidth', 2); % Corrected dimensions of u
xlabel('Time (s)');
ylabel('Force (N)');
grid on;