├── Geometric_Tracking_Quadcopter.m ├── LICENSE ├── QuadDOB.m ├── README.md ├── Report └── Effect_of_DOB_on_Attitude_Control.pdf ├── Scenario1 ├── 1.png ├── 2.png ├── 3.png ├── 4.png └── 5.png ├── Scenario2 ├── 1.png ├── 2.png ├── 3.png ├── 4.png └── 5.png ├── Scenario2a ├── 1.png ├── 2.png ├── 3.png ├── 4.png └── 5.png ├── Tracking_SE3_vs_w_CAM.m ├── simulation.gif └── vwsmc.mat /Geometric_Tracking_Quadcopter.m: -------------------------------------------------------------------------------- 1 | clear 2 | 3 | % Declaring Globals 4 | global e3 R W x v xt yt zt xdt ydt zdt xddt yddt zddt dt time; 5 | global dbarx dbarW timesteps Rdes_previous wdes_previous I Rdes; 6 | global zW zx lW lv Km Ks fb; 7 | animate=0; 8 | 9 | %Initialising all Variables 10 | J = diag([0.0820, 0.0845, 0.1377]); 11 | d = 0.315; 12 | m = 4.34; 13 | g = 9.81; 14 | 15 | kx = 16*m; 16 | kv = 5.6*m; 17 | kR = 8.81; 18 | kw = 2.54; 19 | Km = diag([0.2 0.2 0.05]); 20 | Ks = 0.02; 21 | 22 | dt = 0.01; 23 | time = 8; 24 | 25 | reset_params(); 26 | 27 | desired_path(); 28 | global xdest b1dest vdest adest; 29 | 30 | for n = 1 : length(timesteps) 31 | 32 | % desired values 33 | xdes = xdest(:,n); 34 | b1des = b1dest(:,n); 35 | vdes = vdest(:,n); 36 | ades = adest(:,n); 37 | 38 | % errors 39 | xerr = x - xdes; 40 | verr = v - vdes; 41 | 42 | xerr_hist(n) = xerr(1,1); 43 | yerr_hist(n) = xerr(2,1); 44 | zerr_hist(n) = xerr(3,1); 45 | 46 | b3des = (-kx*xerr - kv*verr - m*g*e3 + m*ades)/norm(-kx*xerr - kv*verr - m*g*e3 + m*ades); 47 | b2des = (cross(b3des, b1des)/norm(cross(b3des, b1des))); 48 | 49 | Rdes = [cross(b2des, b3des) b2des b3des]; 50 | 51 | dRdes = (Rdes - Rdes_previous)/dt; 52 | Rdes_previous = Rdes; 53 | wdes = Vhelper(Rdes'*dRdes); 54 | 55 | werr = W - R'*Rdes*wdes; 56 | rerr = Vhelper(0.5*(Rdes'*R - R'*Rdes)); 57 | 58 | % control inputs 59 | f = -dot((-kx*xerr - kv*verr-m*g*e3 + m*ades), R*e3); 60 | M = -kR*rerr - kw*werr; 61 | 62 | vdot = (m*g*e3 - f*R*e3)/m; 63 | v = v + vdot*dt; 64 | xdot = v; 65 | x = x + xdot*dt; 66 | Wdot = inv(J) * (M - cross(W,J * W)); 67 | W = W + Wdot*dt; 68 | Rdot = R * Whelper(W); 69 | R = R + Rdot*dt; 70 | R = GSnorm(R); 71 | 72 | xt(n) = x(1); 73 | yt(n) = x(2); 74 | zt(n) = x(3); 75 | 76 | xdt(n) = v(1, 1); 77 | ydt(n) = v(2, 1); 78 | zdt(n) = v(3, 1); 79 | 80 | xddt(n) = (xdt(end)-xdt(end-1))/dt; 81 | yddt(n) = (ydt(end)-ydt(end-1))/dt; 82 | zddt(n) = (zdt(end)-zdt(end-1))/dt; 83 | 84 | end 85 | 86 | ax = axes(figure(1)); 87 | plot3(ax, xt,yt,zt); 88 | xlabel('X'); 89 | ylabel('Y'); 90 | zlabel('Z'); 91 | hold(ax, "on"); 92 | 93 | axs = []; 94 | val = [xt; yt; zt; xdt; ydt; zdt; xddt; yddt; zddt]; 95 | ylabels = ["x" "y" "z" "v_x" "v_y" "v_z" "a_x" "a_y" "a_z"]; 96 | fig2 = figure(2); 97 | for i = 1 : 9 98 | axs(i) = subplot(3, 3, i); 99 | plot(axs(i), timesteps, val(i,:)) 100 | xlabel('time') 101 | ylabel(ylabels(i)) 102 | hold(axs(i), "on"); 103 | end 104 | 105 | 106 | erraxs = []; 107 | val2 = [xerr_hist; yerr_hist; zerr_hist]; 108 | ylabels2 = ["Error in x" "Error in y" "Error in z"]; 109 | fig3 = figure(3); 110 | for i = 1 : 3 111 | erraxs(i) = subplot(1, 3, i); 112 | plot(erraxs(i), timesteps, val2(i,:)) 113 | xlabel('time') 114 | ylabel(ylabels2(i)) 115 | hold(erraxs(i), "on"); 116 | end 117 | 118 | if animate 119 | scenario = uavScenario("StopTime", 8, "UpdateRate", 1000); 120 | axnm = axes(figure("Units","normalized", "OuterPosition", [0 0 1 1])); 121 | hold(axnm, "on") 122 | 123 | plot3(axnm, 70*yt', 70*xt', -70*zt'); 124 | end 125 | 126 | 127 | reset_params(); 128 | 129 | for n = 1 : length(timesteps) 130 | [dis_x, dis_W] = dis_gen(timesteps(n)); 131 | 132 | % desired values 133 | xdes = xdest(:,n); 134 | b1des = b1dest(:,n); 135 | vdes = vdest(:,n); 136 | ades = adest(:,n); 137 | 138 | % errors 139 | xerr = x - xdes; 140 | verr = v - vdes; 141 | 142 | xerr_hist(n) = xerr(1,1); 143 | yerr_hist(n) = xerr(2,1); 144 | zerr_hist(n) = xerr(3,1); 145 | 146 | b3des = (-kx*xerr - kv*verr - m*g*e3 + m*ades)/norm(-kx*xerr - kv*verr - m*g*e3 + m*ades); 147 | b2des = (cross(b3des, b1des)/norm(cross(b3des, b1des))); 148 | Rdes = [cross(b2des, b3des) b2des b3des]; 149 | 150 | dRdes = (Rdes - Rdes_previous)/dt; 151 | Rdes_previous = Rdes; 152 | wdes = Vhelper(Rdes'*dRdes); 153 | 154 | werr = W - R'*Rdes*wdes; 155 | rerr = Vhelper(0.5*(Rdes'*R - R'*Rdes)); 156 | 157 | % control inputs 158 | f = -dot((-kx*xerr - kv*verr-m*g*e3 + m*ades), R*e3); 159 | M = -kR*rerr - kw*werr; 160 | 161 | vdot = (m*g*e3 - f*R*e3 - dis_x)/m; 162 | v = v + vdot*dt; 163 | xdot = v; 164 | x = x + xdot*dt; 165 | Wdot = inv(J) * (M - cross(W,J * W)+ dis_W); 166 | W = W + Wdot*dt; 167 | Rdot = R * Whelper(W); 168 | R = R + Rdot*dt; 169 | R = GSnorm(R); 170 | 171 | xt(n) = x(1); 172 | yt(n) = x(2); 173 | zt(n) = x(3); 174 | 175 | xdt(n) = v(1, 1); 176 | ydt(n) = v(2, 1); 177 | zdt(n) = v(3, 1); 178 | 179 | xddt(n) = (xdt(end)-xdt(end-1))/dt; 180 | yddt(n) = (ydt(end)-ydt(end-1))/dt; 181 | zddt(n) = (zdt(end)-zdt(end-1))/dt; 182 | end 183 | 184 | plot3(ax, xt, yt, zt); 185 | 186 | if animate 187 | plot3(axnm, 70*yt', 70*xt', -70*zt'); 188 | end 189 | 190 | val = [xt; yt; zt; xdt; ydt; zdt; xddt; yddt; zddt]; 191 | for i = 1 : 9 192 | plot(axs(i), timesteps, val(i,:)) 193 | end 194 | 195 | val2 = [xerr_hist; yerr_hist; zerr_hist]; 196 | for i = 1:3 197 | plot(erraxs(i), timesteps, val2(i,:)) 198 | end 199 | 200 | reset_params(); 201 | 202 | for n = 1 : length(timesteps) 203 | 204 | [dis_x, dis_W] = dis_gen(timesteps(n)); 205 | 206 | % desired values 207 | xdes = xdest(:,n); 208 | b1des = b1dest(:,n); 209 | vdes = vdest(:,n); 210 | ades = adest(:,n); 211 | 212 | xerr = x - xdes; 213 | verr = v - vdes; 214 | 215 | xerr_hist(n) = xerr(1,1); 216 | yerr_hist(n) = xerr(2,1); 217 | zerr_hist(n) = xerr(3,1); 218 | 219 | b3des = (-kx*xerr - kv*verr - m*g*e3 + m*ades)/norm(-kx*xerr - kv*verr - m*g*e3 + m*ades); 220 | b2des = (cross(b3des, b1des)/norm(cross(b3des, b1des))); 221 | 222 | Rdes = [cross(b2des, b3des) b2des b3des]; 223 | 224 | dRdes = (Rdes - Rdes_previous)/dt; 225 | Rdes_previous = Rdes; 226 | wdes = Vhelper(Rdes'*dRdes); 227 | dwdes = (wdes - wdes_previous)/dt; 228 | wdes_previous = wdes; 229 | 230 | werr = W - R'*Rdes*wdes; 231 | rerr = Vhelper(0.5*(Rdes'*R - R'*Rdes)); 232 | 233 | vdot = (m*g*e3 - R*fb)/m; 234 | v = v + vdot*dt; 235 | xdot = v; 236 | x = x + xdot*dt; 237 | Wdot = inv(J) * (M - cross(W,J * W)); 238 | W = W + Wdot*dt; 239 | Rdot = R * Whelper(W); 240 | R = R + Rdot*dt; 241 | R = GSnorm(R); 242 | 243 | lamdaW = Km * wdes; 244 | lamdav = Ks * vdes; 245 | 246 | M = -kR*rerr - kw*werr + J*dwdes + cross(W, J*W) - dbarW; 247 | zWdot = - lW * [inv(J)*(lamdaW + zW) - inv(J)*(cross(W, J*W)) + inv(J)*M]; 248 | dbarW = zW + lamdaW; 249 | zW = zW + zWdot*dt; 250 | 251 | fb = -dot((-kx*xerr - kv*verr - m*g*e3 + m*ades - dbarx), R*e3) * e3; 252 | 253 | zxdot = -lv * (1/m * (lamdav + zx) - g*e3 + ((R*fb)/m)); 254 | dbarx = zx + lamdav; 255 | zx = zx + zxdot*dt; 256 | 257 | xt(n) = x(1); 258 | yt(n) = x(2); 259 | zt(n) = x(3); 260 | 261 | xdt(n) = v(1, 1); 262 | ydt(n) = v(2, 1); 263 | zdt(n) = v(3, 1); 264 | 265 | xddt(n) = (xdt(end)-xdt(end-1))/dt; 266 | yddt(n) = (ydt(end)-ydt(end-1))/dt; 267 | zddt(n) = (zdt(end)-zdt(end-1))/dt; 268 | 269 | end 270 | 271 | plot3(ax, xt, yt, zt, '--k'); 272 | 273 | xdes_path = xdest(1, :); 274 | y_desired_path = xdest(2, :); 275 | z_desiredpath = xdest(3, :); 276 | plot3(ax, xdes_path, y_desired_path, z_desiredpath) 277 | legend(ax, "Drone in Ideal Conditions", "With Disturbance", "With DOB", "Desired Path") 278 | hold(ax, "off"); 279 | 280 | val = [xt; yt; zt; xdt; ydt; zdt; xddt; yddt; zddt]; 281 | for i = 1 : 9 282 | plot(axs(i), timesteps, val(i,:), '--k') 283 | hold(axs(i), "off"); 284 | end 285 | Lgnd = legend("Drone in Ideal Conditons Ideal", "With Disturbance", "With DOB"); 286 | Lgnd.Position(1) = 0.84; 287 | Lgnd.Position(2) = 0.88; 288 | 289 | val2 = [xerr_hist; yerr_hist; zerr_hist]; 290 | for i = 1:3 291 | plot(erraxs(i), timesteps, val2(i,:), '--k') 292 | hold(erraxs(i), "off"); 293 | end 294 | 295 | Lgnd = legend("Drone in Ideal Conditons Ideal", "With Disturbance", "With DOB"); 296 | Lgnd.Position(1) = 0.84; 297 | Lgnd.Position(2) = 0.88; 298 | 299 | if animate 300 | plot3(axnm, 70*yt', 70*xt', -70*zt', '--k'); 301 | 302 | plat = uavPlatform("UAV", scenario, "Trajectory", ... 303 | waypointTrajectory(70*[xt' yt' zt'], "TimeOfArrival", timesteps)); 304 | updateMesh(plat,"quadrotor", {10}, [1 0 0], eul2tform([0 0 pi])); 305 | 306 | [axnm, plotFrames] = show3D(scenario); 307 | view([-110 30]); 308 | axis equal 309 | 310 | setup(scenario); 311 | updateCounter = 0; 312 | while true 313 | % Advance scenario. 314 | isRunning = advance(scenario); 315 | updateCounter = updateCounter + 1; 316 | 317 | % Update visualization every 10 updates. 318 | if updateCounter > 10 319 | show3D(scenario, "FastUpdate", true, "Parent", axnm); 320 | updateCounter = 0; 321 | drawnow limitrate 322 | end 323 | 324 | if ~isRunning 325 | break; 326 | end 327 | end 328 | hold(axnm, "off") 329 | end 330 | 331 | function Wh = Whelper(W) 332 | Wh = [0 -W(3) W(2); 333 | W(3) 0 -W(1); 334 | -W(2) W(1) 0]; 335 | end 336 | 337 | function Vh = Vhelper(W) 338 | Vh = [W(3, 2); W(1, 3) ; W(2,1)]; 339 | end 340 | 341 | function b = GSnorm(a) 342 | [m,n]=size(a); 343 | b=zeros(m,n); 344 | k=n; 345 | b(:,1)=a(:,1)./sqrt(sum(a(:,1).*a(:,1))); 346 | b(:,2)=a(:,2)-dot(b(:,1),a(:,2))*b(:,1); 347 | b(:,2)=b(:,2)./sqrt(sum(b(:,2).*b(:,2))); 348 | if k>2 349 | for i=3:k 350 | b(:,i)=a(:,i)-dot(b(:,2),a(:,i))*b(:,2)-dot(b(:,1),a(:,i))*b(:,1); 351 | if i>3 352 | for j=i-1:-1:3 353 | b(:,i)=b(:,i)-dot(b(:,j),a(:,i))*b(:,j); 354 | end 355 | 356 | end 357 | b(:,i)=b(:,i)./sqrt(sum(b(:,i).*b(:,i))); 358 | end 359 | end 360 | end 361 | 362 | function reset_params() 363 | global e1 e2 e3 R W Wh x v xt yt zt xdt ydt zdt xddt yddt zddt dt time; 364 | global dbarx dbarW timesteps Rt Rdes_previous wdes_previous I Rdes; 365 | global zW zx lW lv Km Ks fb; 366 | 367 | e1 = [1; 0; 0]; 368 | e2 = [0; 1; 0]; 369 | e3 = [0; 0; 1]; 370 | R = [e1 e2 e3]; 371 | W = [0; 0; 0]; 372 | Wh = [0 -W(3) W(2); 373 | W(3) 0 -W(1); 374 | -W(2) W(1) 0]; 375 | x = [0 0 0]'; 376 | v = [0 0 0]'; 377 | 378 | xt = []; 379 | yt = []; 380 | zt = []; 381 | 382 | xdt = [0 0]; 383 | ydt = [0 0]; 384 | zdt = [0 0]; 385 | 386 | xddt = []; 387 | yddt = []; 388 | zddt = []; 389 | 390 | dbarx = 0; 391 | dbarW = 0; 392 | 393 | xerr_hist = []; 394 | yerr_hist = []; 395 | zerr_hist = []; 396 | 397 | %The timestep calculations 398 | timesteps = 0:dt:time; 399 | 400 | Rt = cell(length(timesteps), 1); 401 | Rdes_previous = zeros(3); 402 | wdes_previous = [0; 0; 0]; 403 | Rdes = zeros(3); 404 | 405 | I = eye(3); 406 | 407 | zW = [0; 0; 0]; 408 | zx = [0; 0; 0]; 409 | 410 | lW = Km * I; 411 | lv = Ks * I; 412 | 413 | fb = [0 0 0]'; 414 | end 415 | 416 | function desired_path() 417 | 418 | global xdest b1dest vdest adest timesteps; 419 | 420 | xdest = []; 421 | b1dest = []; 422 | vdest = []; 423 | adest = []; 424 | 425 | % syms t; 426 | % xdfn = [0.4*t; 0.4*sin(pi*t); 0.6*cos(pi*t)]; 427 | % 428 | for n = 1 : length(timesteps) 429 | xdest(:,n) = [0.4*timesteps(n) 0.4*sin(pi*timesteps(n)) 0.6*cos(pi*timesteps(n))]'; 430 | b1dest(:,n) = [cos(pi*timesteps(n)) sin(pi*timesteps(n)) 0]'; 431 | vdest(:,n) = [0.4 0.4*pi*cos(pi*timesteps(n)) -0.6*pi*sin(pi*timesteps(n))]'; 432 | adest(:,n) = [0 -0.4*pi^2*sin(pi*timesteps(n)) -0.6*pi^2*cos(pi*timesteps(n))]'; 433 | end 434 | 435 | % syms t; 436 | % xdfn = [2*cos((pi*t)/10); 2*sin((pi*t)/10); (t/2)] 437 | % b1fn = diff(xdfn) / norm(diff(xdfn)) 438 | % diff(xdfn) 439 | % diff(diff(xdfn)) 440 | 441 | % for n = 1 : length(timesteps) 442 | % xdest(:,n) = [2*cos((pi*timesteps(n))/10); 2*sin((pi*timesteps(n))/10); (timesteps(n)/2)]; 443 | % b1dest(:,n) = [-(pi*sin((pi*timesteps(n))/10))/(5*((pi^2*abs(cos((timesteps(n)*pi)/10))^2)/25 + (pi^2*abs(sin((timesteps(n)*pi)/10))^2)/25 + 1/4)^(1/2)); 444 | % (pi*cos((pi*timesteps(n))/10))/(5((pi^2*abs(cos((timesteps(n)pi)/10))^2)/25 + (pi^2*abs(sin((timesteps(n)*pi)/10))^2)/25 + 1/4)^(1/2)); 445 | % 1/(2((pi^2*abs(cos((timesteps(n)*pi)/10))^2)/25 + (pi^2*abs(sin((timesteps(n)*pi)/10))^2)/25 + 1/4)^(1/2))]; 446 | % vdest(:,n) = [-(pi*sin((pi*timesteps(n))/10))/5; (pi*cos((pi*timesteps(n))/10))/5; 0.5]; 447 | % adest(:,n) = [-(pi^2*cos((pi*timesteps(n))/10))/50; -(pi^2*sin((pi*timesteps(n))/10))/50; 0]; 448 | % end 449 | 450 | end 451 | 452 | function [disx, disW] = dis_gen(n) 453 | dis_gain_x = 10; 454 | dis_gain_w = 6; 455 | 456 | step_disturbance = 0; 457 | 458 | scenario1 = [0.13*atan(n/2); 0.2*atan(n/2); 0.26*atan(n/2)]; 459 | scenario2 = [0.13; 0.2; 0.26]; 460 | scenario3 = [0.13*cos(0.1*pi*n); 0.2*cos(0.1*pi*n); 0.26*cos(0.1*pi*n)]; 461 | dis_fn = scenario2; 462 | 463 | disx = dis_gain_x * dis_fn; 464 | disW = dis_gain_w * dis_fn; 465 | 466 | if and(or(n < 4, n > 5), step_disturbance) 467 | disx = 0; 468 | disW = 0; 469 | end 470 | end 471 | 472 | v1 = [-5; 2; 2]; 473 | v2 = [-4; -2; 2]; 474 | x = [-5; 3; 17]; 475 | 476 | (((v1 * v1')/(v1' * v1)) + ((v2 * v2')/ (v2' * v2)))*[-5; 3; 17] 477 | 478 | ((dot(x,v1))/dot(v1, v1))*v1 + ((dot(x,v2))/dot(v2, v2))*v2 479 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Walstan Baptista 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /QuadDOB.m: -------------------------------------------------------------------------------- 1 | clear 2 | 3 | % Declaring Globals 4 | global e3 R W x v xt yt zt xdt ydt zdt xddt yddt zddt dt time; 5 | global dbarx dbarW timesteps Rdes_previous wdes_previous I Rdes; 6 | global zW zx lW lv Km Ks fb; 7 | animate=0; 8 | 9 | %Initialising all Variables 10 | J = diag([0.0820, 0.0845, 0.1377]); 11 | d = 0.315; 12 | ctf = 8.004e-4; 13 | m = 4.34; 14 | g = 9.81; 15 | upper_limit = 60; 16 | 17 | kx = 16*m; 18 | kv = 5.6*m; 19 | kR = 8.81; 20 | kw = 2.54; 21 | Km = diag([0.2 0.2 0.05]); 22 | Ks = 0.02; 23 | 24 | dt = 0.01; 25 | time = 20; 26 | 27 | reset_params(); 28 | 29 | desired_path(); 30 | global xdest b1dest vdest adest; 31 | 32 | for n = 1 : length(timesteps) 33 | 34 | % desired values 35 | xdes = xdest(:,n); 36 | b1des = b1dest(:,n); 37 | vdes = vdest(:,n); 38 | ades = adest(:,n); 39 | 40 | % errors 41 | xerr = x - xdes; 42 | verr = v - vdes; 43 | 44 | xerr_hist(n) = xerr(1,1); 45 | yerr_hist(n) = xerr(2,1); 46 | zerr_hist(n) = xerr(3,1); 47 | 48 | b3des = (-kx*xerr - kv*verr - m*g*e3 + m*ades)/norm(-kx*xerr - kv*verr - m*g*e3 + m*ades); 49 | b2des = (cross(b3des, b1des)/norm(cross(b3des, b1des))); 50 | 51 | Rdes = [cross(b2des, b3des) b2des b3des]; 52 | 53 | dRdes = (Rdes - Rdes_previous)/dt; 54 | Rdes_previous = Rdes; 55 | wdes = Vhelper(Rdes'*dRdes); 56 | 57 | werr = W - R'*Rdes*wdes; 58 | rerr = Vhelper(0.5*(Rdes'*R - R'*Rdes)); 59 | 60 | % control inputs 61 | f = -dot((-kx*xerr - kv*verr-m*g*e3 + m*ades), R*e3); 62 | f = min(max(abs(f)), upper_limit)* sign(f); 63 | M = -kR*rerr - kw*werr; 64 | 65 | % f = min(max(abs(f)), upper_limit)/max(abs(f))*f; 66 | % M = min(max(abs(M)), 1)/max(abs(M))*M 67 | 68 | % Control Allocation Matrix 69 | % u = [f M']'; 70 | % 71 | % CAM = [1 1 1 1; 0 -d 0 d; d 0 -d 0; -ctf ctf -ctf ctf]; 72 | % fi = CAM\u; 73 | % 74 | % u = CAM * fi; 75 | % f = u(1) 76 | % M = u(2:4); 77 | 78 | vdot = (m*g*e3 - f*R*e3)/m; 79 | v = v + vdot*dt; 80 | % v = min(max(abs(v)), 1.4)/max(abs(v))*v; 81 | xdot = v; 82 | x = x + xdot*dt; 83 | Wdot = inv(J) * (M - cross(W,J * W)); 84 | W = W + Wdot*dt; 85 | Rdot = R * Whelper(W); 86 | R = R + Rdot*dt; 87 | R = GSnorm(R); 88 | 89 | xt(n) = x(1); 90 | yt(n) = x(2); 91 | zt(n) = x(3); 92 | 93 | xdt(n) = v(1, 1); 94 | ydt(n) = v(2, 1); 95 | zdt(n) = v(3, 1); 96 | 97 | xddt(n) = (xdt(end)-xdt(end-1))/dt; 98 | yddt(n) = (ydt(end)-ydt(end-1))/dt; 99 | zddt(n) = (zdt(end)-zdt(end-1))/dt; 100 | 101 | end 102 | 103 | 104 | ax = axes(figure(1)); 105 | plot3(ax, xt,yt,zt); 106 | xlabel('X'); 107 | ylabel('Y'); 108 | zlabel('Z'); 109 | hold(ax, "on"); 110 | 111 | axs = []; 112 | val = [xt; yt; zt; xdt; ydt; zdt; xddt; yddt; zddt]; 113 | ylabels = ["x" "y" "z" "v_x" "v_y" "v_z" "a_x" "a_y" "a_z"]; 114 | fig2 = figure(2); 115 | for i = 1 : 9 116 | axs(i) = subplot(3, 3, i); 117 | plot(axs(i), timesteps, val(i,:)) 118 | xlabel('time') 119 | ylabel(ylabels(i)) 120 | hold(axs(i), "on"); 121 | end 122 | 123 | 124 | erraxs = []; 125 | val2 = [xerr_hist; yerr_hist; zerr_hist]; 126 | ylabels2 = ["Error in x" "Error in y" "Error in z"]; 127 | fig3 = figure(3); 128 | for i = 1 : 3 129 | erraxs(i) = subplot(1, 3, i); 130 | plot(erraxs(i), timesteps, val2(i,:)) 131 | xlabel('time') 132 | ylabel(ylabels2(i)) 133 | hold(erraxs(i), "on"); 134 | end 135 | 136 | if animate 137 | scenario = uavScenario("StopTime", 8, "UpdateRate", 1000); 138 | axnm = axes(figure("Units","normalized", "OuterPosition", [0 0 1 1])); 139 | hold(axnm, "on") 140 | 141 | plot3(axnm, 70*yt', 70*xt', -70*zt'); 142 | end 143 | 144 | 145 | reset_params(); 146 | 147 | for n = 1 : length(timesteps) 148 | [dis_x, dis_W] = dis_gen(timesteps(n)); 149 | 150 | % desired values 151 | xdes = xdest(:,n); 152 | b1des = b1dest(:,n); 153 | vdes = vdest(:,n); 154 | ades = adest(:,n); 155 | 156 | % errors 157 | xerr = x - xdes; 158 | verr = v - vdes; 159 | 160 | xerr_hist(n) = xerr(1,1); 161 | yerr_hist(n) = xerr(2,1); 162 | zerr_hist(n) = xerr(3,1); 163 | 164 | b3des = (-kx*xerr - kv*verr - m*g*e3 + m*ades)/norm(-kx*xerr - kv*verr - m*g*e3 + m*ades); 165 | b2des = (cross(b3des, b1des)/norm(cross(b3des, b1des))); 166 | Rdes = [cross(b2des, b3des) b2des b3des]; 167 | 168 | dRdes = (Rdes - Rdes_previous)/dt; 169 | Rdes_previous = Rdes; 170 | wdes = Vhelper(Rdes'*dRdes); 171 | 172 | werr = W - R'*Rdes*wdes; 173 | rerr = Vhelper(0.5*(Rdes'*R - R'*Rdes)); 174 | 175 | % control inputs 176 | f = -dot((-kx*xerr - kv*verr-m*g*e3 + m*ades), R*e3); 177 | f = min(max(abs(f)), upper_limit)* sign(f); 178 | M = -kR*rerr - kw*werr; 179 | 180 | % Control Allocation Matrix 181 | % u = [f M']'; 182 | % CAM = [1 1 1 1; 0 -d 0 d; d 0 -d 0; -ctf ctf -ctf ctf]; 183 | % fi = CAM\u; 184 | % fi = min(fi, upper_limit); 185 | % fi = max(fi, lower_limit); 186 | % u = CAM * fi; 187 | % f = u(1); 188 | % M = u(2:4); 189 | 190 | vdot = (m*g*e3 - f*R*e3 + dis_x)/m; 191 | % vdot = min(max(abs(vdot)), 1.4)/max(abs(vdot))*vdot; 192 | v = v + vdot*dt; 193 | xdot = v; 194 | x = x + xdot*dt; 195 | Wdot = inv(J) * (M - cross(W,J * W)+ dis_W); 196 | W = W + Wdot*dt; 197 | Rdot = R * Whelper(W); 198 | R = R + Rdot*dt; 199 | R = GSnorm(R); 200 | 201 | xt(n) = x(1); 202 | yt(n) = x(2); 203 | zt(n) = x(3); 204 | 205 | xdt(n) = v(1, 1); 206 | ydt(n) = v(2, 1); 207 | zdt(n) = v(3, 1); 208 | 209 | xddt(n) = (xdt(end)-xdt(end-1))/dt; 210 | yddt(n) = (ydt(end)-ydt(end-1))/dt; 211 | zddt(n) = (zdt(end)-zdt(end-1))/dt; 212 | end 213 | 214 | plot3(ax, xt, yt, zt); 215 | 216 | if animate 217 | plot3(axnm, 70*yt', 70*xt', -70*zt'); 218 | end 219 | 220 | val = [xt; yt; zt; xdt; ydt; zdt; xddt; yddt; zddt]; 221 | for i = 1 : 9 222 | plot(axs(i), timesteps, val(i,:)) 223 | end 224 | 225 | val2 = [xerr_hist; yerr_hist; zerr_hist]; 226 | for i = 1:3 227 | plot(erraxs(i), timesteps, val2(i,:)) 228 | end 229 | 230 | reset_params(); 231 | 232 | fblog_x = []; 233 | fblog_y = []; 234 | fblog_z = []; 235 | dbarxlog_x = []; 236 | dbarxlog_y = []; 237 | dbarxlog_z = []; 238 | 239 | for n = 1 : length(timesteps) 240 | 241 | [dis_x, dis_W] = dis_gen(timesteps(n)); 242 | 243 | % desired values 244 | xdes = xdest(:,n); 245 | b1des = b1dest(:,n); 246 | vdes = vdest(:,n); 247 | ades = adest(:,n); 248 | 249 | xerr = x - xdes; 250 | verr = v - vdes; 251 | 252 | xerr_hist(n) = xerr(1,1); 253 | yerr_hist(n) = xerr(2,1); 254 | zerr_hist(n) = xerr(3,1); 255 | 256 | b3des = (-kx*xerr - kv*verr - m*g*e3 + m*ades)/norm(-kx*xerr - kv*verr - m*g*e3 + m*ades); 257 | b2des = (cross(b3des, b1des)/norm(cross(b3des, b1des))); 258 | 259 | Rdes = [cross(b2des, b3des) b2des b3des]; 260 | 261 | dRdes = (Rdes - Rdes_previous)/dt; 262 | Rdes_previous = Rdes; 263 | wdes = Vhelper(Rdes'*dRdes); 264 | dwdes = (wdes - wdes_previous)/dt; 265 | wdes_previous = wdes; 266 | 267 | werr = W - R'*Rdes*wdes; 268 | rerr = Vhelper(0.5*(Rdes'*R - R'*Rdes)); 269 | 270 | vdot = (m*g*e3 - R*fb + dis_x)/m; 271 | v = v + vdot*dt; 272 | xdot = v; 273 | % v = min(max(abs(v)), 1.4)/max(abs(v))*v; 274 | x = x + xdot*dt; 275 | Wdot = inv(J) * (M - cross(W,J * W)+ dis_W); 276 | W = W + Wdot*dt; 277 | Rdot = R * Whelper(W); 278 | R = R + Rdot*dt; 279 | R = GSnorm(R); 280 | 281 | lamdaW = Km * wdes; 282 | lamdav = Ks * vdes; 283 | 284 | M = -kR*rerr - kw*werr + J*dwdes + cross(W, J*W) - dbarW; 285 | 286 | f_lim = -dot((-kx*xerr - kv*verr - m*g*e3 + m*ades - dbarx), R*e3); 287 | f_lim = min(max(abs(f_lim)), upper_limit)* sign(f_lim); 288 | fb = f_lim * e3; 289 | 290 | 291 | 292 | 293 | fblog_x(n) = fb(1,1); 294 | fblog_y(n) = fb(2,1); 295 | fblog_z(n) = fb(3,1); 296 | dbarxlog_x(n) = dbarx(1,1); 297 | dbarxlog_y(n) = dbarx(2,1); 298 | dbarxlog_z(n) = dbarx(3,1); 299 | 300 | zWdot = - lW * [inv(J)*(lamdaW + zW) - inv(J)*(cross(W, J*W)) + inv(J)*M]; 301 | dbarW = zW + lamdaW; 302 | zW = zW + zWdot*dt; 303 | 304 | zxdot = -lv * (1/m * (lamdav + zx) - g*e3 + ((R*fb)/m)); 305 | dbarx = zx + lamdav; 306 | zx = zx + zxdot*dt; 307 | 308 | xt(n) = x(1); 309 | yt(n) = x(2); 310 | zt(n) = x(3); 311 | 312 | xdt(n) = v(1, 1); 313 | ydt(n) = v(2, 1); 314 | zdt(n) = v(3, 1); 315 | 316 | xddt(n) = (xdt(end)-xdt(end-1))/dt; 317 | yddt(n) = (ydt(end)-ydt(end-1))/dt; 318 | zddt(n) = (zdt(end)-zdt(end-1))/dt; 319 | 320 | end 321 | 322 | 323 | plot3(ax, xt, yt, zt, '--k'); 324 | 325 | xdes_path = xdest(1, :); 326 | y_desired_path = xdest(2, :); 327 | z_desiredpath = xdest(3, :); 328 | plot3(ax, xdes_path, y_desired_path, z_desiredpath) 329 | legend(ax, "Drone in Ideal Conditions", "With Disturbance", "With DOB", "Desired") 330 | % hold(ax, "off"); 331 | 332 | val = [xt; yt; zt; xdt; ydt; zdt; xddt; yddt; zddt]; 333 | for i = 1 : 9 334 | plot(axs(i), timesteps, val(i,:), '--k') 335 | hold(axs(i), "off"); 336 | end 337 | Lgnd = legend("Drone in Ideal Conditons Ideal", "With Disturbance", "With DOB"); 338 | Lgnd.Position(1) = 0.84; 339 | Lgnd.Position(2) = 0.88; 340 | 341 | val2 = [xerr_hist; yerr_hist; zerr_hist]; 342 | for i = 1:3 343 | plot(erraxs(i), timesteps, val2(i,:), '--k') 344 | hold(erraxs(i), "off"); 345 | end 346 | 347 | Lgnd = legend("Drone in Ideal Conditons Ideal", "With Disturbance", "With DOB"); 348 | Lgnd.Position(1) = 0.84; 349 | Lgnd.Position(2) = 0.88; 350 | 351 | % figure(4) 352 | % 353 | % subplot(2,3,1); 354 | % plot(timesteps, dbarxlog_x, '--k'); 355 | % 356 | % subplot(2,3,4); 357 | % plot(timesteps, fblog_x); 358 | % 359 | % subplot(2,3,2); 360 | % plot(timesteps, dbarxlog_y, '--k'); 361 | % 362 | % subplot(2,3,5); 363 | % plot(timesteps, fblog_y); 364 | % 365 | % subplot(2,3,3); 366 | % plot(timesteps, dbarxlog_z, '--k'); 367 | % 368 | % subplot(2,3,6); 369 | % plot(timesteps, fblog_z); 370 | % 371 | % legend("dbarx", "fb") 372 | 373 | if animate 374 | scale = 1 375 | plot3(axnm, scale*yt', scale*xt', -scale*zt', '--k'); 376 | 377 | plat = uavPlatform("UAV", scenario, "Trajectory", ... 378 | waypointTrajectory(70*[xt' yt' zt'], "TimeOfArrival", timesteps)); 379 | updateMesh(plat,"quadrotor", {10}, [1 0 0], eul2tform([0 0 pi])); 380 | 381 | [axnm, plotFrames] = show3D(scenario); 382 | view([-110 30]); 383 | axis equal 384 | 385 | setup(scenario); 386 | updateCounter = 0; 387 | while true 388 | % Advance scenario. 389 | isRunning = advance(scenario); 390 | updateCounter = updateCounter + 1; 391 | 392 | % Update visualization every 10 updates. 393 | if updateCounter > 10 394 | show3D(scenario, "FastUpdate", true, "Parent", axnm); 395 | updateCounter = 0; 396 | drawnow limitrate 397 | end 398 | 399 | if ~isRunning 400 | break; 401 | end 402 | end 403 | hold(axnm, "off") 404 | end 405 | 406 | function Wh = Whelper(W) 407 | Wh = [0 -W(3) W(2); 408 | W(3) 0 -W(1); 409 | -W(2) W(1) 0]; 410 | end 411 | 412 | function Vh = Vhelper(W) 413 | Vh = [W(3, 2); W(1, 3) ; W(2,1)]; 414 | end 415 | 416 | function b = GSnorm(a) 417 | [m,n]=size(a); 418 | b=zeros(m,n); 419 | k=n; 420 | b(:,1)=a(:,1)./sqrt(sum(a(:,1).*a(:,1))); 421 | b(:,2)=a(:,2)-dot(b(:,1),a(:,2))*b(:,1); 422 | b(:,2)=b(:,2)./sqrt(sum(b(:,2).*b(:,2))); 423 | if k>2 424 | for i=3:k 425 | b(:,i)=a(:,i)-dot(b(:,2),a(:,i))*b(:,2)-dot(b(:,1),a(:,i))*b(:,1); 426 | if i>3 427 | for j=i-1:-1:3 428 | b(:,i)=b(:,i)-dot(b(:,j),a(:,i))*b(:,j); 429 | end 430 | 431 | end 432 | b(:,i)=b(:,i)./sqrt(sum(b(:,i).*b(:,i))); 433 | end 434 | end 435 | end 436 | 437 | function reset_params() 438 | global e1 e2 e3 R W Wh x v xt yt zt xdt ydt zdt xddt yddt zddt dt time; 439 | global dbarx dbarW timesteps Rt Rdes_previous wdes_previous I Rdes; 440 | global zW zx lW lv Km Ks fb; 441 | 442 | e1 = [1; 0; 0]; 443 | e2 = [0; 1; 0]; 444 | e3 = [0; 0; 1]; 445 | R = [e1 e2 e3]; 446 | W = [0; 0; 0]; 447 | Wh = [0 -W(3) W(2); 448 | W(3) 0 -W(1); 449 | -W(2) W(1) 0]; 450 | x = [0 0 0]'; 451 | v = [0 0 0]'; 452 | 453 | xt = []; 454 | yt = []; 455 | zt = []; 456 | 457 | xdt = [0 0]; 458 | ydt = [0 0]; 459 | zdt = [0 0]; 460 | 461 | xddt = []; 462 | yddt = []; 463 | zddt = []; 464 | 465 | dbarx = [0;0;0]; 466 | dbarW = 0; 467 | 468 | xerr_hist = []; 469 | yerr_hist = []; 470 | zerr_hist = []; 471 | 472 | %The timestep calculations 473 | timesteps = 0:dt:time; 474 | 475 | Rt = cell(length(timesteps), 1); 476 | Rdes_previous = zeros(3); 477 | wdes_previous = [0; 0; 0]; 478 | Rdes = zeros(3); 479 | 480 | I = eye(3); 481 | 482 | zW = [0; 0; 0]; 483 | zx = [0; 0; 0]; 484 | 485 | lW = Km * I; 486 | lv = Ks * I; 487 | 488 | fb = [0 0 0]'; 489 | end 490 | 491 | function desired_path() 492 | 493 | global xdest b1dest vdest adest timesteps; 494 | 495 | xdest = []; 496 | b1dest = []; 497 | vdest = []; 498 | adest = []; 499 | 500 | % syms t; 501 | % xdfn = [0.4*t; 0.4*sin(pi*t); 0.6*cos(pi*t)]; 502 | % 503 | % for n = 1 : length(timesteps) 504 | % xdest(:,n) = [0.4*timesteps(n) 0.4*sin(pi*timesteps(n)) 0.6*cos(pi*timesteps(n))]'; 505 | % b1dest(:,n) = [cos(pi*timesteps(n)) sin(pi*timesteps(n)) 0]'; 506 | % vdest(:,n) = [0.4 0.4*pi*cos(pi*timesteps(n)) -0.6*pi*sin(pi*timesteps(n))]'; 507 | % adest(:,n) = [0 -0.4*pi^2*sin(pi*timesteps(n)) -0.6*pi^2*cos(pi*timesteps(n))]'; 508 | % end 509 | 510 | for n = 1 : length(timesteps) 511 | xdest(:,n) = [0.6*cos(pi*timesteps(n)) 0.4*sin(pi*timesteps(n)) 0.6*timesteps(n)]'; 512 | b1dest(:,n) = [0 sin(pi*timesteps(n)) cos(pi*timesteps(n))]'; 513 | vdest(:,n) = [-0.6*pi*sin(pi*timesteps(n)) 0.4*pi*cos(pi*timesteps(n)) 0.6]'; 514 | adest(:,n) = [-0.6*pi^2*cos(pi*timesteps(n)) -0.4*pi^2*sin(pi*timesteps(n)) 0]'; 515 | end 516 | 517 | % syms t; 518 | % xdfn = [2*cos((pi*t)/10); 2*sin((pi*t)/10); (t/2)] 519 | % b1fn = diff(xdfn) / norm(diff(xdfn)) 520 | % diff(xdfn) 521 | % diff(diff(xdfn)) 522 | 523 | % for n = 1 : length(timesteps) 524 | % xdest(:,n) = [2*cos((pi*timesteps(n))/10); 2*sin((pi*timesteps(n))/10); (timesteps(n)/2)]; 525 | % b1dest(:,n) = [-(pi*sin((pi*timesteps(n))/10))/(5*((pi^2*abs(cos((timesteps(n)*pi)/10))^2)/25 + (pi^2*abs(sin((timesteps(n)*pi)/10))^2)/25 + 1/4)^(1/2)); 526 | % (pi*cos((pi*timesteps(n))/10))/(5((pi^2*abs(cos((timesteps(n)pi)/10))^2)/25 + (pi^2*abs(sin((timesteps(n)*pi)/10))^2)/25 + 1/4)^(1/2)); 527 | % 1/(2((pi^2*abs(cos((timesteps(n)*pi)/10))^2)/25 + (pi^2*abs(sin((timesteps(n)*pi)/10))^2)/25 + 1/4)^(1/2))]; 528 | % vdest(:,n) = [-(pi*sin((pi*timesteps(n))/10))/5; (pi*cos((pi*timesteps(n))/10))/5; 0.5]; 529 | % adest(:,n) = [-(pi^2*cos((pi*timesteps(n))/10))/50; -(pi^2*sin((pi*timesteps(n))/10))/50; 0]; 530 | % end 531 | 532 | end 533 | 534 | function [disx, disW] = dis_gen(n) 535 | dis_gain_x = 20; 536 | dis_gain_w = 6; 537 | 538 | step_disturbance = 0; 539 | 540 | scenario1 = [0.13*atan(n/2); 0.2*atan(n/2); 0.26*atan(n/2)]; 541 | scenario2 = [0.13; 0.2; 0.26]; 542 | scenario3 = [0.13*cos(0.1*pi*n); 0.2*cos(0.1*pi*n); 0.26*cos(0.1*pi*n)]; 543 | dis_fn = scenario2; 544 | 545 | disx = dis_gain_x * dis_fn; 546 | disW = dis_gain_w * dis_fn; 547 | 548 | if and(or(n < 5, n > 7), step_disturbance) 549 | disx = 0; 550 | disW = 0; 551 | end 552 | end -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Disturbance Observer-based Attitude Control of Quadrotor Drones 2 | 3 | This project aims to improve the control of quadrotor drones by implementing a disturbance observer-based controller that is capable of detecting and accounting for uncertainties during flight. The proposed controller is designed using state space modeling and takes into account external disturbances in all three axes, making it possible for the drone to perform better in turbulent conditions and provide increased accuracy in its motion control. 4 | 5 | ## Introduction 6 | Quadrotor drones are versatile unmanned aerial vehicles that are used for various applications in a wide range of domains. However, they are inherently an open-loop unstable and under-actuated system, which poses technical challenges that need to be overcome through robust, precise control of the drone at all times to perform given tasks effectively and accurately without human supervision. 7 | 8 | A classical Proportional-Integral-Derivative (PID) controller is commonly used in drones to control their motion. However, its performance is limited in the presence of external disturbances, as the controller does not take into account these external forces. This can cause the drone to deviate from the desired trajectory and oscillate. To address this, a disturbance observer can be used to attenuate the disturbances and reduce the oscillations. The observer's algorithm estimates the disturbance forces based on the measured trajectories and then compensates for the disturbances. The observer can then be used to modify the controller's output, thus allowing the drone to track the desired trajectory more accurately. 9 | 10 | ## Technical challenges 11 | The disturbance observer has been implemented in a limited scope to account for disturbances in 2D, that is, in the X and Y axis or to account for the ground effect in the vertical Z axis during take-off and landing. The challenge lies in attempting to implement a DOB that can handle disturbances in all three coordinate axes during take-off, flight and landing. 12 | 13 | ## Approach 14 | Instead of the classical control methods, a geometric tracking controller will be used. This type of controller uses feedback from the system's sensors to continuously adjust the control signals sent to the actuators to correct any deviations from the desired trajectory. The controller calculates the necessary correction by comparing the desired geometric path with the actual position of the system and computing the error between the two. This error is then used to generate the control signals that will drive the system back onto the desired path. The goal of a geometric tracking controller is to enable the system to follow the desired trajectory as closely as possible, with minimal error and maximum accuracy. 15 | 16 | To design the controller, the dynamics of the quadrotor are first modeled using state space equations. The equations of motion for the quadrotor are derived using the principles of rigid body dynamics and take into account the mass, inertia, and forces acting on the quadrotor. Next, the disturbance observer is designed using the state space equations. The disturbance observer estimates the external disturbances acting on the quadrotor and uses this information to adjust the control inputs to the quadrotor. Finally, the disturbance observer is combined with a feedback controller to form the overall control system. The feedback controller uses the estimated disturbance and the desired position of the quadrotor to compute the control inputs, which are then applied to the quadrotor to maintain a stable flight. 17 | 18 | ## Project overview 19 | This project proposes a disturbance observer-based controller for a quadrotor drone. To evaluate the effectiveness of the controller, simulations are performed in a simulated environment. The inputs to the system include the desired position of the quadrotor and external disturbances in the form of step and impulse responses. The output of the system is expected to be a stable flight along all three axes, even under moderate and unexpected disturbances, such as wind gusts. More details can be found in the [Project Report](report/Effect_of_DOB_on_Attitude_Control.pdf). 20 |
21 | 22 | ## Simulation 23 | ![](simulation.gif) -------------------------------------------------------------------------------- /Report/Effect_of_DOB_on_Attitude_Control.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/walstanb/QuadDOB-control/8830ce4a574bd6b47efc2d529b5ebee0923aeddb/Report/Effect_of_DOB_on_Attitude_Control.pdf -------------------------------------------------------------------------------- /Scenario1/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/walstanb/QuadDOB-control/8830ce4a574bd6b47efc2d529b5ebee0923aeddb/Scenario1/1.png -------------------------------------------------------------------------------- /Scenario1/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/walstanb/QuadDOB-control/8830ce4a574bd6b47efc2d529b5ebee0923aeddb/Scenario1/2.png -------------------------------------------------------------------------------- /Scenario1/3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/walstanb/QuadDOB-control/8830ce4a574bd6b47efc2d529b5ebee0923aeddb/Scenario1/3.png -------------------------------------------------------------------------------- /Scenario1/4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/walstanb/QuadDOB-control/8830ce4a574bd6b47efc2d529b5ebee0923aeddb/Scenario1/4.png -------------------------------------------------------------------------------- /Scenario1/5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/walstanb/QuadDOB-control/8830ce4a574bd6b47efc2d529b5ebee0923aeddb/Scenario1/5.png -------------------------------------------------------------------------------- /Scenario2/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/walstanb/QuadDOB-control/8830ce4a574bd6b47efc2d529b5ebee0923aeddb/Scenario2/1.png -------------------------------------------------------------------------------- /Scenario2/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/walstanb/QuadDOB-control/8830ce4a574bd6b47efc2d529b5ebee0923aeddb/Scenario2/2.png -------------------------------------------------------------------------------- /Scenario2/3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/walstanb/QuadDOB-control/8830ce4a574bd6b47efc2d529b5ebee0923aeddb/Scenario2/3.png -------------------------------------------------------------------------------- /Scenario2/4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/walstanb/QuadDOB-control/8830ce4a574bd6b47efc2d529b5ebee0923aeddb/Scenario2/4.png -------------------------------------------------------------------------------- /Scenario2/5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/walstanb/QuadDOB-control/8830ce4a574bd6b47efc2d529b5ebee0923aeddb/Scenario2/5.png -------------------------------------------------------------------------------- /Scenario2a/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/walstanb/QuadDOB-control/8830ce4a574bd6b47efc2d529b5ebee0923aeddb/Scenario2a/1.png -------------------------------------------------------------------------------- /Scenario2a/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/walstanb/QuadDOB-control/8830ce4a574bd6b47efc2d529b5ebee0923aeddb/Scenario2a/2.png -------------------------------------------------------------------------------- /Scenario2a/3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/walstanb/QuadDOB-control/8830ce4a574bd6b47efc2d529b5ebee0923aeddb/Scenario2a/3.png -------------------------------------------------------------------------------- /Scenario2a/4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/walstanb/QuadDOB-control/8830ce4a574bd6b47efc2d529b5ebee0923aeddb/Scenario2a/4.png -------------------------------------------------------------------------------- /Scenario2a/5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/walstanb/QuadDOB-control/8830ce4a574bd6b47efc2d529b5ebee0923aeddb/Scenario2a/5.png -------------------------------------------------------------------------------- /Tracking_SE3_vs_w_CAM.m: -------------------------------------------------------------------------------- 1 | clear; 2 | clc; 3 | 4 | % skew-symmetric operator 5 | ssmat = @(x) [0 -x(3) x(2); x(3) 0 -x(1); -x(2) x(1) 0]; 6 | % inverse of skew-symmetric operator 7 | vmap = @(x) [-x(2,3) x(1,3) -x(1,2)]'; 8 | 9 | % Vehicle properties 10 | m = 4.34; 11 | J = diag([0.0820, 0.0845, 0.1377]); 12 | 13 | % Misc things 14 | e1 = [1 0 0]'; 15 | e2 = [0 1 0]'; 16 | e3 = [0 0 1]'; 17 | g = 9.8; 18 | 19 | % Initial conditions 20 | x = [0 0 0]'; 21 | v = [0 0 0]'; 22 | R = eul2rotm([0 pi-0.01 0]); 23 | omega = [0 0 0]'; 24 | Rd = zeros(3); 25 | Rd_prev = zeros(3); 26 | 27 | % Setpoint 28 | xd = [0 0 10]'; 29 | vd = [0 0 0]'; 30 | b1d = [1 0 0]'; 31 | 32 | % Controller gains 33 | kx = 10; 34 | kv = 10; 35 | kR = 10; 36 | komega = 10; 37 | 38 | % Main loop 39 | dt = 0.001; 40 | end_time = 10; 41 | 42 | xlist = []; 43 | ylist = []; 44 | zlist = []; 45 | 46 | 47 | for i = 1:end_time/dt 48 | % Translational error computations 49 | ex = x - xd; 50 | ev = v - vd; 51 | 52 | % Compute desired rotation 53 | b3d = -(-kx*ex - kv*ev - m*g*e3)/vecnorm(-kx*ex - kv*ev - m*g*e3); 54 | b2d = cross(b3d, b1d)/vecnorm(cross(b3d, b1d)); 55 | Rd = [cross(b2d, b3d), b2d, b3d]; 56 | 57 | % Compute desired angular velocity 58 | dRd = (Rd - Rd_prev)/dt; 59 | omegad = vmap(Rd'*dRd); 60 | Rd_prev = Rd; 61 | 62 | % Rotational/Angular error computations 63 | eR = 0.5*vmap(Rd'*R-R'*Rd); 64 | eomega = omega - R' * Rd * omegad; 65 | 66 | % Control input computation 67 | f = -dot((-kx*ex - kv*ev - m*g*e3), R*e3); 68 | M = -kR*eR - komega*eomega + cross(omega, J*omega) - J*(ssmat(omega)*R'*Rd*omegad); 69 | 70 | % Control Allocation Matrix 71 | u = [f M']'; 72 | d = 0.25; ctf = 0.1; 73 | CAM = [1 1 1 1; 0 -d 0 d; d 0 -d 0; -ctf ctf -ctf ctf]; 74 | fi = CAM\u; 75 | fi = min(fi, 20); 76 | fi = max(fi, -20); 77 | u = CAM * fi; 78 | f = u(1); 79 | M = u(2:4); 80 | 81 | % System evolution 82 | a = g*e3 - f*R*e3/m; 83 | v = v + a*dt; 84 | x = x + v*dt; 85 | 86 | xlist(i) = x(1); 87 | ylist(i) = x(2); 88 | zlist(i) = x(3); 89 | 90 | domega = J\(M - cross(omega,J*omega)); 91 | omega = omega + domega * dt; 92 | R = R + R * ssmat(omega) * dt; 93 | R = orthnorm(R); 94 | 95 | aHistory(:,i) = a; 96 | vHistory(:,i) = v; 97 | xHistory(:,i) = x; 98 | domegaHistory(:,i) = domega; 99 | omegaHistory(:,i) = omega; 100 | RHistory(:,:,i) = R; 101 | exHistory(:,i) = ex; 102 | evHistory(:,i) = ev; 103 | eRHistory(:,i) = eR; 104 | eomegaHistory(:,i) = eomega; 105 | end 106 | 107 | figure 108 | plot3(xlist, ylist, zlist); 109 | 110 | 111 | 112 | % time = 0:dt:end_time-dt; 113 | % figure(1);clf 114 | % sp331 = subplot(3,3,1); hold on; grid on; grid minor; 115 | % plot(time, aHistory); legend("ax", "ay", "az"); title("Linear Acceleration"); 116 | % sp332 = subplot(3,3,2); hold on; grid on; grid minor; 117 | % plot(time, vHistory); legend("vx", "vy", "vz"); title("Linear Velocity"); 118 | % sp333 = subplot(3,3,3); hold on; grid on; grid minor; 119 | % plot(time, xHistory); legend("x", "y", "z"); title("Linear Position"); 120 | % sp334 = subplot(3,3,4); hold on; grid on; grid minor; 121 | % plot(time, domegaHistory); legend("dp", "dq", "dr"); title("Angular Acceleration"); 122 | % sp335 = subplot(3,3,5); hold on; grid on; grid minor; 123 | % plot(time, omegaHistory); legend("p", "q", "r"); title("Angular Velocity"); 124 | % sp336 = subplot(3,3,6); hold on; grid on; grid minor; 125 | % plot(time, rotm2eul(RHistory)); legend("yaw", "pitch", "roll"); title("Euler Angles"); 126 | % sp337 = subplot(3,3,7); hold on; grid on; grid minor; 127 | % plot(time, exHistory); legend("ex", "ey", "ez"); title("Position Error"); 128 | % sp338 = subplot(3,3,8); hold on; grid on; grid minor; 129 | % plot(time, eRHistory); legend("eRx", "eRy", "eRz"); title("Rotational Error"); 130 | % sp339 = subplot(3,3,9); hold on; grid on; grid minor; 131 | % plot(time, eomegaHistory); legend("eomegax", "eomegay", "eomegaz"); title("Anguler Velocity Error"); 132 | % 133 | 134 | function R = orthnorm(R) 135 | R(:,1) = R(:,1)/vecnorm(R(:,1)); 136 | R(:,2) = cross(R(:,3), R(:,1))/vecnorm(cross(R(:,3), R(:,1))); 137 | R(:,3) = cross(R(:,1), R(:,2))/vecnorm(cross(R(:,1), R(:,2))); 138 | end -------------------------------------------------------------------------------- /simulation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/walstanb/QuadDOB-control/8830ce4a574bd6b47efc2d529b5ebee0923aeddb/simulation.gif -------------------------------------------------------------------------------- /vwsmc.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/walstanb/QuadDOB-control/8830ce4a574bd6b47efc2d529b5ebee0923aeddb/vwsmc.mat --------------------------------------------------------------------------------