iterative linear quadratic regulator for a cart pole system in matlab

main.m
% Define the system parameters
m_c = 1;    % Mass of the cart
m_p = 0.1;  % Mass of the pole
g = 9.81;   % Gravitational acceleration
l = 0.5;    % Length of the pole
d = 0.1;    % Damping coefficient

% Define the state-space model
A = [0 1 0 0;
     0 -d/m_c -m_p*g/m_c 0;
     0 0 0 1;
     0 -l*d/(m_c*l^2) -l*(m_c+m_p)*g/(m_c*l^2) 0];
B = [0; 1/m_c; 0; l/(m_c*l^2)];
C = [1 0 0 0; 0 0 1 0];
D = [0; 0];

% Define the cost function
Q = diag([10 0.1 100 0.1]); % State weights
R = 0.1;                   % Input weight

% Iteratively solve the LQR problem
K = zeros(1,4); % Start with zero gains
iter_max = 100; % Maximum number of iterations
tol = 1e-6;     % Tolerance for convergence
for i = 1:iter_max
    [K_new,~,~] = lqr(A,B,Q,R); % Solve the LQR problem
    if max(abs(K_new - K)) < tol % Check for convergence
        break;
    else
        K = K_new;
    end
end

% Simulate the system with the LQR controller
t = 0:0.01:5;
x0 = [0 0 pi/180 0]; % Initial state
u = -K*x0';          % Compute the control input
[t,x] = ode45(@(t,x) cart_pole(t,x,u,m_c,m_p,g,l,d),t,x0); % Simulate the system with the control input

% Plot the results
figure;
subplot(211); plot(t,x(:,1)); ylabel('Cart Position (m)'); grid on;
subplot(212); plot(t,x(:,3)*180/pi); ylabel('Pole Angle (deg)'); xlabel('Time (s)'); grid on;

% Define the nonlinear dynamics of the cart-pole system
function [dxdt] = cart_pole(t,x,u,m_c,m_p,g,l,d)
    s = sin(x(3));
    c = cos(x(3));
    dxdt = zeros(4,1);
    dxdt(1) = x(2);
    dxdt(2) = (u + m_p*l*(x(4)^2)*s - (m_c+m_p)*g*s - d*x(2))/(m_c + m_p*(1-c^2));
    dxdt(3) = x(4);
    dxdt(4) = ((m_c+m_p)*g*s - c*(u + m_p*l*(x(4)^2)*s - (m_c+m_p)*g*s - d*x(2)))/(l*(m_c + m_p*(1-c^2)));
end
1719 chars
55 lines