Questions about the PID controller of an inverted pendulum

I need help with this problem I am trying to solve. I have implemented two codes, one for a PD controller and another for a PID controller. The first one works correctly without any issues, but the second one gives me a dimension error. I understand that it is due to the difference in sizes of K.A, K.B, K.C, and K.D. I can't find a way to solve the problem, and I would appreciate it if you could help me. I am sharing my code:

Code for PD close-loop controller:

%% Define the process model
% Define the constants values
M = 0.5;  % Mass
g = 9.81; % Acceleration due to gravity
f = 0.25; % Friction coefficient
l = 1;    % Length
% Define the operation parameters, force, and torque.
pos_oper = 0;      % Operating position
torque_oper = 0;   % Operating torque
% Define the structure model for process part
Model.Process.StateEq = @(t,theta,T) [theta(2); g*sin(theta(1)) - f/M*theta(2)+T/M];
Model.Process.OutputEq = @(t,theta,T) theta(1);
Process_Order = 2;
%% Linear PD controller
% Hand-tuned PD controller (with derivative noise filter):
s = tf('s');
Kp = 9.71;      % Proportional gain
Kd = 3.03;    % Derivative gain
filter = 20; % Derivative noise filter
F_transfer = Kp+Kd*s/(s/filter + 1);
K = ss(F_transfer);
Control_Order = size(K.A,1);
%% Define the control model
% Define the model - control - stateEq
Model.Control.StateEq = @(t,theta,y) K.A*theta+K.B*(-(y-pos_oper));
T_limit_sup = 3.5;  % Upper torque limit
T_limit_inf = -3.5; % Lower torque limit
saturate=@(T) max(min(T,T_limit_sup),T_limit_inf);
% Define the model-control-output equation
Model.Control.OutputEq = @(t,theta,y) saturate(torque_oper + K.C*theta + K.D*(-(y-pos_oper)));
%% Numerical simulation
Idx_process=1:Process_Order;
Idx_control = Process_Order + (1:Control_Order);
odefun = @(t,FullState) CL_StateEQ(t,FullState(Idx_process),FullState(Idx_control),Model);
Ini_state_process = [pi/30;0]; % Initial state for the process
Ini_state_control = 0;         % Initial state for the control
x0 = [Ini_state_process;Ini_state_control];
t_fin = 6; % Final time for simulation
CPU_ode45 =cputime;
tic;
[T_ode45,X_ode45] = ode45(odefun, [0 t_fin], x0, odeset('RelTol',1e-6,'AbsTol',1e-6));

Unrecognized function or variable 'CL_StateEQ'.

Error in solution>@(t,FullState)CL_StateEQ(t,FullState(Idx_process),FullState(Idx_control),Model) (line 34)
odefun = @(t,FullState) CL_StateEQ(t,FullState(Idx_process),FullState(Idx_control),Model);

Error in odearguments (line 92)
f0 = ode(t0,y0,args{:}); % ODE15I sets args{1} to yp0.

Error in ode45 (line 104)
odearguments(odeIsFuncHandle,odeTreatAsMFile, solver_name, ode, tspan, y0, options, varargin);

ejecution_time_ode45 = toc;
CPU_ode45 = cputime - CPU_ode45;

Wrong code for PID controller:

%% Define the process model
% Define the constants values
M = 0.5;  % Mass
g = 9.81; % Acceleration due to gravity
f = 0.25; % Friction coefficient
l = 1;    % Length
% Define the operation parameters, force, and torque.
pos_oper = 0;      % Operating position
torque_oper = 0;   % Operating torque
% Define the structure model for process part
Model.Process.StateEq = @(t,theta,T) [theta(2); g*sin(theta(1)) - f/M*theta(2)+T/M];
Model.Process.OutputEq = @(t,theta,T) theta(1);
Process_Order = 2;
%% Linear PD controller
% Hand-tuned PD controller (with derivative noise filter):
s = tf('s');
Kp = 7;      % Proportional gain
Ki = 0.01;
Kd = 1.3;    % Derivative gain
filter = 20; % Derivative noise filter
F_transfer = Kp+Ki/s+Kd*s/(s/filter + 1);
K = ss(F_transfer);
Control_Order = size(K.A,1);
%% Define the control model
% Define the model - control - stateEq
Model.Control.StateEq = @(t,theta,y) K.A*theta+K.B*(-(y-pos_oper));
T_limit_sup = 3.5;  % Upper torque limit
T_limit_inf = -3.5; % Lower torque limit
saturate=@(T) max(min(T,T_limit_sup),T_limit_inf);
% Define the model-control-output equation
Model.Control.OutputEq = @(t,theta,y) saturate(torque_oper + K.C*theta + K.D*(-(y-pos_oper)));
%% Numerical simulation
Idx_process=1:Process_Order;
Idx_control = Process_Order + (1:Control_Order);
odefun = @(t, FullState) [
    Model.Process.StateEq(t, FullState(Idx_process), Model.Control.OutputEq(t, FullState(Idx_control), Model.Process.OutputEq(t, FullState(Idx_process), 0)));
    Model.Control.StateEq(t, FullState(Idx_control), Model.Process.OutputEq(t, FullState(Idx_process), 0), Model.Process.OutputEq)
];
Ini_state_process = [pi/30;0]; % Initial state for the process
Ini_state_control = 0;         % Initial state for the control
x0 = [Ini_state_process;Ini_state_control];
t_fin = 6; % Final time for simulation
CPU_ode45 =cputime;
tic;
[T_ode45,X_ode45] = ode45(odefun, [0 t_fin], x0, odeset('RelTol',1e-6,'AbsTol',1e-6));
ejecution_time_ode45 = toc;
CPU_ode45 = cputime - CPU_ode45;
%%
odefun = @(t,FullState) CL_StateEQ(t,FullState(Idx_process),FullState(Idx_control(2,1)),Model);
Ini_state_process = [pi/30;0]; % Initial state for the process
Ini_state_control = 0;         % Initial state for the control
x0 = [Ini_state_process;Ini_state_control];
t_fin = 6; % Final time for simulation
CPU_ode45 =cputime;
tic;
[T_ode45,X_ode45] = ode45(odefun, [0 t_fin], x0, odeset('RelTol',1e-6,'AbsTol',1e-6));
ejecution_time_ode45 = toc;
CPU_ode45 = cputime - CPU_ode45;

[Jon]

I tried to run the code you attached to see if I could reproduce the problem and help you debug it, but I do not have the function CL_StateEQ. Can you please attach this, and any other code needed to provide a self contained example that reproduces the problem.

[Carmen Serrano Portillo]

Hi, Is true, sorry I added it here. Thanks you so much!

function dxdtbc=CL_StateEQ(t,ProcessState,ControlState, M)

% That indicates de close-loop

Measurements=M.Process.OutputEq(t,ProcessState);

dControllerStatedt=M.Control.StateEq(t,ControlState,Measurements);

u=M.Control.OutputEq(t,ControlState,Measurements);

dSProcesStatedt=M.Process.StateEq(t,ProcessState,u);

dxdtbc=[dSProcesStatedt;dControllerStatedt];

end

Answer 1

[Sam Chak]

Hi @Carmen Serrano Portillo

Could you please check if the current response of the inverted pendulum under the PID controller aligns with the expected result? In the code, I have marked the lines where I made changes with '<--'.

%% Define the process model
% Define the constants values
M = 0.5;  % Mass
g = 9.81; % Acceleration due to gravity
f = 0.25; % Friction coefficient
l = 1;    % Length
% Define the operation parameters, force, and torque.
pos_oper = 0;      % Operating position
torque_oper = 0;   % Operating torque
% Define the structure model for process part
Model.Process.StateEq = @(t,theta,T) [theta(2); g*sin(theta(1)) - f/M*theta(2)+T/M];
Model.Process.OutputEq = @(t,theta,T) theta(1);
Process_Order = 2;

%% Linear PID controller <--
% Hand-tuned PID controller (with derivative noise filter): <--
s = tf('s');
Kp = 7;      % Proportional gain
Ki = 0.01;
Kd = 1.3;    % Derivative gain
filter = 20; % Derivative noise filter
F_transfer = Kp+Ki/s+Kd*s/(s/filter + 1);
K = ss(F_transfer);
Control_Order = size(K.A,1);

%% Define the control model
% Define the model - control - stateEq
Model.Control.StateEq = @(t,theta,y) K.A*theta+K.B*(-(y-pos_oper));
T_limit_sup = 3.5;  % Upper torque limit
T_limit_inf = -3.5; % Lower torque limit
saturate=@(T) max(min(T,T_limit_sup),T_limit_inf);
% Define the model-control-output equation
Model.Control.OutputEq = @(t,theta,y) saturate(torque_oper + K.C*theta + K.D*(-(y-pos_oper)));
%% Numerical simulation
Idx_process=1:Process_Order;
Idx_control = Process_Order + (1:Control_Order);

% odefun = @(t, FullState) [
%     Model.Process.StateEq(t, FullState(Idx_process), Model.Control.OutputEq(t, FullState(Idx_control), Model.Process.OutputEq(t, FullState(Idx_process), 0)));
%     Model.Control.StateEq(t, FullState(Idx_control), Model.Process.OutputEq(t, FullState(Idx_process), 0), Model.Process.OutputEq)
% ];
% Ini_state_process = [pi/30;0]; % Initial state for the process
% Ini_state_control = [0; 0];         % Initial state for the control
% x0 = [Ini_state_process;Ini_state_control];
% t_fin = 6; % Final time for simulation
% CPU_ode45 =cputime;
% tic;
% [T_ode45,X_ode45] = ode45(odefun, [0 t_fin], x0, odeset('RelTol',1e-6,'AbsTol',1e-6));
% ejecution_time_ode45 = toc;
% CPU_ode45 = cputime - CPU_ode45;

%%

odefun = @(t,FullState) CL_StateEQ(t, FullState(Idx_process), FullState(Idx_control), Model); % <--
Ini_state_process = [pi/30;0];      % Initial state for the process
Ini_state_control = [0; 0];         % TWO Initial state for the control <--
x0                = [Ini_state_process;Ini_state_control];
t_fin             = 6;              % Final time for simulation
CPU_ode45         = cputime;
tic;
[T_ode45,X_ode45] = ode45(odefun, [0 t_fin], x0, odeset('RelTol',1e-6,'AbsTol',1e-6));
ejecution_time_ode45 = toc;
CPU_ode45 = cputime - CPU_ode45;

plot(T_ode45, rad2deg(X_ode45(:,1))), grid on   % <-- added to display result

function dxdtbc = CL_StateEQ(t, ProcessState, ControlState, M)
    % That indicates de close-loop
    Measurements        = M.Process.OutputEq(t, ProcessState); 
    dControllerStatedt  = M.Control.StateEq(t, ControlState, Measurements);
    u                   = M.Control.OutputEq(t, ControlState, Measurements);
    dSProcesStatedt     = M.Process.StateEq(t, ProcessState, u);
    dxdtbc              = [dSProcesStatedt; dControllerStatedt];
end

[Carmen Serrano Portillo]

Oh my God, yes, thank you very much! I am very grateful for the help. Best regards