├── Exam 1 ├── DCHW42.m ├── Exam 1.docx ├── Exam 1.pdf ├── ddcExam1.m ├── formationcontrol.m └── practice1.m └── README.md /Exam 1/DCHW42.m: -------------------------------------------------------------------------------- 1 | clear all; clc;close all; 2 | 3 | 4 | % Simulating Dynamics for leader & 4 agents 5 | [t,Zdot] = ode23(@frmctrl,[0:0.01:40],[zeros(2,1);ones(2,1);2*rand(8,1);2*rand(8,1)]); 6 | 7 | % Positions of the leader & agents 8 | figure; 9 | plot(10,10,'o','MarkerFaceColor','green'); 10 | plot(1.5,2.5,'o','MarkerFaceColor','blue'); 11 | plot(2.5,1.5,'o','MarkerFaceColor','blue'); 12 | plot(3,4,'o','MarkerFaceColor','blue'); 13 | plot(4,3,'o','MarkerFaceColor','blue'); 14 | plot(4,5,'o','MarkerFaceColor','blue'); 15 | plot(5.6,4,'o','MarkerFaceColor','blue'); 16 | plot(Zdot(:,5),Zdot(:,6)) % Node 1 17 | hold on 18 | plot(Zdot(:,9),Zdot(:,10)) % Node 2 19 | hold on 20 | plot(Zdot(:,13),Zdot(:,14)) % Node 3 21 | hold on 22 | plot(Zdot(:,17),Zdot(:,18)) % Node 4 23 | hold on 24 | plot(Zdot(:,1),Zdot(:,2)) % Leader 25 | legend('1','2','3','4','Leader') 26 | grid on; 27 | title('Positions of Leader & agents') 28 | xlabel('x');ylabel('y'); 29 | p1 = plot(Zdot(:,5),Zdot(:,6),'o'); 30 | p2 = plot(Zdot(:,9),Zdot(:,10),'o'); 31 | p3 = plot(Zdot(:,13),Zdot(:,14),'o'); 32 | p4 = plot(Zdot(:,17),Zdot(:,18),'o'); 33 | p = plot(Zdot(:,1),Zdot(:,2),'o','MarkerFaceColor','red'); 34 | hold off 35 | % Plot Animations 36 | for k = 1:size(t,1) 37 | p.XData = Zdot(k,1); 38 | p.YData = Zdot(k,2); 39 | p1.XData = Zdot(k,5); 40 | p1.YData = Zdot(k,6); 41 | p2.XData = Zdot(k,9); 42 | p2.YData = Zdot(k,10); 43 | p3.XData = Zdot(k,13); 44 | p3.YData = Zdot(k,14); 45 | p4.XData = Zdot(k,17); 46 | p4.YData = Zdot(k,18); 47 | drawnow limitrate 48 | end 49 | drawnow 50 | % Velocities of Leader & agents 51 | figure; 52 | plot(Zdot(:,7),Zdot(:,8)) % Node 1 53 | hold on 54 | plot(Zdot(:,11),Zdot(:,12)) % Node 2 55 | hold on 56 | plot(Zdot(:,15),Zdot(:,16)) % Node 3 57 | hold on 58 | plot(Zdot(:,19),Zdot(:,20)) % Node 4 59 | hold on 60 | plot(Zdot(:,3),Zdot(:,4)) % Leader 61 | legend('1','2','3','4','Leader') 62 | grid on; 63 | title('Velocities of Leader & agents') 64 | xlabel('V_x');ylabel('V_y'); 65 | 66 | % Leader Position Phase Plot 67 | figure 68 | plot(Zdot(:,1),Zdot(:,2)) 69 | title('Leader position'); 70 | xlabel('x');ylabel('y'); 71 | 72 | 73 | 74 | function Zdot = frmctrl(t,z) 75 | vX = z(3) 76 | vY = z(4) 77 | global kG K kF 78 | 79 | kG = -10; 80 | K = [0.5 -1 3 -1 -2 4.5]; 81 | 82 | obs = [1.5 2.5 3 4 4 5.6; 83 | 2 1 4 3 5 4]; 84 | 85 | %% Vector to Goal at (10,10) 86 | r = sqrt((10-z(1))^2 + (10-z(2))^2); 87 | 88 | %% Force on the robot due to Goal (x-direction) 89 | fXG = (kG*(z(1) - 10))/(r); 90 | %% Force on the robot due to Goal (y-direction) 91 | fYG = (kG*(z(2) - 10))/(r); 92 | FO1x = 0; 93 | FO1y = 0; 94 | 95 | 96 | 97 | %% Force on the leader and follower due to obstacles 98 | for i = 1:6 99 | ObsCord = obs(:,i); 100 | oX = ObsCord(1); 101 | oY = ObsCord(2); 102 | rOL = sqrt((oX-z(1))^2 + ((oY-z(2))^2)); 103 | fOX = (K(i)*(z(1) - oX))/(rOL); 104 | fOY = (K(i)*(z(2) - oY))/(rOL); 105 | FO1x = FO1x + fOX; 106 | FO1y = FO1y + fOY; 107 | end 108 | 109 | %% Total Forces on the robot (x & y direction) 110 | fX = fXG+FO1x; 111 | fY = fYG+FO1y; 112 | 113 | if r<0.1 114 | fX = 0; 115 | fY = 0; 116 | vX = 0; 117 | vY = 0; 118 | 119 | end 120 | 121 | % Adjacency Matrix of the formation graph 122 | A = [0 0 1/2 0; 123 | 1/2 0 0 0; 124 | 1/2 1/2 0 0; 125 | 0 1/2 0 0]; 126 | D = diag(sum(A,2)); 127 | L = (D - A); % Graph Laplacian Matrix 128 | 129 | gamma = 3.75; 130 | % Offset of the agents wrt leader 131 | delta0 = [1 1]; 132 | delta1 = [1 -1]; 133 | delta2 = [-1 -1]; 134 | delta3 = [-1 1]; 135 | 136 | delta =[delta0'; zeros(2,1) ; delta1'; zeros(2,1) ; delta2' ;zeros(2,1) ; delta3'; zeros(2,1)]; 137 | 138 | % PD Gain Matrices 139 | kp = eye(2); 140 | kd = gamma*eye(2); 141 | K = [kp kd]; 142 | 143 | c = 550; 144 | % Agent Node Dynamics Matrices 145 | A_sys = [zeros(2) eye(2); 146 | zeros(2) zeros(2)]; 147 | B_sys = [zeros(2); eye(2)]; 148 | 149 | % Pinning Gain Matrix 150 | G = [0 0 0 0;0 1/2 0 0;0 0 0 0;0 0 0 0]; 151 | 152 | % Kronecker Products 153 | kron1 = kron(eye(4),A_sys); 154 | cL = c*(L + G); 155 | BK = B_sys*K; 156 | kron2 = kron(cL,BK); 157 | Ac = kron1 - kron2; 158 | 159 | % Leader Dynamics 160 | %x0 = A_sys * z(1:4); 161 | 162 | % Node Dynamics equation 163 | z0 = Ac*z(5:20) + kron2 *delta + kron2*([z(1:2); z(3:4); z(1:2); z(3:4); z(1:2); z(3:4); z(1:2); z(3:4)]); 164 | 165 | Zdot = [x0;z0]; 166 | 167 | end 168 | -------------------------------------------------------------------------------- /Exam 1/Exam 1.docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yangwangaaa/Formation-control-and-concencus-in-distributed-system/0b8de0f3d99a82fafcb9232c6a05e525bafa2640/Exam 1/Exam 1.docx -------------------------------------------------------------------------------- /Exam 1/Exam 1.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yangwangaaa/Formation-control-and-concencus-in-distributed-system/0b8de0f3d99a82fafcb9232c6a05e525bafa2640/Exam 1/Exam 1.pdf -------------------------------------------------------------------------------- /Exam 1/ddcExam1.m: -------------------------------------------------------------------------------- 1 | clear all; clc;close all; 2 | 3 | 4 | %% Simulating Dynamics for 4 agents and leader 5 | [t,Zdot] = ode23('formationcontrol',[0:0.01:50],[2*rand(8,1);2*rand(8,1);zeros(2,1);ones(2,1)]); 6 | 7 | %% Positions of the agents and leader 8 | figure; 9 | plot(Zdot(:,1),Zdot(:,2)) %agent1 10 | hold on 11 | plot(Zdot(:,5),Zdot(:,6)) %agent2 12 | plot(Zdot(:,9),Zdot(:,10)) %agent3 13 | plot(Zdot(:,13),Zdot(:,14)) %agent4 14 | plot(Zdot(:,17),Zdot(:,18)) %Leader 15 | 16 | 17 | grid on; 18 | title('Positions of agents and leader') 19 | xlabel('x');ylabel('y'); 20 | 21 | p1 = plot(Zdot(:,1),Zdot(:,2),'s'); 22 | p2 = plot(Zdot(:,5),Zdot(:,6),'s'); 23 | p3 = plot(Zdot(:,9),Zdot(:,10),'s'); 24 | p4 = plot(Zdot(:,13),Zdot(:,14),'s'); 25 | p = plot(Zdot(:,17),Zdot(:,18),'o','MarkerFaceColor',[0.91 0.41 0.17]); 26 | legend('1','2','3','4','Leader') 27 | hold off 28 | %% Plot Animations 29 | for k = 1:size(t,1) 30 | p1.XData = Zdot(k,1); 31 | p1.YData = Zdot(k,2); 32 | p2.XData = Zdot(k,5); 33 | p2.YData = Zdot(k,6); 34 | p3.XData = Zdot(k,9); 35 | p3.YData = Zdot(k,10); 36 | p4.XData = Zdot(k,13); 37 | p4.YData = Zdot(k,14); 38 | p.XData = Zdot(k,17); 39 | p.YData = Zdot(k,18); 40 | 41 | drawnow limitrate 42 | 43 | end 44 | drawnow 45 | %% Velocities of agents and leader 46 | figure; 47 | plot(Zdot(:,3),Zdot(:,4)) %agent1 48 | hold on 49 | plot(Zdot(:,7),Zdot(:,8)) %agent2 50 | plot(Zdot(:,11),Zdot(:,12)) %agent3 51 | plot(Zdot(:,15),Zdot(:,16)) %agent4 52 | plot(Zdot(:,19),Zdot(:,20)) %Leader 53 | legend('1','2','3','4','Leader') 54 | grid on; 55 | title('Velocities of agents and Leader') 56 | xlabel('V_x');ylabel('V_y'); 57 | 58 | %% Leader Position Phase Plot 59 | figure 60 | plot(Zdot(:,17),Zdot(:,18)) 61 | title('Leader position'); 62 | xlabel('x');ylabel('y'); 63 | -------------------------------------------------------------------------------- /Exam 1/formationcontrol.m: -------------------------------------------------------------------------------- 1 | function Zdot = formationcontrol(t,z) 2 | %% Adjacency Matrix of the formation graph 3 | 4 | a = [0 0 0.5 0; 5 | 0.5 0 0 0; 6 | 0.5 0.5 0 0; 7 | 0 0.5 0 0;]; 8 | 9 | d = diag(sum(a,2)); 10 | %% Graph Laplacian Matrix 11 | 12 | l = (d - a); 13 | %% Offset of the agents wrt leader 14 | del0 = [1 1]; 15 | del1 = [1 -1]; 16 | del2 = [-1 -1]; 17 | del3 = [-1 1]; 18 | 19 | del = [del0'; zeros(1,2)';del1'; zeros(1,2)';del2' ;zeros(1,2)';del3'; zeros(1,2)']; 20 | 21 | %% Setting the value of gamma 22 | gamma = 3; 23 | %% Pinning Gain Matrix 24 | G = [0 0 0 0;0 0.5 0 0;0 0 0 0;0 0 0 0]; 25 | 26 | %% PD Gain Matrices 27 | kp = eye(2); 28 | kd = gamma*eye(2); 29 | K = [kp kd]; 30 | 31 | c = 550; 32 | %% Agent Node Dynamics Matrices 33 | aSys = [zeros(2) eye(2); 34 | zeros(2) zeros(2)]; 35 | bSys = [zeros(2); eye(2)]; 36 | 37 | %% Kronecker Products 38 | kron1 = kron(eye(4),aSys); 39 | cL = c*(l + G); 40 | BK = bSys*K; 41 | kron2 = kron(cL,BK); 42 | Ac = kron1-kron2; 43 | 44 | %% Leader Dynamics 45 | x0 = aSys * z(17:20); 46 | 47 | %% Node Dynamics equation 48 | z0 = Ac*z(1:16) + kron2*del + kron2*([z(17:20); z(17:20); z(17:20); z(17:20)]); 49 | Zdot = [z0;x0]; 50 | 51 | end 52 | -------------------------------------------------------------------------------- /Exam 1/practice1.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yangwangaaa/Formation-control-and-concencus-in-distributed-system/0b8de0f3d99a82fafcb9232c6a05e525bafa2640/Exam 1/practice1.m -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Formation-control-and-concencus-in-distributed-system 2 | A MATLAB project on formation control for mobile UAVs 3 | --------------------------------------------------------------------------------