SMR LAB Matlab Code with explanation comments

% Clear workspace, command window, and close all figures
clear; clc; close all;

% Define time parameters
dt = 0.1; % Time step
ts = 10;  % Total simulation time
t = 0:dt:ts; % Time vector

% Initial robot state
x = 0;    % Initial x-position
y = 0;    % Initial y-position
psi = 0;  % Initial orientation angle
eta = [x; y; psi]; % State vector [x, y, psi]

% Robot dimensions
a = 0.1; % Wheel radius
d = 0.2; % Distance from the center to the wheel
l = 0.5; % Length between wheels

% Define the relative positions of robot corners
mr_co = [-l/2, l/2, l/2, -l/2;
         -d,    -d,   d,   d];
% MR_CO FOR OMNI WHEEL NOT NEEDED AS PER SIR
%mr_co = [l, (-sqrt((l^2)-((w/2)^2))),(-sqrt((l^2)-((w/2)^2)));
%        0,                      w/2,                   -w/2];

%For Kinematics use u v r
%u = 0.4; v = 0; r = 0.2;
%zeta=[u;v;r];

% For Kinematics of DIFFERENTIAL bot use this W and omega
%omega = [0.4;0.4];
%W = [a/2,a/2;            0,0;       -a/(2*d), a/(2*d)];
%zeta=(W * omega);

% For Kinematics of OMNI bot use this W and omega
%omega = [0; 0.2; -0.2;]; % angular velocities of three wheels
%W = a/3*[0,-sqrt(3),sqrt(3);     2,-1,-1;      1/l,1/l,1/l]
%zeta=(W * omega);

% For Kinematics of MECHANUM bot use this W and omega
%omega = [0.5;0.5;0.5;0.5]; % angular velocities of four wheels
%W = a/4*[1,1,1,1;       1,-1,1,-1;       -1/(d-l),-1/(d-l),1/(d-l),1/(d-l)];
%zeta=(W * omega);

% Main simulation loop
for i = 1:length(t)
    % Calculate the Jacobian matrix
    Jacobian = [cos(eta(3,i)), -sin(eta(3,i)), 0;
             sin(eta(3,i)), cos(eta(3,i)),  0;
             0,              0,             1];

    % Compute the time derivative of the robot state
    eta_dot = Jacobian * zeta;

    % Update the robot state using Euler integration
    eta(:,i+1) = eta(:,i) + dt * eta_dot;
end

% Animate the robot's trajectory and orientation
figure
for i = 1:length(t)
    % Calculate the positions of robot corners in global coordinates
    pos = [cos(eta(3,i)), -sin(eta(3,i));
           sin(eta(3,i)),  cos(eta(3,i))] * mr_co;

    % Plot the robot
    fill(pos(1,:)+eta(1,i), pos(2,:)+eta(2,i), 'o')
    hold on, grid on, axis equal;
    plot(eta(1,1:i), eta(2,1:i), 'b-');
    axis([-1 5 -1 5]);
    xlabel('x[m]'); ylabel('y[m]');
    legend('MR','Path'), set(gca,'fontsize',24)
    pause(0.001);
    hold off;
end