├── Formation control of multi-agent systems a graph rigidity approach (Cai, Xiaoyu Feemster, Matthew Queiroz etc.) (z-lib.org).pdf ├── README.md ├── code ├── SI_DFM_2D_SYSU │ ├── Desired_S_distance.m │ ├── Desired_U_distance.m │ ├── Desired_Y_distance.m │ ├── Desired_velocity.m │ ├── SI_dynamic_fomation_manv_fig_plot.m │ ├── SI_dynamic_fomation_manv_func.m │ └── SI_dynamic_fomation_manv_main.m ├── SI_dynamic_fomation_manv_results.mat ├── controller1.m ├── controller2.m ├── controller3.m ├── controller4.m ├── controller5.m ├── controller6.m ├── readonly │ ├── 6formation_results.mat │ ├── JudgeStudentsPoint.m │ ├── quadEOM_readonly.m │ └── quadModel_readonly.m ├── run6formation.m ├── test_trajectory.m └── utils │ ├── QuatToRot.m │ ├── RPYtoRot_ZXY.m │ ├── R_to_quaternion.m │ ├── R_to_ypr.m │ ├── RotToQuat.m │ ├── RotToRPY_ZXY.m │ ├── quaternion_to_R.m │ └── ypr_to_R.m └── project instruction.docx /Formation control of multi-agent systems a graph rigidity approach (Cai, Xiaoyu Feemster, Matthew Queiroz etc.) (z-lib.org).pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SYSU-HI-LAB/Introduction_to_multi_agent_control/168686e8608951fc5df224114d3e01949aef2506/Formation control of multi-agent systems a graph rigidity approach (Cai, Xiaoyu Feemster, Matthew Queiroz etc.) (z-lib.org).pdf -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Introduction_to_multi_agent_control 2 | git clone https://github.com/SYSU-HI-LAB/Introduction_to_multi_agent_control.git 3 | 4 | Read the project instruction very very very carefully !!!! 5 | -------------------------------------------------------------------------------- /code/SI_DFM_2D_SYSU/Desired_S_distance.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % This function defines the desired, time-varying distances and their 3 | % derivative 4 | % Input variables: time t, number of agents n, and Adjacency matrix Adj 5 | % Output variables: desired distance vector d(t) and derivative d_dot(t) 6 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 7 | function [d,d_dot] = Desired_S_distance(t,n,Adj) 8 | 9 | % Calculation of d(t) 10 | A = 0.5; % Amplitude of AC component of desired distance 11 | w = 1.3; % Frequency of AC component of desired distance 12 | AC = 0.5; % AC component of desired distances 13 | 14 | % x coordinate of framework F*(t) 15 | x_coor = [0; AC/2; -AC/2;-AC/2; 0; AC/2]; 16 | % y coordinate of framework F*(t) 17 | y_coor = [AC/2+AC*cos(pi/6); AC/2; AC/2;-AC/2; -AC/2-AC*cos(pi/6);-AC/2]; 18 | q_star2 = [x_coor'; y_coor']; % 2xn vector 19 | d = zeros(n,n); % initialize the desired distance 20 | d_dot = zeros(2*n-3,1); 21 | for i = 1:n 22 | for j = 1:n 23 | d(i,j) = sqrt((x_coor(i)-x_coor(j))^2 + (y_coor(i)-y_coor(j))^2); 24 | end 25 | end 26 | 27 | % Calculation of d_dot(t) 28 | ACdot = 0 ; % 29 | 30 | % xdot coordinate of framework F*(t) 31 | x_coor_dot = [0; ACdot/2; -ACdot/2; -ACdot/2; 0; ACdot/2]; 32 | % ydot coordinate of framework F*(t) 33 | y_coor_dot = [ACdot/2+ACdot*cos(pi/6); ACdot/2; ACdot/2;-ACdot/2; -ACdot/2-ACdot*cos(pi/6); -ACdot/2]; 34 | 35 | q_dot_star2 = [x_coor_dot'; y_coor_dot']; % 2xn vector 36 | 37 | for i = 1:n-1 38 | for j = i+1:n 39 | if Adj(i,j) == 1 40 | d_dot(i,j) = (q_star2(:,i)-q_star2(:,j))'... 41 | *(q_dot_star2(:,i)-q_dot_star2(:,j))/d(i,j); 42 | end 43 | end 44 | end -------------------------------------------------------------------------------- /code/SI_DFM_2D_SYSU/Desired_U_distance.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % This function defines the desired, time-varying distances and their 3 | % derivative 4 | % Input variables: time t, number of agents n, and Adjacency matrix Adj 5 | % Output variables: desired distance vector d(t) and derivative d_dot(t) 6 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 7 | function [d,d_dot] = Desired_U_distance(t,n,Adj) 8 | AC = 0.5; % AC component of desired distances 9 | x_coor = [-AC/2; AC/2; AC/2;AC/4; -AC/4; -AC/2]; 10 | y_coor = [AC/2; AC/2; -AC/2;-AC/2; -AC/2; -AC/2]; 11 | q_star2 = [x_coor'; y_coor']; % 2xn vector 12 | 13 | d = zeros(n,n); % initialize the desired distance 14 | d_dot = zeros(2*n-3,1); 15 | 16 | for i = 1:n 17 | for j = 1:n 18 | d(i,j) = sqrt((x_coor(i)-x_coor(j))^2 + (y_coor(i)-y_coor(j))^2); 19 | end 20 | end 21 | 22 | 23 | ACdot = 0 ; 24 | 25 | x_coor_dot = [-ACdot/2; ACdot/2; ACdot/2; ACdot/4; -ACdot/4; -ACdot/2]; 26 | y_coor_dot = [ACdot/2; ACdot/2; -ACdot/2; -ACdot/2; -ACdot/2; -ACdot/2]; 27 | q_dot_star2 = [x_coor_dot'; y_coor_dot']; % 2xn vector 28 | 29 | for i = 1:n-1 30 | for j = i+1:n 31 | if Adj(i,j) == 1 32 | d_dot(i,j) = (q_star2(:,i)-q_star2(:,j))'... 33 | *(q_dot_star2(:,i)-q_dot_star2(:,j))/d(i,j); 34 | end 35 | end 36 | end -------------------------------------------------------------------------------- /code/SI_DFM_2D_SYSU/Desired_Y_distance.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % This function defines the desired, time-varying distances and their 3 | % derivative 4 | % Input variables: time t, number of agents n, and Adjacency matrix Adj 5 | % Output variables: desired distance vector d(t) and derivative d_dot(t) 6 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 7 | function [d,d_dot] = Desired_Y_distance(t,n,Adj) 8 | 9 | % Calculation of d(t) 10 | A = 0.5; % Amplitude of AC component of desired distance 11 | w = 1.3; % Frequency of AC component of desired distance 12 | AC = 0.5; 13 | % x_coor = [-(AC)/2; AC/2; AC/4;0;0;-AC/4]; 14 | % % y coordinate of framework F*(t) 15 | % y_coor = [cos(pi/3)*AC/2; cos(pi/3)*AC/2; 0;-cos(pi/3)*AC/2; -cos(pi/3)*AC*2 ;0 ]; 16 | x_coor = [-(AC); AC; 0;0;0;0]; 17 | % y coordinate of framework F*(t) 18 | y_coor = [cos(pi/6)*AC + AC/2; cos(pi/6)*AC + AC/2; AC/2;0; -AC ;-AC/2 ]; 19 | 20 | 21 | q_star2 = [x_coor'; y_coor']; % 2xn vector 22 | 23 | d = zeros(n,n); % initialize the desired distance 24 | d_dot = zeros(2*n-3,1); 25 | 26 | for i = 1:n 27 | for j = 1:n 28 | d(i,j) = sqrt((x_coor(i)-x_coor(j))^2 + (y_coor(i)-y_coor(j))^2); 29 | end 30 | end 31 | 32 | % Calculation of d_dot(t) 33 | ACdot = 0 ; 34 | 35 | 36 | x_coor_dot = [-(ACdot); ACdot; 0; 0;0;0]; 37 | y_coor_dot = [cos(pi/6)*ACdot + ACdot/2; cos(pi/6)*ACdot+ACdot/2 ;ACdot/2; 0; -ACdot; -ACdot/2 ]; 38 | 39 | 40 | 41 | q_dot_star2 = [x_coor_dot'; y_coor_dot']; % 2xn vector 42 | 43 | for i = 1:n-1 44 | for j = i+1:n 45 | if Adj(i,j) == 1 46 | d_dot(i,j) = (q_star2(:,i)-q_star2(:,j))'... 47 | *(q_dot_star2(:,i)-q_dot_star2(:,j))/d(i,j); 48 | end 49 | end 50 | end -------------------------------------------------------------------------------- /code/SI_DFM_2D_SYSU/Desired_velocity.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % This function defines the desired velocity 3 | % Input variable: time t 4 | % Output variable: desired velocity vd 5 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 6 | function [vd] = Desired_velocity(t) 7 | vd = [1;0.3*cos(t)]; -------------------------------------------------------------------------------- /code/SI_DFM_2D_SYSU/SI_dynamic_fomation_manv_fig_plot.m: -------------------------------------------------------------------------------- 1 | 2 | % plot SYSU 3 | clear 4 | clc 5 | close all 6 | load '../SI_dynamic_fomation_manv_results.mat' 7 | fs = 24; % Font size in the figure 8 | avgMs = 5; % Average marker size 9 | lw = 3.5; % Linewidth 10 | tf = max(t); 11 | Adj_plot_s = [0, 1, 1, 0, 0, 0; ... 12 | 1, 0, 0, 0, 0, 0; ... 13 | 1, 0, 0, 0, 0, 1; ... 14 | 0, 0, 0, 0, 1, 0; ... 15 | 0, 0, 0, 1, 0, 1; ... 16 | 0, 0, 1, 0, 1, 0]; 17 | 18 | %% Plot trajectory 19 | 20 | %% plot S initial 21 | figure 22 | xlim([-1, 25]); 23 | ylim([-2.1, 10]) 24 | hold on 25 | set(gca, 'DataAspectRatio', [1, 1, 1], 'Box', 'on', 'FontSize', fs) 26 | for ii = 1:1:length(t) 27 | for jj = 1:n 28 | plot(xx(ii, jj), yy(ii, jj), 'k.', 'MarkerSize', avgMs); 29 | end 30 | if (ii == 1) || (ii == length(t)) 31 | for i0 = 1:n - 1 32 | for j0 = i0 + 1:n 33 | if Adj_plot_s(i0, j0) == 1 34 | line([xx(ii, i0), xx(ii, j0)], [yy(ii, i0), yy(ii, j0)], ... 35 | 'LineWidth', lw) 36 | 37 | end 38 | end 39 | end 40 | end 41 | % pause(0.05) % Uncomment this if animation is needed 42 | end 43 | for i1 = 1:n 44 | hh1 = plot(xx(3, i1), yy(3, i1), 'rs', 'MarkerSize', 1.4*avgMs); 45 | hh2 = plot(xx(length(t), i1), yy(length(t), i1), 'ro', ... 46 | 'MarkerSize', 1.4*avgMs); 47 | end 48 | 49 | %% plot S->Y 50 | for ii = 1:1:length(t) 51 | for jj = 1:n 52 | plot(xx_Y(ii, jj), yy_Y(ii, jj), 'k.', 'MarkerSize', avgMs); 53 | end 54 | if (ii == 1) 55 | for i0 = 1:n - 1 56 | for j0 = i0 + 1:n 57 | if Adj_plot_s(i0, j0) == 1 58 | line([xx_Y(ii, i0), xx_Y(ii, j0)], [yy_Y(ii, i0), yy_Y(ii, j0)], ... 59 | 'LineWidth', lw) 60 | end 61 | end 62 | end 63 | end 64 | % pause(0.05) % Uncomment this if animation is needed 65 | end 66 | for i1 = 1:n 67 | hh1 = plot(xx_Y(3, i1), yy_Y(3, i1), 'rs', 'MarkerSize', 1.4*avgMs); 68 | hh2 = plot(xx_Y(length(t), i1), yy_Y(length(t), i1), 'ro', ... 69 | 'MarkerSize', 1.4*avgMs); 70 | end 71 | 72 | 73 | %% plot Y --> s 74 | Adj_plot_y = [0, 0, 1, 0, 0, 0; ... 75 | 0, 0, 1, 0, 0, 0; ... 76 | 1, 1, 0, 1, 0, 1; ... 77 | 0, 0, 1, 0, 0, 1; ... 78 | 0, 0, 0, 0, 0, 1; ... 79 | 0, 0, 0, 0, 1, 0]; 80 | 81 | for ii = 1:1:length(t) 82 | for jj = 1:n 83 | plot(xx_S(ii, jj), yy_S(ii, jj), 'k.', 'MarkerSize', avgMs); 84 | end 85 | if (ii == 1) 86 | for i0 = 1:n - 1 87 | for j0 = i0 + 1:n 88 | if Adj_plot_y(i0, j0) == 1 89 | line([xx_S(ii, i0), xx_S(ii, j0)], [yy_S(ii, i0), yy_S(ii, j0)], ... 90 | 'LineWidth', lw) 91 | end 92 | end 93 | end 94 | end 95 | % pause(0.05) % Uncomment this if animation is needed 96 | end 97 | for i1 = 1:n 98 | hh1 = plot(xx_S(2, i1), yy_S(2, i1), 'rs', 'MarkerSize', 1.4*avgMs); 99 | hh2 = plot(xx_S(length(t), i1), yy_S(length(t), i1), 'ro', ... 100 | 'MarkerSize', 1.4*avgMs); 101 | end 102 | 103 | %% plot s--> u 104 | Adj_plot_s = [0, 1, 1, 0, 0, 0; ... 105 | 1, 0, 0, 0, 0, 0; ... 106 | 1, 0, 0, 0, 0, 1; ... 107 | 0, 0, 0, 0, 1, 0; ... 108 | 0, 0, 0, 1, 0, 1; ... 109 | 0, 0, 1, 0, 1, 0]; 110 | Adj_plot_u = [0, 0, 1, 0, 0, 0; ... 111 | 0, 0, 0, 0, 0, 1; ... 112 | 1, 0, 0, 1, 0, 0; ... 113 | 0, 0, 1, 0, 1, 0; ... 114 | 0, 0, 0, 1, 0, 1; ... 115 | 0, 1, 0, 0, 1, 0]; 116 | for ii = 1:1:length(t) 117 | for jj = 1:n 118 | plot(xx_U(ii, jj), yy_U(ii, jj), 'k.', 'MarkerSize', avgMs); 119 | end 120 | if (ii == 1) 121 | for i0 = 1:n - 1 122 | for j0 = i0 + 1:n 123 | if Adj_plot_s(i0, j0) == 1 124 | line([xx_U(ii, i0), xx_U(ii, j0)], [yy_U(ii, i0), yy_U(ii, j0)], ... 125 | 'LineWidth', lw) 126 | end 127 | end 128 | end 129 | end 130 | if ii == length(t) 131 | for i0 = 1:n - 1 132 | for j0 = i0 + 1:n 133 | if Adj_plot_u(i0, j0) == 1 134 | line([xx_U(ii, i0), xx_U(ii, j0)], [yy_U(ii, i0), yy_U(ii, j0)], ... 135 | 'LineWidth', lw) 136 | end 137 | end 138 | end 139 | end 140 | % pause(0.05) % Uncomment this if animation is needed 141 | end 142 | 143 | for i1 = 1:n 144 | hh1 = plot(xx_U(2, i1), yy_U(2, i1), 'rs', 'MarkerSize', 1.4*avgMs); 145 | hh2 = plot(xx_U(length(t), i1), yy_U(length(t), i1), 'ro', ... 146 | 'MarkerSize', 1.4*avgMs); 147 | end 148 | 149 | % %% Plot distance error %% 150 | % nn = 1:1:length(t); % Plot distance error (e) using less points 151 | % figure 152 | % grid on 153 | % hold on 154 | % set(gca, 'Box', 'on', 'FontSize', fs) 155 | % plot(t(nn), e12(nn), 'LineWidth', lw, 'Color', [0, 0, 1]); 156 | % plot(t(nn), e23(nn), 'LineWidth', lw, 'Color', [0, 0.5, 0]); 157 | % plot(t(nn), e34(nn), 'LineWidth', lw, 'Color', [1, 0, 0]); 158 | % plot(t(nn), e45(nn), 'LineWidth', lw, 'Color', [0, 0, 0]); 159 | % plot(t(nn), e15(nn), 'LineWidth', lw, 'Color', [0.75, 0, 0.75]); 160 | % plot(t(nn), e13(nn), 'LineWidth', lw, 'Color', [0, 0, 1]); 161 | % plot(t(nn), e14(nn), 'LineWidth', lw, 'Color', [0, 0.5, 0]); 162 | % plot(t(nn), e24(nn), 'LineWidth', lw, 'Color', [1, 0, 0]); 163 | % plot(t(nn), e25(nn), 'LineWidth', lw, 'Color', [0, 0, 0]); 164 | % plot(t(nn), e35(nn), 'LineWidth', lw, 'Color', [0.75, 0, 0.75]); 165 | % plot(t(nn), e16(nn), 'LineWidth', lw, 'Color', [0, 0, 1]); 166 | % plot(t(nn), e26(nn), 'LineWidth', lw, 'Color', [0, 0.5, 0]); 167 | % plot(t(nn), e36(nn), 'LineWidth', lw, 'Color', [1, 0, 0]); 168 | % plot(t(nn), e46(nn), 'LineWidth', lw, 'Color', [0, 0, 0]); 169 | % plot(t(nn), e56(nn), 'LineWidth', lw, 'Color', [0.75, 0, 0.75]); 170 | % xlabel('Time') 171 | % ylabel('e_i_j') 172 | % xlim([0, 1]) 173 | % 174 | % %% Plot control input %% 175 | % figure 176 | % subplot(211) 177 | % grid on 178 | % hold on 179 | % set(gca, 'Box', 'on', 'FontSize', fs) 180 | % 181 | % plot(t(nn), u(1, nn), 'LineWidth', lw, 'Color', [0, 0, 1]); 182 | % plot(t(nn), u(3, nn), 'LineWidth', lw, 'Color', [0, 0.5, 0]); 183 | % plot(t(nn), u(5, nn), 'LineWidth', lw, 'Color', [1, 0, 0]); 184 | % plot(t(nn), u(7, nn), 'LineWidth', lw, 'Color', [0, 0, 0]); 185 | % plot(t(nn), u(9, nn), 'LineWidth', lw, 'Color', [0.75, 0, 0.75]); 186 | % plot(t(nn), u(11, nn), 'LineWidth', lw, 'Color', [0.75, 0, 0.75]); 187 | % xlim([0, tf]) 188 | % xlabel('Time') 189 | % ylabel('u_i_x') 190 | % legend('u_1_x', 'u_2_x', 'u_3_x', 'u_4_x', 'u_5_x', 'Location', 'NorthEastOutside') 191 | % subplot(212) 192 | % grid on 193 | % hold on 194 | % set(gca, 'Box', 'on', 'FontSize', fs) 195 | % plot(t(nn), u(2, nn), 'LineWidth', lw, 'Color', [0, 0, 1]); 196 | % plot(t(nn), u(4, nn), 'LineWidth', lw, 'Color', [0, 0.5, 0]); 197 | % plot(t(nn), u(6, nn), 'LineWidth', lw, 'Color', [1, 0, 0]); 198 | % plot(t(nn), u(8, nn), 'LineWidth', lw, 'Color', [0, 0, 0]); 199 | % plot(t(nn), u(10, nn), 'LineWidth', lw, 'Color', [0.75, 0, 0.75]); 200 | % plot(t(nn), u(12, nn), 'LineWidth', lw, 'Color', [0.75, 0, 0.75]); 201 | % xlim([0, tf]) 202 | % legend('u_1_y', 'u_2_y', 'u_3_y', 'u_4_y', 'u_5_y', 'Location', 'NorthEastOutside') 203 | % xlabel('Time') 204 | % ylabel('u_i_y') 205 | -------------------------------------------------------------------------------- /code/SI_DFM_2D_SYSU/SI_dynamic_fomation_manv_func.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % This function is for dynamic formation maneuvering of single integrator 3 | % model. 4 | % Input variables: t as time sequence; q_vec as vector of q, 5 | % para is the structured parameters passing from the main 6 | % Output variable: dq is the time derivative of q 7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 8 | function [ u ] = SI_dynamic_fomation_manv_func( t,q_vec,para,Adj,character) 9 | % Obtain parameters from structure para 10 | n = para.n; 11 | kv = para.kv; 12 | Adj = para.Adj; 13 | 14 | % Obtain q from vector q_vec such that 15 | % q(:,i) indicates the coordinate of the ith agent 16 | q = reshape(q_vec,2,[]); % 2xn vector 17 | % 18 | z = zeros(2*n-3,1); % initialize z 19 | R = zeros(2*n-3,2*n); % initialize Rigidity Matrix 20 | e = zeros(n,n); % Distance error matrix 21 | dv = zeros(2*n-3,1); 22 | 23 | %% Choose a formation based on letters 24 | switch (character) 25 | case 'Y' 26 | [d,d_dot] = Desired_Y_distance(t,n,Adj); 27 | case 'S' 28 | [d,d_dot] = Desired_S_distance(t,n,Adj); 29 | case 'U' 30 | [d,d_dot] = Desired_U_distance(t,n,Adj); 31 | 32 | end 33 | ord = 1; 34 | for i = 1:n-1 35 | for j = i+1:n 36 | e(i,j) = sqrt((q(:,i)-q(:,j))'*(q(:,i)-q(:,j)))-d(i,j); 37 | if Adj(i,j) == 1 38 | dv(ord) = d(i,j)*d_dot(i,j); 39 | z(ord) = e(i,j)*(e(i,j)+2*d(i,j)); 40 | R(ord,2*i-1:2*i) = (q(:,i)-q(:,j))'; 41 | R(ord,2*j-1:2*j) = (q(:,j)-q(:,i))'; 42 | ord = ord+1; 43 | end 44 | end 45 | end 46 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 47 | vd = Desired_velocity(t); % desired velocity 48 | % u = -kv*R'*z+kron(ones(n,1),vd); 49 | % u= R'*pinv(R*R')*(-kv*z + 0); 50 | u = R'*pinv(R*R')*(-kv*z + dv)+kron(ones(n,1),vd); % control input 51 | dq = u; -------------------------------------------------------------------------------- /code/SI_DFM_2D_SYSU/SI_dynamic_fomation_manv_main.m: -------------------------------------------------------------------------------- 1 | clear 2 | clc 3 | close all 4 | 5 | %% modify ode45 options to form SYSU, the formation may fail due to the ode45 accuracy and formation init position,especially for y->s 6 | opts = odeset('NormControl', 'on', 'Reltol',1e-13,'AbsTol',1e-13, 'Stats','on'); 7 | 8 | %% Parameter Setting 9 | n = 6; % Number of agents 10 | kv = 6; % Control gain kv 11 | 12 | % connection matrix 13 | Adj = [0, 1, 1, 0, 0, 0; ... 14 | 1, 0, 1, 0, 0, 1; ... 15 | 1, 1, 0, 1, 0, 1; ... 16 | 0, 0, 1, 0, 1, 1; ... 17 | 0, 0, 0, 1, 0, 1; ... 18 | 0, 1, 1, 1, 1, 0]; 19 | 20 | 21 | tfinal = 5; % Simulation ending time 22 | h = 1e-1; % 1e-2 % ODE step 23 | 24 | % Encapusulate the paremeters into a structure 'para' 25 | para.n = n; 26 | para.kv = kv; 27 | para.Adj = Adj; 28 | 29 | % An example initial conditions: q_0 is initial positions and q_0(:,i) is 30 | % the coordinates for the ith agent; 31 | 32 | 33 | q_0 = [0, 0.5, -0.5, -0.5, 0, 0.5; ... 34 | 1, 0.5, 0.5, -0.5, -1, -0.5]; 35 | 36 | %% ODE 37 | q_0_vec = reshape(q_0, 1, []); % reshape q_0 into a 2nx1 vector 38 | time_span = 0:h:tfinal; % simulation time span 39 | [t, q] = ode45(@SI_dynamic_fomation_manv_func, time_span, q_0_vec, opts, para, Adj, 'S'); 40 | 41 | xx = q(:, 2*(0:n - 1)+1); 42 | yy = q(:, 2*(0:n - 1)+2); 43 | 44 | % %% Retrieve the control input 45 | % u = zeros(2*n, length(t)); % Control Input 46 | % for ii = 1:length(t) % loop for time from 0 to tfinal 47 | % e = zeros(n, n); % initialize distance error 48 | % dv = zeros(2*n-3, 1); % initialize dv 49 | % z = zeros(2*n-3, 1); % initialize z 50 | % R = zeros(2*n-3, 2*n); % initialize Rigidity Matrix 51 | % [d, d_dot] = Desired_tv_distance(t(ii), n, Adj); 52 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 53 | % % Construct R, e, and z 54 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 55 | % ord = 1; 56 | % for i = 1:n - 1 57 | % for j = i + 1:n 58 | % e(i, j) = sqrt((xx(ii, i) - xx(ii, j))^2+(yy(ii, i) - yy(ii, j))^2) ... 59 | % -d(i, j); 60 | % if Adj(i, j) == 1 61 | % dv(ord) = d(i, j) * d_dot(i, j); 62 | % z(ord) = e(i, j) * (e(i, j) + 2 * d(i, j)); 63 | % R(ord, 2*i-1:2*i) = [xx(ii, i) - xx(ii, j), yy(ii, i) - yy(ii, j)]; 64 | % R(ord, 2*j-1:2*j) = [xx(ii, j) - xx(ii, i), yy(ii, j) - yy(ii, i)]; 65 | % ord = ord + 1; 66 | % end 67 | % end 68 | % end 69 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 70 | % vd = Desired_velocity(t(ii)); % Set desired velocity by function 71 | % % maneuvering control input for single integrator model 72 | % % u(:,ii) = -kv*R'*z; 73 | % 74 | % u(:, ii) = R' * pinv(R*R') * (-kv * z + dv) + kron(ones(n, 1), vd); 75 | % end 76 | % 77 | % %% Calculate distance errors (e12,e13...) 78 | % d = zeros(length(t), n, n); 79 | % for ii = 1:length(t) 80 | % [d(ii, :, :), ~] = Desired_tv_distance(t(ii), n, Adj); 81 | % end 82 | % for i = 1:n - 1 83 | % for j = i + 1:n 84 | % eval(['e', int2str(i), int2str(j), ... 85 | % '=sqrt((xx(:,i)-xx(:,j)).^2+(yy(:,i)-yy(:,j)).^2)-d(:,i,j);']) 86 | % end 87 | % end 88 | 89 | disp('Simulation complete S!') 90 | 91 | 92 | %% fomation Y 93 | %% connection matrix Y 94 | Adj_Y = [0, 1, 1, 1, 1, 0; ... 95 | 1, 0, 1, 0, 0, 1; ... 96 | 1, 1, 0, 1, 0, 0; ... 97 | 1, 0, 1, 0, 0, 1; ... 98 | 1, 0, 0, 0, 0, 1; ... 99 | 0, 1, 0, 1, 1, 0]; 100 | 101 | %% S last position is Y first position 102 | q_0_Y = [xx(length(t), 1), xx(length(t), 2), xx(length(t), 3), xx(length(t), 4), xx(length(t), 5), xx(length(t), 6); ... 103 | yy(length(t), 1), yy(length(t), 2), yy(length(t), 3), yy(length(t), 4), yy(length(t), 5), yy(length(t), 6)]; 104 | 105 | %% ODE 106 | q_0_vec_Y = reshape(q_0_Y, 1, []); % reshape q_0 into a 2nx1 vector 107 | time_span = 0:h:tfinal; % simulation time span 108 | [t, q_Y] = ode45(@SI_dynamic_fomation_manv_func, time_span, q_0_vec_Y, opts, para, Adj_Y, 'Y'); 109 | 110 | xx_Y = q_Y(:, 2*(0:n - 1)+1); 111 | yy_Y = q_Y(:, 2*(0:n - 1)+2); 112 | disp('Simulation complete Y!') 113 | 114 | % %% Retrieve the control input 115 | % u_y = zeros(2*n, length(t)); % Control Input 116 | % for ii = 1:length(t) % loop for time from 0 to tfinal 117 | % e = zeros(n, n); % initialize distance error 118 | % dv = zeros(2*n-3, 1); % initialize dv 119 | % z = zeros(2*n-3, 1); % initialize z 120 | % R = zeros(2*n-3, 2*n); % initialize Rigidity Matrix 121 | % [d, d_dot] = Desired_Y_distance(t(ii), n, Adj_Y); 122 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 123 | % % Construct R, e, and z 124 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 125 | % ord = 1; 126 | % for i = 1:n - 1 127 | % for j = i + 1:n 128 | % e(i, j) = sqrt((xx_Y(ii, i) - xx_Y(ii, j))^2+(yy_Y(ii, i) - yy_Y(ii, j))^2) ... 129 | % -d(i, j); 130 | % if Adj_Y(i, j) == 1 131 | % dv(ord) = d(i, j) * d_dot(i, j); 132 | % z(ord) = e(i, j) * (e(i, j) + 2 * d(i, j)); 133 | % R(ord, 2*i-1:2*i) = [xx_Y(ii, i) - xx_Y(ii, j), yy_Y(ii, i) - yy_Y(ii, j)]; 134 | % R(ord, 2*j-1:2*j) = [xx_Y(ii, j) - xx_Y(ii, i), yy_Y(ii, j) - yy_Y(ii, i)]; 135 | % ord = ord + 1; 136 | % end 137 | % end 138 | % end 139 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 140 | % vd = Desired_velocity(t(ii)); % Set desired velocity by function 141 | % % maneuvering control input for single integrator model 142 | % u_y(:, ii) = R' * pinv(R*R') * (-kv * z + dv) + kron(ones(n, 1), vd); 143 | % end 144 | 145 | 146 | %% fomation S 147 | %% connection matrix S 148 | 149 | Adj_S = [0, 1, 1, 0, 0, 0; ... 150 | 1, 0, 1, 0, 0, 1; ... 151 | 1, 1, 0, 1, 0, 1; ... 152 | 0, 0, 1, 0, 1, 1; ... 153 | 0, 0, 0, 1, 0, 1; ... 154 | 0, 1, 1, 1, 1, 0]; 155 | 156 | q_0_S = [xx_Y(length(t), 1), xx_Y(length(t), 2), xx_Y(length(t), 3), xx_Y(length(t), 4), xx_Y(length(t), 5), xx_Y(length(t), 6); ... 157 | yy_Y(length(t), 1), yy_Y(length(t), 2), yy_Y(length(t), 3), yy_Y(length(t), 4), yy_Y(length(t), 5), yy_Y(length(t), 6)]; 158 | 159 | %% ODE 160 | q_0_vec_S = reshape(q_0_S, 1, []); % reshape q_0 into a 2nx1 vector 161 | time_span = 0:h:tfinal; % simulation time span 162 | [t, q] = ode45(@SI_dynamic_fomation_manv_func, time_span, q_0_vec_S, opts, para, Adj_S, 'S'); 163 | 164 | xx_S = q(:, 2*(0:n - 1)+1); 165 | yy_S = q(:, 2*(0:n - 1)+2); 166 | 167 | % %% Retrieve the control input 168 | % u_s = zeros(2*n, length(t)); % Control Input 169 | % for ii = 1:length(t) % loop for time from 0 to tfinal 170 | % e = zeros(n, n); % initialize distance error 171 | % dv = zeros(2*n-3, 1); % initialize dv 172 | % z = zeros(2*n-3, 1); % initialize z 173 | % R = zeros(2*n-3, 2*n); % initialize Rigidity Matrix 174 | % [d, d_dot] = Desired_tv_distance(t(ii), n, Adj); 175 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 176 | % % Construct R, e, and z 177 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 178 | % ord = 1; 179 | % for i = 1:n - 1 180 | % for j = i + 1:n 181 | % e(i, j) = sqrt((xx_S(ii, i) - xx_S(ii, j))^2+(yy_S(ii, i) - yy_S(ii, j))^2) ... 182 | % -d(i, j); 183 | % if Adj_S(i, j) == 1 184 | % dv(ord) = d(i, j) * d_dot(i, j); 185 | % z(ord) = e(i, j) * (e(i, j) + 2 * d(i, j)); 186 | % R(ord, 2*i-1:2*i) = [xx_S(ii, i) - xx_S(ii, j), yy_S(ii, i) - yy_S(ii, j)]; 187 | % R(ord, 2*j-1:2*j) = [xx_S(ii, j) - xx_S(ii, i), yy_S(ii, j) - yy_S(ii, i)]; 188 | % ord = ord + 1; 189 | % end 190 | % end 191 | % end 192 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 193 | % vd = Desired_velocity(t(ii)); % Set desired velocity by function 194 | % % maneuvering control input for single integrator model 195 | % u_s(:, ii) = R' * pinv(R*R') * (-kv * z + dv) + kron(ones(n, 1), vd); 196 | % end 197 | % 198 | % %% Calculate distance errors (e12,e13...) 199 | % d = zeros(length(t), n, n); 200 | % for ii = 1:length(t) 201 | % [d(ii, :, :), ~] = Desired_tv_distance(t(ii), n, Adj_S); 202 | % end 203 | % for i = 1:n - 1 204 | % for j = i + 1:n 205 | % eval(['e', int2str(i), int2str(j), ... 206 | % '=sqrt((xx_S(:,i)-xx_S(:,j)).^2+(yy_S(:,i)-yy_S(:,j)).^2)-d(:,i,j);']) 207 | % end 208 | % end 209 | 210 | disp('Simulation complete S!') 211 | 212 | %% fomation U 213 | %% connection matrix U 214 | 215 | %% connection matrix 216 | Adj_U = [0, 1, 1, 1, 0, 1; ... 217 | 1, 0, 1, 0, 1, 0; ... 218 | 1, 1, 0, 1, 0, 0; ... 219 | 1, 0, 1, 0, 1, 0; ... 220 | 0, 1, 0, 1, 0, 1; ... 221 | 1, 0, 0, 0, 1, 0]; 222 | q_0_U = [xx_S(length(t), 1), xx_S(length(t), 2), xx_S(length(t), 3), xx_S(length(t), 4), xx_S(length(t), 5), xx_S(length(t), 6); ... 223 | yy_S(length(t), 1), yy_S(length(t), 2), yy_S(length(t), 3), yy_S(length(t), 4), yy_S(length(t), 5), yy_S(length(t), 6)]; 224 | 225 | %% ODE 226 | q_0_vec_U = reshape(q_0_U, 1, []); % reshape q_0 into a 2nx1 vector 227 | time_span = 0:h:tfinal; % simulation time span 228 | [t, q_U] = ode45(@SI_dynamic_fomation_manv_func, time_span, q_0_vec_U, opts, para, Adj_U, 'U'); 229 | 230 | xx_U = q_U(:, 2*(0:n - 1)+1); 231 | yy_U = q_U(:, 2*(0:n - 1)+2); 232 | 233 | disp('Simulation complete U!') 234 | 235 | % %% Retrieve the control input 236 | % u_u = zeros(2*n, length(t)); % Control Input 237 | % for ii = 1:length(t) % loop for time from 0 to tfinal 238 | % e = zeros(n, n); % initialize distance error 239 | % dv = zeros(2*n-3, 1); % initialize dv 240 | % z = zeros(2*n-3, 1); % initialize z 241 | % R = zeros(2*n-3, 2*n); % initialize Rigidity Matrix 242 | % [d, d_dot] = Desired_U_distance(t(ii), n, Adj); 243 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 244 | % % Construct R, e, and z 245 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 246 | % ord = 1; 247 | % for i = 1:n - 1 248 | % for j = i + 1:n 249 | % e(i, j) = sqrt((xx_U(ii, i) - xx_U(ii, j))^2+(yy_U(ii, i) - yy_U(ii, j))^2) ... 250 | % -d(i, j); 251 | % if Adj_U(i, j) == 1 252 | % dv(ord) = d(i, j) * d_dot(i, j); 253 | % z(ord) = e(i, j) * (e(i, j) + 2 * d(i, j)); 254 | % R(ord, 2*i-1:2*i) = [xx_U(ii, i) - xx_U(ii, j), yy_U(ii, i) - yy_U(ii, j)]; 255 | % R(ord, 2*j-1:2*j) = [xx_U(ii, j) - xx_U(ii, i), yy_U(ii, j) - yy_U(ii, i)]; 256 | % ord = ord + 1; 257 | % end 258 | % end 259 | % end 260 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 261 | % vd = Desired_velocity(t(ii)); % Set desired velocity by function 262 | % % maneuvering control input for single integrator model 263 | % u_u(:, ii) = R' * pinv(R*R') * (-kv * z + dv) + kron(ones(n, 1), vd); 264 | % end 265 | 266 | 267 | x = [xx; xx_Y; xx_S; xx_U]; 268 | y = [yy; yy_Y; yy_S; yy_U]; 269 | 270 | 271 | 272 | % save variables for plot and animation 273 | save('../SI_dynamic_fomation_manv_results.mat') 274 | disp('Simulation complete!') 275 | -------------------------------------------------------------------------------- /code/SI_dynamic_fomation_manv_results.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SYSU-HI-LAB/Introduction_to_multi_agent_control/168686e8608951fc5df224114d3e01949aef2506/code/SI_dynamic_fomation_manv_results.mat -------------------------------------------------------------------------------- /code/controller1.m: -------------------------------------------------------------------------------- 1 | function [F, M] = controller1(t, s, s_des) 2 | 3 | global params 4 | 5 | m = params.mass; 6 | g = params.grav; 7 | I = params.I; 8 | 9 | F = m*g; M = [0.0, 0.0, 0.0]'; % You should calculate the output F and M 10 | 11 | %params 12 | Kp_i = [4,4,15]*5; %2 3 fast 13 | Kd_i = [2,2,3]*5; %0.6 soft 1.0 14 | Kp_phi = 20*5; %20 15 | Kd_phi = 6*5; %4 fast 16 | Kp_theta = 20*5; 17 | Kd_theta = 6*5; 18 | Kp_psi = 20*5; 19 | Kd_psi = 3*5; 20 | persistent sp_last; 21 | if isempty(sp_last) 22 | sp_last = s(1:6); 23 | sp = sp_last; 24 | else 25 | sp = 0.9*s(1:6) + 0.1 * sp_last; 26 | sp_last = sp; 27 | end 28 | %position control 29 | i = 1; 30 | r2c = zeros(3,1); 31 | while(i<4) 32 | r2c(i) = Kp_i(i) * (s_des(i) - sp(i)) + Kd_i(i) * (s_des(i+3) - sp(i+3)); 33 | i = i + 1; 34 | end 35 | F = m * (g + r2c(3)); 36 | 37 | %attitude control 38 | q = s(7:10); 39 | [phi,theta,psi] = RotToRPY_ZXY(quaternion_to_R(q)); 40 | 41 | persistent phi_last; 42 | persistent theta_last; 43 | persistent psi_last; 44 | if isempty(phi_last) 45 | phi_last = phi; 46 | theta_last = theta; 47 | psi_last = psi; 48 | else 49 | phi = 0.1*phi_last + 0.9*phi; 50 | theta = 0.1*theta_last + 0.9*theta; 51 | psi = 0.1*psi_last + 0.9*psi; 52 | phi_last = phi; 53 | theta_last = theta; 54 | psi_last = psi; 55 | end 56 | 57 | phi_c = (r2c(1)*sin(psi) - r2c(2)*cos(psi))/g; 58 | theta_c = (r2c(1)*cos(psi) + r2c(2)*sin(psi))/g; 59 | 60 | persistent phi_error_last theta_error_last; 61 | persistent psi_error_last; 62 | persistent t_last; 63 | 64 | phi_error = phi_c - phi; 65 | theta_error = theta_c - theta; 66 | q_des = s_des(7:10); 67 | [phi_des,theta_des,psi_des] = RotToRPY_ZXY(quaternion_to_R(q_des)'); 68 | psi_error = psi_des - psi; 69 | if psi_error < -pi 70 | psi_error = psi_error + 2*pi; 71 | else 72 | if psi_error > pi 73 | psi_error = psi_error - 2*pi; 74 | end 75 | end 76 | if isempty(phi_error_last) 77 | phi_error_last = phi_error; 78 | theta_error_last = theta_error; 79 | psi_error_last = psi_error; 80 | t_last = t; 81 | else 82 | phi_c2 = Kp_phi*(phi_error) + Kd_phi*(phi_error - phi_error_last)/(t - t_last); 83 | theta_c2 = Kp_theta*(theta_error) + Kd_theta*(theta_error - theta_error_last)/(t - t_last); 84 | psi_c2 = Kp_psi*(psi_error) + Kd_psi*(psi_error - psi_error_last)/(t - t_last); 85 | 86 | phi_error_last = phi_error; 87 | theta_error_last = theta_error; 88 | psi_error_last = psi_error; 89 | t_last = t; 90 | angle_c2 = [phi_c2, 91 | theta_c2, 92 | psi_c2]; 93 | %output filter 94 | persistent phi_c2_last; 95 | persistent theta_c2_last; 96 | persistent psi_c2_last; 97 | if isempty(phi_c2_last) 98 | phi_c2_last = phi_c2; 99 | theta_c2_last = theta_c2; 100 | psi_c2_last = psi_c2; 101 | else 102 | phi_c2 = 0.1*phi_c2_last + 0.9*phi_c2; 103 | theta_c2 = 0.1*theta_c2_last + 0.9*theta_c2; 104 | psi_c2 = 0.1*psi_c2_last + 0.9*psi_c2; 105 | phi_c2_last = phi_c2; 106 | theta_c2_last = theta_c2; 107 | psi_c2_last = psi_c2; 108 | end 109 | w = s(11:13); 110 | M = I * angle_c2 + cross(w,I*w); 111 | end 112 | 113 | 114 | end 115 | -------------------------------------------------------------------------------- /code/controller2.m: -------------------------------------------------------------------------------- 1 | function [F, M] = controller2(t, s, s_des) 2 | 3 | global params 4 | 5 | m = params.mass; 6 | g = params.grav; 7 | I = params.I; 8 | 9 | F = m*g; M = [0.0, 0.0, 0.0]'; % You should calculate the output F and M 10 | 11 | %params 12 | Kp_i = [4,4,15]*5; %2 3 fast 13 | Kd_i = [2,2,3]*5; %0.6 soft 1.0 14 | Kp_phi = 20*5; %20 15 | Kd_phi = 6*5; %4 fast 16 | Kp_theta = 20*5; 17 | Kd_theta = 6*5; 18 | Kp_psi = 20*5; 19 | Kd_psi = 3*5; 20 | persistent sp_last; 21 | if isempty(sp_last) 22 | sp_last = s(1:6); 23 | sp = sp_last; 24 | else 25 | sp = 0.9*s(1:6) + 0.1 * sp_last; 26 | sp_last = sp; 27 | end 28 | %position control 29 | i = 1; 30 | r2c = zeros(3,1); 31 | while(i<4) 32 | r2c(i) = Kp_i(i) * (s_des(i) - sp(i)) + Kd_i(i) * (s_des(i+3) - sp(i+3)); 33 | i = i + 1; 34 | end 35 | F = m * (g + r2c(3)); 36 | 37 | %attitude control 38 | q = s(7:10); 39 | [phi,theta,psi] = RotToRPY_ZXY(quaternion_to_R(q)); 40 | 41 | persistent phi_last; 42 | persistent theta_last; 43 | persistent psi_last; 44 | if isempty(phi_last) 45 | phi_last = phi; 46 | theta_last = theta; 47 | psi_last = psi; 48 | else 49 | phi = 0.1*phi_last + 0.9*phi; 50 | theta = 0.1*theta_last + 0.9*theta; 51 | psi = 0.1*psi_last + 0.9*psi; 52 | phi_last = phi; 53 | theta_last = theta; 54 | psi_last = psi; 55 | end 56 | 57 | phi_c = (r2c(1)*sin(psi) - r2c(2)*cos(psi))/g; 58 | theta_c = (r2c(1)*cos(psi) + r2c(2)*sin(psi))/g; 59 | 60 | persistent phi_error_last theta_error_last; 61 | persistent psi_error_last; 62 | persistent t_last; 63 | 64 | phi_error = phi_c - phi; 65 | theta_error = theta_c - theta; 66 | q_des = s_des(7:10); 67 | [phi_des,theta_des,psi_des] = RotToRPY_ZXY(quaternion_to_R(q_des)'); 68 | psi_error = psi_des - psi; 69 | if psi_error < -pi 70 | psi_error = psi_error + 2*pi; 71 | else 72 | if psi_error > pi 73 | psi_error = psi_error - 2*pi; 74 | end 75 | end 76 | if isempty(phi_error_last) 77 | phi_error_last = phi_error; 78 | theta_error_last = theta_error; 79 | psi_error_last = psi_error; 80 | t_last = t; 81 | else 82 | phi_c2 = Kp_phi*(phi_error) + Kd_phi*(phi_error - phi_error_last)/(t - t_last); 83 | theta_c2 = Kp_theta*(theta_error) + Kd_theta*(theta_error - theta_error_last)/(t - t_last); 84 | psi_c2 = Kp_psi*(psi_error) + Kd_psi*(psi_error - psi_error_last)/(t - t_last); 85 | 86 | phi_error_last = phi_error; 87 | theta_error_last = theta_error; 88 | psi_error_last = psi_error; 89 | t_last = t; 90 | angle_c2 = [phi_c2, 91 | theta_c2, 92 | psi_c2]; 93 | %output filter 94 | persistent phi_c2_last; 95 | persistent theta_c2_last; 96 | persistent psi_c2_last; 97 | if isempty(phi_c2_last) 98 | phi_c2_last = phi_c2; 99 | theta_c2_last = theta_c2; 100 | psi_c2_last = psi_c2; 101 | else 102 | phi_c2 = 0.1*phi_c2_last + 0.9*phi_c2; 103 | theta_c2 = 0.1*theta_c2_last + 0.9*theta_c2; 104 | psi_c2 = 0.1*psi_c2_last + 0.9*psi_c2; 105 | phi_c2_last = phi_c2; 106 | theta_c2_last = theta_c2; 107 | psi_c2_last = psi_c2; 108 | end 109 | w = s(11:13); 110 | M = I * angle_c2 + cross(w,I*w); 111 | end 112 | 113 | 114 | end 115 | -------------------------------------------------------------------------------- /code/controller3.m: -------------------------------------------------------------------------------- 1 | function [F, M] = controller3(t, s, s_des) 2 | 3 | global params 4 | 5 | m = params.mass; 6 | g = params.grav; 7 | I = params.I; 8 | 9 | F = m*g; M = [0.0, 0.0, 0.0]'; % You should calculate the output F and M 10 | 11 | %params 12 | Kp_i = [4,4,15]*5; %2 3 fast 13 | Kd_i = [2,2,3]*5; %0.6 soft 1.0 14 | Kp_phi = 20*5; %20 15 | Kd_phi = 6*5; %4 fast 16 | Kp_theta = 20*5; 17 | Kd_theta = 6*5; 18 | Kp_psi = 20*5; 19 | Kd_psi = 3*5; 20 | persistent sp_last; 21 | if isempty(sp_last) 22 | sp_last = s(1:6); 23 | sp = sp_last; 24 | else 25 | sp = 0.9*s(1:6) + 0.1 * sp_last; 26 | sp_last = sp; 27 | end 28 | %position control 29 | i = 1; 30 | r2c = zeros(3,1); 31 | while(i<4) 32 | r2c(i) = Kp_i(i) * (s_des(i) - sp(i)) + Kd_i(i) * (s_des(i+3) - sp(i+3)); 33 | i = i + 1; 34 | end 35 | F = m * (g + r2c(3)); 36 | 37 | %attitude control 38 | q = s(7:10); 39 | [phi,theta,psi] = RotToRPY_ZXY(quaternion_to_R(q)); 40 | 41 | persistent phi_last; 42 | persistent theta_last; 43 | persistent psi_last; 44 | if isempty(phi_last) 45 | phi_last = phi; 46 | theta_last = theta; 47 | psi_last = psi; 48 | else 49 | phi = 0.1*phi_last + 0.9*phi; 50 | theta = 0.1*theta_last + 0.9*theta; 51 | psi = 0.1*psi_last + 0.9*psi; 52 | phi_last = phi; 53 | theta_last = theta; 54 | psi_last = psi; 55 | end 56 | 57 | phi_c = (r2c(1)*sin(psi) - r2c(2)*cos(psi))/g; 58 | theta_c = (r2c(1)*cos(psi) + r2c(2)*sin(psi))/g; 59 | 60 | persistent phi_error_last theta_error_last; 61 | persistent psi_error_last; 62 | persistent t_last; 63 | 64 | phi_error = phi_c - phi; 65 | theta_error = theta_c - theta; 66 | q_des = s_des(7:10); 67 | [phi_des,theta_des,psi_des] = RotToRPY_ZXY(quaternion_to_R(q_des)'); 68 | psi_error = psi_des - psi; 69 | if psi_error < -pi 70 | psi_error = psi_error + 2*pi; 71 | else 72 | if psi_error > pi 73 | psi_error = psi_error - 2*pi; 74 | end 75 | end 76 | if isempty(phi_error_last) 77 | phi_error_last = phi_error; 78 | theta_error_last = theta_error; 79 | psi_error_last = psi_error; 80 | t_last = t; 81 | else 82 | phi_c2 = Kp_phi*(phi_error) + Kd_phi*(phi_error - phi_error_last)/(t - t_last); 83 | theta_c2 = Kp_theta*(theta_error) + Kd_theta*(theta_error - theta_error_last)/(t - t_last); 84 | psi_c2 = Kp_psi*(psi_error) + Kd_psi*(psi_error - psi_error_last)/(t - t_last); 85 | 86 | phi_error_last = phi_error; 87 | theta_error_last = theta_error; 88 | psi_error_last = psi_error; 89 | t_last = t; 90 | angle_c2 = [phi_c2, 91 | theta_c2, 92 | psi_c2]; 93 | %output filter 94 | persistent phi_c2_last; 95 | persistent theta_c2_last; 96 | persistent psi_c2_last; 97 | if isempty(phi_c2_last) 98 | phi_c2_last = phi_c2; 99 | theta_c2_last = theta_c2; 100 | psi_c2_last = psi_c2; 101 | else 102 | phi_c2 = 0.1*phi_c2_last + 0.9*phi_c2; 103 | theta_c2 = 0.1*theta_c2_last + 0.9*theta_c2; 104 | psi_c2 = 0.1*psi_c2_last + 0.9*psi_c2; 105 | phi_c2_last = phi_c2; 106 | theta_c2_last = theta_c2; 107 | psi_c2_last = psi_c2; 108 | end 109 | w = s(11:13); 110 | M = I * angle_c2 + cross(w,I*w); 111 | end 112 | 113 | 114 | end 115 | -------------------------------------------------------------------------------- /code/controller4.m: -------------------------------------------------------------------------------- 1 | function [F, M] = controller4(t, s, s_des) 2 | 3 | global params 4 | 5 | m = params.mass; 6 | g = params.grav; 7 | I = params.I; 8 | 9 | F = m*g; M = [0.0, 0.0, 0.0]'; % You should calculate the output F and M 10 | 11 | %params 12 | Kp_i = [4,4,15]*5; %2 3 fast 13 | Kd_i = [2,2,3]*5; %0.6 soft 1.0 14 | Kp_phi = 20*5; %20 15 | Kd_phi = 6*5; %4 fast 16 | Kp_theta = 20*5; 17 | Kd_theta = 6*5; 18 | Kp_psi = 20*5; 19 | Kd_psi = 3*5; 20 | persistent sp_last; 21 | if isempty(sp_last) 22 | sp_last = s(1:6); 23 | sp = sp_last; 24 | else 25 | sp = 0.9*s(1:6) + 0.1 * sp_last; 26 | sp_last = sp; 27 | end 28 | %position control 29 | i = 1; 30 | r2c = zeros(3,1); 31 | while(i<4) 32 | r2c(i) = Kp_i(i) * (s_des(i) - sp(i)) + Kd_i(i) * (s_des(i+3) - sp(i+3)); 33 | i = i + 1; 34 | end 35 | F = m * (g + r2c(3)); 36 | 37 | %attitude control 38 | q = s(7:10); 39 | [phi,theta,psi] = RotToRPY_ZXY(quaternion_to_R(q)); 40 | 41 | persistent phi_last; 42 | persistent theta_last; 43 | persistent psi_last; 44 | if isempty(phi_last) 45 | phi_last = phi; 46 | theta_last = theta; 47 | psi_last = psi; 48 | else 49 | phi = 0.1*phi_last + 0.9*phi; 50 | theta = 0.1*theta_last + 0.9*theta; 51 | psi = 0.1*psi_last + 0.9*psi; 52 | phi_last = phi; 53 | theta_last = theta; 54 | psi_last = psi; 55 | end 56 | 57 | phi_c = (r2c(1)*sin(psi) - r2c(2)*cos(psi))/g; 58 | theta_c = (r2c(1)*cos(psi) + r2c(2)*sin(psi))/g; 59 | 60 | persistent phi_error_last theta_error_last; 61 | persistent psi_error_last; 62 | persistent t_last; 63 | 64 | phi_error = phi_c - phi; 65 | theta_error = theta_c - theta; 66 | q_des = s_des(7:10); 67 | [phi_des,theta_des,psi_des] = RotToRPY_ZXY(quaternion_to_R(q_des)'); 68 | psi_error = psi_des - psi; 69 | if psi_error < -pi 70 | psi_error = psi_error + 2*pi; 71 | else 72 | if psi_error > pi 73 | psi_error = psi_error - 2*pi; 74 | end 75 | end 76 | if isempty(phi_error_last) 77 | phi_error_last = phi_error; 78 | theta_error_last = theta_error; 79 | psi_error_last = psi_error; 80 | t_last = t; 81 | else 82 | phi_c2 = Kp_phi*(phi_error) + Kd_phi*(phi_error - phi_error_last)/(t - t_last); 83 | theta_c2 = Kp_theta*(theta_error) + Kd_theta*(theta_error - theta_error_last)/(t - t_last); 84 | psi_c2 = Kp_psi*(psi_error) + Kd_psi*(psi_error - psi_error_last)/(t - t_last); 85 | 86 | phi_error_last = phi_error; 87 | theta_error_last = theta_error; 88 | psi_error_last = psi_error; 89 | t_last = t; 90 | angle_c2 = [phi_c2, 91 | theta_c2, 92 | psi_c2]; 93 | %output filter 94 | persistent phi_c2_last; 95 | persistent theta_c2_last; 96 | persistent psi_c2_last; 97 | if isempty(phi_c2_last) 98 | phi_c2_last = phi_c2; 99 | theta_c2_last = theta_c2; 100 | psi_c2_last = psi_c2; 101 | else 102 | phi_c2 = 0.1*phi_c2_last + 0.9*phi_c2; 103 | theta_c2 = 0.1*theta_c2_last + 0.9*theta_c2; 104 | psi_c2 = 0.1*psi_c2_last + 0.9*psi_c2; 105 | phi_c2_last = phi_c2; 106 | theta_c2_last = theta_c2; 107 | psi_c2_last = psi_c2; 108 | end 109 | w = s(11:13); 110 | M = I * angle_c2 + cross(w,I*w); 111 | end 112 | 113 | 114 | end 115 | -------------------------------------------------------------------------------- /code/controller5.m: -------------------------------------------------------------------------------- 1 | function [F, M] = controller5(t, s, s_des) 2 | 3 | global params 4 | 5 | m = params.mass; 6 | g = params.grav; 7 | I = params.I; 8 | 9 | F = m*g; M = [0.0, 0.0, 0.0]'; % You should calculate the output F and M 10 | 11 | %params 12 | Kp_i = [4,4,15]*5; %2 3 fast 13 | Kd_i = [2,2,3]*5; %0.6 soft 1.0 14 | Kp_phi = 20*5; %20 15 | Kd_phi = 6*5; %4 fast 16 | Kp_theta = 20*5; 17 | Kd_theta = 6*5; 18 | Kp_psi = 20*5; 19 | Kd_psi = 3*5; 20 | persistent sp_last; 21 | if isempty(sp_last) 22 | sp_last = s(1:6); 23 | sp = sp_last; 24 | else 25 | sp = 0.9*s(1:6) + 0.1 * sp_last; 26 | sp_last = sp; 27 | end 28 | %position control 29 | i = 1; 30 | r2c = zeros(3,1); 31 | while(i<4) 32 | r2c(i) = Kp_i(i) * (s_des(i) - sp(i)) + Kd_i(i) * (s_des(i+3) - sp(i+3)); 33 | i = i + 1; 34 | end 35 | F = m * (g + r2c(3)); 36 | 37 | %attitude control 38 | q = s(7:10); 39 | [phi,theta,psi] = RotToRPY_ZXY(quaternion_to_R(q)); 40 | 41 | persistent phi_last; 42 | persistent theta_last; 43 | persistent psi_last; 44 | if isempty(phi_last) 45 | phi_last = phi; 46 | theta_last = theta; 47 | psi_last = psi; 48 | else 49 | phi = 0.1*phi_last + 0.9*phi; 50 | theta = 0.1*theta_last + 0.9*theta; 51 | psi = 0.1*psi_last + 0.9*psi; 52 | phi_last = phi; 53 | theta_last = theta; 54 | psi_last = psi; 55 | end 56 | 57 | phi_c = (r2c(1)*sin(psi) - r2c(2)*cos(psi))/g; 58 | theta_c = (r2c(1)*cos(psi) + r2c(2)*sin(psi))/g; 59 | 60 | persistent phi_error_last theta_error_last; 61 | persistent psi_error_last; 62 | persistent t_last; 63 | 64 | phi_error = phi_c - phi; 65 | theta_error = theta_c - theta; 66 | q_des = s_des(7:10); 67 | [phi_des,theta_des,psi_des] = RotToRPY_ZXY(quaternion_to_R(q_des)'); 68 | psi_error = psi_des - psi; 69 | if psi_error < -pi 70 | psi_error = psi_error + 2*pi; 71 | else 72 | if psi_error > pi 73 | psi_error = psi_error - 2*pi; 74 | end 75 | end 76 | if isempty(phi_error_last) 77 | phi_error_last = phi_error; 78 | theta_error_last = theta_error; 79 | psi_error_last = psi_error; 80 | t_last = t; 81 | else 82 | phi_c2 = Kp_phi*(phi_error) + Kd_phi*(phi_error - phi_error_last)/(t - t_last); 83 | theta_c2 = Kp_theta*(theta_error) + Kd_theta*(theta_error - theta_error_last)/(t - t_last); 84 | psi_c2 = Kp_psi*(psi_error) + Kd_psi*(psi_error - psi_error_last)/(t - t_last); 85 | 86 | phi_error_last = phi_error; 87 | theta_error_last = theta_error; 88 | psi_error_last = psi_error; 89 | t_last = t; 90 | angle_c2 = [phi_c2, 91 | theta_c2, 92 | psi_c2]; 93 | %output filter 94 | persistent phi_c2_last; 95 | persistent theta_c2_last; 96 | persistent psi_c2_last; 97 | if isempty(phi_c2_last) 98 | phi_c2_last = phi_c2; 99 | theta_c2_last = theta_c2; 100 | psi_c2_last = psi_c2; 101 | else 102 | phi_c2 = 0.1*phi_c2_last + 0.9*phi_c2; 103 | theta_c2 = 0.1*theta_c2_last + 0.9*theta_c2; 104 | psi_c2 = 0.1*psi_c2_last + 0.9*psi_c2; 105 | phi_c2_last = phi_c2; 106 | theta_c2_last = theta_c2; 107 | psi_c2_last = psi_c2; 108 | end 109 | w = s(11:13); 110 | M = I * angle_c2 + cross(w,I*w); 111 | end 112 | 113 | 114 | end 115 | -------------------------------------------------------------------------------- /code/controller6.m: -------------------------------------------------------------------------------- 1 | function [F, M] = controller6(t, s, s_des) 2 | 3 | global params 4 | 5 | m = params.mass; 6 | g = params.grav; 7 | I = params.I; 8 | 9 | F = m*g; M = [0.0, 0.0, 0.0]'; % You should calculate the output F and M 10 | %params 11 | Kp_i = [4,4,15]*5; %2 3 fast 12 | Kd_i = [2,2,3]*5; %0.6 soft 1.0 13 | Kp_phi = 20*5; %20 14 | Kd_phi = 6*5; %4 fast 15 | Kp_theta = 20*5; 16 | Kd_theta = 6*5; 17 | Kp_psi = 20*5; 18 | Kd_psi = 3*5; 19 | persistent sp_last; 20 | if isempty(sp_last) 21 | sp_last = s(1:6); 22 | sp = sp_last; 23 | else 24 | sp = 0.9*s(1:6) + 0.1 * sp_last; 25 | sp_last = sp; 26 | end 27 | %position control 28 | i = 1; 29 | r2c = zeros(3,1); 30 | while(i<4) 31 | r2c(i) = Kp_i(i) * (s_des(i) - sp(i)) + Kd_i(i) * (s_des(i+3) - sp(i+3)); 32 | i = i + 1; 33 | end 34 | F = m * (g + r2c(3)); 35 | 36 | %attitude control 37 | q = s(7:10); 38 | [phi,theta,psi] = RotToRPY_ZXY(quaternion_to_R(q)); 39 | 40 | persistent phi_last; 41 | persistent theta_last; 42 | persistent psi_last; 43 | if isempty(phi_last) 44 | phi_last = phi; 45 | theta_last = theta; 46 | psi_last = psi; 47 | else 48 | phi = 0.1*phi_last + 0.9*phi; 49 | theta = 0.1*theta_last + 0.9*theta; 50 | psi = 0.1*psi_last + 0.9*psi; 51 | phi_last = phi; 52 | theta_last = theta; 53 | psi_last = psi; 54 | end 55 | 56 | phi_c = (r2c(1)*sin(psi) - r2c(2)*cos(psi))/g; 57 | theta_c = (r2c(1)*cos(psi) + r2c(2)*sin(psi))/g; 58 | 59 | persistent phi_error_last theta_error_last; 60 | persistent psi_error_last; 61 | persistent t_last; 62 | 63 | phi_error = phi_c - phi; 64 | theta_error = theta_c - theta; 65 | q_des = s_des(7:10); 66 | [phi_des,theta_des,psi_des] = RotToRPY_ZXY(quaternion_to_R(q_des)'); 67 | psi_error = psi_des - psi; 68 | if psi_error < -pi 69 | psi_error = psi_error + 2*pi; 70 | else 71 | if psi_error > pi 72 | psi_error = psi_error - 2*pi; 73 | end 74 | end 75 | if isempty(phi_error_last) 76 | phi_error_last = phi_error; 77 | theta_error_last = theta_error; 78 | psi_error_last = psi_error; 79 | t_last = t; 80 | else 81 | phi_c2 = Kp_phi*(phi_error) + Kd_phi*(phi_error - phi_error_last)/(t - t_last); 82 | theta_c2 = Kp_theta*(theta_error) + Kd_theta*(theta_error - theta_error_last)/(t - t_last); 83 | psi_c2 = Kp_psi*(psi_error) + Kd_psi*(psi_error - psi_error_last)/(t - t_last); 84 | 85 | phi_error_last = phi_error; 86 | theta_error_last = theta_error; 87 | psi_error_last = psi_error; 88 | t_last = t; 89 | angle_c2 = [phi_c2, 90 | theta_c2, 91 | psi_c2]; 92 | %output filter 93 | persistent phi_c2_last; 94 | persistent theta_c2_last; 95 | persistent psi_c2_last; 96 | if isempty(phi_c2_last) 97 | phi_c2_last = phi_c2; 98 | theta_c2_last = theta_c2; 99 | psi_c2_last = psi_c2; 100 | else 101 | phi_c2 = 0.1*phi_c2_last + 0.9*phi_c2; 102 | theta_c2 = 0.1*theta_c2_last + 0.9*theta_c2; 103 | psi_c2 = 0.1*psi_c2_last + 0.9*psi_c2; 104 | phi_c2_last = phi_c2; 105 | theta_c2_last = theta_c2; 106 | psi_c2_last = psi_c2; 107 | end 108 | w = s(11:13); 109 | M = I * angle_c2 + cross(w,I*w); 110 | end 111 | 112 | 113 | end 114 | -------------------------------------------------------------------------------- /code/readonly/6formation_results.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SYSU-HI-LAB/Introduction_to_multi_agent_control/168686e8608951fc5df224114d3e01949aef2506/code/readonly/6formation_results.mat -------------------------------------------------------------------------------- /code/readonly/JudgeStudentsPoint.m: -------------------------------------------------------------------------------- 1 | load '6formation_results.mat' 2 | 3 | [~, num] = size(x0); 4 | pointNum = size(get(thtraj1, 'XData'),2); 5 | k = 0.5; 6 | 7 | %% statistics position error, include x, y and z 8 | % if you add the drone num, you should add the additional position error 9 | absPositionErrormean = (sum(abs(get(thtraj1, 'XData')-get(ehtraj1, 'XData'))+abs(get(thtraj1, 'YData')-get(ehtraj1, 'YData'))+abs(get(thtraj1, 'ZData')-get(ehtraj1, 'ZData')) ... 10 | +abs(get(thtraj2, 'XData')-get(ehtraj2, 'XData'))+abs(get(thtraj2, 'YData')-get(ehtraj2, 'YData'))+abs(get(thtraj2, 'ZData')-get(ehtraj2, 'ZData')) ... 11 | +abs(get(thtraj3, 'XData')-get(ehtraj3, 'XData'))+abs(get(thtraj3, 'YData')-get(ehtraj3, 'YData'))+abs(get(thtraj3, 'ZData')-get(ehtraj3, 'ZData')) ... 12 | +abs(get(thtraj4, 'XData')-get(ehtraj4, 'XData'))+abs(get(thtraj4, 'YData')-get(ehtraj4, 'YData'))+abs(get(thtraj4, 'ZData')-get(ehtraj4, 'ZData')) ... 13 | +abs(get(thtraj5, 'XData')-get(ehtraj5, 'XData'))+abs(get(thtraj5, 'YData')-get(ehtraj5, 'YData'))+abs(get(thtraj5, 'ZData')-get(ehtraj5, 'ZData')) ... 14 | +abs(get(thtraj6, 'XData')-get(ehtraj6, 'XData'))+abs(get(thtraj6, 'YData')-get(ehtraj6, 'YData'))+abs(get(thtraj6, 'ZData')-get(ehtraj6, 'ZData'))))/num/pointNum; % 203.1524 15 | 16 | 17 | 18 | finalPoint = absPositionErrormean*100 + time; 19 | 20 | disp(finalPoint); %20.3076 21 | -------------------------------------------------------------------------------- /code/readonly/quadEOM_readonly.m: -------------------------------------------------------------------------------- 1 | % written by Daniel Mellinger 2 | % July 1, 2011 3 | % 4 | % For details of the controller see the paper: 5 | % 6 | % Daniel Mellinger, Nathan Michael, and Vijay Kumar. 7 | % Trajectory Generation and Control for Precise Aggressive Maneuvers with Quadrotors. 8 | % Int. Symposium on Experimental Robotics, Dec 2010. 9 | % 10 | % For more info take a look at some of the papers posted here: 11 | % 12 | % https://fling.seas.upenn.edu/~dmel/wiki/index.php?n=Main.Publications 13 | 14 | function sdot = quadEOM_readonly(t, true_state, F, M, Fd) 15 | global params 16 | 17 | %************ EQUATIONS OF MOTION ************************ 18 | % FIXME: Ignore motor limit to make sure the desired control input is the 19 | % accelerometer output 20 | % 21 | % % Prop speeds omega2 22 | % omega2 = params.omega2_FM*[F;M]; 23 | % % enforce a max & min prop speed 24 | % if(~isempty(params.maxomega)) 25 | % omega2 = min(omega2,params.maxomega^2*ones(4,1)); 26 | % end 27 | % if(~isempty(params.minomega)) 28 | % omega2 = max(omega2,params.minomega^2*ones(4,1)); 29 | % end 30 | % % Convert back to force & moment 31 | % [FM] = params.FM_omega2*omega2; 32 | % % Re-assemble Control input 33 | % F = FM(1); 34 | % M = FM(2:4); 35 | 36 | % Assign states 37 | % x = true_state(1); 38 | % y = true_state(2); 39 | % z = true_state(3); 40 | xdot = true_state(4); 41 | ydot = true_state(5); 42 | zdot = true_state(6); 43 | qW = true_state(7); 44 | qX = true_state(8); 45 | qY = true_state(9); 46 | qZ = true_state(10); 47 | p = true_state(11); 48 | q = true_state(12); 49 | r = true_state(13); 50 | Rot = QuatToRot([qW,qX,qY,qZ]'); 51 | [phi,theta,yawangle] = RotToRPY_ZXY(Rot); 52 | 53 | %init 54 | sdot = zeros(13,1); 55 | %rotation matrix from world frame to body frame 56 | BRW = [ cos(yawangle)*cos(theta) - sin(phi)*sin(yawangle)*sin(theta), cos(theta)*sin(yawangle) + cos(yawangle)*sin(phi)*sin(theta), -cos(phi)*sin(theta);... 57 | -cos(phi)*sin(yawangle), cos(phi)*cos(yawangle), sin(phi);... 58 | cos(yawangle)*sin(theta) + cos(theta)*sin(phi)*sin(yawangle), sin(yawangle)*sin(theta) - cos(yawangle)*cos(theta)*sin(phi), cos(phi)*cos(theta)]; 59 | WRB = BRW'; 60 | 61 | % Acceleration 62 | accel = 1/params.mass *(WRB * ([0;0;F] + Fd) - [0;0;params.mass*params.grav]); 63 | % Angular velocity 64 | K_quat = 2; %this enforces the magnitude 1 constraint for the quaternion 65 | quaterror = 1 - (qW^2 + qX^2 + qY^2 + qZ^2); 66 | qdot = -1/2*[0,-p,-q,-r;... 67 | p,0,-r,q;... 68 | q,r,0,-p;... 69 | r,-q,p,0]*[qW,qX,qY,qZ]' + K_quat*quaterror*[qW,qX,qY,qZ]'; 70 | % Angular acceleration 71 | omega = [p;q;r]; 72 | pqrdot = params.invI * (M - cross(omega,params.I*omega)); 73 | 74 | % Assemble sdot 75 | sdot(1) = xdot; 76 | sdot(2) = ydot; 77 | sdot(3) = zdot; 78 | sdot(4) = accel(1); 79 | sdot(5) = accel(2); 80 | sdot(6) = accel(3); 81 | sdot(7) = qdot(1); 82 | sdot(8) = qdot(2); 83 | sdot(9) = qdot(3); 84 | sdot(10) = qdot(4); 85 | sdot(11) = pqrdot(1); 86 | sdot(12) = pqrdot(2); 87 | sdot(13) = pqrdot(3); 88 | -------------------------------------------------------------------------------- /code/readonly/quadModel_readonly.m: -------------------------------------------------------------------------------- 1 | % written by Daniel Mellinger 2 | % July 1, 2011 3 | % 4 | % For details of the controller see the paper: 5 | % 6 | % Daniel Mellinger, Nathan Michael, and Vijay Kumar. 7 | % Trajectory Generation and Control for Precise Aggressive Maneuvers with Quadrotors. 8 | % Int. Symposium on Experimental Robotics, Dec 2010. 9 | % 10 | % For more info take a look at some of the papers posted here: 11 | % 12 | % https://fling.seas.upenn.edu/~dmel/wiki/index.php?n=Main.Publications 13 | 14 | function params = quadModel_readonly() 15 | 16 | m = 0.5; 17 | g = 9.81; 18 | I = [2.32e-3,0,0;0,2.32e-3,0;0,0,4e-3]; 19 | params.mass = m; 20 | params.I = I; 21 | params.invI = inv(I); 22 | params.grav = g; 23 | 24 | params.maxangle = 40*pi/180; %you can specify the maximum commanded angle here 25 | params.maxF = 2.5*m*g; 26 | params.minF = 0.05*m*g; 27 | 28 | 29 | %armlength is the distance from the center of the craft to the prop center 30 | params.kforce = 6.11e-8; %in Newton/rpm^2 31 | params.kmoment = 1.5e-9; %in Newton*meter/rpm^2 32 | params.armlength = 0.175; %in meters 33 | 34 | params.FM_omega2 = [params.kforce,params.kforce,params.kforce,params.kforce;... 35 | 0,params.armlength*params.kforce,0,-params.armlength*params.kforce;... 36 | -params.armlength*params.kforce,0,params.armlength*params.kforce,0;... 37 | params.kmoment,-params.kmoment,params.kmoment,-params.kmoment]; 38 | 39 | params.omega2_FM = inv(params.FM_omega2); 40 | 41 | params.maxomega = sqrt(params.maxF/(4*params.kforce)); 42 | params.minomega = sqrt(params.minF/(4*params.kforce)); 43 | -------------------------------------------------------------------------------- /code/run6formation.m: -------------------------------------------------------------------------------- 1 | % Used for HKUST ELEC 5660 2 | 3 | function run6formation(h1, h2, h3, h4, h5, h6, h7, h8, h9, h10, h11, h12, ... 4 | h13, h14, h15, h16, h17, h18, h19, h20, h21, h22, h23, h24, h25, h26, ... 5 | h27, h28, h29, h30, h31, h32, h33, h34, h35, h36, h37, h38, h39, h40, ... 6 | h41, h42, h43, h44, h45, h46, h47, h48, h49) 7 | 8 | addpath('./SI_DFM_2D_SYSU','./readonly','utils') 9 | load 'SI_dynamic_fomation_manv_results.mat' 10 | 11 | % Sensor parameters 12 | fnoise = 1; % Standard deviation of gaussian noise for external disturbance (N) 13 | ifov = 60; % Camera field of view 14 | 15 | % Initialize simulation 16 | global params; 17 | params = quadModel_readonly(); % Quad model 18 | 19 | yaw0 = -30 * pi / 180; 20 | pitch0 = -30 * pi / 180; 21 | roll0 = -30 * pi / 180; 22 | Quat0 = R_to_quaternion(ypr_to_R([yaw0, pitch0, roll0])'); 23 | 24 | %% quadrotor true states initialize 25 | x0 = [0, 0.5, -0.5, -0.5, 0, 0.5; ... 26 | 1, 0.5, 0.5, -0.5, -1, -0.5; ... 27 | 1, 1, 1, 1, 1, 1; ... 28 | 0, 0, 0, 0, 0, 0; ... 29 | 0, 0, 0, 0, 0, 0; ... 30 | 0, 0, 0, 0, 0, 0; ... 31 | Quat0(1), Quat0(1), Quat0(1), Quat0(1), Quat0(1), Quat0(1); ... 32 | Quat0(2), Quat0(2), Quat0(2), Quat0(2), Quat0(2), Quat0(2); ... 33 | Quat0(3), Quat0(3), Quat0(3), Quat0(3), Quat0(3), Quat0(3); ... 34 | Quat0(4), Quat0(4), Quat0(4), Quat0(4), Quat0(4), Quat0(4); ... 35 | 0, 0, 0, 0, 0, 0; ... 36 | 0, 0, 0, 0, 0, 0; ... 37 | 0, 0, 0, 0, 0, 0]; 38 | 39 | true_s = x0; % true state 40 | F1 = params.mass * params.grav; 41 | M1 = [0; 0; 0]; 42 | 43 | F2 = params.mass * params.grav; 44 | M2 = [0; 0; 0]; 45 | 46 | F3 = params.mass * params.grav; 47 | M3 = [0; 0; 0]; 48 | 49 | F4 = params.mass * params.grav; 50 | M4 = [0; 0; 0]; 51 | 52 | F5 = params.mass * params.grav; 53 | M5 = [0; 0; 0]; 54 | 55 | F6 = params.mass * params.grav; 56 | M6 = [0; 0; 0]; 57 | 58 | % Time 59 | tstep = 0.002; % Time step for solving equations of motion // FIXME: not 0.01 60 | cstep = 0.01; % Period of calling student code 61 | vstep = 0.05; % visualization interval 62 | time = 0; % current time 63 | vis_time = 0; % Time of last visualization 64 | time_tol = 25; % Maximum time that the quadrotor is allowed to fly 65 | 66 | % Visualization 67 | vis_init = false; 68 | 69 | 70 | % h1 71 | thprop1 = []; 72 | thprop2 = []; 73 | thprop3 = []; 74 | thprop4 = []; 75 | tharm1 = []; 76 | tharm2 = []; 77 | thfov1 = []; 78 | thfov2 = []; 79 | thfov3 = []; 80 | thfov4 = []; 81 | ehprop1 = []; 82 | ehprop2 = []; 83 | ehprop3 = []; 84 | ehprop4 = []; 85 | eharm1 = []; 86 | eharm2 = []; 87 | ehmap = []; 88 | ehwindow = []; 89 | 90 | % h2 h3 91 | thpitch = []; 92 | throll = []; 93 | 94 | % Start Simulation run_trajectory_readonly 95 | disp('Start Simulation ...'); 96 | while (1) 97 | 98 | % External disturbance 99 | Fd = randn(3, 1) * fnoise; 100 | 101 | % Run simulation for cstep 102 | timeint = time:tstep:time + cstep; 103 | [~, xsave] = ode45(@(t, s) quadEOM_readonly(t, s, F1, M1, Fd), timeint', true_s(:, 1)); 104 | true_s(:, 1) = xsave(end, :)'; 105 | 106 | [~, xsave] = ode45(@(t, s) quadEOM_readonly(t, s, F2, M2, Fd), timeint', true_s(:, 2)); 107 | true_s(:, 2) = xsave(end, :)'; 108 | 109 | [~, xsave] = ode45(@(t, s) quadEOM_readonly(t, s, F3, M3, Fd), timeint', true_s(:, 3)); 110 | true_s(:, 3) = xsave(end, :)'; 111 | 112 | [~, xsave] = ode45(@(t, s) quadEOM_readonly(t, s, F4, M4, Fd), timeint', true_s(:, 4)); 113 | true_s(:, 4) = xsave(end, :)'; 114 | 115 | [~, xsave] = ode45(@(t, s) quadEOM_readonly(t, s, F5, M5, Fd), timeint', true_s(:, 5)); 116 | true_s(:, 5) = xsave(end, :)'; 117 | 118 | [~, xsave] = ode45(@(t, s) quadEOM_readonly(t, s, F6, M6, Fd), timeint', true_s(:, 6)); 119 | true_s(:, 6) = xsave(end, :)'; 120 | 121 | time = time + cstep; 122 | 123 | i = ceil(time/0.1); 124 | 125 | 126 | s_des = [x(i, 1), x(i, 2), x(i, 3), x(i, 4), x(i, 5), x(i, 6); ... 127 | y(i, 1), y(i, 2), y(i, 3), y(i, 4), y(i, 5), y(i, 6); ... 128 | 1, 1, 1, 1, 1, 1; ... 129 | 0, 0, 0, 0, 0, 0; ... 130 | 0, 0, 0, 0, 0, 0; ... 131 | 0, 0, 0, 0, 0, 0; ... 132 | 1, 1, 1, 1, 1, 1; ... 133 | 0, 0, 0, 0, 0, 0; ... 134 | 0, 0, 0, 0, 0, 0; ... 135 | 0, 0, 0, 0, 0, 0; ... 136 | 0, 0, 0, 0, 0, 0; ... 137 | 0, 0, 0, 0, 0, 0; ... 138 | 0, 0, 0, 0, 0, 0]; 139 | 140 | 141 | [F1, M1] = controller1(time , true_s(:, 1), s_des(:, 1)); 142 | 143 | [F2, M2] = controller2(time , true_s(:, 2), s_des(:, 2)); 144 | 145 | [F3, M3] = controller3(time , true_s(:, 3), s_des(:, 3)); 146 | 147 | [F4, M4] = controller4(time , true_s(:, 4), s_des(:, 4)); 148 | 149 | [F5, M5] = controller5(time , true_s(:, 5), s_des(:, 5)); 150 | 151 | [F6, M6] = controller6(time , true_s(:, 6), s_des(:, 6)); 152 | 153 | if (i >= length(t)*4) || (time >= time_tol) 154 | save('./readonly/6formation_results.mat', 'time','thtraj1', 'ehtraj1', 'thtraj2', 'ehtraj2', 'thtraj3', 'ehtraj3', ... 155 | 'thtraj4', 'ehtraj4','thtraj5', 'ehtraj5', 'thtraj6', 'ehtraj6', 'thvx1', 'ehvx1', 'thvx2', 'ehvx2','thvx3', 'ehvx3', ... 156 | 'thvx4', 'ehvx4','thvx5', 'ehvx5','thvx6', 'ehvx6','thvy1', 'ehvy1','thvy2', 'ehvy2','thvy3', 'ehvy3','thvy4', 'ehvy4', ... 157 | 'thvy5', 'ehvy5', 'thvy6', 'ehvy6', 'x0'); 158 | disp("simulation complete!"); 159 | break; 160 | end 161 | 162 | %% Rlot Results 163 | if time - vis_time > vstep 164 | 165 | %% Plot quad, fov, and estimated quad, estimated_map, and sliding window 166 | subplot(h1); 167 | hold on; 168 | if ~vis_init 169 | grid on; 170 | %axis equal; 171 | %axis ([-5 5 -2 12 -1 4]); 172 | %axis auto 173 | end 174 | %% at special time plot S Y S U 175 | if (i == 32) 176 | line([s_des(1, 2), s_des(1, 1)], [s_des(2, 2), s_des(2, 1)], 'Color', [0.5, 0.5, 1], 'LineWidth', 3); 177 | line([s_des(1, 1), s_des(1, 3)], [s_des(2, 1), s_des(2, 3)], 'Color', [0.5, 0.5, 1], 'LineWidth', 3); 178 | line([s_des(1, 3), s_des(1, 6)], [s_des(2, 3), s_des(2, 6)], 'Color', [0.5, 0.5, 1], 'LineWidth', 3); 179 | line([s_des(1, 6), s_des(1, 5)], [s_des(2, 6), s_des(2, 5)], 'Color', [0.5, 0.5, 1], 'LineWidth', 3); 180 | line([s_des(1, 5), s_des(1, 4)], [s_des(2, 5), s_des(2, 4)], 'Color', [0.5, 0.5, 1], 'LineWidth', 3); 181 | 182 | elseif (i == 82) 183 | line([s_des(1, 3), s_des(1, 1)], [s_des(2, 3), s_des(2, 1)], 'Color', [0.5, 0.5, 1], 'LineWidth', 3); 184 | line([s_des(1, 2), s_des(1, 3)], [s_des(2, 2), s_des(2, 3)], 'Color', [0.5, 0.5, 1], 'LineWidth', 3); 185 | line([s_des(1, 3), s_des(1, 4)], [s_des(2, 3), s_des(2, 4)], 'Color', [0.5, 0.5, 1], 'LineWidth', 3); 186 | line([s_des(1, 6), s_des(1, 4)], [s_des(2, 6), s_des(2, 4)], 'Color', [0.5, 0.5, 1], 'LineWidth', 3); 187 | line([s_des(1, 5), s_des(1, 6)], [s_des(2, 5), s_des(2, 6)], 'Color', [0.5, 0.5, 1], 'LineWidth', 3); 188 | 189 | elseif (i == 132) 190 | line([s_des(1, 2), s_des(1, 1)], [s_des(2, 2), s_des(2, 1)], 'Color', [0.5, 0.5, 1], 'LineWidth', 3); 191 | line([s_des(1, 1), s_des(1, 3)], [s_des(2, 1), s_des(2, 3)], 'Color', [0.5, 0.5, 1], 'LineWidth', 3); 192 | line([s_des(1, 3), s_des(1, 6)], [s_des(2, 3), s_des(2, 6)], 'Color', [0.5, 0.5, 1], 'LineWidth', 3); 193 | line([s_des(1, 6), s_des(1, 5)], [s_des(2, 6), s_des(2, 5)], 'Color', [0.5, 0.5, 1], 'LineWidth', 3); 194 | line([s_des(1, 5), s_des(1, 4)], [s_des(2, 5), s_des(2, 4)], 'Color', [0.5, 0.5, 1], 'LineWidth', 3); 195 | 196 | elseif (i == 182) 197 | line([s_des(1, 2), s_des(1, 6)], [s_des(2, 2), s_des(2, 6)], 'Color', [0.5, 0.5, 1], 'LineWidth', 3); 198 | line([s_des(1, 1), s_des(1, 3)], [s_des(2, 1), s_des(2, 3)], 'Color', [0.5, 0.5, 1], 'LineWidth', 3); 199 | line([s_des(1, 3), s_des(1, 4)], [s_des(2, 3), s_des(2, 4)], 'Color', [0.5, 0.5, 1], 'LineWidth', 3); 200 | line([s_des(1, 4), s_des(1, 5)], [s_des(2, 4), s_des(2, 5)], 'Color', [0.5, 0.5, 1], 'LineWidth', 3); 201 | line([s_des(1, 5), s_des(1, 6)], [s_des(2, 5), s_des(2, 6)], 'Color', [0.5, 0.5, 1], 'LineWidth', 3); 202 | end 203 | plot3(s_des(1), s_des(2), s_des(3), 'm-'); 204 | 205 | ll = 0.175 / 3; 206 | rr = 0.1 / 3; 207 | ff = 0.3 / 3; 208 | nprop = 40 / 4; 209 | propangs = linspace(0, 2*pi, nprop); 210 | tR = QuatToRot(true_s(7:10, 1))'; 211 | tpoint1 = tR * [ll; 0; 0]; 212 | tpoint2 = tR * [0; ll; 0]; 213 | tpoint3 = tR * [-ll; 0; 0]; 214 | tpoint4 = tR * [0; -ll; 0]; 215 | tproppts = rr * tR * [cos(propangs); sin(propangs); zeros(1, nprop)]; 216 | twp1 = true_s(1:3, 1) + tpoint1; 217 | twp2 = true_s(1:3, 1) + tpoint2; 218 | twp3 = true_s(1:3, 1) + tpoint3; 219 | twp4 = true_s(1:3, 1) + tpoint4; 220 | tprop1 = tproppts + twp1 * ones(1, nprop); 221 | tprop2 = tproppts + twp2 * ones(1, nprop); 222 | tprop3 = tproppts + twp3 * ones(1, nprop); 223 | tprop4 = tproppts + twp4 * ones(1, nprop); 224 | tfov0 = true_s(1:3, 1); 225 | tfov1 = tR * [ff; ff * tan(ifov*pi/180/2); ff * tan(ifov*pi/180/2)] + true_s(1:3, 1); 226 | tfov2 = tR * [ff; ff * tan(ifov*pi/180/2); -ff * tan(ifov*pi/180/2)] + true_s(1:3, 1); 227 | tfov3 = tR * [ff; -ff * tan(ifov*pi/180/2); -ff * tan(ifov*pi/180/2)] + true_s(1:3, 1); 228 | tfov4 = tR * [ff; -ff * tan(ifov*pi/180/2); ff * tan(ifov*pi/180/2)] + true_s(1:3, 1); 229 | eR = QuatToRot(s_des(7:10, 1))'; 230 | epoint1 = eR * [ll; 0; 0]; 231 | epoint2 = eR * [0; ll; 0]; 232 | epoint3 = eR * [-ll; 0; 0]; 233 | epoint4 = eR * [0; -ll; 0]; 234 | eproppts = rr * eR * [cos(propangs); sin(propangs); zeros(1, nprop)]; 235 | ewp1 = s_des(1:3, 1) + epoint1; 236 | ewp2 = s_des(1:3, 1) + epoint2; 237 | ewp3 = s_des(1:3, 1) + epoint3; 238 | ewp4 = s_des(1:3, 1) + epoint4; 239 | eprop1 = eproppts + ewp1 * ones(1, nprop); 240 | eprop2 = eproppts + ewp2 * ones(1, nprop); 241 | eprop3 = eproppts + ewp3 * ones(1, nprop); 242 | eprop4 = eproppts + ewp4 * ones(1, nprop); 243 | emap = [0; 0; 0]; 244 | ewindow = [0; 0; 0]; 245 | 246 | if ~vis_init 247 | thtraj1 = plot3(true_s(1, 1), true_s(2, 1), true_s(3, 1), 'b-', 'LineWidth', 0.5); 248 | tharm11 = line([twp1(1), twp3(1)], [twp1(2), twp3(2)], [twp1(3), twp3(3)], 'Color', 'b'); %画出无人机交叉臂 249 | tharm12 = line([twp2(1), twp4(1)], [twp2(2), twp4(2)], [twp2(3), twp4(3)], 'Color', 'b'); 250 | thprop11 = plot3(tprop1(1, :), tprop1(2, :), tprop1(3, :), 'r-'); %画出无人机四个圆圈 251 | thprop12 = plot3(tprop2(1, :), tprop2(2, :), tprop2(3, :), 'b-'); 252 | thprop13 = plot3(tprop3(1, :), tprop3(2, :), tprop3(3, :), 'b-'); 253 | thprop14 = plot3(tprop4(1, :), tprop4(2, :), tprop4(3, :), 'b-'); 254 | thfov11 = line([tfov0(1), tfov1(1), tfov2(1)], [tfov0(2), tfov1(2), tfov2(2)], [tfov0(3), tfov1(3), tfov2(3)], 'Color', 'k'); 255 | thfov12 = line([tfov0(1), tfov2(1), tfov3(1)], [tfov0(2), tfov2(2), tfov3(2)], [tfov0(3), tfov2(3), tfov3(3)], 'Color', 'k'); 256 | thfov13 = line([tfov0(1), tfov3(1), tfov4(1)], [tfov0(2), tfov3(2), tfov4(2)], [tfov0(3), tfov3(3), tfov4(3)], 'Color', 'k'); 257 | thfov14 = line([tfov0(1), tfov4(1), tfov1(1)], [tfov0(2), tfov4(2), tfov1(2)], [tfov0(3), tfov4(3), tfov1(3)], 'Color', 'k'); 258 | 259 | ehtraj1 = plot3(s_des(1, 1), s_des(2, 1), s_des(3, 1), 'g-', 'LineWidth', 1); 260 | 261 | eharm11 = line([ewp1(1), ewp3(1)], [ewp1(2), ewp3(2)], [ewp1(3), ewp3(3)], 'Color', 'g'); 262 | eharm12 = line([ewp2(1), ewp4(1)], [ewp2(2), ewp4(2)], [ewp2(3), ewp4(3)], 'Color', 'g'); 263 | ehprop11 = plot3(eprop1(1, :), eprop1(2, :), eprop1(3, :), 'm-'); 264 | ehprop12 = plot3(eprop2(1, :), eprop2(2, :), eprop2(3, :), 'g-'); 265 | ehprop13 = plot3(eprop3(1, :), eprop3(2, :), eprop3(3, :), 'g-'); 266 | ehprop14 = plot3(eprop4(1, :), eprop4(2, :), eprop4(3, :), 'g-'); 267 | ehmap1 = plot3(emap(1, :), emap(2, :), emap(3, :), 'ko', 'MarkerSize', 10, 'MarkerFaceColor', 'k'); 268 | ehwindow1 = plot3(ewindow(1, :), ewindow(2, :), ewindow(3, :), 'ro', 'MarkerSize', 5, 'MarkerFaceColor', 'r'); 269 | else 270 | set(thtraj1, 'XData', [get(thtraj1, 'XData'), true_s(1, 1)]); 271 | set(thtraj1, 'YData', [get(thtraj1, 'YData'), true_s(2, 1)]); 272 | set(thtraj1, 'ZData', [get(thtraj1, 'ZData'), true_s(3, 1)]); 273 | set(thprop11, 'XData', tprop1(1, :)); 274 | set(thprop11, 'YData', tprop1(2, :)); 275 | set(thprop11, 'ZData', tprop1(3, :)); 276 | set(thprop12, 'XData', tprop2(1, :)); 277 | set(thprop12, 'YData', tprop2(2, :)); 278 | set(thprop12, 'ZData', tprop2(3, :)); 279 | set(thprop13, 'XData', tprop3(1, :)); 280 | set(thprop13, 'YData', tprop3(2, :)); 281 | set(thprop13, 'ZData', tprop3(3, :)); 282 | set(thprop14, 'XData', tprop4(1, :)); 283 | set(thprop14, 'YData', tprop4(2, :)); 284 | set(thprop14, 'ZData', tprop4(3, :)); 285 | set(tharm11, 'XData', [twp1(1), twp3(1)]); 286 | set(tharm11, 'YData', [twp1(2), twp3(2)]); 287 | set(tharm11, 'ZData', [twp1(3), twp3(3)]); 288 | set(tharm12, 'XData', [twp2(1), twp4(1)]); 289 | set(tharm12, 'YData', [twp2(2), twp4(2)]); 290 | set(tharm12, 'ZData', [twp2(3), twp4(3)]); 291 | set(thfov11, 'XData', [tfov0(1), tfov1(1), tfov2(1)]); 292 | set(thfov11, 'YData', [tfov0(2), tfov1(2), tfov2(2)]); 293 | set(thfov11, 'ZData', [tfov0(3), tfov1(3), tfov2(3)]); 294 | set(thfov12, 'XData', [tfov0(1), tfov2(1), tfov3(1)]); 295 | set(thfov12, 'YData', [tfov0(2), tfov2(2), tfov3(2)]); 296 | set(thfov12, 'ZData', [tfov0(3), tfov2(3), tfov3(3)]); 297 | set(thfov13, 'XData', [tfov0(1), tfov3(1), tfov4(1)]); 298 | set(thfov13, 'YData', [tfov0(2), tfov3(2), tfov4(2)]); 299 | set(thfov13, 'ZData', [tfov0(3), tfov3(3), tfov4(3)]); 300 | set(thfov14, 'XData', [tfov0(1), tfov4(1), tfov1(1)]); 301 | set(thfov14, 'YData', [tfov0(2), tfov4(2), tfov1(2)]); 302 | set(thfov14, 'ZData', [tfov0(3), tfov4(3), tfov1(3)]); 303 | set(ehtraj1, 'XData', [get(ehtraj1, 'XData'), s_des(1, 1)]); 304 | set(ehtraj1, 'YData', [get(ehtraj1, 'YData'), s_des(2, 1)]); 305 | set(ehtraj1, 'ZData', [get(ehtraj1, 'ZData'), s_des(3, 1)]); 306 | set(ehprop11, 'XData', eprop1(1, :)); 307 | set(ehprop11, 'YData', eprop1(2, :)); 308 | set(ehprop11, 'ZData', eprop1(3, :)); 309 | set(ehprop12, 'XData', eprop2(1, :)); 310 | set(ehprop12, 'YData', eprop2(2, :)); 311 | set(ehprop12, 'ZData', eprop2(3, :)); 312 | set(ehprop13, 'XData', eprop3(1, :)); 313 | set(ehprop13, 'YData', eprop3(2, :)); 314 | set(ehprop13, 'ZData', eprop3(3, :)); 315 | set(ehprop14, 'XData', eprop4(1, :)); 316 | set(ehprop14, 'YData', eprop4(2, :)); 317 | set(ehprop14, 'ZData', eprop4(3, :)); 318 | set(eharm11, 'XData', [ewp1(1), ewp3(1)]); 319 | set(eharm11, 'YData', [ewp1(2), ewp3(2)]); 320 | set(eharm11, 'ZData', [ewp1(3), ewp3(3)]); 321 | set(eharm12, 'XData', [ewp2(1), ewp4(1)]); 322 | set(eharm12, 'YData', [ewp2(2), ewp4(2)]); 323 | set(eharm12, 'ZData', [ewp2(3), ewp4(3)]); 324 | set(ehmap1, 'XData', emap(1, :)); 325 | set(ehmap1, 'YData', emap(2, :)); 326 | set(ehmap1, 'ZData', emap(3, :)); 327 | set(ehwindow1, 'XData', ewindow(1, :)); 328 | set(ehwindow1, 'YData', ewindow(2, :)); 329 | set(ehwindow1, 'ZData', ewindow(3, :)); 330 | end 331 | hold on; 332 | 333 | grid on; 334 | 335 | 336 | twp1 = true_s(1:3, 2) + tpoint1; 337 | twp2 = true_s(1:3, 2) + tpoint2; 338 | twp3 = true_s(1:3, 2) + tpoint3; 339 | twp4 = true_s(1:3, 2) + tpoint4; 340 | tprop1 = tproppts + twp1 * ones(1, nprop); 341 | tprop2 = tproppts + twp2 * ones(1, nprop); 342 | tprop3 = tproppts + twp3 * ones(1, nprop); 343 | tprop4 = tproppts + twp4 * ones(1, nprop); 344 | tfov0 = true_s(1:3, 2); 345 | tfov1 = tR * [ff; ff * tan(ifov*pi/180/2); ff * tan(ifov*pi/180/2)] + true_s(1:3, 2); 346 | tfov2 = tR * [ff; ff * tan(ifov*pi/180/2); -ff * tan(ifov*pi/180/2)] + true_s(1:3, 2); 347 | tfov3 = tR * [ff; -ff * tan(ifov*pi/180/2); -ff * tan(ifov*pi/180/2)] + true_s(1:3, 2); 348 | tfov4 = tR * [ff; -ff * tan(ifov*pi/180/2); ff * tan(ifov*pi/180/2)] + true_s(1:3, 2); 349 | eR = QuatToRot(s_des(7:10, 2))'; 350 | epoint1 = eR * [ll; 0; 0]; 351 | epoint2 = eR * [0; ll; 0]; 352 | epoint3 = eR * [-ll; 0; 0]; 353 | epoint4 = eR * [0; -ll; 0]; 354 | eproppts = rr * eR * [cos(propangs); sin(propangs); zeros(1, nprop)]; 355 | 356 | 357 | ewp1 = s_des(1:3, 2) + epoint1; 358 | ewp2 = s_des(1:3, 2) + epoint2; 359 | ewp3 = s_des(1:3, 2) + epoint3; 360 | ewp4 = s_des(1:3, 2) + epoint4; 361 | 362 | eprop1 = eproppts + ewp1 * ones(1, nprop); 363 | eprop2 = eproppts + ewp2 * ones(1, nprop); 364 | eprop3 = eproppts + ewp3 * ones(1, nprop); 365 | eprop4 = eproppts + ewp4 * ones(1, nprop); 366 | emap = [0; 0; 0]; 367 | ewindow = [0; 0; 0]; 368 | 369 | if ~vis_init 370 | thtraj2 = plot3(true_s(1, 2), true_s(2, 2), true_s(3, 2), 'b-', 'LineWidth', 0.5); 371 | tharm21 = line([twp1(1), twp3(1)], [twp1(2), twp3(2)], [twp1(3), twp3(3)], 'Color', 'b'); 372 | tharm22 = line([twp2(1), twp4(1)], [twp2(2), twp4(2)], [twp2(3), twp4(3)], 'Color', 'b'); 373 | thprop21 = plot3(tprop1(1, :), tprop1(2, :), tprop1(3, :), 'r-'); 374 | thprop22 = plot3(tprop2(1, :), tprop2(2, :), tprop2(3, :), 'b-'); 375 | thprop23 = plot3(tprop3(1, :), tprop3(2, :), tprop3(3, :), 'b-'); 376 | thprop24 = plot3(tprop4(1, :), tprop4(2, :), tprop4(3, :), 'b-'); 377 | thfov21 = line([tfov0(1), tfov1(1), tfov2(1)], [tfov0(2), tfov1(2), tfov2(2)], [tfov0(3), tfov1(3), tfov2(3)], 'Color', 'k'); 378 | thfov22 = line([tfov0(1), tfov2(1), tfov3(1)], [tfov0(2), tfov2(2), tfov3(2)], [tfov0(3), tfov2(3), tfov3(3)], 'Color', 'k'); 379 | thfov23 = line([tfov0(1), tfov3(1), tfov4(1)], [tfov0(2), tfov3(2), tfov4(2)], [tfov0(3), tfov3(3), tfov4(3)], 'Color', 'k'); 380 | thfov24 = line([tfov0(1), tfov4(1), tfov1(1)], [tfov0(2), tfov4(2), tfov1(2)], [tfov0(3), tfov4(3), tfov1(3)], 'Color', 'k'); 381 | 382 | ehtraj2 = plot3(s_des(1, 2), s_des(2, 2), s_des(3, 2), 'g-', 'LineWidth', 1); 383 | 384 | eharm21 = line([ewp1(1), ewp3(1)], [ewp1(2), ewp3(2)], [ewp1(3), ewp3(3)], 'Color', 'g'); 385 | eharm22 = line([ewp2(1), ewp4(1)], [ewp2(2), ewp4(2)], [ewp2(3), ewp4(3)], 'Color', 'g'); 386 | ehprop21 = plot3(eprop1(1, :), eprop1(2, :), eprop1(3, :), 'm-'); 387 | ehprop22 = plot3(eprop2(1, :), eprop2(2, :), eprop2(3, :), 'g-'); 388 | ehprop23 = plot3(eprop3(1, :), eprop3(2, :), eprop3(3, :), 'g-'); 389 | ehprop24 = plot3(eprop4(1, :), eprop4(2, :), eprop4(3, :), 'g-'); 390 | ehmap2 = plot3(emap(1, :), emap(2, :), emap(3, :), 'ko', 'MarkerSize', 10, 'MarkerFaceColor', 'k'); 391 | ehwindow2 = plot3(ewindow(1, :), ewindow(2, :), ewindow(3, :), 'ro', 'MarkerSize', 5, 'MarkerFaceColor', 'r'); 392 | else 393 | set(thtraj2, 'XData', [get(thtraj2, 'XData'), true_s(1, 2)]); 394 | set(thtraj2, 'YData', [get(thtraj2, 'YData'), true_s(2, 2)]); 395 | set(thtraj2, 'ZData', [get(thtraj2, 'ZData'), true_s(3, 2)]); 396 | set(thprop21, 'XData', tprop1(1, :)); 397 | set(thprop21, 'YData', tprop1(2, :)); 398 | set(thprop21, 'ZData', tprop1(3, :)); 399 | set(thprop22, 'XData', tprop2(1, :)); 400 | set(thprop22, 'YData', tprop2(2, :)); 401 | set(thprop22, 'ZData', tprop2(3, :)); 402 | set(thprop23, 'XData', tprop3(1, :)); 403 | set(thprop23, 'YData', tprop3(2, :)); 404 | set(thprop23, 'ZData', tprop3(3, :)); 405 | set(thprop24, 'XData', tprop4(1, :)); 406 | set(thprop24, 'YData', tprop4(2, :)); 407 | set(thprop24, 'ZData', tprop4(3, :)); 408 | set(tharm21, 'XData', [twp1(1), twp3(1)]); 409 | set(tharm21, 'YData', [twp1(2), twp3(2)]); 410 | set(tharm21, 'ZData', [twp1(3), twp3(3)]); 411 | set(tharm22, 'XData', [twp2(1), twp4(1)]); 412 | set(tharm22, 'YData', [twp2(2), twp4(2)]); 413 | set(tharm22, 'ZData', [twp2(3), twp4(3)]); 414 | set(thfov21, 'XData', [tfov0(1), tfov1(1), tfov2(1)]); 415 | set(thfov21, 'YData', [tfov0(2), tfov1(2), tfov2(2)]); 416 | set(thfov21, 'ZData', [tfov0(3), tfov1(3), tfov2(3)]); 417 | set(thfov22, 'XData', [tfov0(1), tfov2(1), tfov3(1)]); 418 | set(thfov22, 'YData', [tfov0(2), tfov2(2), tfov3(2)]); 419 | set(thfov22, 'ZData', [tfov0(3), tfov2(3), tfov3(3)]); 420 | set(thfov23, 'XData', [tfov0(1), tfov3(1), tfov4(1)]); 421 | set(thfov23, 'YData', [tfov0(2), tfov3(2), tfov4(2)]); 422 | set(thfov23, 'ZData', [tfov0(3), tfov3(3), tfov4(3)]); 423 | set(thfov24, 'XData', [tfov0(1), tfov4(1), tfov1(1)]); 424 | set(thfov24, 'YData', [tfov0(2), tfov4(2), tfov1(2)]); 425 | set(thfov24, 'ZData', [tfov0(3), tfov4(3), tfov1(3)]); 426 | set(ehtraj2, 'XData', [get(ehtraj2, 'XData'), s_des(1, 2)]); 427 | set(ehtraj2, 'YData', [get(ehtraj2, 'YData'), s_des(2, 2)]); 428 | set(ehtraj2, 'ZData', [get(ehtraj2, 'ZData'), s_des(3, 2)]); 429 | set(ehprop21, 'XData', eprop1(1, :)); 430 | set(ehprop21, 'YData', eprop1(2, :)); 431 | set(ehprop21, 'ZData', eprop1(3, :)); 432 | set(ehprop22, 'XData', eprop2(1, :)); 433 | set(ehprop22, 'YData', eprop2(2, :)); 434 | set(ehprop22, 'ZData', eprop2(3, :)); 435 | set(ehprop23, 'XData', eprop3(1, :)); 436 | set(ehprop23, 'YData', eprop3(2, :)); 437 | set(ehprop23, 'ZData', eprop3(3, :)); 438 | set(ehprop24, 'XData', eprop4(1, :)); 439 | set(ehprop24, 'YData', eprop4(2, :)); 440 | set(ehprop24, 'ZData', eprop4(3, :)); 441 | set(eharm21, 'XData', [ewp1(1), ewp3(1)]); 442 | set(eharm21, 'YData', [ewp1(2), ewp3(2)]); 443 | set(eharm21, 'ZData', [ewp1(3), ewp3(3)]); 444 | set(eharm22, 'XData', [ewp2(1), ewp4(1)]); 445 | set(eharm22, 'YData', [ewp2(2), ewp4(2)]); 446 | set(eharm22, 'ZData', [ewp2(3), ewp4(3)]); 447 | set(ehmap2, 'XData', emap(1, :)); 448 | set(ehmap2, 'YData', emap(2, :)); 449 | set(ehmap2, 'ZData', emap(3, :)); 450 | set(ehwindow2, 'XData', ewindow(1, :)); 451 | set(ehwindow2, 'YData', ewindow(2, :)); 452 | set(ehwindow2, 'ZData', ewindow(3, :)); 453 | end 454 | 455 | hold on; 456 | 457 | grid on; 458 | 459 | 460 | twp1 = true_s(1:3, 3) + tpoint1; 461 | twp2 = true_s(1:3, 3) + tpoint2; 462 | twp3 = true_s(1:3, 3) + tpoint3; 463 | twp4 = true_s(1:3, 3) + tpoint4; 464 | tprop1 = tproppts + twp1 * ones(1, nprop); 465 | tprop2 = tproppts + twp2 * ones(1, nprop); 466 | tprop3 = tproppts + twp3 * ones(1, nprop); 467 | tprop4 = tproppts + twp4 * ones(1, nprop); 468 | tfov0 = true_s(1:3, 3); 469 | tfov1 = tR * [ff; ff * tan(ifov*pi/180/2); ff * tan(ifov*pi/180/2)] + true_s(1:3, 3); 470 | tfov2 = tR * [ff; ff * tan(ifov*pi/180/2); -ff * tan(ifov*pi/180/2)] + true_s(1:3, 3); 471 | tfov3 = tR * [ff; -ff * tan(ifov*pi/180/2); -ff * tan(ifov*pi/180/2)] + true_s(1:3, 3); 472 | tfov4 = tR * [ff; -ff * tan(ifov*pi/180/2); ff * tan(ifov*pi/180/2)] + true_s(1:3, 3); 473 | eR = QuatToRot(s_des(7:10, 3))'; 474 | epoint1 = eR * [ll; 0; 0]; 475 | epoint2 = eR * [0; ll; 0]; 476 | epoint3 = eR * [-ll; 0; 0]; 477 | epoint4 = eR * [0; -ll; 0]; 478 | eproppts = rr * eR * [cos(propangs); sin(propangs); zeros(1, nprop)]; 479 | 480 | 481 | ewp1 = s_des(1:3, 3) + epoint1; 482 | ewp2 = s_des(1:3, 3) + epoint2; 483 | ewp3 = s_des(1:3, 3) + epoint3; 484 | ewp4 = s_des(1:3, 3) + epoint4; 485 | 486 | eprop1 = eproppts + ewp1 * ones(1, nprop); 487 | eprop2 = eproppts + ewp2 * ones(1, nprop); 488 | eprop3 = eproppts + ewp3 * ones(1, nprop); 489 | eprop4 = eproppts + ewp4 * ones(1, nprop); 490 | emap = [0; 0; 0]; 491 | ewindow = [0; 0; 0]; 492 | 493 | if ~vis_init 494 | thtraj3 = plot3(true_s(1, 3), true_s(2, 3), true_s(3, 3), 'b-', 'LineWidth', 0.5); 495 | tharm31 = line([twp1(1), twp3(1)], [twp1(2), twp3(2)], [twp1(3), twp3(3)], 'Color', 'b'); 496 | tharm32 = line([twp2(1), twp4(1)], [twp2(2), twp4(2)], [twp2(3), twp4(3)], 'Color', 'b'); 497 | thprop31 = plot3(tprop1(1, :), tprop1(2, :), tprop1(3, :), 'r-'); 498 | thprop32 = plot3(tprop2(1, :), tprop2(2, :), tprop2(3, :), 'b-'); 499 | thprop33 = plot3(tprop3(1, :), tprop3(2, :), tprop3(3, :), 'b-'); 500 | thprop34 = plot3(tprop4(1, :), tprop4(2, :), tprop4(3, :), 'b-'); 501 | thfov31 = line([tfov0(1), tfov1(1), tfov2(1)], [tfov0(2), tfov1(2), tfov2(2)], [tfov0(3), tfov1(3), tfov2(3)], 'Color', 'k'); 502 | thfov32 = line([tfov0(1), tfov2(1), tfov3(1)], [tfov0(2), tfov2(2), tfov3(2)], [tfov0(3), tfov2(3), tfov3(3)], 'Color', 'k'); 503 | thfov33 = line([tfov0(1), tfov3(1), tfov4(1)], [tfov0(2), tfov3(2), tfov4(2)], [tfov0(3), tfov3(3), tfov4(3)], 'Color', 'k'); 504 | thfov34 = line([tfov0(1), tfov4(1), tfov1(1)], [tfov0(2), tfov4(2), tfov1(2)], [tfov0(3), tfov4(3), tfov1(3)], 'Color', 'k'); 505 | 506 | ehtraj3 = plot3(s_des(1, 3), s_des(2, 3), s_des(3, 3), 'g-', 'LineWidth', 0.5); 507 | 508 | eharm31 = line([ewp1(1), ewp3(1)], [ewp1(2), ewp3(2)], [ewp1(3), ewp3(3)], 'Color', 'g'); 509 | eharm32 = line([ewp2(1), ewp4(1)], [ewp2(2), ewp4(2)], [ewp2(3), ewp4(3)], 'Color', 'g'); 510 | ehprop31 = plot3(eprop1(1, :), eprop1(2, :), eprop1(3, :), 'm-'); 511 | ehprop32 = plot3(eprop2(1, :), eprop2(2, :), eprop2(3, :), 'g-'); 512 | ehprop33 = plot3(eprop3(1, :), eprop3(2, :), eprop3(3, :), 'g-'); 513 | ehprop34 = plot3(eprop4(1, :), eprop4(2, :), eprop4(3, :), 'g-'); 514 | ehmap3 = plot3(emap(1, :), emap(2, :), emap(3, :), 'ko', 'MarkerSize', 10, 'MarkerFaceColor', 'k'); 515 | ehwindow3 = plot3(ewindow(1, :), ewindow(2, :), ewindow(3, :), 'ro', 'MarkerSize', 5, 'MarkerFaceColor', 'r'); 516 | else 517 | set(thtraj3, 'XData', [get(thtraj3, 'XData'), true_s(1, 3)]); 518 | set(thtraj3, 'YData', [get(thtraj3, 'YData'), true_s(2, 3)]); 519 | set(thtraj3, 'ZData', [get(thtraj3, 'ZData'), true_s(3, 3)]); 520 | set(thprop31, 'XData', tprop1(1, :)); 521 | set(thprop31, 'YData', tprop1(2, :)); 522 | set(thprop31, 'ZData', tprop1(3, :)); 523 | set(thprop32, 'XData', tprop2(1, :)); 524 | set(thprop32, 'YData', tprop2(2, :)); 525 | set(thprop32, 'ZData', tprop2(3, :)); 526 | set(thprop33, 'XData', tprop3(1, :)); 527 | set(thprop33, 'YData', tprop3(2, :)); 528 | set(thprop33, 'ZData', tprop3(3, :)); 529 | set(thprop34, 'XData', tprop4(1, :)); 530 | set(thprop34, 'YData', tprop4(2, :)); 531 | set(thprop34, 'ZData', tprop4(3, :)); 532 | set(tharm31, 'XData', [twp1(1), twp3(1)]); 533 | set(tharm31, 'YData', [twp1(2), twp3(2)]); 534 | set(tharm31, 'ZData', [twp1(3), twp3(3)]); 535 | set(tharm32, 'XData', [twp2(1), twp4(1)]); 536 | set(tharm32, 'YData', [twp2(2), twp4(2)]); 537 | set(tharm32, 'ZData', [twp2(3), twp4(3)]); 538 | set(thfov31, 'XData', [tfov0(1), tfov1(1), tfov2(1)]); 539 | set(thfov31, 'YData', [tfov0(2), tfov1(2), tfov2(2)]); 540 | set(thfov31, 'ZData', [tfov0(3), tfov1(3), tfov2(3)]); 541 | set(thfov32, 'XData', [tfov0(1), tfov2(1), tfov3(1)]); 542 | set(thfov32, 'YData', [tfov0(2), tfov2(2), tfov3(2)]); 543 | set(thfov32, 'ZData', [tfov0(3), tfov2(3), tfov3(3)]); 544 | set(thfov33, 'XData', [tfov0(1), tfov3(1), tfov4(1)]); 545 | set(thfov33, 'YData', [tfov0(2), tfov3(2), tfov4(2)]); 546 | set(thfov33, 'ZData', [tfov0(3), tfov3(3), tfov4(3)]); 547 | set(thfov34, 'XData', [tfov0(1), tfov4(1), tfov1(1)]); 548 | set(thfov34, 'YData', [tfov0(2), tfov4(2), tfov1(2)]); 549 | set(thfov34, 'ZData', [tfov0(3), tfov4(3), tfov1(3)]); 550 | set(ehtraj3, 'XData', [get(ehtraj3, 'XData'), s_des(1, 3)]); 551 | set(ehtraj3, 'YData', [get(ehtraj3, 'YData'), s_des(2, 3)]); 552 | set(ehtraj3, 'ZData', [get(ehtraj3, 'ZData'), s_des(3, 3)]); 553 | set(ehprop31, 'XData', eprop1(1, :)); 554 | set(ehprop31, 'YData', eprop1(2, :)); 555 | set(ehprop31, 'ZData', eprop1(3, :)); 556 | set(ehprop32, 'XData', eprop2(1, :)); 557 | set(ehprop32, 'YData', eprop2(2, :)); 558 | set(ehprop32, 'ZData', eprop2(3, :)); 559 | set(ehprop33, 'XData', eprop3(1, :)); 560 | set(ehprop33, 'YData', eprop3(2, :)); 561 | set(ehprop33, 'ZData', eprop3(3, :)); 562 | set(ehprop34, 'XData', eprop4(1, :)); 563 | set(ehprop34, 'YData', eprop4(2, :)); 564 | set(ehprop34, 'ZData', eprop4(3, :)); 565 | set(eharm31, 'XData', [ewp1(1), ewp3(1)]); 566 | set(eharm31, 'YData', [ewp1(2), ewp3(2)]); 567 | set(eharm31, 'ZData', [ewp1(3), ewp3(3)]); 568 | set(eharm32, 'XData', [ewp2(1), ewp4(1)]); 569 | set(eharm32, 'YData', [ewp2(2), ewp4(2)]); 570 | set(eharm32, 'ZData', [ewp2(3), ewp4(3)]); 571 | set(ehmap3, 'XData', emap(1, :)); 572 | set(ehmap3, 'YData', emap(2, :)); 573 | set(ehmap3, 'ZData', emap(3, :)); 574 | set(ehwindow3, 'XData', ewindow(1, :)); 575 | set(ehwindow3, 'YData', ewindow(2, :)); 576 | set(ehwindow3, 'ZData', ewindow(3, :)); 577 | end 578 | 579 | hold on; 580 | 581 | grid on; 582 | 583 | 584 | twp1 = true_s(1:3, 4) + tpoint1; 585 | twp2 = true_s(1:3, 4) + tpoint2; 586 | twp3 = true_s(1:3, 4) + tpoint3; 587 | twp4 = true_s(1:3, 4) + tpoint4; 588 | tprop1 = tproppts + twp1 * ones(1, nprop); 589 | tprop2 = tproppts + twp2 * ones(1, nprop); 590 | tprop3 = tproppts + twp3 * ones(1, nprop); 591 | tprop4 = tproppts + twp4 * ones(1, nprop); 592 | tfov0 = true_s(1:3, 4); 593 | tfov1 = tR * [ff; ff * tan(ifov*pi/180/2); ff * tan(ifov*pi/180/2)] + true_s(1:3, 4); 594 | tfov2 = tR * [ff; ff * tan(ifov*pi/180/2); -ff * tan(ifov*pi/180/2)] + true_s(1:3, 4); 595 | tfov3 = tR * [ff; -ff * tan(ifov*pi/180/2); -ff * tan(ifov*pi/180/2)] + true_s(1:3, 4); 596 | tfov4 = tR * [ff; -ff * tan(ifov*pi/180/2); ff * tan(ifov*pi/180/2)] + true_s(1:3, 4); 597 | eR = QuatToRot(s_des(7:10, 4))'; 598 | epoint1 = eR * [ll; 0; 0]; 599 | epoint2 = eR * [0; ll; 0]; 600 | epoint3 = eR * [-ll; 0; 0]; 601 | epoint4 = eR * [0; -ll; 0]; 602 | eproppts = rr * eR * [cos(propangs); sin(propangs); zeros(1, nprop)]; 603 | 604 | 605 | ewp1 = s_des(1:3, 4) + epoint1; 606 | ewp2 = s_des(1:3, 4) + epoint2; 607 | ewp3 = s_des(1:3, 4) + epoint3; 608 | ewp4 = s_des(1:3, 4) + epoint4; 609 | 610 | eprop1 = eproppts + ewp1 * ones(1, nprop); 611 | eprop2 = eproppts + ewp2 * ones(1, nprop); 612 | eprop3 = eproppts + ewp3 * ones(1, nprop); 613 | eprop4 = eproppts + ewp4 * ones(1, nprop); 614 | emap = [0; 0; 0]; 615 | ewindow = [0; 0; 0]; 616 | 617 | if ~vis_init 618 | thtraj4 = plot3(true_s(1, 4), true_s(2, 4), true_s(3, 4), 'b-', 'LineWidth', 0.5); 619 | tharm41 = line([twp1(1), twp3(1)], [twp1(2), twp3(2)], [twp1(3), twp3(3)], 'Color', 'b'); 620 | tharm42 = line([twp2(1), twp4(1)], [twp2(2), twp4(2)], [twp2(3), twp4(3)], 'Color', 'b'); 621 | thprop41 = plot3(tprop1(1, :), tprop1(2, :), tprop1(3, :), 'r-'); 622 | thprop42 = plot3(tprop2(1, :), tprop2(2, :), tprop2(3, :), 'b-'); 623 | thprop43 = plot3(tprop3(1, :), tprop3(2, :), tprop3(3, :), 'b-'); 624 | thprop44 = plot3(tprop4(1, :), tprop4(2, :), tprop4(3, :), 'b-'); 625 | thfov41 = line([tfov0(1), tfov1(1), tfov2(1)], [tfov0(2), tfov1(2), tfov2(2)], [tfov0(3), tfov1(3), tfov2(3)], 'Color', 'k'); 626 | thfov42 = line([tfov0(1), tfov2(1), tfov3(1)], [tfov0(2), tfov2(2), tfov3(2)], [tfov0(3), tfov2(3), tfov3(3)], 'Color', 'k'); 627 | thfov43 = line([tfov0(1), tfov3(1), tfov4(1)], [tfov0(2), tfov3(2), tfov4(2)], [tfov0(3), tfov3(3), tfov4(3)], 'Color', 'k'); 628 | thfov44 = line([tfov0(1), tfov4(1), tfov1(1)], [tfov0(2), tfov4(2), tfov1(2)], [tfov0(3), tfov4(3), tfov1(3)], 'Color', 'k'); 629 | 630 | ehtraj4 = plot3(s_des(1, 4), s_des(2, 4), s_des(3, 4), 'g-', 'LineWidth', 0.5); 631 | 632 | eharm41 = line([ewp1(1), ewp3(1)], [ewp1(2), ewp3(2)], [ewp1(3), ewp3(3)], 'Color', 'g'); 633 | eharm42 = line([ewp2(1), ewp4(1)], [ewp2(2), ewp4(2)], [ewp2(3), ewp4(3)], 'Color', 'g'); 634 | ehprop41 = plot3(eprop1(1, :), eprop1(2, :), eprop1(3, :), 'm-'); 635 | ehprop42 = plot3(eprop2(1, :), eprop2(2, :), eprop2(3, :), 'g-'); 636 | ehprop43 = plot3(eprop3(1, :), eprop3(2, :), eprop3(3, :), 'g-'); 637 | ehprop44 = plot3(eprop4(1, :), eprop4(2, :), eprop4(3, :), 'g-'); 638 | ehmap4 = plot3(emap(1, :), emap(2, :), emap(3, :), 'ko', 'MarkerSize', 10, 'MarkerFaceColor', 'k'); 639 | ehwindow4 = plot3(ewindow(1, :), ewindow(2, :), ewindow(3, :), 'ro', 'MarkerSize', 5, 'MarkerFaceColor', 'r'); 640 | else 641 | set(thtraj4, 'XData', [get(thtraj4, 'XData'), true_s(1, 4)]); 642 | set(thtraj4, 'YData', [get(thtraj4, 'YData'), true_s(2, 4)]); 643 | set(thtraj4, 'ZData', [get(thtraj4, 'ZData'), true_s(3, 4)]); 644 | set(thprop41, 'XData', tprop1(1, :)); 645 | set(thprop41, 'YData', tprop1(2, :)); 646 | set(thprop41, 'ZData', tprop1(3, :)); 647 | set(thprop42, 'XData', tprop2(1, :)); 648 | set(thprop42, 'YData', tprop2(2, :)); 649 | set(thprop42, 'ZData', tprop2(3, :)); 650 | set(thprop43, 'XData', tprop3(1, :)); 651 | set(thprop43, 'YData', tprop3(2, :)); 652 | set(thprop43, 'ZData', tprop3(3, :)); 653 | set(thprop44, 'XData', tprop4(1, :)); 654 | set(thprop44, 'YData', tprop4(2, :)); 655 | set(thprop44, 'ZData', tprop4(3, :)); 656 | set(tharm41, 'XData', [twp1(1), twp3(1)]); 657 | set(tharm41, 'YData', [twp1(2), twp3(2)]); 658 | set(tharm41, 'ZData', [twp1(3), twp3(3)]); 659 | set(tharm42, 'XData', [twp2(1), twp4(1)]); 660 | set(tharm42, 'YData', [twp2(2), twp4(2)]); 661 | set(tharm42, 'ZData', [twp2(3), twp4(3)]); 662 | set(thfov41, 'XData', [tfov0(1), tfov1(1), tfov2(1)]); 663 | set(thfov41, 'YData', [tfov0(2), tfov1(2), tfov2(2)]); 664 | set(thfov41, 'ZData', [tfov0(3), tfov1(3), tfov2(3)]); 665 | set(thfov42, 'XData', [tfov0(1), tfov2(1), tfov3(1)]); 666 | set(thfov42, 'YData', [tfov0(2), tfov2(2), tfov3(2)]); 667 | set(thfov42, 'ZData', [tfov0(3), tfov2(3), tfov3(3)]); 668 | set(thfov43, 'XData', [tfov0(1), tfov3(1), tfov4(1)]); 669 | set(thfov43, 'YData', [tfov0(2), tfov3(2), tfov4(2)]); 670 | set(thfov43, 'ZData', [tfov0(3), tfov3(3), tfov4(3)]); 671 | set(thfov44, 'XData', [tfov0(1), tfov4(1), tfov1(1)]); 672 | set(thfov44, 'YData', [tfov0(2), tfov4(2), tfov1(2)]); 673 | set(thfov44, 'ZData', [tfov0(3), tfov4(3), tfov1(3)]); 674 | set(ehtraj4, 'XData', [get(ehtraj4, 'XData'), s_des(1, 4)]); 675 | set(ehtraj4, 'YData', [get(ehtraj4, 'YData'), s_des(2, 4)]); 676 | set(ehtraj4, 'ZData', [get(ehtraj4, 'ZData'), s_des(3, 4)]); 677 | set(ehprop41, 'XData', eprop1(1, :)); 678 | set(ehprop41, 'YData', eprop1(2, :)); 679 | set(ehprop41, 'ZData', eprop1(3, :)); 680 | set(ehprop42, 'XData', eprop2(1, :)); 681 | set(ehprop42, 'YData', eprop2(2, :)); 682 | set(ehprop42, 'ZData', eprop2(3, :)); 683 | set(ehprop43, 'XData', eprop3(1, :)); 684 | set(ehprop43, 'YData', eprop3(2, :)); 685 | set(ehprop43, 'ZData', eprop3(3, :)); 686 | set(ehprop44, 'XData', eprop4(1, :)); 687 | set(ehprop44, 'YData', eprop4(2, :)); 688 | set(ehprop44, 'ZData', eprop4(3, :)); 689 | set(eharm41, 'XData', [ewp1(1), ewp3(1)]); 690 | set(eharm41, 'YData', [ewp1(2), ewp3(2)]); 691 | set(eharm41, 'ZData', [ewp1(3), ewp3(3)]); 692 | set(eharm42, 'XData', [ewp2(1), ewp4(1)]); 693 | set(eharm42, 'YData', [ewp2(2), ewp4(2)]); 694 | set(eharm42, 'ZData', [ewp2(3), ewp4(3)]); 695 | set(ehmap4, 'XData', emap(1, :)); 696 | set(ehmap4, 'YData', emap(2, :)); 697 | set(ehmap4, 'ZData', emap(3, :)); 698 | set(ehwindow4, 'XData', ewindow(1, :)); 699 | set(ehwindow4, 'YData', ewindow(2, :)); 700 | set(ehwindow4, 'ZData', ewindow(3, :)); 701 | end 702 | 703 | 704 | hold on; 705 | 706 | grid on; 707 | 708 | 709 | twp1 = true_s(1:3, 5) + tpoint1; 710 | twp2 = true_s(1:3, 5) + tpoint2; 711 | twp3 = true_s(1:3, 5) + tpoint3; 712 | twp4 = true_s(1:3, 5) + tpoint4; 713 | tprop1 = tproppts + twp1 * ones(1, nprop); 714 | tprop2 = tproppts + twp2 * ones(1, nprop); 715 | tprop3 = tproppts + twp3 * ones(1, nprop); 716 | tprop4 = tproppts + twp4 * ones(1, nprop); 717 | tfov0 = true_s(1:3, 5); 718 | tfov1 = tR * [ff; ff * tan(ifov*pi/180/2); ff * tan(ifov*pi/180/2)] + true_s(1:3, 5); 719 | tfov2 = tR * [ff; ff * tan(ifov*pi/180/2); -ff * tan(ifov*pi/180/2)] + true_s(1:3, 5); 720 | tfov3 = tR * [ff; -ff * tan(ifov*pi/180/2); -ff * tan(ifov*pi/180/2)] + true_s(1:3, 5); 721 | tfov4 = tR * [ff; -ff * tan(ifov*pi/180/2); ff * tan(ifov*pi/180/2)] + true_s(1:3, 5); 722 | eR = QuatToRot(s_des(7:10, 5))'; 723 | epoint1 = eR * [ll; 0; 0]; 724 | epoint2 = eR * [0; ll; 0]; 725 | epoint3 = eR * [-ll; 0; 0]; 726 | epoint4 = eR * [0; -ll; 0]; 727 | eproppts = rr * eR * [cos(propangs); sin(propangs); zeros(1, nprop)]; 728 | 729 | 730 | ewp1 = s_des(1:3, 5) + epoint1; 731 | ewp2 = s_des(1:3, 5) + epoint2; 732 | ewp3 = s_des(1:3, 5) + epoint3; 733 | ewp4 = s_des(1:3, 5) + epoint4; 734 | 735 | eprop1 = eproppts + ewp1 * ones(1, nprop); 736 | eprop2 = eproppts + ewp2 * ones(1, nprop); 737 | eprop3 = eproppts + ewp3 * ones(1, nprop); 738 | eprop4 = eproppts + ewp4 * ones(1, nprop); 739 | emap = [0; 0; 0]; 740 | ewindow = [0; 0; 0]; 741 | 742 | if ~vis_init 743 | thtraj5 = plot3(true_s(1, 5), true_s(2, 5), true_s(3, 5), 'b-', 'LineWidth', 0.5); 744 | tharm51 = line([twp1(1), twp3(1)], [twp1(2), twp3(2)], [twp1(3), twp3(3)], 'Color', 'b'); 745 | tharm52 = line([twp2(1), twp4(1)], [twp2(2), twp4(2)], [twp2(3), twp4(3)], 'Color', 'b'); 746 | thprop51 = plot3(tprop1(1, :), tprop1(2, :), tprop1(3, :), 'r-'); 747 | thprop52 = plot3(tprop2(1, :), tprop2(2, :), tprop2(3, :), 'b-'); 748 | thprop53 = plot3(tprop3(1, :), tprop3(2, :), tprop3(3, :), 'b-'); 749 | thprop54 = plot3(tprop4(1, :), tprop4(2, :), tprop4(3, :), 'b-'); 750 | thfov51 = line([tfov0(1), tfov1(1), tfov2(1)], [tfov0(2), tfov1(2), tfov2(2)], [tfov0(3), tfov1(3), tfov2(3)], 'Color', 'k'); 751 | thfov52 = line([tfov0(1), tfov2(1), tfov3(1)], [tfov0(2), tfov2(2), tfov3(2)], [tfov0(3), tfov2(3), tfov3(3)], 'Color', 'k'); 752 | thfov53 = line([tfov0(1), tfov3(1), tfov4(1)], [tfov0(2), tfov3(2), tfov4(2)], [tfov0(3), tfov3(3), tfov4(3)], 'Color', 'k'); 753 | thfov54 = line([tfov0(1), tfov4(1), tfov1(1)], [tfov0(2), tfov4(2), tfov1(2)], [tfov0(3), tfov4(3), tfov1(3)], 'Color', 'k'); 754 | 755 | ehtraj5 = plot3(s_des(1, 5), s_des(2, 5), s_des(3, 5), 'g-', 'LineWidth', 0.5); 756 | 757 | eharm51 = line([ewp1(1), ewp3(1)], [ewp1(2), ewp3(2)], [ewp1(3), ewp3(3)], 'Color', 'g'); 758 | eharm52 = line([ewp2(1), ewp4(1)], [ewp2(2), ewp4(2)], [ewp2(3), ewp4(3)], 'Color', 'g'); 759 | ehprop51 = plot3(eprop1(1, :), eprop1(2, :), eprop1(3, :), 'm-'); 760 | ehprop52 = plot3(eprop2(1, :), eprop2(2, :), eprop2(3, :), 'g-'); 761 | ehprop53 = plot3(eprop3(1, :), eprop3(2, :), eprop3(3, :), 'g-'); 762 | ehprop54 = plot3(eprop4(1, :), eprop4(2, :), eprop4(3, :), 'g-'); 763 | ehmap5 = plot3(emap(1, :), emap(2, :), emap(3, :), 'ko', 'MarkerSize', 10, 'MarkerFaceColor', 'k'); 764 | ehwindow5 = plot3(ewindow(1, :), ewindow(2, :), ewindow(3, :), 'ro', 'MarkerSize', 5, 'MarkerFaceColor', 'r'); 765 | else 766 | set(thtraj5, 'XData', [get(thtraj5, 'XData'), true_s(1, 5)]); 767 | set(thtraj5, 'YData', [get(thtraj5, 'YData'), true_s(2, 5)]); 768 | set(thtraj5, 'ZData', [get(thtraj5, 'ZData'), true_s(3, 5)]); 769 | set(thprop51, 'XData', tprop1(1, :)); 770 | set(thprop51, 'YData', tprop1(2, :)); 771 | set(thprop51, 'ZData', tprop1(3, :)); 772 | set(thprop52, 'XData', tprop2(1, :)); 773 | set(thprop52, 'YData', tprop2(2, :)); 774 | set(thprop52, 'ZData', tprop2(3, :)); 775 | set(thprop53, 'XData', tprop3(1, :)); 776 | set(thprop53, 'YData', tprop3(2, :)); 777 | set(thprop53, 'ZData', tprop3(3, :)); 778 | set(thprop54, 'XData', tprop4(1, :)); 779 | set(thprop54, 'YData', tprop4(2, :)); 780 | set(thprop54, 'ZData', tprop4(3, :)); 781 | set(tharm51, 'XData', [twp1(1), twp3(1)]); 782 | set(tharm51, 'YData', [twp1(2), twp3(2)]); 783 | set(tharm51, 'ZData', [twp1(3), twp3(3)]); 784 | set(tharm52, 'XData', [twp2(1), twp4(1)]); 785 | set(tharm52, 'YData', [twp2(2), twp4(2)]); 786 | set(tharm52, 'ZData', [twp2(3), twp4(3)]); 787 | set(thfov51, 'XData', [tfov0(1), tfov1(1), tfov2(1)]); 788 | set(thfov51, 'YData', [tfov0(2), tfov1(2), tfov2(2)]); 789 | set(thfov51, 'ZData', [tfov0(3), tfov1(3), tfov2(3)]); 790 | set(thfov52, 'XData', [tfov0(1), tfov2(1), tfov3(1)]); 791 | set(thfov52, 'YData', [tfov0(2), tfov2(2), tfov3(2)]); 792 | set(thfov52, 'ZData', [tfov0(3), tfov2(3), tfov3(3)]); 793 | set(thfov53, 'XData', [tfov0(1), tfov3(1), tfov4(1)]); 794 | set(thfov53, 'YData', [tfov0(2), tfov3(2), tfov4(2)]); 795 | set(thfov53, 'ZData', [tfov0(3), tfov3(3), tfov4(3)]); 796 | set(thfov54, 'XData', [tfov0(1), tfov4(1), tfov1(1)]); 797 | set(thfov54, 'YData', [tfov0(2), tfov4(2), tfov1(2)]); 798 | set(thfov54, 'ZData', [tfov0(3), tfov4(3), tfov1(3)]); 799 | set(ehtraj5, 'XData', [get(ehtraj5, 'XData'), s_des(1, 5)]); 800 | set(ehtraj5, 'YData', [get(ehtraj5, 'YData'), s_des(2, 5)]); 801 | set(ehtraj5, 'ZData', [get(ehtraj5, 'ZData'), s_des(3, 5)]); 802 | set(ehprop51, 'XData', eprop1(1, :)); 803 | set(ehprop51, 'YData', eprop1(2, :)); 804 | set(ehprop51, 'ZData', eprop1(3, :)); 805 | set(ehprop52, 'XData', eprop2(1, :)); 806 | set(ehprop52, 'YData', eprop2(2, :)); 807 | set(ehprop52, 'ZData', eprop2(3, :)); 808 | set(ehprop53, 'XData', eprop3(1, :)); 809 | set(ehprop53, 'YData', eprop3(2, :)); 810 | set(ehprop53, 'ZData', eprop3(3, :)); 811 | set(ehprop54, 'XData', eprop4(1, :)); 812 | set(ehprop54, 'YData', eprop4(2, :)); 813 | set(ehprop54, 'ZData', eprop4(3, :)); 814 | set(eharm51, 'XData', [ewp1(1), ewp3(1)]); 815 | set(eharm51, 'YData', [ewp1(2), ewp3(2)]); 816 | set(eharm51, 'ZData', [ewp1(3), ewp3(3)]); 817 | set(eharm52, 'XData', [ewp2(1), ewp4(1)]); 818 | set(eharm52, 'YData', [ewp2(2), ewp4(2)]); 819 | set(eharm52, 'ZData', [ewp2(3), ewp4(3)]); 820 | set(ehmap5, 'XData', emap(1, :)); 821 | set(ehmap5, 'YData', emap(2, :)); 822 | set(ehmap5, 'ZData', emap(3, :)); 823 | set(ehwindow5, 'XData', ewindow(1, :)); 824 | set(ehwindow5, 'YData', ewindow(2, :)); 825 | set(ehwindow5, 'ZData', ewindow(3, :)); 826 | end 827 | 828 | 829 | hold on; 830 | 831 | grid on; 832 | 833 | 834 | twp1 = true_s(1:3, 6) + tpoint1; 835 | twp2 = true_s(1:3, 6) + tpoint2; 836 | twp3 = true_s(1:3, 6) + tpoint3; 837 | twp4 = true_s(1:3, 6) + tpoint4; 838 | tprop1 = tproppts + twp1 * ones(1, nprop); 839 | tprop2 = tproppts + twp2 * ones(1, nprop); 840 | tprop3 = tproppts + twp3 * ones(1, nprop); 841 | tprop4 = tproppts + twp4 * ones(1, nprop); 842 | tfov0 = true_s(1:3, 6); 843 | tfov1 = tR * [ff; ff * tan(ifov*pi/180/2); ff * tan(ifov*pi/180/2)] + true_s(1:3, 6); 844 | tfov2 = tR * [ff; ff * tan(ifov*pi/180/2); -ff * tan(ifov*pi/180/2)] + true_s(1:3, 6); 845 | tfov3 = tR * [ff; -ff * tan(ifov*pi/180/2); -ff * tan(ifov*pi/180/2)] + true_s(1:3, 6); 846 | tfov4 = tR * [ff; -ff * tan(ifov*pi/180/2); ff * tan(ifov*pi/180/2)] + true_s(1:3, 6); 847 | eR = QuatToRot(s_des(7:10, 6))'; 848 | epoint1 = eR * [ll; 0; 0]; 849 | epoint2 = eR * [0; ll; 0]; 850 | epoint3 = eR * [-ll; 0; 0]; 851 | epoint4 = eR * [0; -ll; 0]; 852 | eproppts = rr * eR * [cos(propangs); sin(propangs); zeros(1, nprop)]; 853 | 854 | 855 | ewp1 = s_des(1:3, 6) + epoint1; 856 | ewp2 = s_des(1:3, 6) + epoint2; 857 | ewp3 = s_des(1:3, 6) + epoint3; 858 | ewp4 = s_des(1:3, 6) + epoint4; 859 | 860 | eprop1 = eproppts + ewp1 * ones(1, nprop); 861 | eprop2 = eproppts + ewp2 * ones(1, nprop); 862 | eprop3 = eproppts + ewp3 * ones(1, nprop); 863 | eprop4 = eproppts + ewp4 * ones(1, nprop); 864 | emap = [0; 0; 0]; 865 | ewindow = [0; 0; 0]; 866 | 867 | if ~vis_init 868 | thtraj6 = plot3(true_s(1, 6), true_s(2, 6), true_s(3, 6), 'b-', 'LineWidth', 0.5); 869 | tharm1 = line([twp1(1), twp3(1)], [twp1(2), twp3(2)], [twp1(3), twp3(3)], 'Color', 'b'); 870 | tharm2 = line([twp2(1), twp4(1)], [twp2(2), twp4(2)], [twp2(3), twp4(3)], 'Color', 'b'); 871 | thprop1 = plot3(tprop1(1, :), tprop1(2, :), tprop1(3, :), 'r-'); 872 | thprop2 = plot3(tprop2(1, :), tprop2(2, :), tprop2(3, :), 'b-'); 873 | thprop3 = plot3(tprop3(1, :), tprop3(2, :), tprop3(3, :), 'b-'); 874 | thprop4 = plot3(tprop4(1, :), tprop4(2, :), tprop4(3, :), 'b-'); 875 | thfov1 = line([tfov0(1), tfov1(1), tfov2(1)], [tfov0(2), tfov1(2), tfov2(2)], [tfov0(3), tfov1(3), tfov2(3)], 'Color', 'k'); 876 | thfov2 = line([tfov0(1), tfov2(1), tfov3(1)], [tfov0(2), tfov2(2), tfov3(2)], [tfov0(3), tfov2(3), tfov3(3)], 'Color', 'k'); 877 | thfov3 = line([tfov0(1), tfov3(1), tfov4(1)], [tfov0(2), tfov3(2), tfov4(2)], [tfov0(3), tfov3(3), tfov4(3)], 'Color', 'k'); 878 | thfov4 = line([tfov0(1), tfov4(1), tfov1(1)], [tfov0(2), tfov4(2), tfov1(2)], [tfov0(3), tfov4(3), tfov1(3)], 'Color', 'k'); 879 | 880 | ehtraj6 = plot3(s_des(1, 6), s_des(2, 6), s_des(3, 6), 'g-', 'LineWidth', 0.5); 881 | 882 | eharm1 = line([ewp1(1), ewp3(1)], [ewp1(2), ewp3(2)], [ewp1(3), ewp3(3)], 'Color', 'g'); 883 | eharm2 = line([ewp2(1), ewp4(1)], [ewp2(2), ewp4(2)], [ewp2(3), ewp4(3)], 'Color', 'g'); 884 | ehprop1 = plot3(eprop1(1, :), eprop1(2, :), eprop1(3, :), 'm-'); 885 | ehprop2 = plot3(eprop2(1, :), eprop2(2, :), eprop2(3, :), 'g-'); 886 | ehprop3 = plot3(eprop3(1, :), eprop3(2, :), eprop3(3, :), 'g-'); 887 | ehprop4 = plot3(eprop4(1, :), eprop4(2, :), eprop4(3, :), 'g-'); 888 | ehmap = plot3(emap(1, :), emap(2, :), emap(3, :), 'ko', 'MarkerSize', 10, 'MarkerFaceColor', 'k'); 889 | ehwindow = plot3(ewindow(1, :), ewindow(2, :), ewindow(3, :), 'ro', 'MarkerSize', 5, 'MarkerFaceColor', 'r'); 890 | else 891 | set(thtraj6, 'XData', [get(thtraj6, 'XData'), true_s(1, 6)]); 892 | set(thtraj6, 'YData', [get(thtraj6, 'YData'), true_s(2, 6)]); 893 | set(thtraj6, 'ZData', [get(thtraj6, 'ZData'), true_s(3, 6)]); 894 | set(thprop1, 'XData', tprop1(1, :)); 895 | set(thprop1, 'YData', tprop1(2, :)); 896 | set(thprop1, 'ZData', tprop1(3, :)); 897 | set(thprop2, 'XData', tprop2(1, :)); 898 | set(thprop2, 'YData', tprop2(2, :)); 899 | set(thprop2, 'ZData', tprop2(3, :)); 900 | set(thprop3, 'XData', tprop3(1, :)); 901 | set(thprop3, 'YData', tprop3(2, :)); 902 | set(thprop3, 'ZData', tprop3(3, :)); 903 | set(thprop4, 'XData', tprop4(1, :)); 904 | set(thprop4, 'YData', tprop4(2, :)); 905 | set(thprop4, 'ZData', tprop4(3, :)); 906 | set(tharm1, 'XData', [twp1(1), twp3(1)]); 907 | set(tharm1, 'YData', [twp1(2), twp3(2)]); 908 | set(tharm1, 'ZData', [twp1(3), twp3(3)]); 909 | set(tharm2, 'XData', [twp2(1), twp4(1)]); 910 | set(tharm2, 'YData', [twp2(2), twp4(2)]); 911 | set(tharm2, 'ZData', [twp2(3), twp4(3)]); 912 | set(thfov1, 'XData', [tfov0(1), tfov1(1), tfov2(1)]); 913 | set(thfov1, 'YData', [tfov0(2), tfov1(2), tfov2(2)]); 914 | set(thfov1, 'ZData', [tfov0(3), tfov1(3), tfov2(3)]); 915 | set(thfov2, 'XData', [tfov0(1), tfov2(1), tfov3(1)]); 916 | set(thfov2, 'YData', [tfov0(2), tfov2(2), tfov3(2)]); 917 | set(thfov2, 'ZData', [tfov0(3), tfov2(3), tfov3(3)]); 918 | set(thfov3, 'XData', [tfov0(1), tfov3(1), tfov4(1)]); 919 | set(thfov3, 'YData', [tfov0(2), tfov3(2), tfov4(2)]); 920 | set(thfov3, 'ZData', [tfov0(3), tfov3(3), tfov4(3)]); 921 | set(thfov4, 'XData', [tfov0(1), tfov4(1), tfov1(1)]); 922 | set(thfov4, 'YData', [tfov0(2), tfov4(2), tfov1(2)]); 923 | set(thfov4, 'ZData', [tfov0(3), tfov4(3), tfov1(3)]); 924 | set(ehtraj6, 'XData', [get(ehtraj6, 'XData'), s_des(1, 6)]); 925 | set(ehtraj6, 'YData', [get(ehtraj6, 'YData'), s_des(2, 6)]); 926 | set(ehtraj6, 'ZData', [get(ehtraj6, 'ZData'), s_des(3, 6)]); 927 | set(ehprop1, 'XData', eprop1(1, :)); 928 | set(ehprop1, 'YData', eprop1(2, :)); 929 | set(ehprop1, 'ZData', eprop1(3, :)); 930 | set(ehprop2, 'XData', eprop2(1, :)); 931 | set(ehprop2, 'YData', eprop2(2, :)); 932 | set(ehprop2, 'ZData', eprop2(3, :)); 933 | set(ehprop3, 'XData', eprop3(1, :)); 934 | set(ehprop3, 'YData', eprop3(2, :)); 935 | set(ehprop3, 'ZData', eprop3(3, :)); 936 | set(ehprop4, 'XData', eprop4(1, :)); 937 | set(ehprop4, 'YData', eprop4(2, :)); 938 | set(ehprop4, 'ZData', eprop4(3, :)); 939 | set(eharm1, 'XData', [ewp1(1), ewp3(1)]); 940 | set(eharm1, 'YData', [ewp1(2), ewp3(2)]); 941 | set(eharm1, 'ZData', [ewp1(3), ewp3(3)]); 942 | set(eharm2, 'XData', [ewp2(1), ewp4(1)]); 943 | set(eharm2, 'YData', [ewp2(2), ewp4(2)]); 944 | set(eharm2, 'ZData', [ewp2(3), ewp4(3)]); 945 | set(ehmap, 'XData', emap(1, :)); 946 | set(ehmap, 'YData', emap(2, :)); 947 | set(ehmap, 'ZData', emap(3, :)); 948 | set(ehwindow, 'XData', ewindow(1, :)); 949 | set(ehwindow, 'YData', ewindow(2, :)); 950 | set(ehwindow, 'ZData', ewindow(3, :)); 951 | end 952 | hold off 953 | 954 | %% Plot roll oriengation 955 | subplot(h2); 956 | true_ypr = R_to_ypr(quaternion_to_R(true_s(7:10, 1))') * 180 / pi; 957 | if ~vis_init 958 | hold on; 959 | throll = plot(time, true_ypr(3), 'r-', 'LineWidth', 1); 960 | hold off; 961 | xlabel('Time (s)'); 962 | ylabel('roll1 degree'); 963 | axis([0, time_tol, -45, 45]); 964 | else 965 | hold on; 966 | set(throll, 'XData', [get(throll, 'XData'), time]); 967 | set(throll, 'YData', [get(throll, 'YData'), true_ypr(3)]); 968 | hold off; 969 | end 970 | 971 | subplot(h10); 972 | true_ypr2 = R_to_ypr(quaternion_to_R(true_s(7:10, 2))') * 180 / pi; 973 | if ~vis_init 974 | hold on; 975 | throll2 = plot(time, true_ypr2(3), 'r-', 'LineWidth', 1); 976 | hold off; 977 | xlabel('Time (s)'); 978 | ylabel('roll2 degree'); 979 | axis([0, time_tol, -45, 45]); 980 | else 981 | hold on; 982 | set(throll2, 'XData', [get(throll2, 'XData'), time]); 983 | set(throll2, 'YData', [get(throll2, 'YData'), true_ypr(3)]); 984 | hold off; 985 | end 986 | 987 | subplot(h18); 988 | true_ypr3 = R_to_ypr(quaternion_to_R(true_s(7:10, 3))') * 180 / pi; 989 | if ~vis_init 990 | hold on; 991 | throll3 = plot(time, true_ypr3(3), 'r-', 'LineWidth', 1); 992 | hold off; 993 | xlabel('Time (s)'); 994 | ylabel('roll3 degree'); 995 | axis([0, time_tol, -45, 45]); 996 | else 997 | hold on; 998 | set(throll3, 'XData', [get(throll3, 'XData'), time]); 999 | set(throll3, 'YData', [get(throll3, 'YData'), true_ypr(3)]); 1000 | hold off; 1001 | end 1002 | 1003 | 1004 | subplot(h26); 1005 | true_ypr4 = R_to_ypr(quaternion_to_R(true_s(7:10, 4))') * 180 / pi; 1006 | if ~vis_init 1007 | hold on; 1008 | throll4 = plot(time, true_ypr4(3), 'r-', 'LineWidth', 1); 1009 | hold off; 1010 | xlabel('Time (s)'); 1011 | ylabel('roll4 degree'); 1012 | axis([0, time_tol, -45, 45]); 1013 | else 1014 | hold on; 1015 | set(throll4, 'XData', [get(throll4, 'XData'), time]); 1016 | set(throll4, 'YData', [get(throll4, 'YData'), true_ypr(3)]); 1017 | hold off; 1018 | end 1019 | 1020 | subplot(h34); 1021 | true_ypr5 = R_to_ypr(quaternion_to_R(true_s(7:10, 5))') * 180 / pi; 1022 | if ~vis_init 1023 | hold on; 1024 | throll5 = plot(time, true_ypr5(3), 'r-', 'LineWidth', 1); 1025 | hold off; 1026 | xlabel('Time (s)'); 1027 | ylabel('roll5 degree'); 1028 | axis([0, time_tol, -45, 45]); 1029 | else 1030 | hold on; 1031 | set(throll5, 'XData', [get(throll5, 'XData'), time]); 1032 | set(throll5, 'YData', [get(throll5, 'YData'), true_ypr(3)]); 1033 | hold off; 1034 | end 1035 | 1036 | subplot(h42); 1037 | true_ypr6 = R_to_ypr(quaternion_to_R(true_s(7:10, 6))') * 180 / pi; 1038 | if ~vis_init 1039 | hold on; 1040 | throll6 = plot(time, true_ypr6(3), 'r-', 'LineWidth', 1); 1041 | hold off; 1042 | xlabel('Time (s)'); 1043 | ylabel('roll6 degree'); 1044 | axis([0, time_tol, -45, 45]); 1045 | else 1046 | hold on; 1047 | set(throll6, 'XData', [get(throll6, 'XData'), time]); 1048 | set(throll6, 'YData', [get(throll6, 'YData'), true_ypr(3)]); 1049 | hold off; 1050 | end 1051 | 1052 | %% Plot pitch orientation 1053 | % quadrotor 1 1054 | subplot(h3); 1055 | if ~vis_init 1056 | hold on; 1057 | thpitch = plot(time, true_ypr(2), 'r-', 'LineWidth', 1); 1058 | hold off; 1059 | xlabel('Time (s)'); 1060 | ylabel('pitch1 degree'); 1061 | axis([0, time_tol, -45, 45]); 1062 | else 1063 | hold on; 1064 | set(thpitch, 'XData', [get(thpitch, 'XData'), time]); 1065 | set(thpitch, 'YData', [get(thpitch, 'YData'), true_ypr(2)]); 1066 | hold off; 1067 | end 1068 | % quadrotor 2 1069 | subplot(h11); 1070 | if ~vis_init 1071 | hold on; 1072 | thpitch2 = plot(time, true_ypr2(2), 'r-', 'LineWidth', 1); 1073 | hold off; 1074 | xlabel('Time (s)'); 1075 | ylabel('pitch2 degree'); 1076 | axis([0, time_tol, -45, 45]); 1077 | else 1078 | hold on; 1079 | set(thpitch2, 'XData', [get(thpitch2, 'XData'), time]); 1080 | set(thpitch2, 'YData', [get(thpitch2, 'YData'), true_ypr(2)]); 1081 | hold off; 1082 | end 1083 | 1084 | % quadrotor 3 1085 | subplot(h19); 1086 | if ~vis_init 1087 | hold on; 1088 | thpitch3 = plot(time, true_ypr3(2), 'r-', 'LineWidth', 1); 1089 | hold off; 1090 | xlabel('Time (s)'); 1091 | ylabel('pitch3 degree'); 1092 | axis([0, time_tol, -45, 45]); 1093 | else 1094 | hold on; 1095 | set(thpitch3, 'XData', [get(thpitch3, 'XData'), time]); 1096 | set(thpitch3, 'YData', [get(thpitch3, 'YData'), true_ypr(2)]); 1097 | hold off; 1098 | end 1099 | % quadrotor 4 1100 | subplot(h27); 1101 | if ~vis_init 1102 | hold on; 1103 | thpitch4 = plot(time, true_ypr4(2), 'r-', 'LineWidth', 1); 1104 | hold off; 1105 | xlabel('Time (s)'); 1106 | ylabel('pitch4 degree'); 1107 | axis([0, time_tol, -45, 45]); 1108 | else 1109 | hold on; 1110 | set(thpitch4, 'XData', [get(thpitch4, 'XData'), time]); 1111 | set(thpitch4, 'YData', [get(thpitch4, 'YData'), true_ypr(2)]); 1112 | hold off; 1113 | end 1114 | 1115 | % quadrotor 5 1116 | subplot(h35); 1117 | if ~vis_init 1118 | hold on; 1119 | thpitch5 = plot(time, true_ypr5(2), 'r-', 'LineWidth', 1); 1120 | hold off; 1121 | xlabel('Time (s)'); 1122 | ylabel('pitch5 degree'); 1123 | axis([0, time_tol, -45, 45]); 1124 | else 1125 | hold on; 1126 | set(thpitch5, 'XData', [get(thpitch5, 'XData'), time]); 1127 | set(thpitch5, 'YData', [get(thpitch5, 'YData'), true_ypr(2)]); 1128 | hold off; 1129 | end 1130 | 1131 | % quadrotor 6 1132 | subplot(h43); 1133 | if ~vis_init 1134 | hold on; 1135 | thpitch6 = plot(time, true_ypr6(2), 'r-', 'LineWidth', 1); 1136 | hold off; 1137 | xlabel('Time (s)'); 1138 | ylabel('pitch6 degree'); 1139 | axis([0, time_tol, -45, 45]); 1140 | else 1141 | hold on; 1142 | set(thpitch6, 'XData', [get(thpitch6, 'XData'), time]); 1143 | set(thpitch6, 'YData', [get(thpitch6, 'YData'), true_ypr(2)]); 1144 | hold off; 1145 | end 1146 | 1147 | %% Plot body frame velocity 1148 | % quadrotor 1 1149 | subplot(h4); 1150 | true_v1 = true_s(4:6, 1); 1151 | des_v1 = s_des(4:6, 1); 1152 | if ~vis_init 1153 | hold on; 1154 | thvx1 = plot(time, true_v1(1), 'r-', 'LineWidth', 1); 1155 | ehvx1 = plot(time, des_v1(1), 'b-', 'LineWidth', 1); 1156 | hold off; 1157 | xlabel('Time (s)'); 1158 | ylabel('X1 World Velocity (m/s)'); 1159 | axis([0, time_tol, -3, 3]); 1160 | else 1161 | hold on; 1162 | set(thvx1, 'XData', [get(thvx1, 'XData'), time]); 1163 | set(thvx1, 'YData', [get(thvx1, 'YData'), true_v1(1)]); 1164 | set(ehvx1, 'XData', [get(ehvx1, 'XData'), time]); 1165 | set(ehvx1, 'YData', [get(ehvx1, 'YData'), des_v1(1)]); 1166 | hold off; 1167 | end 1168 | 1169 | % quadrotor 2 1170 | subplot(h12); 1171 | true_v2 = true_s(4:6, 2); 1172 | des_v2 = s_des(4:6, 2); 1173 | if ~vis_init 1174 | hold on; 1175 | thvx2 = plot(time, true_v2(1), 'r-', 'LineWidth', 1); 1176 | ehvx2 = plot(time, des_v2(1), 'b-', 'LineWidth', 1); 1177 | hold off; 1178 | xlabel('Time (s)'); 1179 | ylabel('X2 World Velocity (m/s)'); 1180 | axis([0, time_tol, -3, 3]); 1181 | else 1182 | hold on; 1183 | set(thvx2, 'XData', [get(thvx2, 'XData'), time]); 1184 | set(thvx2, 'YData', [get(thvx2, 'YData'), true_v2(1)]); 1185 | set(ehvx2, 'XData', [get(ehvx2, 'XData'), time]); 1186 | set(ehvx2, 'YData', [get(ehvx2, 'YData'), des_v2(1)]); 1187 | hold off; 1188 | end 1189 | 1190 | % quadrotor 3 1191 | subplot(h20); 1192 | true_v3 = true_s(4:6, 3); 1193 | des_v3 = s_des(4:6, 3); 1194 | if ~vis_init 1195 | hold on; 1196 | thvx3 = plot(time, true_v3(1), 'r-', 'LineWidth', 1); 1197 | ehvx3 = plot(time, des_v3(1), 'b-', 'LineWidth', 1); 1198 | hold off; 1199 | xlabel('Time (s)'); 1200 | ylabel('X3 World Velocity (m/s)'); 1201 | axis([0, time_tol, -3, 3]); 1202 | else 1203 | hold on; 1204 | set(thvx3, 'XData', [get(thvx3, 'XData'), time]); 1205 | set(thvx3, 'YData', [get(thvx3, 'YData'), true_v3(1)]); 1206 | set(ehvx3, 'XData', [get(ehvx3, 'XData'), time]); 1207 | set(ehvx3, 'YData', [get(ehvx3, 'YData'), des_v3(1)]); 1208 | hold off; 1209 | end 1210 | 1211 | % quadrotor 4 1212 | subplot(h28); 1213 | true_v4 = true_s(4:6, 4); 1214 | des_v4 = s_des(4:6, 4); 1215 | if ~vis_init 1216 | hold on; 1217 | thvx4 = plot(time, true_v4(1), 'r-', 'LineWidth', 1); 1218 | ehvx4 = plot(time, des_v4(1), 'b-', 'LineWidth', 1); 1219 | hold off; 1220 | xlabel('Time (s)'); 1221 | ylabel('X4 World Velocity (m/s)'); 1222 | axis([0, time_tol, -3, 3]); 1223 | else 1224 | hold on; 1225 | set(thvx4, 'XData', [get(thvx4, 'XData'), time]); 1226 | set(thvx4, 'YData', [get(thvx4, 'YData'), true_v4(1)]); 1227 | set(ehvx4, 'XData', [get(ehvx4, 'XData'), time]); 1228 | set(ehvx4, 'YData', [get(ehvx4, 'YData'), des_v4(1)]); 1229 | hold off; 1230 | end 1231 | % quadrotor 5 1232 | subplot(h36); 1233 | true_v5 = true_s(4:6, 5); 1234 | des_v5 = s_des(4:6, 5); 1235 | if ~vis_init 1236 | hold on; 1237 | thvx5 = plot(time, true_v5(1), 'r-', 'LineWidth', 1); 1238 | ehvx5 = plot(time, des_v5(1), 'b-', 'LineWidth', 1); 1239 | hold off; 1240 | xlabel('Time (s)'); 1241 | ylabel('X5 World Velocity (m/s)'); 1242 | axis([0, time_tol, -3, 3]); 1243 | else 1244 | hold on; 1245 | set(thvx5, 'XData', [get(thvx5, 'XData'), time]); 1246 | set(thvx5, 'YData', [get(thvx5, 'YData'), true_v4(1)]); 1247 | set(ehvx5, 'XData', [get(ehvx5, 'XData'), time]); 1248 | set(ehvx5, 'YData', [get(ehvx5, 'YData'), des_v4(1)]); 1249 | hold off; 1250 | end 1251 | % quadrotor 6 1252 | subplot(h44); 1253 | true_v6 = true_s(4:6, 6); 1254 | des_v6 = s_des(4:6, 6); 1255 | if ~vis_init 1256 | hold on; 1257 | thvx6 = plot(time, true_v6(1), 'r-', 'LineWidth', 1); 1258 | ehvx6 = plot(time, des_v6(1), 'b-', 'LineWidth', 1); 1259 | hold off; 1260 | xlabel('Time (s)'); 1261 | ylabel('X6 World Velocity (m/s)'); 1262 | axis([0, time_tol, -3, 3]); 1263 | else 1264 | hold on; 1265 | set(thvx6, 'XData', [get(thvx6, 'XData'), time]); 1266 | set(thvx6, 'YData', [get(thvx6, 'YData'), true_v4(1)]); 1267 | set(ehvx6, 'XData', [get(ehvx6, 'XData'), time]); 1268 | set(ehvx6, 'YData', [get(ehvx6, 'YData'), des_v4(1)]); 1269 | hold off; 1270 | end 1271 | 1272 | %y轴速度 1273 | %quadrotor 1 1274 | subplot(h5); 1275 | if ~vis_init 1276 | hold on; 1277 | thvy1 = plot(time, true_v1(2), 'r-', 'LineWidth', 1); 1278 | ehvy1 = plot(time, des_v1(2), 'b-', 'LineWidth', 1); 1279 | hold off; 1280 | xlabel('Time (s)'); 1281 | ylabel('Y1 World Velocity (m/s)'); 1282 | axis([0, time_tol, -3, 3]); 1283 | else 1284 | hold on; 1285 | set(thvy1, 'XData', [get(thvy1, 'XData'), time]); 1286 | set(thvy1, 'YData', [get(thvy1, 'YData'), true_v1(2)]); 1287 | set(ehvy1, 'XData', [get(ehvy1, 'XData'), time]); 1288 | set(ehvy1, 'YData', [get(ehvy1, 'YData'), des_v1(2)]); 1289 | hold off; 1290 | end 1291 | 1292 | %quadrotor 2 1293 | subplot(h13); 1294 | if ~vis_init 1295 | hold on; 1296 | thvy2 = plot(time, true_v2(2), 'r-', 'LineWidth', 1); 1297 | ehvy2 = plot(time, des_v2(2), 'b-', 'LineWidth', 1); 1298 | hold off; 1299 | xlabel('Time (s)'); 1300 | ylabel('Y2 World Velocity (m/s)'); 1301 | axis([0, time_tol, -3, 3]); 1302 | else 1303 | hold on; 1304 | set(thvy2, 'XData', [get(thvy2, 'XData'), time]); 1305 | set(thvy2, 'YData', [get(thvy2, 'YData'), true_v2(2)]); 1306 | set(ehvy2, 'XData', [get(ehvy2, 'XData'), time]); 1307 | set(ehvy2, 'YData', [get(ehvy2, 'YData'), des_v2(2)]); 1308 | hold off; 1309 | end 1310 | 1311 | 1312 | %quadrotor 3 1313 | subplot(h21); 1314 | if ~vis_init 1315 | hold on; 1316 | thvy3 = plot(time, true_v3(2), 'r-', 'LineWidth', 1); 1317 | ehvy3 = plot(time, des_v3(2), 'b-', 'LineWidth', 1); 1318 | hold off; 1319 | xlabel('Time (s)'); 1320 | ylabel('Y3 World Velocity (m/s)'); 1321 | axis([0, time_tol, -3, 3]); 1322 | else 1323 | hold on; 1324 | set(thvy3, 'XData', [get(thvy3, 'XData'), time]); 1325 | set(thvy3, 'YData', [get(thvy3, 'YData'), true_v3(2)]); 1326 | set(ehvy3, 'XData', [get(ehvy3, 'XData'), time]); 1327 | set(ehvy3, 'YData', [get(ehvy3, 'YData'), des_v3(2)]); 1328 | hold off; 1329 | end 1330 | 1331 | 1332 | %quadrotor 4 1333 | subplot(h29); 1334 | if ~vis_init 1335 | hold on; 1336 | thvy4 = plot(time, true_v1(2), 'r-', 'LineWidth', 1); 1337 | ehvy4 = plot(time, des_v1(2), 'b-', 'LineWidth', 1); 1338 | hold off; 1339 | xlabel('Time (s)'); 1340 | ylabel('Y4 World Velocity (m/s)'); 1341 | axis([0, time_tol, -3, 3]); 1342 | else 1343 | hold on; 1344 | set(thvy4, 'XData', [get(thvy4, 'XData'), time]); 1345 | set(thvy4, 'YData', [get(thvy4, 'YData'), true_v4(2)]); 1346 | set(ehvy4, 'XData', [get(ehvy4, 'XData'), time]); 1347 | set(ehvy4, 'YData', [get(ehvy4, 'YData'), des_v4(2)]); 1348 | hold off; 1349 | end 1350 | 1351 | %quadrotor 5 1352 | subplot(h37); 1353 | if ~vis_init 1354 | hold on; 1355 | thvy5 = plot(time, true_v5(2), 'r-', 'LineWidth', 1); 1356 | ehvy5 = plot(time, des_v5(2), 'b-', 'LineWidth', 1); 1357 | hold off; 1358 | xlabel('Time (s)'); 1359 | ylabel('Y5 World Velocity (m/s)'); 1360 | axis([0, time_tol, -3, 3]); 1361 | else 1362 | hold on; 1363 | set(thvy5, 'XData', [get(thvy5, 'XData'), time]); 1364 | set(thvy5, 'YData', [get(thvy5, 'YData'), true_v5(2)]); 1365 | set(ehvy5, 'XData', [get(ehvy5, 'XData'), time]); 1366 | set(ehvy5, 'YData', [get(ehvy5, 'YData'), des_v5(2)]); 1367 | hold off; 1368 | end 1369 | 1370 | %quadrotor 6 1371 | subplot(h45); 1372 | if ~vis_init 1373 | hold on; 1374 | thvy6 = plot(time, true_v6(2), 'r-', 'LineWidth', 1); 1375 | ehvy6 = plot(time, des_v6(2), 'b-', 'LineWidth', 1); 1376 | hold off; 1377 | xlabel('Time (s)'); 1378 | ylabel('Y6 World Velocity (m/s)'); 1379 | axis([0, time_tol, -3, 3]); 1380 | else 1381 | hold on; 1382 | set(thvy6, 'XData', [get(thvy6, 'XData'), time]); 1383 | set(thvy6, 'YData', [get(thvy6, 'YData'), true_v6(2)]); 1384 | set(ehvy6, 'XData', [get(ehvy6, 'XData'), time]); 1385 | set(ehvy6, 'YData', [get(ehvy6, 'YData'), des_v6(2)]); 1386 | hold off; 1387 | end 1388 | 1389 | %quadrotor 1 1390 | subplot(h6); 1391 | if ~vis_init 1392 | hold on; 1393 | thvz1 = plot(time, true_v1(3), 'r-', 'LineWidth', 1); 1394 | ehvz1 = plot(time, des_v1(3), 'b-', 'LineWidth', 1); 1395 | hold off; 1396 | xlabel('Time (s)'); 1397 | ylabel('Z1 World Velocity (m/s)'); 1398 | axis([0, time_tol, -1, 1]); 1399 | else 1400 | hold on; 1401 | set(thvz1, 'XData', [get(thvz1, 'XData'), time]); 1402 | set(thvz1, 'YData', [get(thvz1, 'YData'), true_v1(3)]); 1403 | set(ehvz1, 'XData', [get(ehvz1, 'XData'), time]); 1404 | set(ehvz1, 'YData', [get(ehvz1, 'YData'), des_v1(3)]); 1405 | hold off; 1406 | end 1407 | 1408 | %quadrotor 2 1409 | subplot(h14); 1410 | if ~vis_init 1411 | hold on; 1412 | thvz2 = plot(time, true_v2(3), 'r-', 'LineWidth', 1); 1413 | ehvz2 = plot(time, des_v2(3), 'b-', 'LineWidth', 1); 1414 | hold off; 1415 | xlabel('Time (s)'); 1416 | ylabel('Z2 World Velocity (m/s)'); 1417 | axis([0, time_tol, -1, 1]); 1418 | else 1419 | hold on; 1420 | set(thvz2, 'XData', [get(thvz2, 'XData'), time]); 1421 | set(thvz2, 'YData', [get(thvz2, 'YData'), true_v2(3)]); 1422 | set(ehvz2, 'XData', [get(ehvz2, 'XData'), time]); 1423 | set(ehvz2, 'YData', [get(ehvz2, 'YData'), des_v2(3)]); 1424 | hold off; 1425 | end 1426 | 1427 | %quadrotor 3 1428 | subplot(h22); 1429 | if ~vis_init 1430 | hold on; 1431 | thvz3 = plot(time, true_v3(3), 'r-', 'LineWidth', 1); 1432 | ehvz3 = plot(time, des_v3(3), 'b-', 'LineWidth', 1); 1433 | hold off; 1434 | xlabel('Time (s)'); 1435 | ylabel('Z3 World Velocity (m/s)'); 1436 | axis([0, time_tol, -1, 1]); 1437 | else 1438 | hold on; 1439 | set(thvz3, 'XData', [get(thvz3, 'XData'), time]); 1440 | set(thvz3, 'YData', [get(thvz3, 'YData'), true_v3(3)]); 1441 | set(ehvz3, 'XData', [get(ehvz3, 'XData'), time]); 1442 | set(ehvz3, 'YData', [get(ehvz3, 'YData'), des_v3(3)]); 1443 | hold off; 1444 | end 1445 | 1446 | %quadrotor 4 1447 | subplot(h30); 1448 | if ~vis_init 1449 | hold on; 1450 | thvz4 = plot(time, true_v4(3), 'r-', 'LineWidth', 1); 1451 | ehvz4 = plot(time, des_v4(3), 'b-', 'LineWidth', 1); 1452 | hold off; 1453 | xlabel('Time (s)'); 1454 | ylabel('Z4 World Velocity (m/s)'); 1455 | axis([0, time_tol, -1, 1]); 1456 | else 1457 | hold on; 1458 | set(thvz4, 'XData', [get(thvz4, 'XData'), time]); 1459 | set(thvz4, 'YData', [get(thvz4, 'YData'), true_v4(3)]); 1460 | set(ehvz4, 'XData', [get(ehvz4, 'XData'), time]); 1461 | set(ehvz4, 'YData', [get(ehvz4, 'YData'), des_v4(3)]); 1462 | hold off; 1463 | end 1464 | 1465 | %quadrotor 5 1466 | subplot(h38); 1467 | if ~vis_init 1468 | hold on; 1469 | thvz5 = plot(time, true_v5(3), 'r-', 'LineWidth', 1); 1470 | ehvz5 = plot(time, des_v5(3), 'b-', 'LineWidth', 1); 1471 | hold off; 1472 | xlabel('Time (s)'); 1473 | ylabel('Z5 World Velocity (m/s)'); 1474 | axis([0, time_tol, -1, 1]); 1475 | else 1476 | hold on; 1477 | set(thvz5, 'XData', [get(thvz5, 'XData'), time]); 1478 | set(thvz5, 'YData', [get(thvz5, 'YData'), true_v5(3)]); 1479 | set(ehvz5, 'XData', [get(ehvz5, 'XData'), time]); 1480 | set(ehvz5, 'YData', [get(ehvz5, 'YData'), des_v5(3)]); 1481 | hold off; 1482 | end 1483 | 1484 | %quadrotor 6 1485 | subplot(h46); 1486 | if ~vis_init 1487 | hold on; 1488 | thvz6 = plot(time, true_v6(3), 'r-', 'LineWidth', 1); 1489 | ehvz6 = plot(time, des_v6(3), 'b-', 'LineWidth', 1); 1490 | hold off; 1491 | xlabel('Time (s)'); 1492 | ylabel('Z6 World Velocity (m/s)'); 1493 | axis([0, time_tol, -1, 1]); 1494 | else 1495 | hold on; 1496 | set(thvz6, 'XData', [get(thvz6, 'XData'), time]); 1497 | set(thvz6, 'YData', [get(thvz6, 'YData'), true_v6(3)]); 1498 | set(ehvz6, 'XData', [get(ehvz6, 'XData'), time]); 1499 | set(ehvz6, 'YData', [get(ehvz6, 'YData'), des_v6(3)]); 1500 | hold off; 1501 | end 1502 | 1503 | %% Plot world frame position 1504 | 1505 | %% quadrotor 1 1506 | subplot(h7); 1507 | true_p1 = true_s(1:3, 1); 1508 | des_p1 = s_des(1:3, 1); 1509 | if ~vis_init 1510 | hold on; 1511 | thpx1 = plot(time, true_p1(1), 'r-', 'LineWidth', 1); 1512 | ehpx1 = plot(time, des_p1(1), 'b-', 'LineWidth', 1); 1513 | hold off; 1514 | xlabel('Time (s)'); 1515 | ylabel('X1 World Position (m)'); 1516 | axis([0, time_tol, -3, 20]); 1517 | else 1518 | hold on; 1519 | set(thpx1, 'XData', [get(thpx1, 'XData'), time]); 1520 | set(thpx1, 'YData', [get(thpx1, 'YData'), true_p1(1)]); 1521 | set(ehpx1, 'XData', [get(ehpx1, 'XData'), time]); 1522 | set(ehpx1, 'YData', [get(ehpx1, 'YData'), des_p1(1)]); 1523 | hold off; 1524 | end 1525 | 1526 | subplot(h8); 1527 | axis([0, 20, -3, 10]); 1528 | if ~vis_init 1529 | hold on; 1530 | thpy1 = plot(time, true_p1(2), 'r-', 'LineWidth', 1); 1531 | ehpy1 = plot(time, des_p1(2), 'b-', 'LineWidth', 1); 1532 | hold off; 1533 | xlabel('Time (s) '); 1534 | ylabel('Y1 World Position (m)'); 1535 | axis([0, time_tol, -3, 3]); 1536 | else 1537 | hold on; 1538 | set(thpy1, 'XData', [get(thpy1, 'XData'), time]); 1539 | set(thpy1, 'YData', [get(thpy1, 'YData'), true_p1(2)]); 1540 | set(ehpy1, 'XData', [get(ehpy1, 'XData'), time]); 1541 | set(ehpy1, 'YData', [get(ehpy1, 'YData'), des_p1(2)]); 1542 | hold off; 1543 | end 1544 | subplot(h9); 1545 | if ~vis_init 1546 | hold on; 1547 | thpz1 = plot(time, true_p1(3), 'r-', 'LineWidth', 1); 1548 | ehpz1 = plot(time, des_p1(3), 'b-', 'LineWidth', 1); 1549 | hold off; 1550 | xlabel('Time (s)'); 1551 | ylabel('Z1 World Position (m)'); 1552 | axis([0, time_tol, -3, 3]); 1553 | else 1554 | hold on; 1555 | set(thpz1, 'XData', [get(thpz1, 'XData'), time]); 1556 | set(thpz1, 'YData', [get(thpz1, 'YData'), true_p1(3)]) 1557 | set(ehpz1, 'XData', [get(ehpz1, 'XData'), time]); 1558 | set(ehpz1, 'YData', [get(ehpz1, 'YData'), des_p1(3)]); 1559 | hold off; 1560 | end 1561 | 1562 | %% quadrotor 2 1563 | subplot(h15); 1564 | true_p2 = true_s(1:3, 2); 1565 | des_p2 = s_des(1:3, 2); 1566 | if ~vis_init 1567 | hold on; 1568 | thpx2 = plot(time, true_p2(1), 'r-', 'LineWidth', 1); 1569 | ehpx2 = plot(time, des_p2(1), 'b-', 'LineWidth', 1); 1570 | hold off; 1571 | xlabel('Time (s)'); 1572 | ylabel('X2 World Position (m)'); 1573 | axis([0, time_tol, -3, 20]); 1574 | else 1575 | hold on; 1576 | set(thpx2, 'XData', [get(thpx2, 'XData'), time]); 1577 | set(thpx2, 'YData', [get(thpx2, 'YData'), true_p2(1)]); 1578 | set(ehpx2, 'XData', [get(ehpx2, 'XData'), time]); 1579 | set(ehpx2, 'YData', [get(ehpx2, 'YData'), des_p2(1)]); 1580 | hold off; 1581 | end 1582 | 1583 | subplot(h16); 1584 | axis([0, 20, -3, 10]); 1585 | if ~vis_init 1586 | hold on; 1587 | thpy2 = plot(time, true_p2(2), 'r-', 'LineWidth', 1); 1588 | ehpy2 = plot(time, des_p2(2), 'b-', 'LineWidth', 1); 1589 | hold off; 1590 | xlabel('Time (s) '); 1591 | ylabel('Y2 World Position (m)'); 1592 | axis([0, time_tol, -3, 3]); 1593 | else 1594 | hold on; 1595 | set(thpy2, 'XData', [get(thpy2, 'XData'), time]); 1596 | set(thpy2, 'YData', [get(thpy2, 'YData'), true_p2(2)]); 1597 | set(ehpy2, 'XData', [get(ehpy2, 'XData'), time]); 1598 | set(ehpy2, 'YData', [get(ehpy2, 'YData'), des_p2(2)]); 1599 | hold off; 1600 | end 1601 | subplot(h17); 1602 | if ~vis_init 1603 | hold on; 1604 | thpz2 = plot(time, true_p2(3), 'r-', 'LineWidth', 1); 1605 | ehpz2 = plot(time, des_p2(3), 'b-', 'LineWidth', 1); 1606 | hold off; 1607 | xlabel('Time (s)'); 1608 | ylabel('Z2 World Position (m)'); 1609 | axis([0, time_tol, -3, 3]); 1610 | else 1611 | hold on; 1612 | set(thpz2, 'XData', [get(thpz2, 'XData'), time]); 1613 | set(thpz2, 'YData', [get(thpz2, 'YData'), true_p2(3)]) 1614 | set(ehpz2, 'XData', [get(ehpz2, 'XData'), time]); 1615 | set(ehpz2, 'YData', [get(ehpz2, 'YData'), des_p2(3)]); 1616 | hold off; 1617 | end 1618 | 1619 | %% quadrotor 3 1620 | subplot(h23); 1621 | true_p3 = true_s(1:3, 3); 1622 | des_p3 = s_des(1:3, 3); 1623 | if ~vis_init 1624 | hold on; 1625 | thpx3 = plot(time, true_p3(1), 'r-', 'LineWidth', 1); 1626 | ehpx3 = plot(time, des_p3(1), 'b-', 'LineWidth', 1); 1627 | hold off; 1628 | xlabel('Time (s)'); 1629 | ylabel('X3 World Position (m)'); 1630 | axis([0, time_tol, -3, 20]); 1631 | else 1632 | hold on; 1633 | set(thpx3, 'XData', [get(thpx3, 'XData'), time]); 1634 | set(thpx3, 'YData', [get(thpx3, 'YData'), true_p3(1)]); 1635 | set(ehpx3, 'XData', [get(ehpx3, 'XData'), time]); 1636 | set(ehpx3, 'YData', [get(ehpx3, 'YData'), des_p3(1)]); 1637 | hold off; 1638 | end 1639 | 1640 | subplot(h24); 1641 | axis([0, 20, -3, 10]); 1642 | if ~vis_init 1643 | hold on; 1644 | thpy3 = plot(time, true_p3(2), 'r-', 'LineWidth', 1); 1645 | ehpy3 = plot(time, des_p3(2), 'b-', 'LineWidth', 1); 1646 | hold off; 1647 | xlabel('Time (s) '); 1648 | ylabel('Y3 World Position (m)'); 1649 | axis([0, time_tol, -3, 3]); 1650 | else 1651 | hold on; 1652 | set(thpy3, 'XData', [get(thpy3, 'XData'), time]); 1653 | set(thpy3, 'YData', [get(thpy3, 'YData'), true_p3(2)]); 1654 | set(ehpy3, 'XData', [get(ehpy3, 'XData'), time]); 1655 | set(ehpy3, 'YData', [get(ehpy3, 'YData'), des_p3(2)]); 1656 | hold off; 1657 | end 1658 | subplot(h25); 1659 | if ~vis_init 1660 | hold on; 1661 | thpz3 = plot(time, true_p3(3), 'r-', 'LineWidth', 1); 1662 | ehpz3 = plot(time, des_p3(3), 'b-', 'LineWidth', 1); 1663 | hold off; 1664 | xlabel('Time (s)'); 1665 | ylabel('Z3 World Position (m)'); 1666 | axis([0, time_tol, -3, 3]); 1667 | else 1668 | hold on; 1669 | set(thpz3, 'XData', [get(thpz3, 'XData'), time]); 1670 | set(thpz3, 'YData', [get(thpz3, 'YData'), true_p3(3)]) 1671 | set(ehpz3, 'XData', [get(ehpz3, 'XData'), time]); 1672 | set(ehpz3, 'YData', [get(ehpz3, 'YData'), des_p3(3)]); 1673 | hold off; 1674 | end 1675 | 1676 | %% quadrotor 4 1677 | subplot(h31); 1678 | true_p4 = true_s(1:3, 4); 1679 | des_p4 = s_des(1:3, 4); 1680 | if ~vis_init 1681 | hold on; 1682 | thpx4 = plot(time, true_p4(1), 'r-', 'LineWidth', 1); 1683 | ehpx4 = plot(time, des_p4(1), 'b-', 'LineWidth', 1); 1684 | hold off; 1685 | xlabel('Time (s)'); 1686 | ylabel('X4 World Position (m)'); 1687 | axis([0, time_tol, -3, 20]); 1688 | else 1689 | hold on; 1690 | set(thpx4, 'XData', [get(thpx4, 'XData'), time]); 1691 | set(thpx4, 'YData', [get(thpx4, 'YData'), true_p4(1)]); 1692 | set(ehpx4, 'XData', [get(ehpx4, 'XData'), time]); 1693 | set(ehpx4, 'YData', [get(ehpx4, 'YData'), des_p4(1)]); 1694 | hold off; 1695 | end 1696 | 1697 | subplot(h32); 1698 | axis([0, 20, -3, 10]); 1699 | if ~vis_init 1700 | hold on; 1701 | thpy4 = plot(time, true_p4(2), 'r-', 'LineWidth', 1); 1702 | ehpy4 = plot(time, des_p4(2), 'b-', 'LineWidth', 1); 1703 | hold off; 1704 | xlabel('Time (s)'); 1705 | ylabel('Y4 World Position (m)'); 1706 | axis([0, time_tol, -3, 3]); 1707 | else 1708 | hold on; 1709 | set(thpy4, 'XData', [get(thpy4, 'XData'), time]); 1710 | set(thpy4, 'YData', [get(thpy4, 'YData'), true_p4(2)]); 1711 | set(ehpy4, 'XData', [get(ehpy4, 'XData'), time]); 1712 | set(ehpy4, 'YData', [get(ehpy4, 'YData'), des_p4(2)]); 1713 | hold off; 1714 | end 1715 | subplot(h33); 1716 | if ~vis_init 1717 | hold on; 1718 | thpz4 = plot(time, true_p4(3), 'r-', 'LineWidth', 1); 1719 | ehpz4 = plot(time, des_p4(3), 'b-', 'LineWidth', 1); 1720 | hold off; 1721 | xlabel('Time (s)'); 1722 | ylabel('Z4 World Position (m)'); 1723 | axis([0, time_tol, -3, 3]); 1724 | else 1725 | hold on; 1726 | set(thpz4, 'XData', [get(thpz4, 'XData'), time]); 1727 | set(thpz4, 'YData', [get(thpz4, 'YData'), true_p4(3)]) 1728 | set(ehpz4, 'XData', [get(ehpz4, 'XData'), time]); 1729 | set(ehpz4, 'YData', [get(ehpz4, 'YData'), des_p4(3)]); 1730 | hold off; 1731 | end 1732 | 1733 | %% quadrotor 5 1734 | subplot(h39); 1735 | true_p5 = true_s(1:3, 5); 1736 | des_p5 = s_des(1:3, 5); 1737 | if ~vis_init 1738 | hold on; 1739 | thpx5 = plot(time, true_p5(1), 'r-', 'LineWidth', 1); 1740 | ehpx5 = plot(time, des_p5(1), 'b-', 'LineWidth', 1); 1741 | hold off; 1742 | xlabel('Time (s)'); 1743 | ylabel('X5 World Position (m)'); 1744 | axis([0, time_tol, -3, 20]); 1745 | else 1746 | hold on; 1747 | set(thpx5, 'XData', [get(thpx5, 'XData'), time]); 1748 | set(thpx5, 'YData', [get(thpx5, 'YData'), true_p5(1)]); 1749 | set(ehpx5, 'XData', [get(ehpx5, 'XData'), time]); 1750 | set(ehpx5, 'YData', [get(ehpx5, 'YData'), des_p5(1)]); 1751 | hold off; 1752 | end 1753 | 1754 | subplot(h40); 1755 | axis([0, 20, -3, 10]); 1756 | if ~vis_init 1757 | hold on; 1758 | thpy5 = plot(time, true_p5(2), 'r-', 'LineWidth', 1); 1759 | ehpy5 = plot(time, des_p5(2), 'b-', 'LineWidth', 1); 1760 | hold off; 1761 | xlabel('Time (s)'); 1762 | ylabel('Y5 World Position (m)'); 1763 | axis([0, time_tol, -3, 3]); 1764 | else 1765 | hold on; 1766 | set(thpy5, 'XData', [get(thpy5, 'XData'), time]); 1767 | set(thpy5, 'YData', [get(thpy5, 'YData'), true_p5(2)]); 1768 | set(ehpy5, 'XData', [get(ehpy5, 'XData'), time]); 1769 | set(ehpy5, 'YData', [get(ehpy5, 'YData'), des_p5(2)]); 1770 | hold off; 1771 | end 1772 | subplot(h41); 1773 | if ~vis_init 1774 | hold on; 1775 | thpz5 = plot(time, true_p5(3), 'r-', 'LineWidth', 1); 1776 | ehpz5 = plot(time, des_p5(3), 'b-', 'LineWidth', 1); 1777 | hold off; 1778 | xlabel('Time (s)'); 1779 | ylabel('Z5 World Position (m)'); 1780 | axis([0, time_tol, -3, 3]); 1781 | else 1782 | hold on; 1783 | set(thpz5, 'XData', [get(thpz5, 'XData'), time]); 1784 | set(thpz5, 'YData', [get(thpz5, 'YData'), true_p5(3)]) 1785 | set(ehpz5, 'XData', [get(ehpz5, 'XData'), time]); 1786 | set(ehpz5, 'YData', [get(ehpz5, 'YData'), des_p5(3)]); 1787 | hold off; 1788 | end 1789 | 1790 | %% quadrotor 6 1791 | subplot(h47); 1792 | true_p6 = true_s(1:3, 6); 1793 | des_p6 = s_des(1:3, 6); 1794 | if ~vis_init 1795 | hold on; 1796 | thpx6 = plot(time, true_p6(1), 'r-', 'LineWidth', 1); 1797 | ehpx6 = plot(time, des_p6(1), 'b-', 'LineWidth', 1); 1798 | hold off; 1799 | xlabel('Time (s)'); 1800 | ylabel('X6 World Position (m)'); 1801 | axis([0, time_tol, -3, 20]); 1802 | else 1803 | hold on; 1804 | set(thpx6, 'XData', [get(thpx6, 'XData'), time]); 1805 | set(thpx6, 'YData', [get(thpx6, 'YData'), true_p6(1)]); 1806 | set(ehpx6, 'XData', [get(ehpx6, 'XData'), time]); 1807 | set(ehpx6, 'YData', [get(ehpx6, 'YData'), des_p6(1)]); 1808 | hold off; 1809 | end 1810 | 1811 | subplot(h48); 1812 | axis([0, 20, -3, 10]); 1813 | if ~vis_init 1814 | hold on; 1815 | thpy6 = plot(time, true_p6(2), 'r-', 'LineWidth', 1); 1816 | ehpy6 = plot(time, des_p6(2), 'b-', 'LineWidth', 1); 1817 | hold off; 1818 | xlabel('Time (s)'); 1819 | ylabel('Y6 World Position (m)'); 1820 | axis([0, time_tol, -3, 3]); 1821 | else 1822 | hold on; 1823 | set(thpy6, 'XData', [get(thpy6, 'XData'), time]); 1824 | set(thpy6, 'YData', [get(thpy6, 'YData'), true_p6(2)]); 1825 | set(ehpy6, 'XData', [get(ehpy6, 'XData'), time]); 1826 | set(ehpy6, 'YData', [get(ehpy6, 'YData'), des_p6(2)]); 1827 | hold off; 1828 | end 1829 | subplot(h49); 1830 | if ~vis_init 1831 | hold on; 1832 | thpz6 = plot(time, true_p6(3), 'r-', 'LineWidth', 1); 1833 | ehpz6 = plot(time, des_p6(3), 'b-', 'LineWidth', 1); 1834 | hold off; 1835 | xlabel('Time (s)'); 1836 | ylabel('Z6 World Position (m)'); 1837 | axis([0, time_tol, -3, 3]); 1838 | else 1839 | hold on; 1840 | set(thpz6, 'XData', [get(thpz6, 'XData'), time]); 1841 | set(thpz6, 'YData', [get(thpz6, 'YData'), true_p6(3)]) 1842 | set(ehpz6, 'XData', [get(ehpz6, 'XData'), time]); 1843 | set(ehpz6, 'YData', [get(ehpz6, 'YData'), des_p6(3)]); 1844 | hold off; 1845 | end 1846 | 1847 | %% Render 1848 | drawnow; 1849 | vis_time = time; 1850 | vis_init = true; 1851 | end 1852 | end 1853 | -------------------------------------------------------------------------------- /code/test_trajectory.m: -------------------------------------------------------------------------------- 1 | % Used for HKUST ELEC 5660 2 | 3 | close all 4 | clear all 5 | 6 | clc; 7 | addpath('./utils', './readonly','/SI_DFM_2D_SYSU'); 8 | 9 | h1 = subplot(6, 12, [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24]); 10 | %第一架飞机数据分析图 11 | h2 = subplot(6, 12, 25); 12 | h3 = subplot(6, 12, 26); 13 | h4 = subplot(6, 12, 27); 14 | h5 = subplot(6, 12, 28); 15 | 16 | h6 = subplot(6, 12, 37); 17 | h7 = subplot(6, 12, 38); 18 | h8 = subplot(6, 12, 39); 19 | h9 = subplot(6, 12, 40); 20 | 21 | %第二架飞机数据飞行图 22 | h10 = subplot(6, 12, 29); 23 | h11 = subplot(6, 12, 30); 24 | h12 = subplot(6, 12, 31); 25 | h13 = subplot(6, 12, 32); 26 | 27 | h14 = subplot(6, 12, 41); 28 | h15 = subplot(6, 12, 42); 29 | h16 = subplot(6, 12, 43); 30 | h17 = subplot(6, 12, 44); 31 | 32 | %第三架飞机数据飞行图 33 | h18 = subplot(6, 12, 33); 34 | h19 = subplot(6, 12, 34); 35 | h20 = subplot(6, 12, 35); 36 | h21 = subplot(6, 12, 36); 37 | 38 | h22 = subplot(6, 12, 45); 39 | h23 = subplot(6, 12, 46); 40 | h24 = subplot(6, 12, 47); 41 | h25 = subplot(6, 12, 48); 42 | 43 | %第四架飞机数据飞行图 44 | h26 = subplot(6, 12, 49); 45 | h27 = subplot(6, 12, 50); 46 | h28 = subplot(6, 12, 51); 47 | h29 = subplot(6, 12, 52); 48 | 49 | h30 = subplot(6, 12, 61); 50 | h31 = subplot(6, 12, 62); 51 | h32 = subplot(6, 12, 63); 52 | h33 = subplot(6, 12, 64); 53 | 54 | %第五架飞机数据飞行图 55 | h34 = subplot(6, 12, 53); 56 | h35 = subplot(6, 12, 54); 57 | h36 = subplot(6, 12, 55); 58 | h37 = subplot(6, 12, 56); 59 | 60 | h38 = subplot(6, 12, 65); 61 | h39 = subplot(6, 12, 66); 62 | h40 = subplot(6, 12, 67); 63 | h41 = subplot(6, 12, 68); 64 | 65 | %第六架飞机数据飞行图 66 | h42 = subplot(6, 12, 57); 67 | h43 = subplot(6, 12, 58); 68 | h44 = subplot(6, 12, 59); 69 | h45 = subplot(6, 12, 60); 70 | 71 | h46 = subplot(6, 12, 69); 72 | h47 = subplot(6, 12, 70); 73 | h48 = subplot(6, 12, 71); 74 | h49 = subplot(6, 12, 72); 75 | 76 | set(gcf, 'Renderer', 'painters'); 77 | set(gcf, 'Position', [100, 100, 1400, 1000]); 78 | 79 | 80 | run6formation(h1, h2, h3, h4, h5, h6, h7, h8, h9, h10, h11, h12, h13, h14, ... 81 | h15, h16, h17, h18, h19, h20, h21, h22, h23, h24, h25, h26, h27, h28, h29, ... 82 | h30, h31, h32, h33, h34, h35, h36, h37, h38, h39, h40, h41, h42, h43, h44, ... 83 | h45, h46, h47, h48, h49); 84 | 85 | JudgeStudentsPoint 86 | -------------------------------------------------------------------------------- /code/utils/QuatToRot.m: -------------------------------------------------------------------------------- 1 | function R = QuatToRot(q) 2 | %written by Daniel Mellinger 3 | %normalize q 4 | 5 | q = q./sqrt(sum(q.^2)); 6 | 7 | qahat(1,2) = -q(4); 8 | qahat(1,3) = q(3); 9 | qahat(2,3) = -q(2); 10 | qahat(2,1) = q(4); 11 | qahat(3,1) = -q(3); 12 | qahat(3,2) = q(2); 13 | 14 | R = eye(3) + 2*qahat*qahat + 2*q(1)*qahat; -------------------------------------------------------------------------------- /code/utils/RPYtoRot_ZXY.m: -------------------------------------------------------------------------------- 1 | function R = RPYtoRot_ZXY(phi,theta,psi) 2 | %written by Daniel Mellinger 3 | 4 | % BRW = [ cos(psi)*cos(theta) - sin(phi)*sin(psi)*sin(theta), 5 | % cos(theta)*sin(psi) + cos(psi)*sin(phi)*sin(theta), -cos(phi)*sin(theta)] 6 | % 7 | % [ -cos(phi)*sin(psi), 8 | % cos(phi)*cos(psi), sin(phi)] 9 | % 10 | % [ cos(psi)*sin(theta) + cos(theta)*sin(phi)*sin(psi), 11 | % sin(psi)*sin(theta) - cos(psi)*cos(theta)*sin(phi), 12 | % cos(phi)*cos(theta)] 13 | 14 | R = [ cos(psi)*cos(theta) - sin(phi)*sin(psi)*sin(theta),... 15 | cos(theta)*sin(psi) + cos(psi)*sin(phi)*sin(theta), -cos(phi)*sin(theta);... 16 | -cos(phi)*sin(psi),... 17 | cos(phi)*cos(psi), sin(phi);... 18 | cos(psi)*sin(theta) + cos(theta)*sin(phi)*sin(psi),... 19 | sin(psi)*sin(theta) - cos(psi)*cos(theta)*sin(phi),... 20 | cos(phi)*cos(theta)]; 21 | -------------------------------------------------------------------------------- /code/utils/R_to_quaternion.m: -------------------------------------------------------------------------------- 1 | % written by Shaojie Shen 2 | % Feb 1, 2014 3 | % 4 | % For details of this code see the paper: 5 | % 6 | % S. Shen, Y. Mulgaonkar, N. Michael, and V. Kumar 7 | % Initialization-free monocular visual-inertial estimation with application to autonomous MAVs 8 | % International Symposium on Experimental Robotics (ISER), Marrakech, Morocco, June 2014. 9 | % 10 | % For more info take a look at some of the papers posted here: 11 | % 12 | % http://www.ee.ust.hk/~eeshaojie 13 | % 14 | % *** Internal Use Only - Do Not Distribute *** 15 | 16 | function q = R_to_quaternion(R) 17 | 18 | tr = R(1,1) + R(2,2) + R(3,3); 19 | 20 | if (tr > 0) 21 | S = sqrt(tr+1.0) * 2; % S=4*qw 22 | qw = 0.25 * S; 23 | qx = (R(3,2) - R(2,3)) / S; 24 | qy = (R(1,3) - R(3,1)) / S; 25 | qz = (R(2,1) - R(1,2)) / S; 26 | elseif ((R(1,1) > R(2,2)) && (R(1,1) > R(3,3))) 27 | S = sqrt(1.0 + R(1,1) - R(2,2) - R(3,3)) * 2; % S=4*qx 28 | qw = (R(3,2) - R(2,3)) / S; 29 | qx = 0.25 * S; 30 | qy = (R(1,2) + R(2,1)) / S; 31 | qz = (R(1,3) + R(3,1)) / S; 32 | elseif (R(2,2) > R(3,3)) 33 | S = sqrt(1.0 + R(2,2) - R(1,1) - R(3,3)) * 2; % S=4*qy 34 | qw = (R(1,3) - R(3,1)) / S; 35 | qx = (R(1,2) + R(2,1)) / S; 36 | qy = 0.25 * S; 37 | qz = (R(2,3) + R(3,2)) / S; 38 | else 39 | S = sqrt(1.0 + R(3,3) - R(1,1) - R(2,2)) * 2; % S=4*qz 40 | qw = (R(2,1) - R(1,2)) / S; 41 | qx = (R(1,3) + R(3,1)) / S; 42 | qy = (R(2,3) + R(3,2)) / S; 43 | qz = 0.25 * S; 44 | end 45 | 46 | q = [qw,qx,qy,qz]'; 47 | q = q*sign(qw); 48 | 49 | end 50 | 51 | -------------------------------------------------------------------------------- /code/utils/R_to_ypr.m: -------------------------------------------------------------------------------- 1 | % written by Shaojie Shen 2 | % Feb 1, 2014 3 | % 4 | % For details of this code see the paper: 5 | % 6 | % S. Shen, Y. Mulgaonkar, N. Michael, and V. Kumar 7 | % Initialization-free monocular visual-inertial estimation with application to autonomous MAVs 8 | % International Symposium on Experimental Robotics (ISER), Marrakech, Morocco, June 2014. 9 | % 10 | % For more info take a look at some of the papers posted here: 11 | % 12 | % http://www.ee.ust.hk/~eeshaojie 13 | % 14 | % *** Internal Use Only - Do Not Distribute *** 15 | 16 | function ypr = R_to_ypr(R) 17 | 18 | n = R(:,1); 19 | o = R(:,2); 20 | a = R(:,3); 21 | 22 | y = atan2(n(2), n(1)); 23 | p = atan2(-n(3), n(1)*cos(y)+n(2)*sin(y)); 24 | r = atan2(a(1)*sin(y)-a(2)*cos(y), -o(1)*sin(y)+o(2)*cos(y)); 25 | ypr = [y;p;r]; 26 | 27 | end 28 | -------------------------------------------------------------------------------- /code/utils/RotToQuat.m: -------------------------------------------------------------------------------- 1 | function q = RotToQuat(R) 2 | %written by Daniel Mellinger 3 | %from the following website, deals with the case when tr<0 4 | %http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixT 5 | %oQuaternion/index.htm 6 | 7 | %takes in W_R_B rotation matrix 8 | 9 | tr = R(1,1) + R(2,2) + R(3,3); 10 | 11 | if (tr > 0) 12 | S = sqrt(tr+1.0) * 2; % S=4*qw 13 | qw = 0.25 * S; 14 | qx = (R(3,2) - R(2,3)) / S; 15 | qy = (R(1,3) - R(3,1)) / S; 16 | qz = (R(2,1) - R(1,2)) / S; 17 | elseif ((R(1,1) > R(2,2))&(R(1,1) > R(3,3))) 18 | S = sqrt(1.0 + R(1,1) - R(2,2) - R(3,3)) * 2; % S=4*qx 19 | qw = (R(3,2) - R(2,3)) / S; 20 | qx = 0.25 * S; 21 | qy = (R(1,2) + R(2,1)) / S; 22 | qz = (R(1,3) + R(3,1)) / S; 23 | elseif (R(2,2) > R(3,3)) 24 | S = sqrt(1.0 + R(2,2) - R(1,1) - R(3,3)) * 2; % S=4*qy 25 | qw = (R(1,3) - R(3,1)) / S; 26 | qx = (R(1,2) + R(2,1)) / S; 27 | qy = 0.25 * S; 28 | qz = (R(2,3) + R(3,2)) / S; 29 | else 30 | S = sqrt(1.0 + R(3,3) - R(1,1) - R(2,2)) * 2; % S=4*qz 31 | qw = (R(2,1) - R(1,2)) / S; 32 | qx = (R(1,3) + R(3,1)) / S; 33 | qy = (R(2,3) + R(3,2)) / S; 34 | qz = 0.25 * S; 35 | end 36 | 37 | q = [qw,qx,qy,qz]'; 38 | q = q*sign(qw); 39 | -------------------------------------------------------------------------------- /code/utils/RotToRPY_ZXY.m: -------------------------------------------------------------------------------- 1 | function [phi,theta,psi] = RotToRPY_ZXY(R) 2 | %written by Daniel Mellinger 3 | 4 | % 5 | % R = [ cos(psi)*cos(theta) - sin(phi)*sin(psi)*sin(theta), 6 | % cos(theta)*sin(psi) + cos(psi)*sin(phi)*sin(theta), -cos(phi)*sin(theta)] 7 | % 8 | % [ -cos(phi)*sin(psi), 9 | % cos(phi)*cos(psi), sin(phi)] 10 | % 11 | % [ cos(psi)*sin(theta) + cos(theta)*sin(phi)*sin(psi), 12 | % sin(psi)*sin(theta) - cos(psi)*cos(theta)*sin(phi), cos(phi)*cos(theta)] 13 | 14 | phi = asin(R(2,3)); 15 | psi = atan2(-R(2,1)/cos(phi),R(2,2)/cos(phi)); 16 | theta = atan2(-R(1,3)/cos(phi),R(3,3)/cos(phi)); 17 | -------------------------------------------------------------------------------- /code/utils/quaternion_to_R.m: -------------------------------------------------------------------------------- 1 | % written by Shaojie Shen 2 | % Feb 1, 2014 3 | % 4 | % For details of this code see the paper: 5 | % 6 | % S. Shen, Y. Mulgaonkar, N. Michael, and V. Kumar 7 | % Initialization-free monocular visual-inertial estimation with application to autonomous MAVs 8 | % International Symposium on Experimental Robotics (ISER), Marrakech, Morocco, June 2014. 9 | % 10 | % For more info take a look at some of the papers posted here: 11 | % 12 | % http://www.ee.ust.hk/~eeshaojie 13 | % 14 | % *** Internal Use Only - Do Not Distribute *** 15 | 16 | function R = quaternion_to_R(q) 17 | 18 | q = q/norm(q); % Ensure Q has unit norm 19 | 20 | % Set up convenience variables 21 | w = q(1); x = q(2); y = q(3); z = q(4); 22 | w2 = w^2; x2 = x^2; y2 = y^2; z2 = z^2; 23 | xy = x*y; xz = x*z; yz = y*z; 24 | wx = w*x; wy = w*y; wz = w*z; 25 | 26 | R = [w2+x2-y2-z2 , 2*(xy - wz) , 2*(wy + xz) ; ... 27 | 2*(wz + xy) , w2-x2+y2-z2 , 2*(yz - wx) ; ... 28 | 2*(xz - wy) , 2*(wx + yz) , w2-x2-y2+z2]; 29 | 30 | end 31 | 32 | -------------------------------------------------------------------------------- /code/utils/ypr_to_R.m: -------------------------------------------------------------------------------- 1 | % written by Shaojie Shen 2 | % Feb 1, 2014 3 | % 4 | % For details of this code see the paper: 5 | % 6 | % S. Shen, Y. Mulgaonkar, N. Michael, and V. Kumar 7 | % Initialization-free monocular visual-inertial estimation with application to autonomous MAVs 8 | % International Symposium on Experimental Robotics (ISER), Marrakech, Morocco, June 2014. 9 | % 10 | % For more info take a look at some of the papers posted here: 11 | % 12 | % http://www.ee.ust.hk/~eeshaojie 13 | % 14 | % *** Internal Use Only - Do Not Distribute *** 15 | 16 | function R = ypr_to_R(ypr) 17 | 18 | y = ypr(1); 19 | p = ypr(2); 20 | r = ypr(3); 21 | 22 | Rz = [cos(y) -sin(y) 0; ... 23 | sin(y) cos(y) 0; ... 24 | 0 0 1]; 25 | 26 | Ry = [cos(p) 0 sin(p); ... 27 | 0 1 0; ... 28 | -sin(p) 0 cos(p)]; 29 | 30 | Rx = [1 0 0; ... 31 | 0 cos(r) -sin(r); ... 32 | 0 sin(r) cos(r)]; 33 | 34 | R = Rz*Ry*Rx; 35 | 36 | end 37 | -------------------------------------------------------------------------------- /project instruction.docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SYSU-HI-LAB/Introduction_to_multi_agent_control/168686e8608951fc5df224114d3e01949aef2506/project instruction.docx --------------------------------------------------------------------------------