├── report.pdf ├── sat.m ├── system_state1.m ├── system_state2.m ├── README.md ├── regulation.m └── tracking.m /report.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kpetridis24/sliding-mode-control/HEAD/report.pdf -------------------------------------------------------------------------------- /sat.m: -------------------------------------------------------------------------------- 1 | function res = sat(x, e) 2 | if x > -e && x < e 3 | res = x / e; 4 | elseif x <= e 5 | res = -1; 6 | else 7 | res = 1; 8 | end 9 | end 10 | -------------------------------------------------------------------------------- /system_state1.m: -------------------------------------------------------------------------------- 1 | function ret = system_state1(t, x, u, H, C, g, qd, s) 2 | x1 = [x(1); x(3)]; 3 | x2 = [x(2); x(4)]; 4 | 5 | s_value = s(x1, x2, qd, [0; 0]); 6 | 7 | dx = zeros(2, 2); 8 | 9 | dx(1, :) = x2'; 10 | dx(2, :) = (H(x1(1), x1(2)) \ u(x1, x2, qd, [0; 0], [0; 0], s_value) - ... 11 | H(x1(1), x1(2)) \ C(x1(1), x1(2), x2(1), x2(2)) * x2 - ... 12 | H(x1(1), x1(2)) \ g(x1(1), x1(2)))'; 13 | 14 | ret = [dx(1, 1); dx(2, 1); dx(1, 2); dx(2, 2)]; 15 | end -------------------------------------------------------------------------------- /system_state2.m: -------------------------------------------------------------------------------- 1 | function ret = system_state2(t, x, u, H, C, g, qd, qd_dot, qd_ddot, s) 2 | x1 = [x(1); x(3)]; 3 | x2 = [x(2); x(4)]; 4 | 5 | s_value = s(x1, x2, qd(t), qd_dot(t)); 6 | 7 | dx = zeros(2, 2); 8 | 9 | dx(1, :) = x2'; 10 | dx(2, :) = (H(x1(1), x1(2)) \ u(x1, x2, qd(t), qd_dot(t), qd_ddot(t), s_value) - ... 11 | H(x1(1), x1(2)) \ C(x1(1), x1(2), x2(1), x2(2)) * x2 - ... 12 | H(x1(1), x1(2)) \ g(x1(1), x1(2)))'; 13 | 14 | ret = [dx(1, 1); dx(2, 1); dx(1, 2); dx(2, 2)]; 15 | end -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # sliding-mode-control 2 | Implementation of Sliding Mode Control and testing on both regulation and tracking problems 3 | 4 | This is a MATLAB implementation of the [Sliding Mode Control](https://en.wikipedia.org/wiki/Sliding_mode_control) method for controlling dynamic systems, 5 | which include uncertainties. This means that we know the ODE of the control system, but we don't acquire any knowledge about the exact values of the system's parameters. 6 | For the exhibition of the method, i simulated the dynamic system of a robotic arm, with 2 freedom degrees, and designed a controller following the Sliding Mode Control 7 | method. The controller is tested on both a regulation and a tracking problem. Regulation refers to us, dictating that the system's output is going to converge into a specific point and remain there. Tracking is the problem where, we desire to make the system track a specific orbit with high accuracy, after some time t. 8 | -------------------------------------------------------------------------------- /regulation.m: -------------------------------------------------------------------------------- 1 | clear; 2 | format shortG; 3 | 4 | %% parameters 5 | 6 | % constant parameters 7 | m1 = 6; 8 | m2 = 4; 9 | l1 = 0.5; 10 | l2 = 0.4; 11 | grav = 9.81; 12 | 13 | % system simulation parameters 14 | lc1 = 0.2; 15 | lc2 = 0.1; 16 | I1 = 0.43; 17 | I2 = 0.05; 18 | ml = 0.5; 19 | 20 | % max estimation error of parameters 21 | lc1_error = 0.3; 22 | lc2_error = 0.25; 23 | I1_error = 0.48; 24 | I2_error = 0.14; 25 | ml_error = 2; 26 | 27 | %% system matrices 28 | 29 | % H matrix: inertia matrix of the robot 30 | h11 = @(q1, q2) m1 * lc1^2 + m2 * (lc2 ^ 2 + l1^2 + 2 * l1 * l2 * cos(q2)) + ml * (l2^2 + l1^2 +2 * l1 * l2 * cos(q2)) + I1 + I2; 31 | h12 = @(q1, q2) m2 * lc2 * (lc2 + l1 * cos(q2)) + ml * l2 * (l2 + l1 * cos(q2)) + I2; 32 | h22 = @(q1, q2) lc2^2 * m2 + l2^2 * ml + I2; 33 | 34 | H = @(q1, q2) [h11(q1, q2) h12(q1, q2); h12(q1, q2) h22(q1, q2)]; 35 | 36 | % C matrix: centripetal & Coriolis forces 37 | c11 = @(q1, q2, q1_dot, q2_dot) -l1 * (m2 * lc2 + ml * l2) * sin(q2) * q2_dot; 38 | c12 = @(q1, q2, q1_dot, q2_dot) -l1 * (m2 * lc2 + ml * l2) * sin(q2) * (q2_dot + q1_dot); 39 | c21 = @(q1, q2, q1_dot, q2_dot) l1 * (m2 * lc2 + ml * l2) * sin(q2) * q1_dot; 40 | c22 = @(q1, q2, q1_dot, q2_dot) 0; 41 | 42 | C = @(q1, q2, q1_dot, q2_dot) [c11(q1, q2, q1_dot, q2_dot) c12(q1, q2, q1_dot, q2_dot); c21(q1, q2, q1_dot, q2_dot) c22(q1, q2, q1_dot, q2_dot)]; 43 | 44 | % g array: gravitational forces 45 | g1 = @(q1, q2) (m2 * lc2 + ml * l2) * grav * cos(q1 + q2) + (m2 * l1 + ml * l1 + m1 * lc1) * grav * cos(q1); 46 | g2 = @(q1, q2) (m2 * lc2 + ml * l2) * grav * cos(q1 + q2); 47 | 48 | g = @(q1, q2) [g1(q1, q2); g2(q1, q2)]; 49 | 50 | %% controller matrices 51 | 52 | % H maximum error 53 | h11_error = @(q1, q2) m1 * lc1_error + m2 * (lc2_error + 2 * l1 * cos(q2) * lc2_error) + ... 54 | ml_error * (l1^2 + l2^2 + 2 * l1 * l2 * cos(q2)) + I1_error + I2_error; 55 | h12_error = @(q1, q2) m2 * (lc2_error + l1 * cos(q2) * lc2_error) + ... 56 | l2 * ml_error * (l2 + l1 * cos(q2)) + I2_error; 57 | h22_error = @(q1, q2) ... 58 | m2 * lc2_error + l2^2 * ml_error + I2_error; 59 | 60 | e1_max = @(q1, q2) ... 61 | [h11_error(q1, q2) h12_error(q1, q2); ... 62 | h12_error(q1, q2) h22_error(q1, q2)]; 63 | 64 | % C maximum error 65 | c11 = @(q1, q2, q1_dot, q2_dot) ... 66 | -q2_dot; 67 | 68 | c12 = @(q1, q2, q1_dot, q2_dot) ... 69 | -(q2_dot + q1_dot); 70 | 71 | c21 = @(q1, q2, q1_dot, q2_dot) ... 72 | q1_dot; 73 | 74 | e2_max = @(q1, q2, q1_dot, q2_dot) ... 75 | (m2 * lc2_error + ml_error * l2) * (l1 * sin(q2)) * ... 76 | [c11(q1, q2, q1_dot, q2_dot) c12(q1, q2, q1_dot, q2_dot); ... 77 | c21(q1, q2, q1_dot, q2_dot) 0]; 78 | 79 | % g maximum error 80 | g1_error = @(q1, q2) ... 81 | (m2 * lc2_error + ml_error * l2) * grav * cos(q1 + q2) + ... 82 | (ml_error * l1 + m1 * lc1_error) * grav * cos(q1); 83 | 84 | g2_error = @(q1, q2) ... 85 | (m2 * lc2_error + ml_error * l2) * grav * cos(q1 + q2); 86 | 87 | e3_max = @(q1, q2) [g1_error(q1, q2); 88 | g2_error(q1, q2)]; 89 | 90 | % e, e_dot (e = q - qd) 91 | e = @(q, qd) q - qd; 92 | e_dot = @(q_dot, qd_dot) q_dot - qd_dot; 93 | 94 | % r 95 | L = [5, 0; 0, 5]; 96 | C_const = 0.5; 97 | r = @(q, q_dot, qd, qd_dot, qd_ddot) ... 98 | e1_max(q(1), q(2)) * abs(qd_ddot - L * e_dot(q_dot, qd_dot)) + ... 99 | e2_max(q(1), q(2), q_dot(1), q_dot(2)) * abs(q_dot) + e3_max(q(1), q(2)) + C_const; 100 | 101 | % parameter estimations 102 | lc1_hat = 0.25; 103 | lc2_hat = 0.15; 104 | I1_hat = 0.2; 105 | I2_hat = 0.1; 106 | ml_hat = 1; 107 | 108 | % C approximation 109 | c11_hat = @(q1, q2, q1_dot, q2_dot) -l1 * (m2 * lc2_hat + ml_hat * l2) * sin(q2) * q2_dot; 110 | c12_hat = @(q1, q2, q1_dot, q2_dot) -l1 * (m2 * lc2_hat + ml_hat * l2) * sin(q2) * (q2_dot + q1_dot); 111 | c21_hat = @(q1, q2, q1_dot, q2_dot) l1 * (m2 * lc2_hat + ml_hat * l2) * sin(q2) * q1_dot; 112 | c22_hat = @(q1, q2, q1_dot, q2_dot) 0; 113 | 114 | C_hat = @(q, q_dot) ... 115 | [c11_hat(q(1), q(2), q_dot(1), q_dot(2)) c12_hat(q(1), q(2), q_dot(1), q_dot(2)); ... 116 | c21_hat(q(1), q(2), q_dot(1), q_dot(2)) c22_hat(q(1), q(2), q_dot(1), q_dot(2))]; 117 | 118 | % H approximation 119 | h11_hat = @(q1, q2) m1 * lc1_hat^2 + m2 * (lc2_hat ^ 2 + l1^2 + 2 * l1 * l2 * cos(q2)) + ml_hat * (l2^2 + l1^2 +2 * l1 * l2 * cos(q2)) + I1_hat + I2_hat; 120 | h12_hat = @(q1, q2) m2 * lc2_hat * (lc2_hat + l1 * cos(q2)) + ml_hat * l2 * (l2 + l1 * cos(q2)) + I2_hat; 121 | h22_hat = @(q1, q2) lc2_hat^2 * m2 + l2^2 * ml_hat + I2_hat; 122 | 123 | H_hat = @(q) ... 124 | [h11_hat(q(1), q(2)) h12_hat(q(1), q(2)); ... 125 | h12_hat(q(1), q(2)) h22_hat(q(1), q(2))]; 126 | 127 | % g approximation 128 | g1_hat = @(q1, q2) (m2 * lc2_hat + ml_hat * l2) * grav * cos(q1 + q2) + ... 129 | (m2 * l1 + ml_hat * l1 + m1 * lc1_hat) * grav * cos(q1); 130 | g2_hat = @(q1, q2) (m2 * lc2_hat + ml_hat * l2) * grav * cos(q1 + q2); 131 | 132 | g_hat = @(q) [g1_hat(q(1), q(2)); g2_hat(q(1), q(2))]; 133 | 134 | % sliding surface of the system 135 | s = @(q, q_dot, qd, qd_dot) ... 136 | e_dot(q_dot, qd_dot) + L * e(q, qd); 137 | 138 | %% controller design 139 | 140 | x0 = [pi/3 pi/3; 0 0]; 141 | t_range = [0, 10]; 142 | epsilon = 0.1; 143 | 144 | u = @(q, q_dot, qd, qd_dot, qd_ddot, s_cur) ... 145 | C_hat(q, q_dot) * q_dot + g_hat(q) + H_hat(q) * qd_ddot - ... 146 | H_hat(q) * L * e_dot(q_dot, qd_dot) - ... 147 | r(q, q_dot, qd, qd_dot, qd_ddot) .* ... 148 | [sat(s_cur(1), epsilon); sat(s_cur(2), epsilon)]; 149 | 150 | %% calculate states of the system for different times 151 | 152 | qd = [pi/2; -pi/3]; 153 | [t, x] = ode45(@(t, x) system_state1(t, x, u, H, C, g, qd, s), t_range, x0); 154 | 155 | %% plots 156 | 157 | q1 = x(:, 1); 158 | q2 = x(:, 3); 159 | q1_dot = x(:, 2); 160 | q2_dot = x(:, 4); 161 | 162 | qd1 = pi/2; 163 | qd2 = -pi/3; 164 | 165 | % system state variables plot 166 | figure(1); 167 | plot(t, q1, 'linewidth', 0.8); 168 | hold on; 169 | plot(t, q2, 'linewidth', 0.8); 170 | hold on; 171 | fplot(qd1, [0 10], '--', 'linewidth', 0.8); 172 | hold on; 173 | fplot(qd2, [0 10], '--', 'linewidth', 0.8); 174 | 175 | legend('q_1', 'q_2', 'q_{d1}', 'q_{d2}'); 176 | xlabel('t (sec)') 177 | ylabel('rad') 178 | 179 | % deviation of q1 from qd, relative to time 180 | e1 = zeros(size(t)); 181 | e2 = zeros(size(t)); 182 | 183 | for i = 1: length(t) 184 | e1(i) = abs(q1(i) - qd1); 185 | e2(i) = abs(q2(i) - qd2); 186 | end 187 | 188 | figure(2); 189 | plot(t, e1, 'linewidth', 0.8); 190 | 191 | legend('e_1'); 192 | xlabel('t (sec)') 193 | ylabel('rad') 194 | 195 | % deviation of q2 from qd, relative to time 196 | figure(3) 197 | plot(t, e2, 'linewidth', 0.8); 198 | 199 | legend('e_2'); 200 | xlabel('t (sec)') 201 | ylabel('rad') 202 | 203 | % phase plane plot 204 | e1 = e([q1 q2], [qd1 qd2]); 205 | e1 = e1(:, 1); 206 | 207 | e1_dot = e_dot([q1_dot q2_dot], [0 0]); 208 | e1_dot = e1_dot(:, 1); 209 | 210 | e2 = e([q1 q2], [qd1 qd2]); 211 | e2 = e2(:, 2); 212 | 213 | e2_dot = e_dot([q1_dot q2_dot], [0 0]); 214 | e2_dot = e2_dot(:, 2); 215 | 216 | figure(4); 217 | sliding_surface = @(x) -L(1,1) * x; 218 | fplot(sliding_surface, '--', 'linewidth', 0.8); 219 | hold on; 220 | plot(e1, e1_dot, 'linewidth', 0.8); 221 | hold on; 222 | plot(e2, e2_dot, 'linewidth', 0.8); 223 | 224 | hl = legend('sliding surface', '$\dot{e_1}$', '$\dot{e_2}$'); 225 | set(hl, 'Interpreter', 'latex'); 226 | 227 | % controller values plot 228 | figure(5); 229 | u_values = zeros(length(t), 2); 230 | for i = 1:size(t) 231 | s_value = s([q1(i); q2(i)], [q1_dot(i); q2_dot(i)], [qd1; qd2], [0; 0]); 232 | u_values(i, :) = u([q1(i); q2(i)], [q1_dot(i); q2_dot(i)], [qd1; qd2], [0; 0], [0; 0], s_value); 233 | end 234 | 235 | plot(t, u_values, 'linewidth', 0.8); 236 | legend('u_1', 'u_2'); 237 | xlabel('t (sec)') 238 | ylabel('exerted force') 239 | 240 | -------------------------------------------------------------------------------- /tracking.m: -------------------------------------------------------------------------------- 1 | clear; 2 | format shortG; 3 | 4 | %% parameters 5 | 6 | % constant parameters 7 | m1 = 6; 8 | m2 = 4; 9 | l1 = 0.5; 10 | l2 = 0.4; 11 | grav = 9.81; 12 | 13 | % system simulation parameters 14 | lc1 = 0.2; 15 | lc2 = 0.1; 16 | I1 = 0.43; 17 | I2 = 0.05; 18 | ml = 0.5; 19 | 20 | % max estimation error of parameters 21 | lc1_error = 0.3; 22 | lc2_error = 0.25; 23 | I1_error = 0.48; 24 | I2_error = 0.14; 25 | ml_error = 2; 26 | 27 | %% system matrices 28 | 29 | % H matrix: inertia matrix of the robot 30 | h11 = @(q1, q2) m1 * lc1^2 + m2 * (lc2 ^ 2 + l1^2 + 2 * l1 * l2 * cos(q2)) + ml * (l2^2 + l1^2 +2 * l1 * l2 * cos(q2)) + I1 + I2; 31 | h12 = @(q1, q2) m2 * lc2 * (lc2 + l1 * cos(q2)) + ml * l2 * (l2 + l1 * cos(q2)) + I2; 32 | h22 = @(q1, q2) lc2^2 * m2 + l2^2 * ml + I2; 33 | 34 | H = @(q1, q2) [h11(q1, q2) h12(q1, q2); h12(q1, q2) h22(q1, q2)]; 35 | 36 | % C matrix: centripetal & Coriolis forces 37 | c11 = @(q1, q2, q1_dot, q2_dot) -l1 * (m2 * lc2 + ml * l2) * sin(q2) * q2_dot; 38 | c12 = @(q1, q2, q1_dot, q2_dot) -l1 * (m2 * lc2 + ml * l2) * sin(q2) * (q2_dot + q1_dot); 39 | c21 = @(q1, q2, q1_dot, q2_dot) l1 * (m2 * lc2 + ml * l2) * sin(q2) * q1_dot; 40 | c22 = @(q1, q2, q1_dot, q2_dot) 0; 41 | 42 | C = @(q1, q2, q1_dot, q2_dot) [c11(q1, q2, q1_dot, q2_dot) c12(q1, q2, q1_dot, q2_dot); c21(q1, q2, q1_dot, q2_dot) c22(q1, q2, q1_dot, q2_dot)]; 43 | 44 | % g array: gravitational forces 45 | g1 = @(q1, q2) (m2 * lc2 + ml * l2) * grav * cos(q1 + q2) + (m2 * l1 + ml * l1 + m1 * lc1) * grav * cos(q1); 46 | g2 = @(q1, q2) (m2 * lc2 + ml * l2) * grav * cos(q1 + q2); 47 | 48 | g = @(q1, q2) [g1(q1, q2); g2(q1, q2)]; 49 | 50 | %% controller matrices 51 | 52 | % H maximum error 53 | h11_error = @(q1, q2) m1 * lc1_error + m2 * (lc2_error + 2 * l1 * cos(q2) * lc2_error) + ... 54 | ml_error * (l1^2 + l2^2 + 2 * l1 * l2 * cos(q2)) + I1_error + I2_error; 55 | h12_error = @(q1, q2) m2 * (lc2_error + l1 * cos(q2) * lc2_error) + ... 56 | l2 * ml_error * (l2 + l1 * cos(q2)) + I2_error; 57 | h22_error = @(q1, q2) ... 58 | m2 * lc2_error + l2^2 * ml_error + I2_error; 59 | 60 | e1_max = @(q1, q2) ... 61 | [h11_error(q1, q2) h12_error(q1, q2); ... 62 | h12_error(q1, q2) h22_error(q1, q2)]; 63 | 64 | % C maximum error 65 | c11 = @(q1, q2, q1_dot, q2_dot) ... 66 | -q2_dot; 67 | 68 | c12 = @(q1, q2, q1_dot, q2_dot) ... 69 | -(q2_dot + q1_dot); 70 | 71 | c21 = @(q1, q2, q1_dot, q2_dot) ... 72 | q1_dot; 73 | 74 | e2_max = @(q1, q2, q1_dot, q2_dot) ... 75 | (m2 * lc2_error + ml_error * l2) * (l1 * sin(q2)) * ... 76 | [c11(q1, q2, q1_dot, q2_dot) c12(q1, q2, q1_dot, q2_dot); ... 77 | c21(q1, q2, q1_dot, q2_dot) 0]; 78 | 79 | % g maximum error 80 | g1_error = @(q1, q2) ... 81 | (m2 * lc2_error + ml_error * l2) * grav * cos(q1 + q2) + ... 82 | (ml_error * l1 + m1 * lc1_error) * grav * cos(q1); 83 | 84 | g2_error = @(q1, q2) ... 85 | (m2 * lc2_error + ml_error * l2) * grav * cos(q1 + q2); 86 | 87 | e3_max = @(q1, q2) [g1_error(q1, q2); 88 | g2_error(q1, q2)]; 89 | 90 | % e, e_dot (e = q - qd) 91 | e = @(q, qd) q - qd; 92 | e_dot = @(q_dot, qd_dot) q_dot - qd_dot; 93 | 94 | % r 95 | L = [11 0; 0 11]; 96 | C_const = 0.5; 97 | r = @(q, q_dot, qd, qd_dot, qd_ddot) ... 98 | e1_max(q(1), q(2)) * abs(qd_ddot - L * e_dot(q_dot, qd_dot)) + ... 99 | e2_max(q(1), q(2), q_dot(1), q_dot(2)) * abs(q_dot) + e3_max(q(1), q(2)) + C_const; 100 | 101 | % parameter estimations 102 | lc1_hat = 0.25; 103 | lc2_hat = 0.15; 104 | I1_hat = 0.2; 105 | I2_hat = 0.1; 106 | ml_hat = 1; 107 | 108 | % C apporximation 109 | c11_hat = @(q1, q2, q1_dot, q2_dot) -l1 * (m2 * lc2_hat + ml_hat * l2) * sin(q2) * q2_dot; 110 | c12_hat = @(q1, q2, q1_dot, q2_dot) -l1 * (m2 * lc2_hat + ml_hat * l2) * sin(q2) * (q2_dot + q1_dot); 111 | c21_hat = @(q1, q2, q1_dot, q2_dot) l1 * (m2 * lc2_hat + ml_hat * l2) * sin(q2) * q1_dot; 112 | c22_hat = @(q1, q2, q1_dot, q2_dot) 0; 113 | 114 | C_hat = @(q, q_dot) ... 115 | [c11_hat(q(1), q(2), q_dot(1), q_dot(2)) c12_hat(q(1), q(2), q_dot(1), q_dot(2)); ... 116 | c21_hat(q(1), q(2), q_dot(1), q_dot(2)) c22_hat(q(1), q(2), q_dot(1), q_dot(2))]; 117 | 118 | % H apporximation 119 | h11_hat = @(q1, q2) m1 * lc1_hat^2 + m2 * (lc2_hat ^ 2 + l1^2 + 2 * l1 * l2 * cos(q2)) + ml_hat * (l2^2 + l1^2 +2 * l1 * l2 * cos(q2)) + I1_hat + I2_hat; 120 | h12_hat = @(q1, q2) m2 * lc2_hat * (lc2_hat + l1 * cos(q2)) + ml_hat * l2 * (l2 + l1 * cos(q2)) + I2_hat; 121 | h22_hat = @(q1, q2) lc2_hat^2 * m2 + l2^2 * ml_hat + I2_hat; 122 | 123 | H_hat = @(q) ... 124 | [h11_hat(q(1), q(2)) h12_hat(q(1), q(2)); ... 125 | h12_hat(q(1), q(2)) h22_hat(q(1), q(2))]; 126 | 127 | % g apporximation 128 | g1_hat = @(q1, q2) (m2 * lc2_hat + ml_hat * l2) * grav * cos(q1 + q2) + ... 129 | (m2 * l1 + ml_hat * l1 + m1 * lc1_hat) * grav * cos(q1); 130 | g2_hat = @(q1, q2) (m2 * lc2_hat + ml_hat * l2) * grav * cos(q1 + q2); 131 | 132 | g_hat = @(q) [g1_hat(q(1), q(2)); g2_hat(q(1), q(2))]; 133 | 134 | % sliding surface of the system 135 | s = @(q, q_dot, qd, qd_dot) ... 136 | e_dot(q_dot, qd_dot) + L * e(q, qd); 137 | 138 | %% controller design 139 | 140 | x0 = [pi/3 pi/3; 0 0]; 141 | t_range = [0, 10]; 142 | epsilon = 0.1; 143 | 144 | u = @(q, q_dot, qd, qd_dot, qd_ddot, s_cur) ... 145 | C_hat(q, q_dot) * q_dot + g_hat(q) + H_hat(q) * qd_ddot - ... 146 | H_hat(q) * L * e_dot(q_dot, qd_dot) - ... 147 | r(q, q_dot, qd, qd_dot, qd_ddot) .* ... 148 | [sat(s_cur(1), epsilon); sat(s_cur(2), epsilon)]; 149 | 150 | %% compute states by solving the differential equations of the system 151 | 152 | qd = @(t) [pi/4 + pi/6 * sin(0.2*pi*t); ... 153 | -pi/3 + pi/3 * cos(0.2*pi*t)]; 154 | 155 | qd_dot = @(t) [pi^2 / 30 * cos(0.2*pi*t); ... 156 | -pi^2 / 15 * sin(0.2*pi*t)]; 157 | 158 | qd_ddot = @(t) [-pi^3 / 150 * sin(0.2*pi*t); ... 159 | -pi^3 / 75 * cos(0.2*pi*t)]; 160 | 161 | [t, x] = ode45(@(t, x) system_state2(t, x, u, H, C, g, qd, qd_dot, qd_ddot, s), t_range, x0); 162 | 163 | %% plots 164 | 165 | q1 = x(:, 1); 166 | q2 = x(:, 3); 167 | q1_dot = x(:, 2); 168 | q2_dot = x(:, 4); 169 | 170 | qd1 = @(t) pi/4 + pi/6 * sin(0.2*pi*t); 171 | qd2 = @(t) -pi/3 + pi/3 * cos(0.2*pi*t); 172 | 173 | % system state variables plot 174 | figure(1); 175 | plot(t, q1, 'linewidth', 0.8); 176 | hold on; 177 | plot(t, q2, 'linewidth', 0.8); 178 | hold on 179 | fplot(qd1, [0 20], '--', 'linewidth', 0.8) 180 | hold on 181 | fplot(qd2, [0 20], '--', 'linewidth', 0.8) 182 | 183 | legend('q1', 'q2', 'qd1', 'qd2'); 184 | xlabel('t (sec)') 185 | ylabel('rad') 186 | 187 | % deviation of q1 from qd, relative to time 188 | e1 = zeros(size(t)); 189 | e2 = zeros(size(t)); 190 | 191 | for i = 1: length(t) 192 | e1(i) = abs(q1(i) - qd1(t(i))); 193 | e2(i) = abs(q2(i) - qd2(t(i))); 194 | end 195 | 196 | figure(2); 197 | plot(t, e1, 'linewidth', 0.8); 198 | 199 | legend('e1'); 200 | xlabel('t (sec)') 201 | ylabel('rad') 202 | 203 | % deviation of q2 from qd, relative to time 204 | figure(3) 205 | plot(t, e2, 'linewidth', 0.8); 206 | 207 | legend('e2'); 208 | xlabel('t (sec)') 209 | ylabel('rad') 210 | 211 | % controller values plot 212 | figure(4); 213 | u_values = zeros(length(t), 2); 214 | for i = 1:size(t) 215 | s_value = s([q1(i); q2(i)], [q1_dot(i); q2_dot(i)], ... 216 | [qd1(t(i)); qd2(t(i))], qd_dot(t(i))); 217 | u_values(i, :) = u([q1(i); q2(i)], [q1_dot(i); q2_dot(i)], ... 218 | [qd1(t(i)); qd2(t(i))], qd_dot(t(i)), qd_ddot(t(i)), s_value); 219 | end 220 | 221 | plot(t, u_values, 'linewidth', 0.8); 222 | legend('u_1', 'u_2'); 223 | xlabel('t (sec)') 224 | ylabel('exerted force') 225 | 226 | % phase plane plot 227 | qd1_vals = zeros(size(t)); 228 | qd2_vals = zeros(size(t)); 229 | 230 | qd1_dot = @(t) pi^2/30 * cos(0.2*pi*t); 231 | qd2_dot = @(t) -pi^2/15 * sin(0.2*pi*t); 232 | 233 | qd1_dot_vals = zeros(size(t)); 234 | qd2_dot_vals = zeros(size(t)); 235 | 236 | for i = 1:length(t) 237 | qd1_vals(i) = qd1(t(i)); 238 | qd2_vals(i) = qd2(t(i)); 239 | qd1_dot_vals(i) = qd1_dot(t(i)); 240 | qd2_dot_vals(i) = qd2_dot(t(i)); 241 | end 242 | 243 | e1 = e([q1 q2], [qd1_vals qd2_vals]); 244 | e1 = e1(:, 1); 245 | e1_dot = e_dot([q1_dot q2_dot], [qd1_dot_vals qd2_dot_vals]); 246 | e1_dot = e1_dot(:, 1); 247 | 248 | e2 = e([q1 q2], [qd1_vals qd2_vals]); 249 | e2 = e2(:, 2); 250 | e2_dot = e_dot([q1_dot q2_dot], [qd1_dot_vals qd2_dot_vals]); 251 | e2_dot = e2_dot(:, 2); 252 | 253 | figure(5); 254 | sliding_surface1 = @(x) -L(1,1) * x; 255 | sliding_surface2 = @(x) -L(2,2) * x; 256 | fplot(sliding_surface1, '--', 'linewidth', 0.8); 257 | hold on; 258 | fplot(sliding_surface2, '--', 'linewidth', 0.8); 259 | hold on; 260 | plot(e1, e1_dot, 'linewidth', 0.8); 261 | hold on; 262 | plot(e2, e2_dot, 'linewidth', 0.8); 263 | 264 | hl = legend('sliding surface', '$\dot{e_1}$', '$\dot{e_2}$'); 265 | set(hl, 'Interpreter', 'latex'); 266 | 267 | --------------------------------------------------------------------------------