example.m

% MPC Tracking example script
% by Marco Tallone
% Please see https://github.com/marcotallone/mpc-tracking for more information

% The following is a simple example hands-on guide on how to use the implemented
% MPC algorithm to perfom trajectory tracking on a non-linear system in 5 steps.


% 1. Step 1: Load the system parameters
% First of all we need to load the system, which must be a child class of 
% the abstract DynamicalSystem class. 
% In this case we will use the Unicycle class, which is a non-linear unicycle model.
% The following parameters are required to instantiate the model:
r  = 0.03;                          % wheel radius
L  = 0.3;                           % distance between wheels
Ts = 0.1;                           % sampling time
x_constraints = [
    -2 2;                           % x position constraints
    -2 2;                           % y position constraints
    -pi 3*pi;                       % heading angle constraints
];
u_constraints = [-50, 50];          % angular velocity constraints
states = 3;                         % number of states
outputs = 2;                        % number of outputs
Q_tilde = 0.75*1e-3*eye(states);    % Process noise covariance
R_tilde = 1e-2*eye(outputs);        % Measurement noise covariance
P0 = eye(states);                   % Initial state covariance

model = Unicycle(r, L, Ts, x_constraints, u_constraints, P0, Q_tilde, R_tilde);


% 2. Step 2: Generate a reference trajectory
% The reference trajectory is generated by the model itself, using the 
% built-in generate_trajectory method. As an example we are here going 
% to generate a circular trajectory of radius 0.5 and 100 reference points.
N_guide = 100;
radius = 0.5;
shape = "circle";
[x_ref, u_ref, Tend] = model.generate_trajectory(N_guide, shape, radius);


% 3. Step 3: Instantiate the MPC controller
% We then need to initialize an MPC controller object to perform the 
% simulation with the MPC algorithm of the non-linear system. We need 
% to decide an initial condition and some cost matrices here.

% Initial condition
x0 = zeros(model.n,1);              % origin initial state

% MPC parameters
N  = 10;                            % prediction horizon
Q  = 1e3*eye(model.n);              % state cost
R  = eye(model.m);                  % input cost
preview = 1;                        % MPC preview flag
formulation = 0;                    % MPC formulation flag
noise = 0;                          % MPC noise flag
debug = 0;                          % MPC debug flag

% Simulation time and steps
x_ref = [x_ref; x_ref(1:N+1, :)];   % add N steps to complete a full loop
u_ref = [u_ref; u_ref(1:N+1, :)];   % add N steps to complete a full loop
Tend = Tend + (N+1)*Ts;
t = 0:Ts:Tend;                      % vector of time steps
Nsteps = length(t) - (N+1);         % number of MPC optimization steps

mpc = MPC(model, x0, Tend, N, Q, R, x_ref, u_ref, preview, formulation, noise, debug);


% 4. Step 4: Optimization. Now we simulate for the chosen time and the MPC 
% will use the optimal input at each time step and compute the associated state vector.
[x, u] = mpc.optimize();


% 5. Step 5: Plot the results and see your system tracking the reference trajectory.
figure(1);

% Reference trajectory
ref_points = scatter(x_ref(:, 1), x_ref(:, 2), 5, 'filled', 'MarkerFaceColor', '#808080');
hold on;
arrow_length = 0.01;
for i = 1:length(x_ref)
    x_arrow = arrow_length * cos(x_ref(i, 3));
    y_arrow = arrow_length * sin(x_ref(i, 3));
    quiver(x_ref(i, 1), x_ref(i, 2), x_arrow, y_arrow, 'AutoScale', 'off', 'Color', '#808080');
end
legend(ref_points,{'Reference trajectory'}, 'Location', 'northwest');

% Labels
title('Trajectory Tracking with MPC (Non-Linear Unicycle System)');
xlabel('x'); ylabel('y');
grid on;
axis equal;
hold on;

% Set plot limits
xlim([-0.6, 0.6]);
ylim([-0.6, 0.6]);
hold on;

% ────────────────────
% Wait for figure here
pause(1);

% Real trajectory
for i = 1:Nsteps
    x_line = plot(x(1:i, 1), x(1:i, 2), 'blue', 'LineWidth', 1);
    x_line.Color(4) = 0.5; % line transparency 50%
    hold on;
    x_points = scatter(x(1:i, 1), x(1:i, 2), 5, 'blue', 'filled');
    hold on;
    quiver(x(1:i, 1), x(1:i, 2), arrow_length * cos(x(1:i, 3)), arrow_length * sin(x(1:i, 3)), 'AutoScale', 'off', 'Color', 'blue');
    legend([ref_points, x_points],{'Reference trajectory', 'Real trajectory'}, 'Location', 'northwest');
    hold on;

    pause(0.05);
    if i < Nsteps
        delete(x_line);
    end
end