├── CCA3D_straight.m ├── IFDS.m ├── PlotObject.m ├── PlotPath.m ├── README.md ├── WeatherMat_3.mat ├── WeatherMat_321.mat ├── WeatherMat_8187.mat ├── Weather_map_Generator.m ├── allTraj_opt.mat ├── allTraj_opt2.mat ├── for_experimenting ├── test_optimize_dist_fmincon.m └── weather_test.m ├── initialize_constraint_matrix.m ├── main.m ├── main_sg_generation.m ├── norm_ubar.m ├── path_dist_objective_v2.m ├── path_distance_objective.m ├── plotting_everything.m ├── real-time_analysis ├── realtime_analysis.m ├── time_test10.mat ├── time_test11.mat ├── time_test12.mat ├── time_test12sp.mat ├── time_test13.mat ├── time_test14.mat ├── time_test15.mat ├── time_test16.mat ├── time_test4.mat └── time_test5.mat ├── time_journal_dyna_1.mat ├── time_journal_dyna_2.mat └── time_optim_journal_dyna_2.mat /CCA3D_straight.m: -------------------------------------------------------------------------------- 1 | function [x_final, y_final, z_final, psi_final, gamma_final, timeSpent] = CCA3D_straight(Wi, Wf, x0, y0, z0, psi0, gamma0, V, tuning) 2 | 3 | dt = 0.01; 4 | timeSpent = 0; 5 | animation = 0; 6 | %.. Time 7 | t(1) = 0 ; % Simulation Time [s] 8 | %.. Position and Velocity of UAV 9 | va = V ; % UAV Velocity [m/s] 10 | 11 | %.. Maximum Lateral Acceleration of UAV 12 | Rmin = 10; % UAV Minimum Turn Radius [m] (lower is better) 13 | % Rmin = 13 ; % UAV Minimum Turn Radius [m] 14 | umax = va^2 / Rmin ; % UAV Maximum Lateral Acceleration [m] 15 | del_psi(1)=0; 16 | 17 | %-----------------------Design Parameters--------------------------------- 18 | kappa = tuning(1); 19 | delta = tuning(2); 20 | kd = tuning(3); 21 | % ------------------------------------------------------------------------ 22 | 23 | % Path Following Algorithm - CCA (Carrot Chasing Algorithm) 24 | 25 | i = 0 ; % Time Index 26 | 27 | x(1) = x0; % Initial UAV X Position [m] 28 | y(1) = y0; % Initial UAV Y Position [m] 29 | z(1) = z0; 30 | psi(1) = psi0 ; % Initial UAV Heading Angle [rad] 31 | gamma(1) = gamma0; % Initial UAV Pitch Angle [rad] 32 | p(:,1) = [ x(1), y(1), z(1) ]' ; % UAV Position Initialization [m] 33 | 34 | 35 | % ------------------------------------------- 36 | % Normal plane checker 37 | % Find an equation of the plane through the point Wf and perpendicular to 38 | % the vector (Wf - Wi) 39 | Rw_vect = Wf - Wi; 40 | ox = Wf(1); 41 | oy = Wf(2); 42 | oz = Wf(3); 43 | a = Rw_vect(1); 44 | b = Rw_vect(2); 45 | c = Rw_vect(3); 46 | 47 | check = 1; 48 | 49 | while a*(x(i+1) - ox) + b*(y(i+1) - oy) + c*(z(i+1) - oz) < 0 50 | % while (x(i+1) < Wf(1)) 51 | % while 1 52 | 53 | i = i + 1; 54 | 55 | %==============================================================================% 56 | %.. Path Following Algorithm 57 | 58 | % Step 1 59 | % Distance between initial waypoint and current UAV position, Ru 60 | Ru_vect = Wi - p(:,i); 61 | Ru = norm(Wi - p(:,i)); 62 | 63 | % Step 2 64 | % Orientation of vector from initial waypoint to final waypoint, theta 65 | theta1 = atan2(Wf(2) - Wi(2), Wf(1) - Wi(1)); 66 | theta2 = atan2(Wf(3) - Wi(3), sqrt( (Wf(1) - Wi(1))^2 + (Wf(2)-Wi(2))^2 )); 67 | 68 | % Step 3 69 | % Orientation of vector from initial waypoint to current UAV position, theta_u 70 | theta_u1 = atan2(p(2,i) - Wi(2), p(1,i) - Wi(1)); 71 | theta_u2 = atan2(p(3,i) - Wi(3), sqrt( (p(1,i) - Wi(1))^2 + (p(2,i)-Wi(2))^2 )); 72 | % Difference between theta and theatu, DEL_theta 73 | DEL_theta1 = theta1 - theta_u1; 74 | DEL_thata2 = theta2 - theta_u2; 75 | 76 | % Step 4 77 | % Distance between initial waypoint and q, R 78 | if (norm(Ru_vect) ~= 0) && (norm(Rw_vect) ~= 0) 79 | alpha = real(acos( dot(Ru_vect, Rw_vect)/( norm(Ru_vect) * norm(Rw_vect)) )); 80 | else 81 | alpha = 0; 82 | end 83 | 84 | R = sqrt( Ru^2 - (Ru*sin(alpha))^2 ); 85 | 86 | % Step 5 87 | % Carrot position, s = ( xt, yt ) 88 | xt = Wi(1) + (R + delta) * cos(theta2)*cos(theta1); 89 | yt = Wi(2) + (R + delta) * cos(theta2)*sin(theta1); 90 | zt = Wi(3) + (R + delta) * sin(theta2); 91 | 92 | % Step 6 93 | % Desired heading angle, psi_d 94 | 95 | psi_d = atan2(yt - p(2,i), xt - p(1,i)); 96 | % Desired pitch angle gamma_d ( CHECK AGAIN!! ) 97 | gamma_d = atan2(zt - p(3,i), sqrt( (xt - p(1,i))^2 + (yt - p(2,i))^2 )); 98 | 99 | % Wrapping up psid 100 | psi_d = rem(psi_d, 2*pi); 101 | gamma_d = rem(gamma_d, 2*pi); 102 | 103 | if psi_d < -pi 104 | psi_d = psi_d + 2*pi; 105 | elseif psi_d > pi 106 | psi_d = psi_d-2*pi; 107 | end 108 | 109 | if gamma_d < -pi 110 | gamma_d = gamma_d + 2*pi; 111 | elseif gamma_d > pi 112 | gamma_d = gamma_d-2*pi; 113 | end 114 | 115 | % Limit turning angle 116 | if psi_d > pi/2 117 | psi_d = pi/2; 118 | elseif psi_d < -pi/2 119 | psi_d = -pi/2; 120 | end 121 | 122 | % Limit pitching angle 123 | if gamma_d > pi/2 124 | gamma_d = pi/2; 125 | elseif gamma_d < -pi/2 126 | gamma_d = -pi/2; 127 | end 128 | 129 | % Step7 130 | % Guidance Yaw command, u1 131 | % u(i) = kappa*(psi_d - psi(i))*va; 132 | del_psi(i+1) = (psi_d - psi(i)); 133 | u1(i) = (kappa*del_psi(i+1) + kd*(del_psi(i+1)-del_psi(i))/dt)*va; 134 | 135 | % Guidance Pitch command, u2 136 | del_gam(i+1) = (gamma_d - gamma(i)); 137 | u2(i) = (kappa*del_gam(i+1) + kd*(del_gam(i+1)-del_gam(i))/dt)*va; 138 | 139 | % Limit u1 140 | if u1(i) > umax 141 | u1(i) = umax; 142 | elseif u1(i) < -umax 143 | u1(i) = - umax; 144 | end 145 | 146 | % Limit u2 147 | if u2(i) > umax 148 | u2(i) = umax; 149 | elseif u2(i) < -umax 150 | u2(i) = - umax; 151 | end 152 | %==============================================================================% 153 | 154 | %.. UAV Dynamics 155 | % Dynamic Model of UAV 156 | dx = va * cos( gamma(i) ) * cos( psi(i) ) ; 157 | dy = va * cos( gamma(i) ) * sin( psi(i) ) ; 158 | dz = va * sin( gamma(i)); 159 | dpsi = u1(i) / (va*cos(gamma(i))) ; 160 | dgam = u2(i) / va; 161 | 162 | % UAV State Update 163 | x(i+1) = x(i) + dx * dt; 164 | y(i+1) = y(i) + dy * dt; 165 | z(i+1) = z(i) + dz * dt; 166 | psi(i+1) = psi(i) + dpsi * dt ; 167 | gamma(i+1) = gamma(i) + dgam * dt; 168 | 169 | % UAV Position Vector Update 170 | p(:,i+1) = [ x(i+1), y(i+1), z(i+1) ]' ; 171 | 172 | %.. Time Update 173 | t(i+1) = t(i) + dt ; 174 | 175 | psi_final = psi(i+1); 176 | gamma_final = gamma(i+1); 177 | x_final = x(i+1); 178 | y_final = y(i+1); 179 | z_final = z(i+1); 180 | 181 | timeSpent = timeSpent + dt; 182 | 183 | 184 | if animation 185 | ss = scatter(x(i),y(i), 'filled', 'MarkerFaceColor','blue'); 186 | pause(0.01) 187 | delete(ss) 188 | end 189 | 190 | % if norm([x(i+1) y(i+1) z(i+1)]' - Wf) < 1 191 | % break; 192 | % end 193 | if i > 10000 194 | break; 195 | end 196 | end 197 | 198 | 199 | % subtitle({'CCA',['\delta = ' num2str(delta) ' \kappa = ' num2str(kappa)]}) 200 | end -------------------------------------------------------------------------------- /IFDS.m: -------------------------------------------------------------------------------- 1 | function [Paths, Object, totalLength, foundPath] = IFDS(rho0, sigma0, loc_final, rt, Wp, Paths, Param, L, Object, weatherMat, dwdx, dwdy) 2 | 3 | % Read the parameters 4 | simMode = Param.simMode; 5 | scene = Param.scene; 6 | sf = Param.sf; 7 | targetThresh = Param.targetThresh; 8 | tsim = Param.tsim; 9 | dt = Param.dt; 10 | C = Param.C; 11 | showDisp = Param.showDisp; 12 | useOptimizer = Param.useOptimizer; 13 | delta_g = Param.Rg; 14 | k = Param.k; 15 | B_U = Param.B_U; 16 | B_L = Param.B_L; 17 | 18 | % Initialization 19 | xd = loc_final(1); 20 | yd = loc_final(2); 21 | zd = loc_final(3); 22 | 23 | foundPath = 0; 24 | 25 | switch simMode 26 | case 1 % Simulate by time 27 | 28 | 29 | for t = 1:tsim 30 | Wp(:,t) = real(Wp(:,t)); 31 | xx = Wp(1,t); 32 | yy = Wp(2,t); 33 | zz = Wp(3,t); 34 | 35 | if t>1000 36 | break 37 | end 38 | Object = create_scene(scene, Object, xx, yy, zz, rt); 39 | if norm([xx yy zz] - [xd yd zd]) < targetThresh 40 | % disp('Target destination reached!') 41 | Wp = Wp(:,1:t); 42 | Paths{L,rt} = Wp; % Save into cell array 43 | foundPath = 1; 44 | break 45 | else 46 | % --------------- Weather constraints ------------ 47 | if k~=0 48 | omega = weatherMat(xx+1, yy+101); 49 | dwdx_now = dwdx(xx+1, yy+101); 50 | dwdy_now = dwdy(xx+1, yy+101); 51 | 52 | for j = 1:Param.numObj 53 | Gm = Object(j).Gamma; 54 | dGdx = Object(j).n(1); 55 | dGdy = Object(j).n(2); 56 | dGdz = Object(j).n(3); 57 | dGx_p = dGdx + k*exp( (B_L - omega)/(B_L - B_U) * log((Gm-1)/k +1) ) * ... 58 | ( log((Gm-1)/k +1)/(B_L-B_U) * dwdx_now - ((B_L-omega)/((Gm-1+k)*(B_L - B_U))) *dGdx ); 59 | 60 | dGy_p = dGdy + k*exp( (B_L - omega)/(B_L - B_U) * log((Gm-1)/k +1) ) * ... 61 | ( log((Gm-1)/k+1)/(B_L-B_U) * dwdy_now - ((B_L-omega)/((Gm-1+k)*(B_L - B_U))) *dGdy ); 62 | 63 | dGz_p = dGdz + k*exp( (B_L - omega)/(B_L - B_U) * log((Gm-1)/k +1) ) * ... 64 | ( -((B_L-omega)/((Gm-1+k)*(B_L - B_U))) *dGdz ); 65 | 66 | 67 | Object(j).Gamma = Object(j).Gamma - k* (exp( (B_L - omega)/(B_L - B_U) * log((Gm-1)/k +1) ) -1); 68 | 69 | Object(j).n = [dGx_p; dGy_p; dGz_p]; 70 | Object(j).t = [dGy_p; -dGx_p; 0]; 71 | end 72 | end 73 | 74 | [UBar, rho0, sigma0, errFlag] = calc_ubar(xx, yy, zz, xd, yd, zd, ... 75 | Object, rho0, sigma0, useOptimizer, delta_g, C, sf, t); 76 | if errFlag == 1 77 | break 78 | end 79 | Wp(:,t+1) = Wp(:,t) + UBar * dt; 80 | end 81 | % t = t+1; 82 | end 83 | Wp = Wp(:,1:t); 84 | Paths{L,rt} = Wp; % Save into cell array 85 | if errFlag == 0 %&& all([Object(:).Gamma] > 0.5) 86 | foundPath = 1; 87 | else 88 | foundPath = 0; 89 | end 90 | 91 | case 2 % simulate by reaching distance 92 | 93 | t = 1; 94 | while true 95 | Wp(:,t) = real(Wp(:,t)); 96 | xx = Wp(1,t); 97 | yy = Wp(2,t); 98 | zz = Wp(3,t); 99 | 100 | if t>1000 101 | break 102 | end 103 | Object = create_scene(scene, Object, xx, yy, zz, rt); 104 | if norm([xx yy zz] - [xd yd zd]) < targetThresh 105 | % disp('Target destination reached!') 106 | Wp = Wp(:,1:t); 107 | Paths{L,rt} = Wp; % Save into cell array 108 | foundPath = 1; 109 | break 110 | else 111 | % --------------- Weather constraints ------------ 112 | if k~=0 113 | omega = weatherMat(xx+1, yy+101); 114 | dwdx_now = dwdx(xx+1, yy+101); 115 | dwdy_now = dwdy(xx+1, yy+101); 116 | 117 | for j = 1:Param.numObj 118 | Gm = Object(j).Gamma; 119 | dGdx = Object(j).n(1); 120 | dGdy = Object(j).n(2); 121 | dGdz = Object(j).n(3); 122 | dGx_p = dGdx + k*exp( (B_L - omega)/(B_L - B_U) * log((Gm-1)/k +1) ) * ... 123 | ( log((Gm-1)/k +1)/(B_L-B_U) * dwdx_now - ((B_L-omega)/((Gm-1+k)*(B_L - B_U))) *dGdx ); 124 | 125 | dGy_p = dGdy + k*exp( (B_L - omega)/(B_L - B_U) * log((Gm-1)/k +1) ) * ... 126 | ( log((Gm-1)/k+1)/(B_L-B_U) * dwdy_now - ((B_L-omega)/((Gm-1+k)*(B_L - B_U))) *dGdy ); 127 | 128 | dGz_p = dGdz + k*exp( (B_L - omega)/(B_L - B_U) * log((Gm-1)/k +1) ) * ... 129 | ( -((B_L-omega)/((Gm-1+k)*(B_L - B_U))) *dGdz ); 130 | 131 | 132 | Object(j).Gamma = Object(j).Gamma - k* (exp( (B_L - omega)/(B_L - B_U) * log((Gm-1)/k +1) ) -1); 133 | 134 | Object(j).n = [dGx_p; dGy_p; dGz_p]; 135 | Object(j).t = [dGy_p; -dGx_p; 0]; 136 | end 137 | end 138 | 139 | [UBar, rho0, sigma0, errFlag] = calc_ubar(xx, yy, zz, xd, yd, zd, ... 140 | Object, rho0, sigma0, useOptimizer, delta_g, C, sf, t); 141 | % if errFlag == 1 142 | % break 143 | % end 144 | Wp(:,t+1) = Wp(:,t) + UBar * dt; 145 | end 146 | t = t+1; 147 | end 148 | Wp = Wp(:,1:t); 149 | Paths{L,rt} = Wp; % Save into cell array 150 | % if errFlag == 0 151 | % foundPath = 1; 152 | % else 153 | % foundPath = 0; 154 | % end 155 | end 156 | 157 | %======================= post-Calculation ============================= 158 | if foundPath == 1 159 | waypoints = Paths{1,rt}; 160 | % waypoints = Paths{1,1}'; % Calculate pairwise distances between waypoints 161 | % differences = diff(waypoints); % the differences between consecutive waypoints 162 | % squaredDistances = sum(differences.^2, 2); 163 | % 164 | % % Calculate the total path length 165 | % totalLength = sum(sqrt(squaredDistances)); 166 | 167 | 168 | differences = diff(waypoints, 1, 2); % Calculate differences between consecutive points 169 | totalLength = sum(sqrt(sum(differences.^2, 1))); % Calculate Euclidean distance and sum 170 | 171 | % Display the total path length 172 | if showDisp 173 | fprintf('Total path length: %.2f m\n', totalLength); 174 | fprintf('Total flight time: %.2f s\n', totalLength/C); 175 | end 176 | else 177 | totalLength = 0; 178 | end 179 | 180 | end 181 | 182 | function [UBar, rho0, sigma0, errFlag] = calc_ubar(X, Y, Z, xd, yd, zd, Obj, rho0, sigma0, useOptimizer, delta_g, C, sf, time) 183 | 184 | dist = sqrt((X - xd)^2 + (Y - yd)^2 + (Z - zd)^2); 185 | 186 | u = -[C*(X - xd)/dist, C*(Y - yd)/dist, C*(Z - zd)/dist]'; 187 | 188 | %% Pre-allocation 189 | numObj = size(Obj,2); 190 | Mm = zeros(3); 191 | sum_w = 0; 192 | errFlag = 0; 193 | 194 | for j = 1:numObj 195 | 196 | % Reading Gamma for each object 197 | Gamma = Obj(j).Gamma; 198 | 199 | % Unit normal vector and Unit tangential vector 200 | n = Obj(j).n; 201 | t = Obj(j).t; 202 | 203 | % Object Distance from UAV 204 | x0 = Obj(j).origin(1); 205 | y0 = Obj(j).origin(2); 206 | z0 = Obj(j).origin(3); 207 | 208 | dist_obj = sqrt((X - x0)^2 + (Y - y0)^2 + (Z - z0)^2); 209 | 210 | % Modular Matrix (Perturbation Matrix 211 | ntu = n' * u; 212 | if ntu < 0 || sf == 1 213 | % ---- optimize the rho0, sigma0 for each object 214 | if useOptimizer == 2 215 | if mod(time,5)==0 % optimize every 5 waypoints 216 | [rho0, sigma0] = path_opt2(Gamma, n, t, u, dist, dist_obj, rho0, sigma0) 217 | end 218 | end 219 | % --------------------------------------------------- 220 | 221 | % if useOptimizer == 0 222 | % Add Gap Constraint 223 | Rstar = Obj(j).Rstar; 224 | rho0_star = log(abs(Gamma))/(log(abs(Gamma - ((Rstar + delta_g)/Rstar)^2 + 1))) * rho0; 225 | rho = rho0_star * exp(1 - 1/(dist_obj * dist)); 226 | % else 227 | % % Without SafeGuard 228 | % rho = rho0 * exp(1 - 1/(dist_obj * dist)); 229 | % end 230 | 231 | sigma = sigma0 * exp(1 - 1/(dist_obj * dist)); 232 | 233 | M = eye(3) - n*n'/(abs(Gamma)^(1/rho)*(n')*n)... 234 | + t*n'/(abs(Gamma)^(1/sigma)*norm(t)*norm(n)); % tao is removed for now 235 | elseif ntu >= 0 && sf == 0 236 | M = eye(3); 237 | else 238 | errFlag = 1; 239 | UBar = u; 240 | return 241 | break 242 | end 243 | 244 | % Weight 245 | w = 1; 246 | for i = 1:numObj 247 | if i == j 248 | continue 249 | else 250 | w = w * (Obj(i).Gamma - 1)/... 251 | ((Obj(j).Gamma - 1) + (Obj(i).Gamma - 1)); 252 | end 253 | end 254 | sum_w = sum_w + w; 255 | 256 | % Saving to Field 257 | Obj(j).n = n; 258 | Obj(j).t = t; 259 | Obj(j).dist = dist_obj; 260 | % Obj(j).rho = rho; 261 | % Obj(j).sigma = sigma; 262 | Obj(j).M = M; 263 | Obj(j).w = w; 264 | 265 | end 266 | 267 | for j = 1:numObj 268 | Obj(j).w_tilde = Obj(j).w/sum_w; 269 | Mm = Mm + Obj(j).w_tilde * Obj(j).M; 270 | end 271 | 272 | UBar = Mm*u; 273 | 274 | function [rho0, sigma0] = path_opt2(Gamma, n, t, u, dist, dist_obj, rho0, sigma0) 275 | 276 | xg = [rho0; sigma0]; 277 | 278 | lower_bound_rho = 0.05; % <0.05 issue started to occur 279 | lower_bound_sigma = 0; 280 | 281 | upper_bound_rho = 2; 282 | upper_bound_sigma = 1; 283 | 284 | % Set up the optimization problem 285 | problem.objective = @(x) norm_ubar(x(1), x(2), Gamma, n, t, u, dist, dist_obj); 286 | problem.x0 = xg; 287 | problem.Aineq = []; 288 | problem.bineq = []; 289 | problem.Aeq = []; 290 | problem.beq = []; 291 | problem.lb = [lower_bound_rho; lower_bound_sigma]; 292 | problem.ub = [upper_bound_rho; upper_bound_sigma]; 293 | problem.nonlcon = []; 294 | problem.solver = 'fmincon'; % specify the solver 295 | problem.options = optimoptions('fmincon', ... 296 | 'Algorithm', 'interior-point', ... % option2: sqp 297 | 'Display', 'off'); 298 | 299 | % Call fmincon 300 | [xOpt, fval, exitflag, output] = fmincon(problem); 301 | % disp(output) 302 | rho0 = xOpt(1); 303 | sigma0 = xOpt(2); 304 | 305 | end 306 | 307 | end 308 | 309 | function Obj = create_scene(num, Obj, X, Y, Z, rt) 310 | switch num 311 | case 0 312 | Obj(1) = create_ceiling(100, 0, 50, 200, 10, Obj(1)); 313 | 314 | case 1 % Single object 315 | % Obj(1) = create_cone(100, 5, 0, 50, 80, Obj(1)); 316 | 317 | % Obj(1) = create_sphere(100, 5, 0, 50, Obj(1)); 318 | Obj(1) = create_sphere(100, 75, 0, 50, Obj(1)); 319 | 320 | 321 | case 2 % 2 objects 322 | Obj(1) = create_cylinder(60, 5, 0, 30, 50, Obj(1)); 323 | Obj(2) = create_sphere(120, -10, 0, 50, Obj(2)); 324 | 325 | % Obj(1) = create_cylinder(60, 100, 0, 30, 50, Obj(1)); 326 | % Obj(2) = create_sphere(120, -100, 0, 50, Obj(2)); 327 | 328 | case 3 % 3 objects 329 | Obj(1) = create_cylinder(60, 5, 0, 30, 50, Obj(1)); 330 | Obj(2) = create_sphere(120, -10, 0, 50, Obj(2)); 331 | Obj(3) = create_cone(168, 0, 0, 25, 80, Obj(3)); 332 | 333 | case 4 % single(complex) object 334 | Obj(1) = create_cylinder(100, 5, 0, 25, 200, Obj(1)); 335 | Obj(2) = create_pipe(60, 20, 60, 80, 5, Obj(2)); 336 | Obj(3) = create_pipe(130, -30, 30, 100, 50, Obj(3)); 337 | case 5 338 | Obj(1) = create_cylinder(50, -20, 0, 30, 50, Obj(1)); 339 | Obj(2) = create_cone(100, -20, 0, 30, 50, Obj(2)); 340 | Obj(3) = create_pipe(150, -20, 0, 30, 50, Obj(3)); 341 | 342 | case 12 % 12 objects 343 | Obj(1) = create_cylinder(100, 5, 0, 30, 50, Obj(1)); 344 | Obj(2) = create_pipe(140, 20, 0, 40,10, Obj(2)); 345 | Obj(3) = create_pipe(20, 20, 0, 24, 40, Obj(3)); 346 | Obj(4) = create_pipe(55, -20, 0, 28, 50, Obj(4)); 347 | Obj(5) = create_sphere(53, -60, 0, 50, Obj(5)); 348 | Obj(6) = create_pipe(150, -80, 0, 40, 50, Obj(6)); 349 | Obj(7) = create_cone(100, -35, 0, 50,45, Obj(7)); 350 | Obj(8) = create_cone(170, 2, 0, 20,50, Obj(8)); 351 | Obj(9) = create_cone(60, 35, 0, 50,30, Obj(9)); 352 | Obj(10) = create_cylinder(110, 70, 0, 60, 50, Obj(10)); 353 | Obj(11) = create_pipe(170, 60, 0, 40, 27, Obj(11)); 354 | Obj(12) = create_cone(150, -30, 0, 32, 45, Obj(12)); 355 | case 7 % 7 objects 356 | Obj(1) = create_cone(60,8, 0, 70, 50, Obj(1)); 357 | Obj(2) = create_cone(100,-24, 0, 89, 100, Obj(2)); 358 | Obj(3) = create_cone(160,40, -4, 100, 30, Obj(3)); 359 | Obj(4) = create_cone(100,100, -10, 150, 100, Obj(4)); 360 | Obj(5) = create_cone(180,-70, -10, 150, 20, Obj(5)); 361 | Obj(6) = create_cone(75,-75, -10, 150, 40, Obj(6)); 362 | Obj(7) = create_cylinder(170, -6, 0, 34, 100, Obj(7)); 363 | case 41 364 | % Oy = -50 + 2*single(rt); 365 | % Ox = 90 - 2*single(rt); 366 | Obj(1) = create_cylinder(100 + 50*sin(rt/8), 0 + 50*cos(rt/8), 0, 20, 80, Obj(1)); 367 | Obj(2) = create_sphere(100, 0, 0, 30, Obj(2)); 368 | Obj(3) = create_cylinder(100 - 50*sin(rt/8), 0 - 50*cos(rt/8), 0, 20, 50, Obj(3)); 369 | 370 | case 42 371 | % Original 372 | % Oy1 = -5 + 60*cos(0.4*single(rt)); 373 | % Oy2 = -20 - 20*sin(0.8*single(rt)); 374 | % Oz2 = 60 + 20*cos(0.8*single(rt)); 375 | % Obj(1) = create_cylinder(60, 5, 0, 30, 50, Obj(1)); 376 | % Obj(2) = create_cylinder(110, -10, 0, 25, 80,Obj(2)); 377 | % Obj(3) = create_cylinder(80, Oy1, 0, 20, 60, Obj(3)); 378 | % Obj(4) = create_sphere(160, Oy2, Oz2, 30, Obj(4)); 379 | 380 | % New 381 | Oy1 = -5 + 60*cos(0.4*single(rt)); 382 | Oy2 = -20 - 20*sin(0.8*single(rt)); 383 | Oz2 = 60 + 20*cos(0.8*single(rt)); 384 | Obj(1) = create_cylinder(40, 5, 0, 30, 40, Obj(1)); 385 | Obj(2) = create_cone(120, -10, 0, 25, 80,Obj(2)); 386 | Obj(3) = create_cylinder(80, Oy1, 0, 10, 60, Obj(3)); 387 | Obj(4) = create_sphere(160, Oy2, Oz2, 20, Obj(4)); 388 | case 44 389 | Oy1 = 0 + 60*sin(0.7*single(rt)); 390 | Oy2 = 0 + 60*cos(0.7*single(rt)); 391 | shift = 40*sin(0.5*single(rt)); 392 | Obj(1) = create_cylinder(40, 5, 0, 30, 80, Obj(1)); 393 | Obj(2) = create_pipe(40, -50, 0, 50, 30, Obj(2)); 394 | Obj(3) = create_pipe(150, 50, 0, 40, 60, Obj(3)); 395 | Obj(4) = create_pipe(150,-10, 0, 40, 80, Obj(4)); 396 | Obj(5) = create_pipe(110, Oy1, 0, 20, 50, Obj(5)); 397 | Obj(6) = create_pipe(80, Oy2, 0, 30, 30, Obj(6)); 398 | Obj(7) = create_sphere(100 + shift, 0 + shift, 60, 30, Obj(7)); 399 | 400 | case 69 401 | Obj(1) = create_cylinder(100, 5, 0, 30, 80, Obj(1)); 402 | Obj(2) = create_sphere(100, 30, 0, 40, Obj(2)); 403 | Obj(3) = create_sphere(100, -20, 0, 40, Obj(3)); 404 | Obj(4) = create_sphere(100, 5, 80, 30, Obj(4)); 405 | case 6969 406 | Obj(1) = create_sphere(100 + 30*sin(rt/8), 0 + 30*cos(rt/8), 0, 40, Obj(1)); 407 | Obj(2) = create_cylinder(100, 0, 0, 40, 80, Obj(2)); 408 | Obj(3) = create_sphere(100 - 30*sin(rt/8), 0 - 30*cos(rt/8), 0, 40, Obj(3)); 409 | end 410 | 411 | function Obj = create_sphere(x0, y0, z0, D, Obj) 412 | 413 | a = D/2; b = D/2; c = D/2; % Object's axis length 414 | p = 1; q = 1; r = 1; % Index parameters 415 | 416 | % Object Shape Equation 417 | Gamma = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 418 | % Differential 419 | [dGdx, dGdy, dGdz] = calc_dG(); 420 | 421 | n = [dGdx; dGdy; dGdz]; 422 | t = [dGdy; -dGdx; 0]; 423 | %----------------------------------------------------------------------- 424 | 425 | % Save to Field 426 | Obj.origin(rt,:) = [x0, y0, z0]; 427 | Obj.Gamma = Gamma; 428 | Obj.n = n; 429 | Obj.t = t; 430 | Obj.a = a; 431 | Obj.b = b; 432 | Obj.c = c; 433 | Obj.p = p; 434 | Obj.q = q; 435 | Obj.r = r; 436 | Obj.Rstar = min([a,b,c]); 437 | function [dGdx, dGdy, dGdz] = calc_dG() 438 | dGdx = (2*p*((X - x0)/a).^(2*p - 1))/a; 439 | dGdy = (2*q*((Y - y0)/b).^(2*q - 1))/b; 440 | dGdz = (2*r*((Z - z0)/c).^(2*r - 1))/c; 441 | end 442 | end 443 | 444 | function Obj = create_cylinder(x0, y0, z0, D, h, Obj) 445 | 446 | a = D/2; b = D/2; c = h; % Object's axis length 447 | p = 1; q = 1; r = 4; % Index parameters 448 | 449 | % Object Shape Equation 450 | Gamma = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 451 | 452 | % Differential 453 | [dGdx, dGdy, dGdz] = calc_dG(); 454 | 455 | n = [dGdx; dGdy; dGdz]; 456 | t = [dGdy; -dGdx; 0]; 457 | 458 | 459 | % Save to Field 460 | Obj.origin(rt,:) = [x0, y0, z0]; 461 | Obj.Gamma = Gamma; 462 | Obj.n = n; 463 | Obj.t = t; 464 | Obj.a = a; 465 | Obj.b = b; 466 | Obj.c = c; 467 | Obj.p = p; 468 | Obj.q = q; 469 | Obj.r = r; 470 | Obj.Rstar = min([a,b,c]); 471 | function [dGdx, dGdy, dGdz] = calc_dG() 472 | dGdx = (2*p*((X - x0)/a).^(2*p - 1))/a; 473 | dGdy = (2*q*((Y - y0)/b).^(2*q - 1))/b; 474 | dGdz = (2*r*((Z - z0)/c).^(2*r - 1))/c; 475 | end 476 | end 477 | 478 | function Obj = create_cone(x0, y0, z0, D, h, Obj) 479 | 480 | a = D/2; b = D/2; c = h; % Object's axis length 481 | p = 1; q = 1; r = 0.5; % Index parameters 482 | 483 | % Object Shape Equation 484 | Gamma = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 485 | 486 | % Differential 487 | [dGdx, dGdy, dGdz] = calc_dG(); 488 | 489 | % n and t 490 | n = [dGdx; dGdy; dGdz]; 491 | t = [dGdy; -dGdx; 0]; 492 | 493 | % Save to Field 494 | Obj.origin(rt,:) = [x0, y0, z0]; 495 | Obj.Gamma = Gamma; 496 | Obj.n = n; 497 | Obj.t = t; 498 | Obj.a = a; 499 | Obj.b = b; 500 | Obj.c = c; 501 | Obj.p = p; 502 | Obj.q = q; 503 | Obj.r = r; 504 | Obj.Rstar = min([a,b,c]); 505 | function [dGdx, dGdy, dGdz] = calc_dG() 506 | dGdx = (2*p*((X - x0)/a).^(2*p - 1))/a; 507 | dGdy = (2*q*((Y - y0)/b).^(2*q - 1))/b; 508 | dGdz = (2*r*((Z - z0)/c).^(2*r - 1))/c; 509 | end 510 | end 511 | 512 | function Obj = create_pipe(x0, y0, z0, D, h, Obj) 513 | 514 | a = D/2; b = D/2; c = h; % Object's axis length 515 | p = 2; q = 2; r = 2; % Index parameters 516 | 517 | % Object Shape Equation 518 | Gamma = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 519 | 520 | % Differential 521 | [dGdx, dGdy, dGdz] = calc_dG(); 522 | 523 | % n and t 524 | n = [dGdx; dGdy; dGdz]; 525 | t = [dGdy; -dGdx; 0]; 526 | 527 | % Save to Field 528 | Obj.origin(rt,:) = [x0, y0, z0]; 529 | Obj.Gamma = Gamma; 530 | Obj.n = n; 531 | Obj.t = t; 532 | Obj.a = a; 533 | Obj.b = b; 534 | Obj.c = c; 535 | Obj.p = p; 536 | Obj.q = q; 537 | Obj.r = r; 538 | Obj.Rstar = min([a,b,c]); 539 | function [dGdx, dGdy, dGdz] = calc_dG() 540 | dGdx = (2*p*((X - x0)/a).^(2*p - 1))/a; 541 | dGdy = (2*q*((Y - y0)/b).^(2*q - 1))/b; 542 | dGdz = (2*r*((Z - z0)/c).^(2*r - 1))/c; 543 | end 544 | end 545 | 546 | function Obj = create_ceiling(x0, y0, z0, D, h, Obj) 547 | 548 | z0 = z0 + h; 549 | a = D/2; b = D/2; c = h; % Object's axis length 550 | p = 20; q = 20; r = 20; % Index parameters 551 | 552 | % Object Shape Equation 553 | Gamma = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 554 | 555 | % Differential 556 | [dGdx, dGdy, dGdz] = calc_dG(); 557 | 558 | % n and t 559 | n = [dGdx; dGdy; dGdz]; 560 | t = [dGdy; -dGdx; 0]; 561 | 562 | % Save to Field 563 | Obj.origin(rt,:) = [x0, y0, z0]; 564 | Obj.Gamma = Gamma; 565 | Obj.n = n; 566 | Obj.t = t; 567 | Obj.a = a; 568 | Obj.b = b; 569 | Obj.c = c; 570 | Obj.p = p; 571 | Obj.q = q; 572 | Obj.r = r; 573 | Obj.Rstar = min([a,b,c]); 574 | function [dGdx, dGdy, dGdz] = calc_dG() 575 | dGdx = (2*p*((X - x0)/a).^(2*p - 1))/a; 576 | dGdy = (2*q*((Y - y0)/b).^(2*q - 1))/b; 577 | dGdz = (2*r*((Z - z0)/c).^(2*r - 1))/c; 578 | end 579 | end 580 | 581 | 582 | end 583 | -------------------------------------------------------------------------------- /PlotObject.m: -------------------------------------------------------------------------------- 1 | function [Gamma, Gamma_star] = PlotObject(Object, Rg, rt, rtsim, X, Y, Z, Gamma, Gamma_star) 2 | for j = 1:size(Object,2) 3 | x0 = Object(j).origin(rt, 1); 4 | y0 = Object(j).origin(rt, 2); 5 | z0 = Object(j).origin(rt, 3); 6 | a = Object(j).a; 7 | b = Object(j).b; 8 | c = Object(j).c; 9 | p = Object(j).p; 10 | q = Object(j).q; 11 | r = Object(j).r; 12 | 13 | Rstar = Object(j).Rstar; 14 | 15 | Gamma(X, Y, Z) = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 16 | Gamma_star(X, Y, Z) = Gamma - ( (Rstar + Rg)/Rstar )^2 + 1; 17 | 18 | % if rtsim > 1 19 | % fimplicit3(Gamma == 1,'EdgeColor','k','FaceAlpha',1,'MeshDensity',20), hold on 20 | % fimplicit3(Gamma_star == 1, 'EdgeColor','k','FaceAlpha',0,'MeshDensity',20) 21 | % else 22 | fimplicit3(Gamma == 1,'EdgeColor','none','FaceAlpha',1,'MeshDensity',100, 'FaceColor', 'w'), hold on 23 | fimplicit3(Gamma_star == 1, 'EdgeColor','none','FaceAlpha',0.2,'MeshDensity',100, 'FaceColor', 'w') 24 | % end 25 | 26 | xlim([0 200]) 27 | ylim([-100 100]) 28 | zlim([0 100]) 29 | end 30 | 31 | end -------------------------------------------------------------------------------- /PlotPath.m: -------------------------------------------------------------------------------- 1 | function pltPath = PlotPath(rt, Paths, Xini, Yini, Zini, destin, multiTarget) 2 | if multiTarget 3 | pltPath(1) = plot3(Paths{1,rt}(1,:), Paths{1,rt}(2,:), Paths{1,rt}(3,:),'b', 'LineWidth', 1.8); 4 | hold on 5 | pltPath(2) = plot3(Paths{2,rt}(1,:), Paths{2,rt}(2,:), Paths{2,rt}(3,:),'b', 'LineWidth', 1.8); 6 | pltPath(3) = plot3(Paths{3,rt}(1,:), Paths{3,rt}(2,:), Paths{3,rt}(3,:),'b', 'LineWidth', 1.8); 7 | pltPath(4) = plot3(Paths{4,rt}(1,:), Paths{4,rt}(2,:), Paths{4,rt}(3,:),'b', 'LineWidth', 1.8); 8 | pltPath(5) = plot3(Paths{5,rt}(1,:), Paths{5,rt}(2,:), Paths{5,rt}(3,:),'b', 'LineWidth', 1.8); 9 | pltPath(6) = plot3(Paths{6,rt}(1,:), Paths{6,rt}(2,:), Paths{6,rt}(3,:),'b', 'LineWidth', 1.8); 10 | pltPath(7) = plot3(Paths{7,rt}(1,:), Paths{7,rt}(2,:), Paths{7,rt}(3,:),'b', 'LineWidth', 1.8); 11 | pltPath(8) = plot3(Paths{8,rt}(1,:), Paths{8,rt}(2,:), Paths{8,rt}(3,:),'b', 'LineWidth', 1.8); 12 | pltPath(9) = plot3(Paths{9,rt}(1,:), Paths{9,rt}(2,:), Paths{9,rt}(3,:),'b', 'LineWidth', 1.8); 13 | scatter3(Xini, Yini, Zini, 'filled', 'r') 14 | scatter3(destin(1,1),destin(1,2),destin(1,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 15 | scatter3(destin(2,1),destin(2,2),destin(2,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 16 | scatter3(destin(3,1),destin(3,2),destin(3,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 17 | scatter3(destin(4,1),destin(4,2),destin(4,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 18 | scatter3(destin(5,1),destin(5,2),destin(5,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 19 | scatter3(destin(6,1),destin(6,2),destin(6,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 20 | scatter3(destin(7,1),destin(7,2),destin(7,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 21 | scatter3(destin(8,1),destin(8,2),destin(8,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 22 | scatter3(destin(9,1),destin(9,2),destin(9,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 23 | else 24 | if length(Paths{1,rt}) ~= length(Paths{1,1}) 25 | lineColor = 'r'; 26 | else 27 | lineColor = 'b'; 28 | end 29 | pltPath = plot3(Paths{1,rt}(1,:), Paths{1,rt}(2,:), Paths{1,rt}(3,:),'b--', 'LineWidth', 1.8); 30 | hold on 31 | % axis equal, grid on, grid minor 32 | scatter3(Paths{1,rt}(1,end), Paths{1,rt}(2,end), Paths{1,rt}(3,end),'s', 'sizedata', 150, 'LineWidth', 1.5, 'MarkerEdgeColor', lineColor) 33 | scatter3(Xini, Yini, Zini, 'filled', 'r', 'xr', 'sizedata', 150) 34 | scatter3(destin(1,1),destin(1,2),destin(1,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 35 | end 36 | 37 | xlim([0 200]) 38 | ylim([-100 100]) 39 | zlim([0 100]) 40 | % xlabel('X [m]'); ylabel('Y [m]'); zlabel('Z [m]'); 41 | % hold off 42 | end -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Final version for IRP Dynamic Autorouting\ 2 | Results Video: https://youtu.be/XtmcNa-w4-0?si=V0FAj7HmrgcvlQuK 3 | 4 | # This version 5 | - [x] Constraints matrix has been introduced (e.g. Weather data) 6 | - [x] Path following and UAV dynamics have been introduced 7 | - [x] Evaluation methods are considered (e.g. real-time performance) 8 | - [x] Added Position-Holding feature 9 | - [x] Added Global vs Local path adaptability based on scenarios 10 | 11 | # Not finished 12 | - [ ] Fuel consumption and flight time cost not considered 13 | - [ ] Overlapped shape problem not fixed 14 | - [ ] Stagnation problem not fixed (e.g. when path is orthogonal to cylinder surface) 15 | 16 | 17 | # Problem Noticed 18 | - **Problem1**: The effect of overlapped object ruined the path planning result 19 | - **Problem2**: The Barrier of the cylinder, cone, and parallel piped are not uniformly enclosed -> This is because of how the safeguard function is derived from sphere 20 | - **Problem3**: After restructuring the code, the Global Optimizer ran a lot slower (but maybe more accurate than `verion2_legacy` since the objective function considers the SafeGuard and identical to the IFDS algorithm called in the main file) 21 | 22 | # Possible Solutions 23 | - **Problem1**: Follow the literature to solve the overlapped problem 24 | - **Problem2**: Derive the barrier for the cylinder case, or even for the general case 25 | 26 | # Results 27 | ![image](https://github.com/komxun/IFDS-Algorithm/assets/133139057/078c3a5d-717b-4cf6-a459-22dee9d5c450) 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /WeatherMat_3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/komxun/IFDS-Algorithm/9906efe91a2b0ad577fa04da683390ffecc82094/WeatherMat_3.mat -------------------------------------------------------------------------------- /WeatherMat_321.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/komxun/IFDS-Algorithm/9906efe91a2b0ad577fa04da683390ffecc82094/WeatherMat_321.mat -------------------------------------------------------------------------------- /WeatherMat_8187.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/komxun/IFDS-Algorithm/9906efe91a2b0ad577fa04da683390ffecc82094/WeatherMat_8187.mat -------------------------------------------------------------------------------- /Weather_map_Generator.m: -------------------------------------------------------------------------------- 1 | clc, clear, close all 2 | 3 | saveVid = 0; % 1: save 2: no 4 | 5 | fontSize = 24; 6 | 7 | % number of point 8 | Nx = 200; 9 | Ny = 200; 10 | tmax = 30; 11 | 12 | weatherMat = zeros(Nx, Ny, tmax); 13 | % numSeed = 8187; 14 | numSeed = 69; 15 | 16 | figure(88) 17 | axis equal tight 18 | colormap turbo 19 | 20 | for t = 1:tmax 21 | f = 0.015; % Increase the randomness control parameter 22 | % f = 0.05; % Lower = higher frequency 23 | rand('seed', numSeed) 24 | 25 | nx1 = 1 + floor(0.999*f*Nx); 26 | nx2 = Nx - nx1 + 1; 27 | ny1 = 1 + floor(0.999*f*Nx); 28 | ny2 = Nx - nx1 + 1; 29 | 30 | F = rand(Nx,Ny); 31 | F = fft2(F); % 2D FFT 32 | F(nx1:nx2 - 0.5*sin(t/2), :) = 0; 33 | F(:, ny1:ny2 - 0.5*cos(t/2)) = 0; 34 | 35 | F = real(ifft2(F)); % 2D iFFT 36 | 37 | % Scale the values to the desired range (0.2 - 1) 38 | F = (F - min(F(:))) ./ (max(F(:)) - min(F(:))); 39 | F = F * 1.22; 40 | % F = F * 1.15; 41 | F(F>1) = 1; 42 | 43 | imagesc(F) 44 | 45 | hold on 46 | % [C2,h2] = contourf(1:200, 1:200, F, [1, 1], 'FaceAlpha',0,... 47 | % 'LineColor', 'w', 'LineWidth', 2); 48 | 49 | % clabel(C2,h2,'FontSize',15,'Color','w') 50 | axis equal tight 51 | 52 | 53 | title("numSeed = " + num2str(numSeed) + ", f = " + num2str(f)... 54 | + num2str(t,', time = %4.1f s')) 55 | set(gca, 'YDir', 'normal', 'FontSize', fontSize) 56 | colorbar 57 | pause(0.01) 58 | hold off 59 | 60 | % Video saving 61 | if saveVid 62 | frm(t) = getframe(gcf) ; 63 | drawnow 64 | end 65 | 66 | weatherMat(:,:,t) = F; 67 | end 68 | 69 | set(gca, 'YDir', 'normal', 'LineWidth', 2) 70 | fileName = "WeatherMat_" + num2str(numSeed) +".mat"; 71 | % save(fileName, 'weatherMat') 72 | 73 | if saveVid 74 | video_name = "WeatherMat_" + num2str(numSeed) + ".avi"; 75 | % create the video writer with 1 fps 76 | writerObj = VideoWriter(video_name); 77 | writerObj.FrameRate = 30; 78 | % set the seconds per image 79 | % open the video writer 80 | open(writerObj); 81 | % write the frames to the video 82 | for i=1:length(frm) 83 | % convert the image to a frame 84 | frame = frm(i) ; 85 | writeVideo(writerObj, frame); 86 | end 87 | % close the writer object 88 | close(writerObj); 89 | end 90 | 91 | 92 | 93 | -------------------------------------------------------------------------------- /allTraj_opt.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/komxun/IFDS-Algorithm/9906efe91a2b0ad577fa04da683390ffecc82094/allTraj_opt.mat -------------------------------------------------------------------------------- /allTraj_opt2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/komxun/IFDS-Algorithm/9906efe91a2b0ad577fa04da683390ffecc82094/allTraj_opt2.mat -------------------------------------------------------------------------------- /for_experimenting/test_optimize_dist_fmincon.m: -------------------------------------------------------------------------------- 1 | clc, clear, close all 2 | 3 | % Initial condition 4 | % Set-up Parameters 5 | tsim = uint16(400); % [s] simulation time for the path 6 | rtsim = 1; % [s] (50) time for the whole scenario 7 | dt = 0.1; % [s] simulation time step 8 | C = 30; % [m/s] UAV cruising speed 9 | targetThresh = 2.5; % [m] allowed error for final target distance 10 | simMode = uint8(1); % 1: by time, 2: by target distance 11 | multiTarget = uint8(0); % 1: multi-target 0: single-target 12 | scene = uint8(2); % Scenario selection 13 | % 1) 1 cone, 2) 1 complex object 14 | % 7) non-urban 12) urban environment 15 | 16 | % Starting location 17 | Xini = 0; 18 | Yini = 0; 19 | Zini = 0; 20 | 21 | % Target Destination 22 | Xfinal = 200; 23 | Yfinal = 0; 24 | Zfinal = 50; 25 | 26 | % Tuning Parameters 27 | sf = uint8(1); % Shape-following demand (1=on, 0=off) 28 | rho0 = 1.3; % Repulsive parameter (rho >= 0) 29 | sigma0 = 0.01; % Tangential parameter 30 | 31 | %----------- Note ------------- 32 | % Good: rho0 = 2, simga0 = 0.01 33 | % Algorihtm stills doesnt work for overlapped objects 34 | %------------------------------ 35 | 36 | % Save to table Param 37 | Param = table; 38 | Param.tsim = tsim; 39 | Param.rtsim = rtsim; 40 | Param.dt = dt; 41 | Param.C = C; 42 | Param.targetThresh = targetThresh; 43 | Param.simMode = simMode; 44 | Param.multiTarget = multiTarget; 45 | Param.scene = scene; 46 | Param.sf = sf; 47 | Param.Xini = Xini; 48 | Param.Yini = Yini; 49 | Param.Zini = Zini; 50 | Param.Xfinal = Xfinal; 51 | Param.Yfinal = Yfinal; 52 | Param.Zfinal = Zfinal; 53 | 54 | % Initial guess for decision variables rho and gamma 55 | initial_rho0 = 1; 56 | initial_sigma0 = 0.01; 57 | x0 = [initial_rho0; initial_sigma0]; 58 | 59 | lower_bound_rho = 0; 60 | lower_bound_sigma = 0; 61 | 62 | upper_bound_rho = 2; 63 | upper_bound_sigma = 1; 64 | 65 | % Set up the optimization problem 66 | problem.objective = @(x) path_dist_objective_v2(x(1), x(2), Param); 67 | problem.x0 = x0; 68 | problem.Aineq = []; 69 | problem.bineq = []; 70 | problem.Aeq = []; 71 | problem.beq = []; 72 | problem.lb = [lower_bound_rho; lower_bound_sigma]; 73 | problem.ub = [upper_bound_rho; upper_bound_sigma]; 74 | problem.nonlcon = []; 75 | problem.solver = 'fmincon'; % specify the solver 76 | problem.options = optimoptions('fmincon', ... 77 | 'Algorithm', 'interior-point', ... % option2: sqp 78 | 'Display', 'iter'); 79 | 80 | 81 | % Call fmincon 82 | [xOpt, fval, exitflag, output] = fmincon(problem); 83 | path_dist_objective_v2(rho0, sigma0, Param) 84 | %% Display the results 85 | disp('Optimized decision variables (rho0, sigma0):'); 86 | disp(xOpt); 87 | disp('Optimized path distance (m):'); 88 | disp(fval); 89 | disp("Original path distance = " + path_dist_objective_v2(x0(1),x0(2), Param) + " m") 90 | -------------------------------------------------------------------------------- /for_experimenting/weather_test.m: -------------------------------------------------------------------------------- 1 | clc, clear, close all 2 | % Load data 3 | % load WeatherMat_0.mat % 200 x 200 x (t=100) matrix 4 | load WeatherMat_321.mat 5 | 6 | % Set-up Parameters 7 | tsim = uint16(400); % [s] simulation time for the path 8 | rtsim = 1; % [s] (max 100) time for the whole scenario 9 | dt = 0.1; % [s] simulation time step 10 | C = 30; % [m/s] UAV cruising speed 11 | targetThresh = 2.5; % [m] allowed error for final target distance 12 | simMode = uint8(1); % 1: by time, 2: by target distance 13 | multiTarget = uint8(0); % 1: multi-target 0: single-target 14 | scene = uint8(0); % Scenario selection 15 | % 0) 1 cone, 2) 1 complex object 16 | % 7) non-urban 12) urban environment 17 | 18 | useOptimizer = 0; % 0:Off 1:Global optimized 2: Local optimized 19 | 20 | % Starting location 21 | Xini = 0; 22 | Yini = 0; 23 | Zini = 0; 24 | 25 | % Target Destination 26 | Xfinal = 200; 27 | Yfinal = 0; 28 | Zfinal = 10; 29 | 30 | % Tuning Parameters 31 | sf = uint8(0); % Shape-following demand (1=on, 0=off) 32 | rho0 = 0.5; % Repulsive parameter (rho >= 0) 33 | sigma0 = 0.01; % Tangential parameter 34 | Rg = 5; % [m] minimum allowed gap distance 35 | 36 | x_guess = [rho0; sigma0]; 37 | 38 | %----------- Note ------------- 39 | % Good: rho0 = 2, simga0 = 0.01 40 | % The algorihtm still doesnt work for overlapped objects 41 | %------------------------------ 42 | 43 | % Save to table Param 44 | Param = table; 45 | Param.tsim = tsim; 46 | Param.rtsim = rtsim; 47 | Param.dt = dt; 48 | Param.C = C; 49 | Param.targetThresh = targetThresh; 50 | Param.simMode = simMode; 51 | Param.multiTarget = multiTarget; 52 | Param.scene = scene; 53 | Param.sf = sf; 54 | Param.Rg = Rg; 55 | Param.rho0_initial = rho0; 56 | Param.sigma0_initial = sigma0; 57 | Param.Xini = Xini; 58 | Param.Yini = Yini; 59 | Param.Zini = Zini; 60 | Param.Xfinal = Xfinal; 61 | Param.Yfinal = Yfinal; 62 | Param.Zfinal = Zfinal; 63 | Param.useOptimizer = useOptimizer; 64 | 65 | % Structure Pre-allocation 66 | switch scene 67 | case 0, numObj = 1; 68 | case 1, numObj = 1; 69 | case 2, numObj = 2; 70 | case 3, numObj = 3; 71 | case 7, numObj = 7; 72 | case 12, numObj = 12; 73 | case 41, numObj = 1; 74 | case 42, numObj = 4; 75 | case 69, numObj = 4; 76 | end 77 | 78 | Object(numObj) = struct('origin',zeros(rtsim,3),'Gamma',0,'n',[],'t',[],... 79 | 'a',0,'b',0,'c',0,'p',0,'q',0,'r',0,'Rstar',0); 80 | 81 | disp(['Number of object: ' num2str(size(Object,2))]) 82 | if sf == 0, disp("Shape-following: Off") 83 | else, disp("Shape-following: On") 84 | end 85 | 86 | %% Original Fluid 87 | 88 | if multiTarget 89 | destin = [200 0 20; 90 | 200 20 20; 91 | 200 -20 20; 92 | 200 20 30; 93 | 200 -20 30; 94 | 200 0 30; 95 | 200 0 40; 96 | 200 20 40; 97 | 200 -20 40;]; 98 | else 99 | destin = [Xfinal Yfinal Zfinal]; 100 | end 101 | 102 | numLine = size(destin,1); 103 | disp("Generating paths for " + num2str(numLine) + " destinations . . .") 104 | disp("*Timer started*") 105 | timer = zeros(1,numLine); 106 | 107 | % Pre-allocate waypoints and path 108 | Wp = zeros(3, tsim+1); 109 | Paths = cell(numLine,rtsim); 110 | 111 | for rt = 1:rtsim 112 | tic 113 | Wp(:,1,rt) = [Xini; Yini; Zini]; % can change this to current uav pos 114 | 115 | %---------------Path Optimization----------------- 116 | switch useOptimizer 117 | case 0 118 | disp("Path optimization: Off") 119 | case 1 120 | disp("Path optimization: Global"); 121 | [rho0, sigma0] = path_optimizing(Param); 122 | case 2 123 | disp("Path optimization: Local") 124 | end 125 | %------------------------------------------------ 126 | for L = 1:numLine 127 | % disp("Calculating path for destination #" + num2str(L)) 128 | xd = (destin(L,1)); 129 | yd = (destin(L,2)); 130 | zd = (destin(L,3)); 131 | 132 | switch simMode 133 | case 1 % Simulate by time 134 | % disp("Simulating by time for " + num2str(tsim) + " s.") 135 | for t = 1:tsim 136 | xx = Wp(1,t); 137 | yy = Wp(2,t); 138 | zz = Wp(3,t); 139 | 140 | % --------------- Weather constraints ------------ 141 | omega = weatherMat(min(round(xx)+1,200),min(round(yy)+100+1,200),rt); 142 | 143 | 144 | Object = create_scene(scene, Object, xx, yy, zz, rt); 145 | 146 | %----------Modified Gamma considering Weather-------- 147 | Object.Gamma = 1+ Object.Gamma - exp(omega * log(Object.Gamma)); 148 | GAMMA_prime = Object.Gamma 149 | %---------------------------------------------------- 150 | 151 | [UBar, rho0, sigma0] = calc_ubar(xx, yy, zz, xd, yd, zd, Object, rho0, sigma0, Param, t); 152 | 153 | if norm([xx yy zz] - [xd yd zd]) < targetThresh 154 | % disp('Target destination reached!') 155 | Wp = Wp(:,1:t); 156 | Paths{L,rt} = Wp; % Save into cell array 157 | break 158 | else 159 | Wp(:,t+1) = Wp(:,t) + UBar * dt; 160 | end 161 | 162 | end 163 | 164 | case 2 % simulate by reaching distance 165 | disp("Simulating by distance until " + num2str(targetThresh) + " m error range") 166 | t = 1; 167 | 168 | while norm([Wp(1,t) Wp(2,t) Wp(3,t)] - double([xd yd zd])) > targetThresh 169 | 170 | xx = Wp(1,t); 171 | yy = Wp(2,t); 172 | zz = Wp(3,t); 173 | 174 | if n(xx,yy,zz)'*u(xx,yy,zz) < 0 || sf == 1 175 | % disp('case 1 activated') 176 | Wp(:,t+1) = Wp(:,t) + double(UBar(Wp(1,t), Wp(2,t), Wp(3,t))) * dt; 177 | 178 | elseif n(xx,yy,zz)'*u(xx,yy,zz) >= 0 && sf == 0 179 | % disp('case 2 activated') 180 | Wp(:,t+1) = Wp(:,t) + double(u(Wp(1,t), Wp(2,t), Wp(3,t))) * dt; 181 | end 182 | t = t+1; 183 | end 184 | % Removing extra space 185 | Wp = Wp(:,1:t,:); 186 | disp('Target destination reached!') 187 | end 188 | 189 | timer(L) = toc; 190 | end 191 | 192 | disp("Average computed time = " + num2str(mean(timer)) + " s") 193 | 194 | % Plotting the path 195 | figure(70) 196 | % set(gcf, 'Position', get(0, 'Screensize')); 197 | plot3(Paths{1,rt}(1,:), Paths{1,rt}(2,:), Paths{1,rt}(3,:),'b', 'LineWidth', 1.5) 198 | hold on, grid on, grid minor, axis equal tight 199 | scatter3(Xini, Yini, Zini, 'filled', 'r') 200 | scatter3(destin(1,1),destin(1,2),destin(1,3), 'xr') 201 | if multiTarget 202 | plot3(Paths{2,rt}(1,:), Paths{2,rt}(2,:), Paths{2,rt}(3,:),'b', 'LineWidth', 1.5) 203 | plot3(Paths{3,rt}(1,:), Paths{3,rt}(2,:), Paths{3,rt}(3,:),'b', 'LineWidth', 1.5) 204 | plot3(Paths{4,rt}(1,:), Paths{4,rt}(2,:), Paths{4,rt}(3,:),'b', 'LineWidth', 1.5) 205 | plot3(Paths{5,rt}(1,:), Paths{5,rt}(2,:), Paths{5,rt}(3,:),'b', 'LineWidth', 1.5) 206 | plot3(Paths{6,rt}(1,:), Paths{6,rt}(2,:), Paths{6,rt}(3,:),'b', 'LineWidth', 1.5) 207 | plot3(Paths{7,rt}(1,:), Paths{7,rt}(2,:), Paths{7,rt}(3,:),'b', 'LineWidth', 1.5) 208 | plot3(Paths{8,rt}(1,:), Paths{8,rt}(2,:), Paths{8,rt}(3,:),'b', 'LineWidth', 1.5) 209 | plot3(Paths{9,rt}(1,:), Paths{9,rt}(2,:), Paths{9,rt}(3,:),'b', 'LineWidth', 1.5) 210 | scatter3(destin(2,1),destin(2,2),destin(2,3), 'xr', 'sizedata', 100) 211 | scatter3(destin(3,1),destin(3,2),destin(3,3), 'xr', 'sizedata', 100) 212 | scatter3(destin(4,1),destin(4,2),destin(4,3), 'xr', 'sizedata', 100) 213 | scatter3(destin(5,1),destin(5,2),destin(5,3), 'xr', 'sizedata', 100) 214 | scatter3(destin(6,1),destin(6,2),destin(6,3), 'xr', 'sizedata', 100) 215 | scatter3(destin(7,1),destin(7,2),destin(7,3), 'xr', 'sizedata', 100) 216 | scatter3(destin(8,1),destin(8,2),destin(8,3), 'xr', 'sizedata', 100) 217 | scatter3(destin(9,1),destin(9,2),destin(9,3), 'xr', 'sizedata', 100) 218 | end 219 | imagesc(0:200, -100:100, weatherMat(:,:,rt)) 220 | colormap turbo 221 | colorbar 222 | title(num2str(rt,'time = %4.1f s')) 223 | xlim([0 200]) 224 | ylim([-100 100]) 225 | zlim([0 100]) 226 | % pause(0.05) 227 | xlabel('X [m]'); ylabel('Y [m]'); zlabel('Z [m]'); 228 | hold off 229 | 230 | % figure 231 | % subplot(3,1,1) 232 | % plot(Wp(1,:),'o-') 233 | % subplot(3,1,2) 234 | % plot(Wp(2,:),'o-') 235 | % subplot(3,1,3) 236 | % plot(Wp(3,:),'o-') 237 | end 238 | 239 | %% post-Calculation 240 | % Calculate pairwise distances between waypoints 241 | waypoints = Paths{1,1}'; 242 | 243 | % Calculate the differences between consecutive waypoints 244 | differences = diff(waypoints); 245 | 246 | % Calculate the squared distances for each coordinate 247 | squaredDistances = sum(differences.^2, 2); 248 | 249 | % Calculate the total path length 250 | totalLength = sum(sqrt(squaredDistances)); 251 | 252 | % Display the total path length 253 | fprintf('Total path length: %.2f m\n', totalLength); 254 | fprintf('Total flight time: %.2f s\n', totalLength/C); 255 | 256 | %% ----------------------- Plotting Results ------------------------------- 257 | animation = 1; 258 | 259 | syms X Y Z Gamma(X,Y,Z) Gamma_star(X,Y,Z) 260 | figure(69) 261 | % set(gcf, 'Position', get(0, 'Screensize')); 262 | view(-43,52) 263 | for rt = 1:rtsim 264 | 265 | if multiTarget 266 | plot_multi(rt, Paths, Xini, Yini, Zini, destin) 267 | else 268 | plot_single(rt, Paths, Xini, Yini, Zini, destin) 269 | end 270 | 271 | for j = 1:size(Object,2) 272 | x0 = Object(j).origin(rt, 1); 273 | y0 = Object(j).origin(rt, 2); 274 | z0 = Object(j).origin(rt, 3); 275 | a = Object(j).a; 276 | b = Object(j).b; 277 | c = Object(j).c; 278 | p = Object(j).p; 279 | q = Object(j).q; 280 | r = Object(j).r; 281 | 282 | Rstar = Object(j).Rstar; 283 | 284 | Gamma(X, Y, Z) = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 285 | Gamma_star(X, Y, Z) = Gamma - ( (Rstar + Rg)/Rstar )^2 + 1; 286 | % figure(69) 287 | fimplicit3(Gamma == 1,'EdgeColor','none','FaceAlpha',1,'MeshDensity',80), hold on 288 | colormap pink 289 | fimplicit3(Gamma_star == 1, 'EdgeColor','k','FaceAlpha',0.2,'MeshDensity',30) 290 | xlim([0 200]) 291 | ylim([-100 100]) 292 | zlim([0 100]) 293 | % hold on, grid on, grid minor, axis equal 294 | end 295 | 296 | imagesc(0:200, -100:100, weatherMat(:,:,rt)) 297 | title(['IFDS, \rho_0 = ' num2str(rho0) ', \sigma_0 = ' num2str(sigma0) ', SF = ' num2str(sf)]); 298 | subtitle(num2str(rt,'time = %4.1f s')) 299 | xlabel('X [m]'); ylabel('Y [m]'); zlabel('Z [m]'); 300 | 301 | hold off 302 | 303 | end 304 | 305 | 306 | % title(['IFDS, \rho_0 = ' num2str(rho0) ', \sigma_0 = ' num2str(sigma0)],... 307 | % 'FontSize',26); 308 | % subtitle(['SF = ' num2str(sf)], 'FontSize', 24) 309 | set(gca,'FontSize', 24) 310 | camlight 311 | 312 | %% ------------------------------Function--------------------------------- 313 | 314 | function [rho0, sigma0] = path_optimizing(Param) 315 | 316 | xg = [Param.rho0_initial; Param.sigma0_initial]; 317 | 318 | lower_bound_rho = 0.05; % <0.05 issue started to occur 319 | lower_bound_sigma = 0; 320 | 321 | upper_bound_rho = 2; 322 | upper_bound_sigma = 1; 323 | 324 | % Set up the optimization problem 325 | problem.objective = @(x) path_dist_objective_v2(x(1), x(2), Param); 326 | problem.x0 = xg; 327 | problem.Aineq = []; 328 | problem.bineq = []; 329 | problem.Aeq = []; 330 | problem.beq = []; 331 | problem.lb = [lower_bound_rho; lower_bound_sigma]; 332 | problem.ub = [upper_bound_rho; upper_bound_sigma]; 333 | problem.nonlcon = []; 334 | problem.solver = 'fmincon'; % specify the solver 335 | problem.options = optimoptions('fmincon', ... 336 | 'Algorithm', 'interior-point', ... % option2: sqp 337 | 'Display', 'off'); 338 | 339 | % Call fmincon 340 | [xOpt, fval, exitflag, output] = fmincon(problem); 341 | disp(output) 342 | rho0 = xOpt(1); 343 | sigma0 = xOpt(2); 344 | end 345 | 346 | 347 | function [UBar, rho0, sigma0] = calc_ubar(X, Y, Z, xd, yd, zd, Obj, rho0, sigma0, Param, time) 348 | 349 | 350 | % rho0 = Param.rho0_initial; 351 | % sigma0 = Param.sigma0_initial; 352 | 353 | useOptimizer = Param.useOptimizer; 354 | Rg = Param.Rg; 355 | C = Param.C; 356 | sf = Param.sf; 357 | dist = sqrt((X - xd)^2 + (Y - yd)^2 + (Z - zd)^2); 358 | 359 | u = -[C*(X - xd)/dist, C*(Y - yd)/dist, C*(Z - zd)/dist]'; 360 | 361 | %% Components 362 | numObj = size(Obj,2); 363 | 364 | 365 | Mm = zeros(3); 366 | sum_w = 0; 367 | 368 | for j = 1:numObj 369 | 370 | % Reading Gamma for each object 371 | Gamma = Obj(j).Gamma; 372 | 373 | % Unit normal vector and Unit tangential vector 374 | n = Obj(j).n; 375 | t = Obj(j).t; 376 | 377 | % Object Distance from UAV 378 | x0 = Obj(j).origin(1); 379 | y0 = Obj(j).origin(2); 380 | z0 = Obj(j).origin(3); 381 | 382 | dist_obj = sqrt((X - x0)^2 + (Y - y0)^2 + (Z - z0)^2); 383 | 384 | % Modular Matrix (Perturbation Matrix 385 | ntu = n' * u; 386 | if ntu < 0 || sf == 1 387 | % ---- optimize the rho0, sigma0 for each object 388 | if useOptimizer == 2 389 | if mod(time,5)==0 % optimize every 5 waypoints 390 | [rho0, sigma0] = path_opt2(Gamma, n, t, u, dist, dist_obj, rho0, sigma0); 391 | end 392 | end 393 | % --------------------------------------------------- 394 | % Add Gap Constraint 395 | Rstar = Obj(j).Rstar; 396 | rho0_star = log(abs(Gamma))/(log(abs(Gamma - ((Rstar + Rg)/Rstar)^2 + 1))) * rho0; 397 | %---------------------------------------------------- 398 | % rho = rho0 * exp(1 - 1/(dist_obj * dist)); 399 | rho_star = rho0_star * exp(1 - 1/(dist_obj * dist)); 400 | sigma = sigma0 * exp(1 - 1/(dist_obj * dist)); 401 | 402 | M = eye(3) - n*n'/(abs(Gamma)^(1/rho_star)*(n')*n)... 403 | + t*n'/(abs(Gamma)^(1/sigma)*norm(t)*norm(n)); % tao is removed for now 404 | elseif ntu >= 0 && sf == 0 405 | M = eye(3); 406 | end 407 | 408 | 409 | 410 | % Weight 411 | w = 1; 412 | for i = 1:numObj 413 | if i == j 414 | continue 415 | else 416 | w = w * (Obj(i).Gamma - 1)/... 417 | ((Obj(j).Gamma - 1) + (Obj(i).Gamma - 1)); 418 | end 419 | end 420 | sum_w = sum_w + w; 421 | 422 | % Saving to Field 423 | Obj(j).n = n; 424 | Obj(j).t = t; 425 | Obj(j).dist = dist_obj; 426 | % Obj(j).rho = rho; 427 | % Obj(j).sigma = sigma; 428 | Obj(j).M = M; 429 | Obj(j).w = w; 430 | 431 | end 432 | 433 | for j = 1:numObj 434 | Obj(j).w_tilde = Obj(j).w/sum_w; 435 | Mm = Mm + Obj(j).w_tilde * Obj(j).M; 436 | end 437 | 438 | UBar = Mm*u; 439 | 440 | function [rho0, sigma0] = path_opt2(Gamma, n, t, u, dist, dist_obj, rho0, sigma0) 441 | 442 | xg = [rho0; sigma0]; 443 | 444 | lower_bound_rho = 0.05; % <0.05 issue started to occur 445 | lower_bound_sigma = 0; 446 | 447 | upper_bound_rho = 2; 448 | upper_bound_sigma = 1; 449 | 450 | % Set up the optimization problem 451 | problem.objective = @(x) norm_ubar(x(1), x(2), Gamma, n, t, u, dist, dist_obj); 452 | problem.x0 = xg; 453 | problem.Aineq = []; 454 | problem.bineq = []; 455 | problem.Aeq = []; 456 | problem.beq = []; 457 | problem.lb = [lower_bound_rho; lower_bound_sigma]; 458 | problem.ub = [upper_bound_rho; upper_bound_sigma]; 459 | problem.nonlcon = []; 460 | problem.solver = 'fmincon'; % specify the solver 461 | problem.options = optimoptions('fmincon', ... 462 | 'Algorithm', 'interior-point', ... % option2: sqp 463 | 'Display', 'off'); 464 | 465 | % Call fmincon 466 | [xOpt, fval, exitflag, output] = fmincon(problem); 467 | % disp(output) 468 | rho0 = xOpt(1); 469 | sigma0 = xOpt(2); 470 | 471 | end 472 | 473 | end 474 | 475 | function Obj = create_scene(num, Obj, X, Y, Z, rt) 476 | switch num 477 | case 0 % Single object 478 | % Obj(1) = create_cone(100, 5, 0, 50, 80, Obj(1)); 479 | Obj(1) = create_sphere(100, 80, 0, 50, Obj(1)); 480 | % Obj(1) = create_cylinder(100, 5, 0, 25, 60, Obj(1)); 481 | 482 | case 1 % single(complex) object 483 | Obj(1) = create_cylinder(100, 5, 0, 25, 200, Obj(1)); 484 | Obj(2) = create_pipe(60, 20, 60, 80, 5, Obj(2)); 485 | Obj(3) = create_pipe(130, -30, 30, 100, 5, Obj(3)); 486 | 487 | case 2 % 2 objects 488 | Obj(1) = create_cylinder(60, 5, 0, 30, 50, Obj(1)); 489 | Obj(2) = create_sphere(120, -10, 0, 50, Obj(2)); 490 | 491 | case 3 % 3 objects 492 | Obj(1) = create_cylinder(60, 5, 0, 30, 50, Obj(1)); 493 | Obj(2) = create_sphere(120, -10, 0, 50, Obj(2)); 494 | Obj(3) = create_pipe(168, 0, 0, 25, 80, Obj(3)); 495 | 496 | case 12 % 12 objects 497 | Obj(1) = create_cylinder(100, 5, 0, 30, 50, Obj(1)); 498 | Obj(2) = create_pipe(140, 20, 0, 40,10, Obj(2)); 499 | Obj(3) = create_pipe(20, 20, 0, 24, 40, Obj(3)); 500 | Obj(4) = create_pipe(55, -20, 0, 28, 50, Obj(4)); 501 | Obj(5) = create_sphere(53, -60, 0, 50, Obj(5)); 502 | Obj(6) = create_pipe(150, -80, 0, 40, 50, Obj(6)); 503 | Obj(7) = create_cone(100, -35, 0, 50,45, Obj(7)); 504 | Obj(8) = create_cone(170, 2, 0, 20,50, Obj(8)); 505 | Obj(9) = create_cone(60, 35, 0, 50,30, Obj(9)); 506 | Obj(10) = create_cylinder(110, 70, 0, 60, 50, Obj(10)); 507 | Obj(11) = create_pipe(170, 60, 0, 40, 27, Obj(11)); 508 | Obj(12) = create_cone(150, -30, 0, 32, 45, Obj(12)); 509 | case 7 % 7 objects 510 | Obj(1) = create_cone(60,8, 0, 70, 50, Obj(1)); 511 | Obj(2) = create_cone(100,-24, 0, 89, 100, Obj(2)); 512 | Obj(3) = create_cone(160,40, -4, 100, 30, Obj(3)); 513 | Obj(4) = create_cone(100,100, -10, 150, 100, Obj(4)); 514 | Obj(5) = create_cone(180,-70, -10, 150, 20, Obj(5)); 515 | Obj(6) = create_cone(75,-75, -10, 150, 40, Obj(6)); 516 | Obj(7) = create_cylinder(170, -6, 0, 34, 100, Obj(7)); 517 | case 41 518 | Oy = -80 + 2*single(rt); 519 | Obj(1) = create_cylinder(100, Oy, 0, 50, 80, Obj(1)); 520 | case 42 521 | Oy1 = 0 + 50*sin(0.2*single(rt)); 522 | Oy2 = -20 - 20*sin(0.5*single(rt)); 523 | Oz2 = 40 + 20*cos(0.5*single(rt)); 524 | Obj(1) = create_cylinder(60, 5, 0, 30, 50, Obj(1)); 525 | Obj(2) = create_cylinder(110, -10, 0, 25, 80,Obj(2)); 526 | Obj(3) = create_cylinder(80, Oy1, 0, 20, 60, Obj(3)); 527 | Obj(4) = create_sphere(160, Oy2, Oz2, 30, Obj(4)); 528 | 529 | case 69 %pp 530 | Obj(1) = create_cylinder(100, 5, 0, 30, 80, Obj(1)); 531 | Obj(2) = create_sphere(100, 30, 0, 40, Obj(2)); 532 | Obj(3) = create_sphere(100, -20, 0, 40, Obj(3)); 533 | Obj(4) = create_sphere(100, 5, 80, 30, Obj(4)); 534 | end 535 | 536 | function Obj = create_sphere(x0, y0, z0, D, Obj) 537 | 538 | a = D/2; b = D/2; c = D/2; % Object's axis length 539 | p = 1; q = 1; r = 1; % Index parameters 540 | 541 | % Object Shape Equation 542 | Gamma = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 543 | 544 | % Differential 545 | [dGdx, dGdy, dGdz] = calc_dG(); 546 | 547 | % n and t 548 | n = [dGdx; dGdy; dGdz]; 549 | t = [dGdy; -dGdx; 0]; 550 | 551 | 552 | % Save to Field 553 | Obj.origin(rt,:) = [x0, y0, z0]; 554 | Obj.Gamma = Gamma; 555 | Obj.n = n; 556 | Obj.t = t; 557 | Obj.a = a; 558 | Obj.b = b; 559 | Obj.c = c; 560 | Obj.p = p; 561 | Obj.q = q; 562 | Obj.r = r; 563 | Obj.Rstar = min([a,b,c]); 564 | function [dGdx, dGdy, dGdz] = calc_dG() 565 | dGdx = (2*p*((X - x0)/a).^(2*p - 1))/a; 566 | dGdy = (2*q*((Y - y0)/b).^(2*q - 1))/b; 567 | dGdz = (2*r*((Z - z0)/c).^(2*r - 1))/c; 568 | end 569 | end 570 | 571 | function Obj = create_cylinder(x0, y0, z0, D, h, Obj) 572 | 573 | a = D/2; b = D/2; c = h; % Object's axis length 574 | p = 1; q = 1; r = 5; % Index parameters 575 | 576 | % Object Shape Equation 577 | Gamma = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 578 | 579 | % Differential 580 | [dGdx, dGdy, dGdz] = calc_dG(); 581 | 582 | % n and t 583 | n = [dGdx; dGdy; dGdz]; 584 | t = [dGdy; -dGdx; 0]; 585 | 586 | % Save to Field 587 | Obj.origin(rt,:) = [x0, y0, z0]; 588 | Obj.Gamma = Gamma; 589 | Obj.n = n; 590 | Obj.t = t; 591 | Obj.a = a; 592 | Obj.b = b; 593 | Obj.c = c; 594 | Obj.p = p; 595 | Obj.q = q; 596 | Obj.r = r; 597 | Obj.Rstar = min([a,b,c]); 598 | function [dGdx, dGdy, dGdz] = calc_dG() 599 | dGdx = (2*p*((X - x0)/a).^(2*p - 1))/a; 600 | dGdy = (2*q*((Y - y0)/b).^(2*q - 1))/b; 601 | dGdz = (2*r*((Z - z0)/c).^(2*r - 1))/c; 602 | end 603 | end 604 | 605 | function Obj = create_cone(x0, y0, z0, D, h, Obj) 606 | 607 | a = D/2; b = D/2; c = h; % Object's axis length 608 | p = 1; q = 1; r = 0.5; % Index parameters 609 | 610 | % Object Shape Equation 611 | Gamma = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 612 | 613 | % Differential 614 | [dGdx, dGdy, dGdz] = calc_dG(); 615 | 616 | % n and t 617 | n = [dGdx; dGdy; dGdz]; 618 | t = [dGdy; -dGdx; 0]; 619 | 620 | % Save to Field 621 | Obj.origin(rt,:) = [x0, y0, z0]; 622 | Obj.Gamma = Gamma; 623 | Obj.n = n; 624 | Obj.t = t; 625 | Obj.a = a; 626 | Obj.b = b; 627 | Obj.c = c; 628 | Obj.p = p; 629 | Obj.q = q; 630 | Obj.r = r; 631 | Obj.Rstar = min([a,b,c]); 632 | function [dGdx, dGdy, dGdz] = calc_dG() 633 | dGdx = (2*p*((X - x0)/a).^(2*p - 1))/a; 634 | dGdy = (2*q*((Y - y0)/b).^(2*q - 1))/b; 635 | dGdz = (2*r*((Z - z0)/c).^(2*r - 1))/c; 636 | end 637 | end 638 | 639 | function Obj = create_pipe(x0, y0, z0, D, h, Obj) 640 | 641 | a = D/2; b = D/2; c = h; % Object's axis length 642 | p = 2; q = 2; r = 2; % Index parameters 643 | 644 | % Object Shape Equation 645 | Gamma = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 646 | 647 | % Differential 648 | [dGdx, dGdy, dGdz] = calc_dG(); 649 | 650 | % n and t 651 | n = [dGdx; dGdy; dGdz]; 652 | t = [dGdy; -dGdx; 0]; 653 | 654 | % Save to Field 655 | Obj.origin(rt,:) = [x0, y0, z0]; 656 | Obj.Gamma = Gamma; 657 | Obj.n = n; 658 | Obj.t = t; 659 | Obj.a = a; 660 | Obj.b = b; 661 | Obj.c = c; 662 | Obj.p = p; 663 | Obj.q = q; 664 | Obj.r = r; 665 | Obj.Rstar = min([a,b,c]); 666 | function [dGdx, dGdy, dGdz] = calc_dG() 667 | dGdx = (2*p*((X - x0)/a).^(2*p - 1))/a; 668 | dGdy = (2*q*((Y - y0)/b).^(2*q - 1))/b; 669 | dGdz = (2*r*((Z - z0)/c).^(2*r - 1))/c; 670 | end 671 | end 672 | 673 | 674 | end 675 | function plot_multi(rt, Paths, Xini, Yini, Zini, destin) 676 | figure(69) 677 | plot3(Paths{1,rt}(1,:), Paths{1,rt}(2,:), Paths{1,rt}(3,:),'b', 'LineWidth', 1.5) 678 | hold on, grid on, grid minor, axis equal 679 | plot3(Paths{2,rt}(1,:), Paths{2,rt}(2,:), Paths{2,rt}(3,:),'b', 'LineWidth', 1.5) 680 | plot3(Paths{3,rt}(1,:), Paths{3,rt}(2,:), Paths{3,rt}(3,:),'b', 'LineWidth', 1.5) 681 | plot3(Paths{4,rt}(1,:), Paths{4,rt}(2,:), Paths{4,rt}(3,:),'b', 'LineWidth', 1.5) 682 | plot3(Paths{5,rt}(1,:), Paths{5,rt}(2,:), Paths{5,rt}(3,:),'b', 'LineWidth', 1.5) 683 | plot3(Paths{6,rt}(1,:), Paths{6,rt}(2,:), Paths{6,rt}(3,:),'b', 'LineWidth', 1.5) 684 | plot3(Paths{7,rt}(1,:), Paths{7,rt}(2,:), Paths{7,rt}(3,:),'b', 'LineWidth', 1.5) 685 | plot3(Paths{8,rt}(1,:), Paths{8,rt}(2,:), Paths{8,rt}(3,:),'b', 'LineWidth', 1.5) 686 | plot3(Paths{9,rt}(1,:), Paths{9,rt}(2,:), Paths{9,rt}(3,:),'b', 'LineWidth', 1.5) 687 | scatter3(Xini, Yini, Zini, 'filled', 'r') 688 | scatter3(destin(1,1),destin(1,2),destin(1,3), 'xr', 'xr', 'sizedata', 100) 689 | scatter3(destin(2,1),destin(2,2),destin(2,3), 'xr', 'xr', 'sizedata', 100) 690 | scatter3(destin(3,1),destin(3,2),destin(3,3), 'xr', 'xr', 'sizedata', 100) 691 | scatter3(destin(4,1),destin(4,2),destin(4,3), 'xr', 'xr', 'sizedata', 100) 692 | scatter3(destin(5,1),destin(5,2),destin(5,3), 'xr', 'xr', 'sizedata', 100) 693 | scatter3(destin(6,1),destin(6,2),destin(6,3), 'xr', 'xr', 'sizedata', 100) 694 | scatter3(destin(7,1),destin(7,2),destin(7,3), 'xr', 'xr', 'sizedata', 100) 695 | scatter3(destin(8,1),destin(8,2),destin(8,3), 'xr', 'xr', 'sizedata', 100) 696 | scatter3(destin(9,1),destin(9,2),destin(9,3), 'xr', 'xr', 'sizedata', 100) 697 | title(num2str(rt,'time = %4.1f s')) 698 | xlim([0 200]) 699 | ylim([-100 100]) 700 | zlim([0 100]) 701 | end 702 | 703 | function plot_single(rt, Paths, Xini, Yini, Zini, destin) 704 | figure(69) 705 | plot3(Paths{1,rt}(1,:), Paths{1,rt}(2,:), Paths{1,rt}(3,:),'b', 'LineWidth', 1.5) 706 | hold on, grid on, grid minor, axis equal 707 | scatter3(Xini, Yini, Zini, 'filled', 'r', 'xr', 'sizedata', 100) 708 | scatter3(destin(1,1),destin(1,2),destin(1,3), 'xr', 'xr', 'sizedata', 100) 709 | title(num2str(rt,'time = %4.1f s')) 710 | xlim([0 200]) 711 | ylim([-100 100]) 712 | zlim([0 100]) 713 | end 714 | -------------------------------------------------------------------------------- /initialize_constraint_matrix.m: -------------------------------------------------------------------------------- 1 | % load WeatherMat_3.mat % Good! (ok for rt = 100!) 2 | % load WeatherMat_8187.mat % Good for static plot (issue in rt>20) 3 | load WeatherMat_321.mat 4 | 5 | warning off 6 | weatherMatMod = weatherMat; 7 | weatherMatMod(weatherMatModB_U) = 1; 9 | 10 | 11 | xspace = 1:200; 12 | yspace = 1:200; 13 | [xgrid, ygrid] = meshgrid(xspace, yspace); 14 | 15 | WMCell = cell(1,size(weatherMat,3)); 16 | dwdxCell = cell(1,size(weatherMat,3)); 17 | dwdyCell = cell(1,size(weatherMat,3)); 18 | 19 | for j = 1:size(weatherMat,3) 20 | WMCell{j} = griddedInterpolant(weatherMat(:,:,j)'); 21 | z_values = WMCell{j}(xgrid,ygrid); 22 | [grad_x, grad_y] = gradient(z_values, xspace, yspace); 23 | dwdxCell{j} = griddedInterpolant(grad_x'); 24 | dwdyCell{j} = griddedInterpolant(grad_y'); 25 | end 26 | 27 | 28 | figure(88) 29 | subplot(1,2,1) 30 | set(gca, 'YDir', 'normal') 31 | colormap turbo 32 | contourf(1:200,1:200,weatherMat(:,:,1), 30) 33 | axis equal tight 34 | colorbar 35 | hold on 36 | [C2,h2] = contourf(1:200, 1:200, weatherMat(:,:,1), [1, 1], 'FaceAlpha',0,'LineColor', 'w', 'LineWidth', 2); 37 | clabel(C2,h2,'FontSize',15,'Color','w') 38 | title("Original Constraint Matrix") 39 | set(gca, 'YDir', 'normal', 'FontSize', fontSize) 40 | 41 | subplot(1,2,2) 42 | set(gca, 'YDir', 'normal') 43 | colormap turbo 44 | contourf(1:200,1:200,weatherMatMod(:,:,1), 30) 45 | hold on 46 | [C2,h2] = contourf(1:200, 1:200, weatherMat(:,:,1), [B_U, B_U], 'FaceAlpha',0,'LineColor', 'w', 'LineWidth', 2); 47 | 48 | 49 | 50 | title("Filtered Constraint Matrix: B_L = " + num2str(B_L) + ", B_U = " + num2str(B_U)) 51 | set(gca, 'YDir', 'normal', 'FontSize', fontSize) 52 | axis equal tight 53 | colorbar 54 | 55 | 56 | % Gradient 57 | figure 58 | % Define the X, Y, and Z coordinates 59 | X = 1:200; 60 | Y = 1:200; 61 | Z = zeros(length(Y), length(X)); 62 | 63 | % Compute the gradient of the matrix numerically 64 | dwdx = diff(weatherMat(:,:,1), 1, 2); 65 | dwdy = diff(weatherMat(:,:,1), 1, 1); 66 | 67 | % Pad the gradient matrices to match the size of the original matrix 68 | dwdx = [dwdx, zeros(size(dwdx, 1), 1)]; 69 | dwdy = [dwdy; zeros(1, size(dwdy, 2))]; 70 | 71 | % Plot the gradient vectors 72 | [X_grid, Y_grid] = meshgrid(X, Y); 73 | quiver(X_grid(1:5:end), Y_grid(1:5:end), dwdx(1:5:end), dwdy(1:5:end),2); 74 | axis equal tight; 75 | 76 | % Set the correct axis limits and labels 77 | xlim([0, 200]); 78 | % ylim([-100, 100]); 79 | xlabel('X'); 80 | ylabel('Y'); 81 | 82 | % Add a title 83 | title('Numerical Gradient of the Matrix'); -------------------------------------------------------------------------------- /main.m: -------------------------------------------------------------------------------- 1 | %% Dynamic Autorouting - Komsun Tamanakijprasart (2023) 2 | % For Static Results 3 | clc, clear, close all 4 | 5 | % ___________________Simulation Set-up Parameters__________________________ 6 | fontSize = 20; 7 | saveVid = 0; 8 | animation = 0; % Figure(69)m 1: see the simulation 9 | showDisp = 1; 10 | tsim = 100; % [s] simulation time for the path 11 | dt = 0.1; % [s] IFDS time step 12 | dt_traj = 1; % [s] Trajectory time step 13 | rtsim = 50 / dt_traj; % [s] (50) time for the whole scenario 14 | simMode = 1; % 1: by time, 2: by target distance 15 | targetThresh = 2; % [m] allowed error for final target distance 16 | multiTarget = uint8(0); % 1: multi-target 0: single-target 17 | scene = 44; % Scenario selection 18 | % 0) NO object 1) 1 object, 2) 2 objects 19 | % 3) 3 objects 4) 3 complex objects 20 | % 7) non-urban 12) urban environment 21 | 22 | % ___________________Features Control Parameters___________________________ 23 | useOptimizer = 0; % 0:Off 1:Global optimized 2: Local optimized 24 | delta_g = 10; % [m] minimum allowed gap distance 25 | k = 0.5; % Higher(1000) = more effect from weather 26 | % Lower(~0.01) = less effect 0 = no weather effect 27 | 28 | env = "dynamic"; % "static" "dynamic" 29 | 30 | % ______________________IFDS Tuning Parameters_____________________________ 31 | sf = uint8(0); % Shape-following demand (1=on, 0=off) 32 | rho0 = 2.5; % Repulsive parameter (rho >= 0) 33 | sigma0 = 0.01; % Tangential parameter 34 | 35 | % Good: rho0 = 2, simga0 = 0.01 36 | % The algorihtm still doesnt work for overlapped objects 37 | 38 | % _________________Constraint Matrix Tuning Paramters______________________ 39 | B_U = 0.9; % [B_L < B_U <= 1] 40 | B_L = 0; % [0 < B_L < B_U] 41 | 42 | % Good: k=1 | B_u=0.7 | B_L = 0 43 | % : k=100 | B_u=1 | B_L = 0.5 44 | 45 | % _______________________CCA Tuning Parameters_____________________________ 46 | ccaTuning = 5; 47 | switch ccaTuning 48 | case 1 49 | kappa = 10 ; % Gain 50 | delta = 2; % Carrot Distance 51 | kd = 0; 52 | case 2 53 | kappa = 20; 54 | delta = 5; 55 | kd = 0; 56 | case 3 57 | kappa = 10; 58 | delta = 10; 59 | kd = 0.1; 60 | case 4 61 | kappa = 100; 62 | delta = 1; 63 | kd = 0; 64 | case 5 65 | kappa = 50; 66 | delta = 20; 67 | kd = 0; 68 | end 69 | 70 | tuning = [kappa, delta, kd]; 71 | 72 | % _______________________ UAV Parameters _________________________________ 73 | C = 10; % [m/s] UAV cruising speed (30) 74 | % Starting location 75 | Xini = 0; 76 | Yini = 0; 77 | Zini = 0; 78 | 79 | % Target Destination 80 | Xfinal = 200; 81 | Yfinal = 0; 82 | % Zfinal = 10; 83 | Zfinal = 50; 84 | 85 | % UAV's Initial State 86 | x_i = 0; 87 | y_i = -20; 88 | % y_i = 0; 89 | z_i = 5; 90 | psi_i = 0; % [rad] Initial Yaw angle 91 | gamma_i = 0; % [rad] Initial Pitch angle 92 | 93 | 94 | x_guess = [rho0; sigma0]; 95 | 96 | % Initialize constraint matrix (separate file) 97 | initialize_constraint_matrix 98 | 99 | 100 | % Save to table Param 101 | Param = table; 102 | Param.showDisp = showDisp; 103 | Param.tsim = tsim; 104 | Param.rtsim = rtsim; 105 | Param.dt = dt; 106 | Param.C = C; 107 | Param.targetThresh = targetThresh; 108 | Param.simMode = simMode; 109 | Param.multiTarget = multiTarget; 110 | Param.scene = scene; 111 | Param.sf = sf; 112 | Param.Rg = delta_g; 113 | Param.rho0_initial = rho0; 114 | Param.sigma0_initial = sigma0; 115 | Param.Xini = Xini; 116 | Param.Yini = Yini; 117 | Param.Zini = Zini; 118 | Param.Xfinal = Xfinal; 119 | Param.Yfinal = Yfinal; 120 | Param.Zfinal = Zfinal; 121 | Param.useOptimizer = useOptimizer; 122 | Param.k = k; 123 | Param.B_U = B_U; 124 | Param.B_L = B_L; 125 | 126 | 127 | % Structure Pre-allocation for each scene 128 | switch scene 129 | case 0, numObj = 1; 130 | case 1, numObj = 1; 131 | case 2, numObj = 2; 132 | case 3, numObj = 3; 133 | case 4, numObj = 3; 134 | case 5, numObj = 3; 135 | case 7, numObj = 7; 136 | case 12, numObj = 12; 137 | case 41, numObj = 3; 138 | case 42, numObj = 4; 139 | case 44, numObj = 7; 140 | case 69, numObj = 4; 141 | case 6969, numObj = 3; 142 | end 143 | Param.numObj = numObj; 144 | Object(numObj) = struct('origin',zeros(rtsim,3),'Gamma',0,'n',[],'t',[],... 145 | 'a',0,'b',0,'c',0,'p',0,'q',0,'r',0,'Rstar',0); 146 | 147 | disp(['Number of object: ' num2str(size(Object,2))]) 148 | if sf == 0, disp("Shape-following: Off") 149 | else, disp("Shape-following: On") 150 | end 151 | 152 | switch useOptimizer 153 | case 0, disp("Path optimization: Off") 154 | case 1, disp("Path optimization: Global"); 155 | case 2, disp("Path optimization: Local") 156 | end 157 | 158 | if multiTarget 159 | destin = [200 0 20; 160 | 200 20 20; 161 | 200 -20 20; 162 | 200 20 30; 163 | 200 -20 30; 164 | 200 0 30; 165 | 200 0 40; 166 | 200 20 40; 167 | 200 -20 40;]; 168 | else 169 | destin = [Xfinal Yfinal Zfinal]; 170 | end 171 | 172 | numLine = size(destin,1); 173 | disp("Generating paths for " + num2str(numLine) + " destinations . . .") 174 | disp("*Timer started*") 175 | timer = zeros(1,rtsim); 176 | 177 | % Pre-allocate waypoints and path 178 | Wp = zeros(3, tsim+1); 179 | Paths = cell(numLine,rtsim); 180 | 181 | %% ====================== Main Path-Planning Program ====================== 182 | 183 | traj = cell(1,rtsim); 184 | traj{1} = [x_i, y_i, z_i]; 185 | errn = cell(1,rtsim); 186 | plengthT = []; 187 | 188 | for rt = 1:rtsim 189 | err = []; 190 | tic 191 | if norm([x_i y_i z_i] - [Xfinal Yfinal Zfinal]) < targetThresh % [m] 192 | disp("Target destination reached at t = " + num2str(rt) + " s") 193 | traj = traj(~cellfun('isempty',traj)); 194 | break 195 | end 196 | 197 | if scene == 41 || scene == 42 || (k ~= 0 && env == "dynamic") || scene == 44 198 | Wp(:,1) = [x_i; y_i; z_i]; 199 | else 200 | Wp(:,1) = [Xini; Yini; Zini]; % can change this to current uav pos 201 | end 202 | 203 | if isempty(traj{rt}) 204 | traj{rt} = traj{rt-1}(:,end); 205 | end 206 | 207 | for L = 1:numLine 208 | 209 | loc_final = destin(L,:)'; 210 | %------------Global Path Optimization------------- 211 | if useOptimizer == 1 212 | [rho0, sigma0] = path_optimizing(loc_final, rt, Wp, Paths, Param, Object, WMCell{rt}, dwdxCell{rt}, dwdyCell{rt}); 213 | end 214 | %------------------------------------------------ 215 | 216 | % Compute the IFDS Algorithm 217 | if env == "dynamic" 218 | [Paths, Object, plength, foundPath] = IFDS(rho0, sigma0, loc_final, rt, Wp, Paths, Param, L, Object, WMCell{rt}, dwdxCell{rt}, dwdyCell{rt}); 219 | elseif env == "static" 220 | [Paths, Object, ~, foundPath] = IFDS(rho0, sigma0, loc_final, rt, Wp, Paths, Param, L, Object, WMCell{15}, dwdxCell{15}, dwdyCell{15}); 221 | end 222 | % timer(L) = toc; 223 | plengthT = [plengthT; plength]; 224 | end 225 | 226 | if foundPath ~= 1 || isempty(Paths{rt}) || size(Paths{rt},2)==1 227 | disp("CAUTION : Path not found at t = " +num2str(rt) + " s") 228 | disp("*UAV is standing by*") 229 | continue 230 | end 231 | 232 | % Compute Path Following Algorithm 233 | trajectory = zeros(3, length(Paths{rt})); 234 | trajectory(:,1) = [x_i; y_i; z_i]; 235 | 236 | i = 1; 237 | dtcum = 0; 238 | 239 | for j = 1:length(Paths{rt})-1 240 | if dtcum >= dt_traj 241 | break 242 | end 243 | Wi = Paths{rt}(:,j); 244 | Wf = Paths{rt}(:,j+1); 245 | 246 | path_vect = Wf - Wi; 247 | a = path_vect(1); 248 | b = path_vect(2); 249 | c = path_vect(3); 250 | 251 | % Check if the waypoint is ahead of current position 252 | if a*(x_i - Wf(1)) + b*(y_i - Wf(2)) + c*(z_i - Wf(3)) < 0 253 | 254 | err = [err, norm([x_i-Wf(1), y_i-Wf(2), z_i-Wf(3)])]; 255 | [x, y, z, psi, gamma, timeSpent] = CCA3D_straight(Wi, Wf, x_i, y_i, z_i, psi_i, gamma_i, C, tuning); 256 | x_i = x(end); 257 | y_i = y(end); 258 | z_i = z(end); 259 | psi_i = psi(end); 260 | gamma_i = gamma(end); 261 | dtcum = dtcum + timeSpent; 262 | 263 | trajectory(:,i+1) = [x y z]'; 264 | i = i+1; 265 | else 266 | % disp("skip waypoint #" + num2str(j)) 267 | end 268 | end 269 | errn{rt} = err; 270 | trajectory = trajectory(:,1:i); % remove extra element 271 | traj{rt} = trajectory; 272 | timer(rt) = toc; 273 | disp("Computed time = " + num2str((timer(rt))) + " s") 274 | 275 | end 276 | 277 | % timer(timer==0)=[]; 278 | disp("Average computed time = " + num2str(mean(timer(timer~=0))) + " s") 279 | 280 | 281 | %% CCA3D Error Analysis - For static path only!! 282 | errn = errn(~cellfun('isempty',errn)); 283 | figure(91) 284 | for j = 1:length(errn) 285 | if mod(j,2) == 0 286 | styl = 'o-k'; 287 | else 288 | styl = 'o-b'; 289 | end 290 | plot(linspace(j-1,j,size(errn{j},2)), errn{j}, styl, 'LineWidth', 1), hold on 291 | end 292 | grid on, grid minor 293 | %% =======================Plotting Results================================= 294 | 295 | for rt = 1:size(traj,2) 296 | 297 | % Plotting the path 298 | figure(70) 299 | set(gcf, 'Position', get(0, 'Screensize')); 300 | subplot(1,2,1) 301 | 302 | quiver3(traj{rt}(1,1), traj{rt}(2,1), traj{rt}(3,1),... 303 | traj{rt}(1,end)-traj{rt}(1,1), traj{rt}(2,end)-traj{rt}(2,1),... 304 | traj{rt}(3,end)-traj{rt}(3,1), 'ok','filled', 'LineWidth', 1.5, 'MaxHeadSize',100,'AutoScaleFactor', 2,... 305 | 'Alignment','tail', 'MarkerSize', 10, 'MarkerFaceColor','w','ShowArrowHead','on') 306 | 307 | hold on, grid on, axis equal 308 | 309 | if ~isempty(Paths{rt}) 310 | PlotPath(rt, Paths, Xini, Yini, Zini, destin, multiTarget); 311 | end 312 | if rt>1 313 | prevTraj = [traj{1:rt-1}]; 314 | plot3(prevTraj(1,:), prevTraj(2,:), prevTraj(3,:), 'k', 'LineWidth', 1.2) 315 | end 316 | 317 | scatter3(destin(1,1),destin(1,2),destin(1,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 318 | 319 | if k~=0 320 | set(gca, 'YDir', 'normal') 321 | colormap turbo 322 | if env == "dynamic" 323 | contourf(1:200,-100:99,weatherMatMod(:,:,rt), 30) 324 | [C2,h2] = contourf(1:200, -100:99, weatherMat(:,:,rt), [B_U, B_U], 'FaceAlpha',0,'LineColor', 'w', 'LineWidth', 2); 325 | elseif env == "static" 326 | contourf(1:200,-100:99,weatherMatMod(:,:,15), 30) 327 | [C2,h2] = contourf(1:200, -100:99, weatherMat(:,:,15), [B_U, B_U], 'FaceAlpha',0,'LineColor', 'w', 'LineWidth', 2); 328 | end 329 | end 330 | 331 | xlim([0 200]), ylim([-100 100]), zlim([0 100]) 332 | title(num2str(rt*dt_traj,'time = %4.2f s')) 333 | xlabel('X [m]'); ylabel('Y [m]'); zlabel('Z [m]'); 334 | set(gca, 'LineWidth', 2, 'FontSize', fontSize-6) 335 | % view(0,90) 336 | hold off 337 | 338 | subplot(1,2,2) 339 | 340 | quiver3(traj{rt}(1,1), traj{rt}(2,1), traj{rt}(3,1),... 341 | traj{rt}(1,end)-traj{rt}(1,1), traj{rt}(2,end)-traj{rt}(2,1),... 342 | traj{rt}(3,end)-traj{rt}(3,1), 'ok','filled', 'LineWidth', 1.5, 'MaxHeadSize',100,'AutoScaleFactor', 2,... 343 | 'Alignment','tail', 'MarkerSize', 10, 'MarkerFaceColor','w','ShowArrowHead','on') 344 | 345 | hold on, grid on, axis equal 346 | 347 | if ~isempty(Paths{rt}) 348 | PlotPath(rt, Paths, Xini, Yini, Zini, destin, multiTarget); 349 | end 350 | if rt>1 351 | prevTraj = [traj{1:rt-1}]; 352 | plot3(prevTraj(1,:), prevTraj(2,:), prevTraj(3,:), 'k', 'LineWidth', 1.2) 353 | 354 | end 355 | 356 | scatter3(destin(1,1),destin(1,2),destin(1,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 357 | 358 | if k~=0 359 | set(gca, 'YDir', 'normal') 360 | colormap turbo 361 | 362 | if env == "dynamic" 363 | contourf(1:200,-100:99,weatherMatMod(:,:,rt), 30) 364 | [C2,h2] = contourf(1:200, -100:99, weatherMat(:,:,rt), [B_U, B_U], 'FaceAlpha',0,'LineColor', 'w', 'LineWidth', 2); 365 | elseif env == "static" 366 | contourf(1:200,-100:99,weatherMatMod(:,:,15), 30) 367 | [C2,h2] = contourf(1:200, -100:99, weatherMat(:,:,15), [B_U, B_U], 'FaceAlpha',0,'LineColor', 'w', 'LineWidth', 2); 368 | end 369 | 370 | clabel(C2,h2,'FontSize',15,'Color','w') 371 | end 372 | 373 | 374 | xlim([0 200]) 375 | ylim([-100 100]) 376 | zlim([0 100]) 377 | 378 | title(num2str(rt*dt_traj,'time = %4.2f s')) 379 | xlabel('X [m]'); ylabel('Y [m]'); zlabel('Z [m]'); 380 | set(gca, 'LineWidth', 2, 'FontSize', fontSize-6) 381 | view(0,90) 382 | hold off 383 | % pause(0.1) 384 | 385 | end 386 | 387 | syms X Y Z Gamma(X,Y,Z) Gamma_star(X,Y,Z) Gamma_prime(X,Y,Z) 388 | syms omega(X,Y) wet(X,Y) 389 | 390 | %% 391 | figure(69) 392 | if animation 393 | simulate = 1:size(traj,2); 394 | else 395 | simulate = size(traj,2); 396 | end 397 | % for rt = simulate 398 | for rt = 23 399 | if rt>2 400 | prevTraj = [traj{1:rt-1}]; 401 | end 402 | 403 | figure(69) 404 | set(gcf, 'Position', get(0, 'Screensize')); 405 | subplot(7,2,[1 3 5 7]) 406 | plotting_everything 407 | 408 | if k~=0 409 | hold on 410 | set(gca, 'YDir', 'normal') 411 | 412 | if env == "dynamic" 413 | contourf(1:200,-100:99,weatherMatMod(:,:,rt),30,'LineStyle', '-') 414 | [C2,h2] = contourf(1:200, -100:99, weatherMat(:,:,rt), [B_U, B_U], 'FaceAlpha',0,'LineColor', 'w', 'LineWidth', 2); 415 | contourf(1:200,-100:99,weatherMatMod(:,:,rt), 30) 416 | elseif env == "static" 417 | contourf(1:200,-100:99,weatherMatMod(:,:,15),30,'LineStyle', '-') 418 | [C2,h2] = contourf(1:200, -100:99, weatherMat(:,:,15), [B_U, B_U], 'FaceAlpha',0,'LineColor', 'w', 'LineWidth', 2); 419 | end 420 | hold off 421 | end 422 | % set(gca, "FontSize", 18) 423 | subplot(7,2,[2 4 6 8]); 424 | plotting_everything 425 | if k~=0 426 | hold on 427 | set(gca, 'YDir', 'normal') 428 | % colormap(flipud(bone)) 429 | colormap turbo 430 | if env == "dynamic" 431 | contourf(1:200,-100:99,weatherMatMod(:,:,rt),30,'LineStyle', '-') 432 | [C2,h2] = contourf(1:200, -100:99, weatherMat(:,:,rt), [B_U, B_U], 'FaceAlpha',0,'LineColor', 'w', 'LineWidth', 2); 433 | contourf(1:200,-100:99,weatherMatMod(:,:,rt), 30) 434 | elseif env == "static" 435 | contourf(1:200,-100:99,weatherMatMod(:,:,15),30,'LineStyle', '-') 436 | [C2,h2] = contourf(1:200, -100:99, weatherMat(:,:,15), [B_U, B_U], 'FaceAlpha',0,'LineColor', 'w', 'LineWidth', 2); 437 | end 438 | clabel(C2,h2,'FontSize',15,'Color','w') 439 | colorbar 440 | hold off 441 | end 442 | if ~animation 443 | if rt>1 444 | legend([pltDestin, pltPath, pltTraj], "Destination", "IFDS Path", "UAV Trajectory",'Position',[0.757 0.917 0.09 0.04]) 445 | % legend([pltDestin, pltTraj], "Destination", "UAV Trajectory",'Position',[0.757 0.917 0.09 0.04]) 446 | else 447 | legend([pltDestin, pltPath], "Destination", "IFDS Path",'Position',[0.757 0.917 0.09 0.04]) 448 | end 449 | end 450 | 451 | view(0,90) 452 | % set(gca, "FontSize", 18) 453 | 454 | % subplot(7,2,[9 11 13]) 455 | % plotting_everything 456 | % view(90,0) 457 | % % set(gca, "FontSize", 18) 458 | % 459 | % subplot(7,2,[10 12 14]) 460 | % plotting_everything 461 | % view(0,0) 462 | % % set(gca, "FontSize", 18) 463 | 464 | 465 | if k ~=0 466 | sgtitle([['IFDS, \rho_0 = ' num2str(rho0) ', \sigma_0 = ' num2str(sigma0) ', SF = ' num2str(sf),', \delta_g = ', num2str(delta_g), 'm, ', num2str(rt,'time = %4.1f s')]; ... 467 | "Constraint Matrix, k = " + num2str(k) + ", B_U = " + num2str(B_U) + ", B_L = " + num2str(B_L)], 'FontSize', fontSize+2); 468 | else 469 | sgtitle(['IFDS, \rho_0 = ' num2str(rho0) ', \sigma_0 = ' num2str(sigma0) ', SF = ' num2str(sf),', \delta_g = ', num2str(delta_g), 'm, ', num2str(rt,'time = %4.1f s')],'FontSize', fontSize+2); 470 | end 471 | % Video saving 472 | if saveVid 473 | frm(rt) = getframe(gcf) ; 474 | drawnow 475 | end 476 | end 477 | 478 | 479 | 480 | % title(['IFDS, \rho_0 = ' num2str(rho0) ', \sigma_0 = ' num2str(sigma0)],... 481 | % 'FontSize',26); 482 | % subtitle(['SF = ' num2str(sf)], 'FontSize', 24) 483 | 484 | 485 | if saveVid 486 | if k~=0 487 | text = "_weather"; 488 | else 489 | text = ""; 490 | end 491 | video_name = "Result_scene_" + num2str(scene) + text + ".avi"; 492 | disp("Video saved: " + video_name); 493 | % create the video writer with 1 fps 494 | writerObj = VideoWriter(video_name); 495 | % writerObj.FrameRate = 30; 496 | writerObj.FrameRate = 2; 497 | % set the seconds per image 498 | % open the video writer 499 | open(writerObj); 500 | % write the frames to the video 501 | for i=1:length(frm) 502 | % convert the image to a frame 503 | frame = frm(i) ; 504 | writeVideo(writerObj, frame); 505 | end 506 | % close the writer object 507 | close(writerObj); 508 | end 509 | 510 | %% Plot Gamma Distribution 511 | 512 | % figure(96) 513 | % PlotGamma(Gamma, Gamma_star, X, Y, Z, fontSize - 8, weatherMatMod, k, B_U, B_L) 514 | 515 | 516 | %% Realtime analysis 517 | s = load('time_journal_dyna_2.mat'); 518 | figure 519 | stem(timer(1:25), 'LineWidth', 2) 520 | hold on, grid on, grid minor 521 | stem(s.timer(1:25), 'LineWidth', 2) 522 | legend("Optimised IFDS Local Path", "IFDS Local Path") 523 | xlabel("Elapsed Simulation Time (s)", 'FontSize', 20) 524 | ylabel("Computed Time (s)", 'FontSize', 20) 525 | set(gca, 'FontSize', 30, 'LineWidth', 1.5) 526 | 527 | 528 | %% Traj Compare 529 | allTraj = [traj{1:rt}]; 530 | tr = load('allTraj_opt.mat'); 531 | figure 532 | subplot(1,2,1) 533 | pltOpt = plot3(tr.allTraj(1,:),tr.allTraj(2,:), tr.allTraj(3,:), 'LineWidth', 2.5); 534 | hold on, grid on, grid minor, axis equal 535 | pltOg = plot3(allTraj(1,:),allTraj(2,:), allTraj(3,:),'r--', 'LineWidth', 2.5); 536 | % Obstacle 537 | % PlotObject(Object, delta_g, rt, rtsim, X, Y, Z, Gamma, Gamma_star); 538 | camlight 539 | xlabel('X [m]'); ylabel('Y [m]'); zlabel('Z [m]') 540 | set(gca, 'FontSize', 20, 'LineWidth', 1.5) 541 | 542 | subplot(1,2,2) 543 | pltOpt = plot3(tr.allTraj(1,:),tr.allTraj(2,:), tr.allTraj(3,:), 'LineWidth', 2.5); 544 | hold on, grid on, grid minor, axis equal 545 | pltOg = plot3(allTraj(1,:),allTraj(2,:), allTraj(3,:),'r--', 'LineWidth', 2.5); 546 | % Obstacle 547 | % PlotObject(Object, delta_g, rt, rtsim, X, Y, Z, Gamma, Gamma_star); 548 | xlabel('X [m]'); ylabel('Y [m]'); zlabel('Z [m]'); camlight 549 | view(0,90) 550 | legend([pltOpt, pltOg],"Optimised Trajectory", "Non-optimised Trajectory") 551 | set(gca, 'FontSize', 20, 'LineWidth', 1.5) 552 | 553 | 554 | 555 | 556 | 557 | 558 | % Assuming traj is your matrix 559 | traj_diff = diff(allTraj, 1, 2); % Calculate differences between consecutive points 560 | length_traj = sum(sqrt(sum(traj_diff.^2, 1))) % Calculate Euclidean distance and sum 561 | 562 | 563 | traj_diff_opt = diff(tr.allTraj, 1, 2); 564 | length_traj_opt = sum(sqrt(sum(traj_diff_opt.^2, 1))) % Calculate Euclidean distance and sum 565 | 566 | %% 567 | cumTraj = zeros(1,rt); 568 | for j = 1:rt 569 | trajRT = [traj{1:j}]; 570 | 571 | temDiff = diff(trajRT, 1, 2); 572 | cumTraj(j) = sum(sqrt(sum(temDiff.^2, 1))); 573 | end 574 | cumTraj 575 | %% ------------------------------Function--------------------------------- 576 | 577 | 578 | function [rho0, sigma0] = path_optimizing(loc_final, rt, Wp, Paths, Param, Object, weatherMat, dwdx, dwdy) 579 | 580 | xg = [Param.rho0_initial; Param.sigma0_initial]; 581 | 582 | lower_bound_rho = 0.05; % <0.05 issue started to occur 583 | lower_bound_sigma = 0; 584 | 585 | upper_bound_rho = 2.5; 586 | upper_bound_sigma = 2; 587 | 588 | % Set up the optimization problem 589 | problem.objective = @(x) PathDistObjective(x(1), x(2)); 590 | problem.x0 = xg; 591 | problem.Aineq = []; 592 | problem.bineq = []; 593 | problem.Aeq = []; 594 | problem.beq = []; 595 | problem.lb = [lower_bound_rho; lower_bound_sigma]; 596 | problem.ub = [upper_bound_rho; upper_bound_sigma]; 597 | problem.nonlcon = []; 598 | problem.solver = 'fmincon'; % specify the solver 599 | problem.options = optimoptions('fmincon', ... 600 | 'Algorithm', 'interior-point', ... % option2: sqp 601 | 'Display', 'off', 'MaxIterations',1); 602 | 603 | % Call fmincon 604 | [xOpt, fval, exitflag, output] = fmincon(problem); 605 | % disp(output) 606 | rho0 = xOpt(1); 607 | sigma0 = xOpt(2); 608 | 609 | function totalLength = PathDistObjective(rho0, sigma0) 610 | Param.showDisp = 0; 611 | Param.useOptimizer = 0; 612 | [~, ~, totalLength] = ... 613 | IFDS(rho0, sigma0, loc_final, rt, Wp, Paths, Param, 1, Object, weatherMat, dwdx, dwdy); 614 | 615 | end 616 | 617 | end 618 | 619 | function PlotGamma(Gamma, Gamma_star, X, Y, Z, fontSize, weatherMat, k, B_U, B_L) 620 | xr = 0:1:200; 621 | yr = -100:1:100; 622 | % zr = 0:100; 623 | zr = 0:200; 624 | 625 | % fixed plane 626 | xfixed = 100; 627 | yfixed = 0; 628 | zfixed = 0; 629 | 630 | % limits of the rectangular plane 631 | x_plane_limits = [min(xr), max(xr)]; 632 | y_plane_limits = [min(yr), max(yr)]; 633 | z_plane_limits = [min(zr), max(zr)]; 634 | 635 | xy_vertices = [x_plane_limits(1), y_plane_limits(1), zfixed; 636 | x_plane_limits(1), y_plane_limits(2), zfixed; 637 | x_plane_limits(2), y_plane_limits(2), zfixed; 638 | x_plane_limits(2), y_plane_limits(1), zfixed]; 639 | yz_vertices = [xfixed, y_plane_limits(1), z_plane_limits(1); 640 | xfixed, y_plane_limits(2), z_plane_limits(1); 641 | xfixed, y_plane_limits(2), z_plane_limits(2); 642 | xfixed, y_plane_limits(1), z_plane_limits(2)]; 643 | xz_vertices = [x_plane_limits(1), yfixed, z_plane_limits(1); 644 | x_plane_limits(1), yfixed, z_plane_limits(2); 645 | x_plane_limits(2), yfixed, z_plane_limits(2); 646 | x_plane_limits(2), yfixed, z_plane_limits(1)]; 647 | faces = [1,2,3,4]; 648 | 649 | 650 | % Create a grid of X and Y values for X-Y, Y-Z, and X-Z plane 651 | [X_grid_xy, Y_grid_xy] = meshgrid(xr, yr); 652 | [Y_grid_yz, Z_grid_yz] = meshgrid(yr, zr); 653 | [X_grid_xz, Z_grid_xz] = meshgrid(xr, zr); 654 | 655 | % Convert the symbolic function to a numerical function 656 | Gamma_numeric = matlabFunction(Gamma, 'Vars', [X, Y, Z]); 657 | 658 | 659 | % Gamma_numeric = Gamma_numeric + 1 - exp(omega * log(Gamma_numeric)); 660 | 661 | % Calculate Gamma for each pair plane 662 | Gamma_values_XY_og = Gamma_numeric(X_grid_xy, Y_grid_xy, zfixed); 663 | Gamma_values_XY = Gamma_numeric_mod(X_grid_xy, Y_grid_xy, zfixed); 664 | Gamma_values_YZ = Gamma_numeric_mod(xfixed, Y_grid_yz, Z_grid_yz); 665 | Gamma_values_XZ = Gamma_numeric_mod(X_grid_xz, yfixed, Z_grid_xz); 666 | 667 | % Define the number of countour levels 668 | num_levels = 30; 669 | max_Gamm = max([max(max(Gamma_values_XY)), max(max(Gamma_values_YZ)), max(max(Gamma_values_XZ))]); 670 | 671 | % Plot the Gamma distribution 672 | sp1 = subplot(2,3,1); 673 | fimplicit3(Gamma == 1,'EdgeColor','none','FaceAlpha',1,'MeshDensity',80), hold on 674 | fimplicit3(Gamma_star == 1, 'EdgeColor','none','FaceAlpha',0.2,'MeshDensity',30) 675 | patch('Vertices', xy_vertices, 'Faces', faces, 'FaceColor', 'blue', 'FaceAlpha', 0.2, 'EdgeColor', 'none'); 676 | patch('Vertices', yz_vertices, 'Faces', faces, 'FaceColor', 'red', 'FaceAlpha', 0.2, 'EdgeColor', 'none'); 677 | patch('Vertices', xz_vertices, 'Faces', faces, 'FaceColor', 'green', 'FaceAlpha', 0.2, 'EdgeColor', 'none'); 678 | axis equal 679 | xlim([0 200]) 680 | ylim([-100 100]) 681 | zlim([0 100]) 682 | xlabel('X [m]'); ylabel('Y [m]'); zlabel('Z [m]') 683 | set(gca, 'FontSize', fontSize+2) 684 | 685 | subplot(2,3,2) 686 | colormap(flipud(turbo)) 687 | contourf(X_grid_xy, Y_grid_xy, Gamma_values_XY_og, num_levels), hold on 688 | [C1,h1] = contourf(X_grid_xy, Y_grid_xy, Gamma_values_XY_og, [1, 1], 'FaceAlpha',0,... 689 | 'LineColor', 'w', 'LineWidth', 2); 690 | clabel(C1,h1,'FontSize',15,'Color','w') 691 | xlabel('X [m]'), ylabel('Y [m]') 692 | title("Original \Gamma - Top view (Z = " + num2str(zfixed) + ")"); 693 | grid on, grid minor, axis equal tight, hold off 694 | colorbar 695 | set(gca, 'FontSize', fontSize+2) 696 | 697 | sp3 = subplot(2,3,3); 698 | set(gca, 'YDir', 'normal') 699 | contourf(1:200,1:200, weatherMat(:,:,1), num_levels); 700 | hold on 701 | [C2,h2] = contourf(1:200, 1:200, weatherMat(:,:,1), [1, 1], 'FaceAlpha',0,... 702 | 'LineColor', 'w', 'LineWidth', 2); 703 | clabel(C2,h2,'FontSize',15,'Color','w') 704 | xlabel('X [m]'), ylabel('Y [m]') 705 | title("Original Constraint Matrix"); 706 | grid on, grid minor, axis equal tight, hold off 707 | set(gca, 'FontSize', fontSize+2) 708 | colormap(sp3,turbo) 709 | colorbar 710 | 711 | subplot(2,3,4) 712 | contourf(X_grid_xy, Y_grid_xy, Gamma_values_XY, num_levels), hold on 713 | [C3,h3] = contourf(X_grid_xy, Y_grid_xy, Gamma_values_XY, [0, 1.00001], 'FaceAlpha',0,... 714 | 'LineColor', 'w', 'LineWidth', 2); 715 | colorbar 716 | % clim([0 max_Gamm]) 717 | clabel(C3,h3,'FontSize',15,'Color','w') 718 | xlabel('X [m]'), ylabel('Y [m]') 719 | title("\Gamma_p - Top view (Z = " + num2str(zfixed) + ")"); 720 | grid on, grid minor, axis equal tight, hold off 721 | set(gca, 'FontSize', fontSize+2) 722 | 723 | % Plot the Y-Z plane distribution 724 | subplot(2,3,5) 725 | contourf(Y_grid_yz, Z_grid_yz, Gamma_values_YZ, num_levels), hold on 726 | [C4,h4] = contourf(Y_grid_yz, Z_grid_yz, Gamma_values_YZ, [1, 1], 'FaceAlpha',0,... 727 | 'LineColor', 'w', 'LineWidth', 2); 728 | colorbar 729 | % clim([0 max_Gamm]) 730 | clabel(C4,h4,'FontSize',15,'Color','w') 731 | xlabel('Y [m]'), ylabel('Z [m]') 732 | title("\Gamma_p - Front view (X = " + num2str(xfixed) + ")"); 733 | grid on, grid minor, axis equal tight, hold off 734 | set(gca, 'FontSize', fontSize+2, 'XDir', 'reverse') 735 | 736 | 737 | % Plot the X-Z plane distribution 738 | subplot(2,3,6) 739 | contourf(X_grid_xz, Z_grid_xz, Gamma_values_XZ, num_levels), hold on 740 | [C5,h5] = contourf(X_grid_xz, Z_grid_xz, Gamma_values_XZ, [1, 1], 'FaceAlpha',0,... 741 | 'LineColor', 'w', 'LineWidth', 2); 742 | colorbar 743 | % clim([0 max_Gamm]) 744 | clabel(C5,h5,'FontSize',15,'Color','w') 745 | xlabel('X [m]'), ylabel('Z [m]'); 746 | title("\Gamma_p - Side view (Y = " + num2str(yfixed) +")"); 747 | grid on, grid minor, axis equal tight, hold off 748 | set(gca, 'FontSize', fontSize+2) 749 | 750 | colormap(sp1, pink) 751 | sgt = sgtitle('Distribution of the boundary function'); 752 | sgt.FontSize = fontSize + 10; 753 | 754 | function gam = Gamma_numeric_mod(X,Y,Z) 755 | gam = Gamma_numeric(X,Y,Z); 756 | 757 | if size(X,1) > 1 && size(Y,1) >1 758 | 759 | xid = round(X)+1; 760 | yid = round(Y)+100+1; 761 | 762 | xid(xid>200) = 200; 763 | yid(yid>200) = 200; 764 | omega = zeros(size(xid)); 765 | 766 | for i = 1:size(xid,1) 767 | 768 | for j = 1:size(xid,2) 769 | omega(i,j) = weatherMat(yid(i,j), xid(i,j), 1); 770 | end 771 | end 772 | 773 | gam = 1 + gam - exp(omega .* log(gam)); 774 | % gam = gam - k* (exp( (B_L - omega)/(B_L - B_U) * log((gam-1)/k +1) ) -1); 775 | 776 | end 777 | 778 | 779 | end 780 | end 781 | 782 | % function [Gamma, Gamma_star] = PlotObject(Object, Rg, rt, rtsim, X, Y, Z, Gamma, Gamma_star) 783 | % for j = 1:size(Object,2) 784 | % x0 = Object(j).origin(rt, 1); 785 | % y0 = Object(j).origin(rt, 2); 786 | % z0 = Object(j).origin(rt, 3); 787 | % a = Object(j).a; 788 | % b = Object(j).b; 789 | % c = Object(j).c; 790 | % p = Object(j).p; 791 | % q = Object(j).q; 792 | % r = Object(j).r; 793 | % 794 | % Rstar = Object(j).Rstar; 795 | % 796 | % Gamma(X, Y, Z) = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 797 | % Gamma_star(X, Y, Z) = Gamma - ( (Rstar + Rg)/Rstar )^2 + 1; 798 | % 799 | % % if rtsim > 1 800 | % % fimplicit3(Gamma == 1,'EdgeColor','k','FaceAlpha',1,'MeshDensity',20), hold on 801 | % % fimplicit3(Gamma_star == 1, 'EdgeColor','k','FaceAlpha',0,'MeshDensity',20) 802 | % % else 803 | % fimplicit3(Gamma == 1,'EdgeColor','none','FaceAlpha',0.9,'MeshDensity',80), hold on 804 | % fimplicit3(Gamma_star == 1, 'EdgeColor','none','FaceAlpha',0.2,'MeshDensity',30) 805 | % % end 806 | % % colormap pink 807 | % 808 | % xlim([0 200]) 809 | % ylim([-100 100]) 810 | % zlim([0 100]) 811 | % end 812 | % 813 | % end 814 | -------------------------------------------------------------------------------- /main_sg_generation.m: -------------------------------------------------------------------------------- 1 | clc, clear, close all 2 | 3 | % Set-up Parameters 4 | fontSize = 20; 5 | showDisp = 1; 6 | tsim = uint16(400); % [s] simulation time for the path 7 | rtsim = 1; % [s] (50) time for the whole scenario 8 | dt = 0.1; % [s] simulation time step 9 | C = 30; % [m/s] UAV cruising speed 10 | targetThresh = 2.5; % [m] allowed error for final target distance 11 | simMode = uint8(1); % 1: by time, 2: by target distance 12 | multiTarget = uint8(1); % 1: multi-target 0: single-target 13 | scene = uint8(2); % Scenario selection 14 | % 0) 1 cone, 2) 1 complex object 15 | % 7) non-urban 12) urban environment 16 | 17 | useOptimizer = 0; % 0:Off 1:Global optimized 2: Local optimized 18 | 19 | % Starting location 20 | Xini = 0; 21 | Yini = 0; 22 | Zini = 0; 23 | 24 | % Target Destination 25 | Xfinal = 200; 26 | Yfinal = 0; 27 | Zfinal = 10; 28 | 29 | % Tuning Parameters 30 | sf = uint8(0); % Shape-following demand (1=on, 0=off) 31 | rho0 = 1; % Repulsive parameter (rho >= 0) 32 | sigma0 = 0.01; % Tangential parameter 33 | Rg = 10; % [m] minimum allowed gap distance 34 | 35 | x_guess = [rho0; sigma0]; 36 | 37 | %----------- Note ------------- 38 | % Good: rho0 = 2, simga0 = 0.01 39 | % The algorihtm still doesnt work for overlapped objects 40 | %------------------------------ 41 | 42 | % Save to table Param 43 | Param = table; 44 | Param.showDisp = showDisp; 45 | Param.tsim = tsim; 46 | Param.rtsim = rtsim; 47 | Param.dt = dt; 48 | Param.C = C; 49 | Param.targetThresh = targetThresh; 50 | Param.simMode = simMode; 51 | Param.multiTarget = multiTarget; 52 | Param.scene = scene; 53 | Param.sf = sf; 54 | Param.Rg = Rg; 55 | Param.rho0_initial = rho0; 56 | Param.sigma0_initial = sigma0; 57 | Param.Xini = Xini; 58 | Param.Yini = Yini; 59 | Param.Zini = Zini; 60 | Param.Xfinal = Xfinal; 61 | Param.Yfinal = Yfinal; 62 | Param.Zfinal = Zfinal; 63 | Param.useOptimizer = useOptimizer; 64 | 65 | % Structure Pre-allocation for each scene 66 | switch scene 67 | case 0, numObj = 1; 68 | case 1, numObj = 1; 69 | case 2, numObj = 2; 70 | case 3, numObj = 3; 71 | case 7, numObj = 7; 72 | case 12, numObj = 12; 73 | case 41, numObj = 1; 74 | case 42, numObj = 4; 75 | case 69, numObj = 4; 76 | end 77 | 78 | Object(numObj) = struct('origin',zeros(rtsim,3),'Gamma',0,'n',[],'t',[],... 79 | 'a',0,'b',0,'c',0,'p',0,'q',0,'r',0,'Rstar',0); 80 | 81 | disp(['Number of object: ' num2str(size(Object,2))]) 82 | if sf == 0, disp("Shape-following: Off") 83 | else, disp("Shape-following: On") 84 | end 85 | 86 | switch useOptimizer 87 | case 0, disp("Path optimization: Off") 88 | case 1, disp("Path optimization: Global"); 89 | case 2, disp("Path optimization: Local") 90 | end 91 | 92 | 93 | %% Original Fluid 94 | 95 | if multiTarget 96 | destin = [200 0 20; 97 | 200 20 20; 98 | 200 -20 20; 99 | 200 20 30; 100 | 200 -20 30; 101 | 200 0 30; 102 | 200 0 40; 103 | 200 20 40; 104 | 200 -20 40;]; 105 | else 106 | destin = [Xfinal Yfinal Zfinal]; 107 | end 108 | 109 | numLine = size(destin,1); 110 | disp("Generating paths for " + num2str(numLine) + " destinations . . .") 111 | disp("*Timer started*") 112 | timer = zeros(1,numLine); 113 | 114 | % Pre-allocate waypoints and path 115 | Wp = zeros(3, tsim+1); 116 | Paths = cell(numLine,rtsim); 117 | 118 | %% ====================== Main Path-Planning Program ====================== 119 | 120 | for rt = 1:rtsim 121 | tic 122 | Wp(:,1,rt) = [Xini; Yini; Zini]; % can change this to current uav pos 123 | 124 | for L = 1:numLine 125 | 126 | loc_final = destin(L,:)'; 127 | 128 | %------------Global Path Optimization------------- 129 | if useOptimizer == 1 130 | [rho0, sigma0] = path_optimizing(loc_final, rt, Wp, Paths, Param, Object); 131 | end 132 | %------------------------------------------------ 133 | 134 | % Compute the IFDS Algorithm 135 | [Paths, Object, ~] = IFDS(rho0, sigma0, loc_final, rt, Wp, Paths, Param, L, Object); 136 | timer(L) = toc; 137 | 138 | end 139 | 140 | disp("Average computed time = " + num2str(mean(timer)) + " s") 141 | % Plotting the path 142 | figure(70) 143 | PlotPath(rt, Paths, Xini, Yini, Zini, destin, multiTarget) 144 | title(num2str(rt,'time = %4.1f s')) 145 | xlabel('X [m]'); ylabel('Y [m]'); zlabel('Z [m]'); 146 | view(0,90) 147 | 148 | end 149 | % ========================================================================= 150 | %% Plotting Results 151 | animation = 1; 152 | 153 | syms X Y Z Gamma(X,Y,Z) Gamma_star(X,Y,Z) 154 | 155 | 156 | figure(69) 157 | % set(gcf, 'Position', get(0, 'Screensize')); 158 | for rt = 1:rtsim 159 | figure(69) 160 | subplot(2,2,1) 161 | PlotPath(rt, Paths, Xini, Yini, Zini, destin, multiTarget) 162 | [Gamma, Gamma_star] = PlotObject(Object, Rg, rt, rtsim, X, Y, Z, Gamma, Gamma_star); 163 | xlabel('X [m]'); ylabel('Y [m]'); zlabel('Z [m]'); camlight 164 | grid minor 165 | set(gca, 'LineWidth', 2) 166 | hold off 167 | 168 | subplot(2,2,2); 169 | PlotPath(rt, Paths, Xini, Yini, Zini, destin, multiTarget) 170 | [Gamma, Gamma_star] = PlotObject(Object, Rg, rt, rtsim, X, Y, Z, Gamma, Gamma_star); 171 | xlabel('X [m]'); ylabel('Y [m]'); zlabel('Z [m]'); camlight 172 | grid minor 173 | set(gca, 'LineWidth', 2) 174 | view(0,90) 175 | hold off 176 | 177 | subplot(2,2,3) 178 | PlotPath(rt, Paths, Xini, Yini, Zini, destin, multiTarget) 179 | [Gamma, Gamma_star] = PlotObject(Object, Rg, rt, rtsim, X, Y, Z, Gamma, Gamma_star); 180 | xlabel('X [m]'); ylabel('Y [m]'); zlabel('Z [m]'); camlight 181 | grid minor 182 | set(gca, 'LineWidth', 2) 183 | view(90,0) 184 | hold off 185 | 186 | subplot(2,2,4); 187 | PlotPath(rt, Paths, Xini, Yini, Zini, destin, multiTarget) 188 | [Gamma, Gamma_star] = PlotObject(Object, Rg, rt, rtsim, X, Y, Z, Gamma, Gamma_star); 189 | xlabel('X [m]'); ylabel('Y [m]'); zlabel('Z [m]'); camlight 190 | grid minor 191 | set(gca, 'LineWidth', 2) 192 | view(0,0) 193 | hold off 194 | 195 | sgtitle(['IFDS, \rho_0 = ' num2str(rho0) ', \sigma_0 = ' num2str(sigma0) ', SF = ' ... 196 | num2str(sf),', r_g = ', num2str(Rg), 'm, ', num2str(rt,'time = %4.1f s')], 'Fontsize', fontSize+2); 197 | end 198 | 199 | % title(['IFDS, \rho_0 = ' num2str(rho0) ', \sigma_0 = ' num2str(sigma0)],... 200 | % 'FontSize',26); 201 | % subtitle(['SF = ' num2str(sf)], 'FontSize', 24) 202 | % set(gca,'FontSize', fontSize) 203 | 204 | 205 | %% Plot Gamma Distribution 206 | figure(96) 207 | PlotGamma(Gamma, Gamma_star, X, Y, Z, fontSize - 8) 208 | 209 | %% ------------------------------Function--------------------------------- 210 | 211 | 212 | function [rho0, sigma0] = path_optimizing(loc_final, rt, Wp, Paths, Param, Object) 213 | 214 | xg = [Param.rho0_initial; Param.sigma0_initial]; 215 | 216 | lower_bound_rho = 0.05; % <0.05 issue started to occur 217 | lower_bound_sigma = 0; 218 | 219 | upper_bound_rho = 2; 220 | upper_bound_sigma = 1; 221 | 222 | % Set up the optimization problem 223 | problem.objective = @(x) PathDistObjective(x(1), x(2)); 224 | problem.x0 = xg; 225 | problem.Aineq = []; 226 | problem.bineq = []; 227 | problem.Aeq = []; 228 | problem.beq = []; 229 | problem.lb = [lower_bound_rho; lower_bound_sigma]; 230 | problem.ub = [upper_bound_rho; upper_bound_sigma]; 231 | problem.nonlcon = []; 232 | problem.solver = 'fmincon'; % specify the solver 233 | problem.options = optimoptions('fmincon', ... 234 | 'Algorithm', 'interior-point', ... % option2: sqp 235 | 'Display', 'off'); 236 | 237 | % Call fmincon 238 | [xOpt, fval, exitflag, output] = fmincon(problem); 239 | disp(output) 240 | rho0 = xOpt(1); 241 | sigma0 = xOpt(2); 242 | 243 | function totalLength = PathDistObjective(rho0, sigma0) 244 | Param.showDisp = 0; 245 | Param.useOptimizer = 0; 246 | [~, ~, totalLength] = IFDS(rho0, sigma0, loc_final, rt, Wp, Paths, Param, 1, Object); 247 | 248 | end 249 | 250 | end 251 | 252 | function PlotGamma(Gamma, Gamma_star, X, Y, Z, fontSize) 253 | xr = 0:200; 254 | yr = -100:100; 255 | % zr = 0:100; 256 | zr = 0:200; 257 | 258 | % fixed plane 259 | xfixed = 100; 260 | yfixed = 0; 261 | zfixed = 0; 262 | 263 | % limits of the rectangular plane 264 | x_plane_limits = [min(xr), max(xr)]; 265 | y_plane_limits = [min(yr), max(yr)]; 266 | z_plane_limits = [min(zr), max(zr)]; 267 | 268 | xy_vertices = [x_plane_limits(1), y_plane_limits(1), zfixed; 269 | x_plane_limits(1), y_plane_limits(2), zfixed; 270 | x_plane_limits(2), y_plane_limits(2), zfixed; 271 | x_plane_limits(2), y_plane_limits(1), zfixed]; 272 | yz_vertices = [xfixed, y_plane_limits(1), z_plane_limits(1); 273 | xfixed, y_plane_limits(2), z_plane_limits(1); 274 | xfixed, y_plane_limits(2), z_plane_limits(2); 275 | xfixed, y_plane_limits(1), z_plane_limits(2)]; 276 | xz_vertices = [x_plane_limits(1), yfixed, z_plane_limits(1); 277 | x_plane_limits(1), yfixed, z_plane_limits(2); 278 | x_plane_limits(2), yfixed, z_plane_limits(2); 279 | x_plane_limits(2), yfixed, z_plane_limits(1)]; 280 | faces = [1,2,3,4]; 281 | 282 | % Create a grid of X and Y values for X-Y, Y-Z, and X-Z plane 283 | [X_grid_xy, Y_grid_xy] = meshgrid(xr, yr); 284 | [Y_grid_yz, Z_grid_yz] = meshgrid(yr, zr); 285 | [X_grid_xz, Z_grid_xz] = meshgrid(xr, zr); 286 | 287 | % Convert the symbolic function to a numerical function 288 | Gamma_numeric = matlabFunction(Gamma, 'Vars', [X, Y, Z]); 289 | 290 | % Calculate Gamma for each pair plane 291 | Gamma_values_XY = Gamma_numeric(X_grid_xy, Y_grid_xy, zfixed); 292 | Gamma_values_YZ = Gamma_numeric(xfixed, Y_grid_yz, Z_grid_yz); 293 | Gamma_values_XZ = Gamma_numeric(X_grid_xz, yfixed, Z_grid_xz); 294 | 295 | % Define the number of countour levels 296 | num_levels = 30; 297 | max_Gamm = max([max(max(Gamma_values_XY)), max(max(Gamma_values_YZ)), max(max(Gamma_values_XZ))]); 298 | 299 | % Plot the Gamma distribution 300 | sp1 = subplot(2,2,1); 301 | fimplicit3(Gamma == 1,'EdgeColor','none','FaceAlpha',1,'MeshDensity',80), hold on 302 | fimplicit3(Gamma_star == 1, 'EdgeColor','none','FaceAlpha',0.2,'MeshDensity',30) 303 | patch('Vertices', xy_vertices, 'Faces', faces, 'FaceColor', 'blue', 'FaceAlpha', 0.2, 'EdgeColor', 'none'); 304 | patch('Vertices', yz_vertices, 'Faces', faces, 'FaceColor', 'red', 'FaceAlpha', 0.2, 'EdgeColor', 'none'); 305 | patch('Vertices', xz_vertices, 'Faces', faces, 'FaceColor', 'green', 'FaceAlpha', 0.2, 'EdgeColor', 'none'); 306 | axis equal 307 | xlim([0 200]) 308 | ylim([-100 100]) 309 | zlim([0 100]) 310 | xlabel('X [m]'); ylabel('Y [m]'); zlabel('Z [m]') 311 | set(gca, 'FontSize', fontSize) 312 | 313 | subplot(2,2,2) 314 | colormap(flipud(jet)) 315 | contourf(X_grid_xy, Y_grid_xy, Gamma_values_XY, num_levels), hold on 316 | [C3,h3] = contourf(X_grid_xy, Y_grid_xy, Gamma_values_XY, [1, 1], 'FaceAlpha',0,... 317 | 'LineColor', 'w', 'LineWidth', 2); 318 | colorbar 319 | clim([0 max_Gamm]) 320 | clabel(C3,h3,'FontSize',15,'Color','w') 321 | xlabel('X [m]'), ylabel('Y [m]') 322 | title("Top view (Z = " + num2str(zfixed) + ")"); 323 | grid on, grid minor, axis equal tight, hold off 324 | set(gca, 'FontSize', fontSize) 325 | 326 | % Plot the Y-Z plane distribution 327 | subplot(2,2,3) 328 | contourf(Y_grid_yz, Z_grid_yz, Gamma_values_YZ, num_levels), hold on 329 | [C4,h4] = contourf(Y_grid_yz, Z_grid_yz, Gamma_values_YZ, [1, 1], 'FaceAlpha',0,... 330 | 'LineColor', 'w', 'LineWidth', 2); 331 | colorbar 332 | clim([0 max_Gamm]) 333 | clabel(C4,h4,'FontSize',15,'Color','w') 334 | xlabel('Y [m]'), ylabel('Z [m]') 335 | title("Front view (X = " + num2str(xfixed) + ")"); 336 | grid on, grid minor, axis equal tight, hold off 337 | set(gca, 'FontSize', fontSize, 'XDir', 'reverse') 338 | 339 | % Plot the X-Z plane distribution 340 | subplot(2,2,4) 341 | contourf(X_grid_xz, Z_grid_xz, Gamma_values_XZ, num_levels), hold on 342 | [C5,h5] = contourf(X_grid_xz, Z_grid_xz, Gamma_values_XZ, [1, 1], 'FaceAlpha',0,... 343 | 'LineColor', 'w', 'LineWidth', 2); 344 | colorbar 345 | clim([0 max_Gamm]) 346 | clabel(C5,h5,'FontSize',15,'Color','w') 347 | xlabel('X [m]'), ylabel('Z [m]'); 348 | title("Side view (Y = " + num2str(yfixed) +")"); 349 | grid on, grid minor, axis equal tight, hold off 350 | set(gca, 'FontSize', fontSize) 351 | 352 | colormap(sp1, pink) 353 | sgt = sgtitle('Distribution of \Gamma(X, Y, Z)'); 354 | sgt.FontSize = fontSize + 2; 355 | end 356 | 357 | 358 | 359 | function [Gamma, Gamma_star] = PlotObject(Object, Rg, rt, rtsim, X, Y, Z, Gamma, Gamma_star) 360 | for j = 1:size(Object,2) 361 | x0 = Object(j).origin(rt, 1); 362 | y0 = Object(j).origin(rt, 2); 363 | z0 = Object(j).origin(rt, 3); 364 | a = Object(j).a; 365 | b = Object(j).b; 366 | c = Object(j).c; 367 | p = Object(j).p; 368 | q = Object(j).q; 369 | r = Object(j).r; 370 | 371 | Rstar = Object(j).Rstar; 372 | 373 | Gamma(X, Y, Z) = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 374 | Gamma_star(X, Y, Z) = Gamma - ( (Rstar + Rg)/Rstar )^2 + 1; 375 | 376 | if rtsim > 1 377 | fimplicit3(Gamma == 1,'EdgeColor','k','FaceAlpha',1,'MeshDensity',20), hold on 378 | fimplicit3(Gamma_star == 1, 'EdgeColor','k','FaceAlpha',0,'MeshDensity',20) 379 | else 380 | fimplicit3(Gamma == 1,'EdgeColor','none','FaceAlpha',1,'MeshDensity',80), hold on 381 | fimplicit3(Gamma_star == 1, 'EdgeColor','none','FaceAlpha',0.2,'MeshDensity',30) 382 | end 383 | colormap pink 384 | xlim([0 200]) 385 | ylim([-100 100]) 386 | zlim([0 100]) 387 | end 388 | 389 | end 390 | 391 | function PlotPath(rt, Paths, Xini, Yini, Zini, destin, multiTarget) 392 | if multiTarget 393 | plot3(Paths{1,rt}(1,:), Paths{1,rt}(2,:), Paths{1,rt}(3,:),'b', 'LineWidth', 1.5) 394 | hold on, grid on, grid minor, axis equal 395 | plot3(Paths{2,rt}(1,:), Paths{2,rt}(2,:), Paths{2,rt}(3,:),'b', 'LineWidth', 1.5) 396 | plot3(Paths{3,rt}(1,:), Paths{3,rt}(2,:), Paths{3,rt}(3,:),'b', 'LineWidth', 1.5) 397 | plot3(Paths{4,rt}(1,:), Paths{4,rt}(2,:), Paths{4,rt}(3,:),'b', 'LineWidth', 1.5) 398 | plot3(Paths{5,rt}(1,:), Paths{5,rt}(2,:), Paths{5,rt}(3,:),'b', 'LineWidth', 1.5) 399 | plot3(Paths{6,rt}(1,:), Paths{6,rt}(2,:), Paths{6,rt}(3,:),'b', 'LineWidth', 1.5) 400 | plot3(Paths{7,rt}(1,:), Paths{7,rt}(2,:), Paths{7,rt}(3,:),'b', 'LineWidth', 1.5) 401 | plot3(Paths{8,rt}(1,:), Paths{8,rt}(2,:), Paths{8,rt}(3,:),'b', 'LineWidth', 1.5) 402 | plot3(Paths{9,rt}(1,:), Paths{9,rt}(2,:), Paths{9,rt}(3,:),'b', 'LineWidth', 1.5) 403 | scatter3(Xini, Yini, Zini, 'filled', 'r') 404 | scatter3(destin(1,1),destin(1,2),destin(1,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 405 | scatter3(destin(2,1),destin(2,2),destin(2,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 406 | scatter3(destin(3,1),destin(3,2),destin(3,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 407 | scatter3(destin(4,1),destin(4,2),destin(4,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 408 | scatter3(destin(5,1),destin(5,2),destin(5,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 409 | scatter3(destin(6,1),destin(6,2),destin(6,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 410 | scatter3(destin(7,1),destin(7,2),destin(7,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 411 | scatter3(destin(8,1),destin(8,2),destin(8,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 412 | scatter3(destin(9,1),destin(9,2),destin(9,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5) 413 | else 414 | plot3(Paths{1,rt}(1,:), Paths{1,rt}(2,:), Paths{1,rt}(3,:),'b', 'LineWidth', 1.5) 415 | hold on, grid on, grid minor, axis equal 416 | scatter3(Xini, Yini, Zini, 'filled', 'r', 'xr', 'sizedata', 150) 417 | scatter3(destin(1,1),destin(1,2),destin(1,3), 'xr', 'xr', 'sizedata', 150) 418 | end 419 | 420 | xlim([0 200]) 421 | ylim([-100 100]) 422 | zlim([0 100]) 423 | % xlabel('X [m]'); ylabel('Y [m]'); zlabel('Z [m]'); 424 | % hold off 425 | end 426 | -------------------------------------------------------------------------------- /norm_ubar.m: -------------------------------------------------------------------------------- 1 | function length = norm_ubar(rho0, sigma0, Gamma, n, t, u, d, d0) 2 | 3 | % c1 = u'*(n)*n'*u; 4 | % c2 = u'*(t)*n'*u; 5 | % c3 = u'*(n)*n'*t*n'*u; 6 | % c4 = u'*(n)*t'*u; 7 | % c5 = u'*(n)*t'*(n)*n'*u; 8 | % c6 = u'*(n)*(t)'*t*n'*u; 9 | % c7 = n'*n; 10 | % 11 | % nn = norm(n); 12 | % tt = norm(t); 13 | % 14 | rho = rho0 * exp(1-1/(d0*d)); 15 | sigma = sigma0 * exp(1-1/(d0*d)); 16 | % 17 | % s1 = -2*c1/(abs(Gamma)^(1/rho)*c7); 18 | % s2 = (c2+c4)/(abs(Gamma)^(1/sigma)*tt*nn); 19 | % s3 = c1/(abs(Gamma)^(2/rho)*c7); 20 | % s4 = -c3/(abs(Gamma)^(1/rho)*abs(Gamma)^(1/sigma)*tt^2*nn^2); 21 | % s5 = -c5/(abs(Gamma)^(1/rho)*abs(Gamma)^(1/sigma)*tt*nn*c7); 22 | % s6 = c6/(abs(Gamma)^(2/sigma)*tt^2*nn^2); 23 | % 24 | % length = s1 + s2 + s3 + s4 + s5 + s6; 25 | 26 | M = eye(3) - n*n'/(abs(Gamma)^(1/rho)*(n')*n)... 27 | + t*n'/(abs(Gamma)^(1/sigma)*norm(t)*norm(n)); 28 | length = (M*u)'*(M*u); 29 | % length = sqrt(length)/(d^2); 30 | 31 | end -------------------------------------------------------------------------------- /path_dist_objective_v2.m: -------------------------------------------------------------------------------- 1 | function totalLength = path_dist_objective_v2(rho0, sigma0, Param) 2 | 3 | showPlot = 0; 4 | 5 | % Set-up Parameters 6 | tsim = Param.tsim; % [s] simulation time for the path 7 | rtsim = Param.rtsim; % [s] (50) time for the whole scenario 8 | dt = Param.dt; % [s] simulation time step 9 | C = Param.C; % [m/s] UAV cruising speed 10 | targetThresh = Param.targetThresh; % [m] allowed error for final target distance 11 | simMode = Param.simMode; % 1: by time, 2: by target distance 12 | multiTarget = Param.multiTarget; % 1: multi-target 0: single-target 13 | scene = Param.scene; % Scenario selection 14 | % 1) 1 cone, 2) 1 complex object 15 | % 7) non-urban 12) urban environment 16 | 17 | % Starting location 18 | Xini = Param.Xini; 19 | Yini = Param.Yini; 20 | Zini = Param.Zini; 21 | 22 | % Target Destination 23 | Xfinal = Param.Xfinal; 24 | Yfinal = Param.Yfinal; 25 | Zfinal = Param.Zfinal; 26 | 27 | % Tuning Parameters 28 | sf = Param.sf; % Shape-following demand (1=on, 0=off) 29 | 30 | %----------- Note ------------- 31 | % Good: rho0 = 2, simga0 = 0.01 32 | % The algorihtm still doesnt work for overlapped objects 33 | %------------------------------ 34 | 35 | % Structure Pre-allocation 36 | switch scene 37 | case 0, numObj = 1; 38 | case 1, numObj = 1; 39 | case 2, numObj = 2; 40 | case 3, numObj = 3; 41 | case 7, numObj = 7; 42 | case 12, numObj = 12; 43 | case 41, numObj = 1; 44 | case 42, numObj = 4; 45 | case 69, numObj = 4; 46 | end 47 | 48 | Object(numObj) = struct('origin',zeros(rtsim,3),'Gamma',0,'n',[],'t',[],'a',0,'b',0,'c',0,'p',0,'q',0,'r',0); 49 | 50 | %% Original Fluid 51 | 52 | if multiTarget 53 | destin = [200 0 20; 54 | 200 20 20; 55 | 200 -20 20; 56 | 200 20 30; 57 | 200 -20 30; 58 | 200 0 30; 59 | 200 0 40; 60 | 200 20 40; 61 | 200 -20 40;]; 62 | else 63 | destin = [Xfinal Yfinal Zfinal]; 64 | end 65 | 66 | numLine = size(destin,1); 67 | % L = 1; 68 | 69 | % Pre-allocate waypoints and path 70 | Wp = zeros(3, tsim+1); 71 | Paths = cell(numLine,rtsim); 72 | 73 | for rt = 1:rtsim 74 | tic 75 | Wp(:,1,rt) = [Xini; Yini; Zini]; % can change this to current uav pos 76 | for L = 1:numLine 77 | % disp("Calculating path for destination #" + num2str(L)) 78 | xd = (destin(L,1)); 79 | yd = (destin(L,2)); 80 | zd = (destin(L,3)); 81 | 82 | switch simMode 83 | case 1 % Simulate by time 84 | % disp("Simulating by time for " + num2str(tsim) + " s.") 85 | for t = 1:tsim 86 | xx = Wp(1,t); 87 | yy = Wp(2,t); 88 | zz = Wp(3,t); 89 | 90 | Object = create_scene(scene, Object, xx, yy, zz, rt); 91 | UBar = calc_ubar(xx, yy, zz, xd, yd, zd, Object, rho0, sigma0, C, sf); 92 | 93 | 94 | if norm([xx yy zz] - [xd yd zd]) < targetThresh 95 | % disp('Target destination reached!') 96 | Wp = Wp(:,1:t); 97 | Paths{L,rt} = Wp; % Save into cell array 98 | break 99 | else 100 | Wp(:,t+1) = Wp(:,t) + UBar * dt; 101 | end 102 | 103 | end 104 | 105 | case 2 % simulate by reaching distance 106 | disp("Simulating by distance until " + num2str(targetThresh) + " m error range") 107 | t = 1; 108 | 109 | while norm([Wp(1,t) Wp(2,t) Wp(3,t)] - double([xd yd zd])) > targetThresh 110 | 111 | xx = Wp(1,t); 112 | yy = Wp(2,t); 113 | zz = Wp(3,t); 114 | 115 | if n(xx,yy,zz)'*u(xx,yy,zz) < 0 || sf == 1 116 | % disp('case 1 activated') 117 | Wp(:,t+1) = Wp(:,t) + double(UBar(Wp(1,t), Wp(2,t), Wp(3,t))) * dt; 118 | 119 | elseif n(xx,yy,zz)'*u(xx,yy,zz) >= 0 && sf == 0 120 | % disp('case 2 activated') 121 | Wp(:,t+1) = Wp(:,t) + double(u(Wp(1,t), Wp(2,t), Wp(3,t))) * dt; 122 | end 123 | t = t+1; 124 | end 125 | % Removing extra space 126 | Wp = Wp(:,1:t,:); 127 | disp('Target destination reached!') 128 | end 129 | 130 | end 131 | 132 | % Plotting the path 133 | if showPlot 134 | figure(70) 135 | % set(gcf, 'Position', get(0, 'Screensize')); 136 | plot3(Paths{1,rt}(1,:), Paths{1,rt}(2,:), Paths{1,rt}(3,:),'b', 'LineWidth', 1.5) 137 | hold on, grid on, grid minor, axis equal 138 | scatter3(Xini, Yini, Zini, 'filled', 'r') 139 | scatter3(destin(1,1),destin(1,2),destin(1,3), 'xr') 140 | if multiTarget 141 | plot3(Paths{2,rt}(1,:), Paths{2,rt}(2,:), Paths{2,rt}(3,:),'b', 'LineWidth', 1.5) 142 | plot3(Paths{3,rt}(1,:), Paths{3,rt}(2,:), Paths{3,rt}(3,:),'b', 'LineWidth', 1.5) 143 | plot3(Paths{4,rt}(1,:), Paths{4,rt}(2,:), Paths{4,rt}(3,:),'b', 'LineWidth', 1.5) 144 | plot3(Paths{5,rt}(1,:), Paths{5,rt}(2,:), Paths{5,rt}(3,:),'b', 'LineWidth', 1.5) 145 | plot3(Paths{6,rt}(1,:), Paths{6,rt}(2,:), Paths{6,rt}(3,:),'b', 'LineWidth', 1.5) 146 | plot3(Paths{7,rt}(1,:), Paths{7,rt}(2,:), Paths{7,rt}(3,:),'b', 'LineWidth', 1.5) 147 | plot3(Paths{8,rt}(1,:), Paths{8,rt}(2,:), Paths{8,rt}(3,:),'b', 'LineWidth', 1.5) 148 | plot3(Paths{9,rt}(1,:), Paths{9,rt}(2,:), Paths{9,rt}(3,:),'b', 'LineWidth', 1.5) 149 | scatter3(destin(2,1),destin(2,2),destin(2,3), 'xr', 'sizedata', 100) 150 | scatter3(destin(3,1),destin(3,2),destin(3,3), 'xr', 'sizedata', 100) 151 | scatter3(destin(4,1),destin(4,2),destin(4,3), 'xr', 'sizedata', 100) 152 | scatter3(destin(5,1),destin(5,2),destin(5,3), 'xr', 'sizedata', 100) 153 | scatter3(destin(6,1),destin(6,2),destin(6,3), 'xr', 'sizedata', 100) 154 | scatter3(destin(7,1),destin(7,2),destin(7,3), 'xr', 'sizedata', 100) 155 | scatter3(destin(8,1),destin(8,2),destin(8,3), 'xr', 'sizedata', 100) 156 | scatter3(destin(9,1),destin(9,2),destin(9,3), 'xr', 'sizedata', 100) 157 | end 158 | title(num2str(rt,'time = %4.1f s')) 159 | xlim([0 200]) 160 | ylim([-100 100]) 161 | zlim([0 100]) 162 | % pause(0.05) 163 | xlabel('X [m]'); ylabel('Y [m]'); zlabel('Z [m]'); 164 | hold off 165 | end 166 | end 167 | 168 | %% post-Calculation 169 | % Calculate pairwise distances between waypoints 170 | waypoints = Paths{1,1}'; 171 | 172 | % Calculate the differences between consecutive waypoints 173 | differences = diff(waypoints); 174 | 175 | % Calculate the squared distances for each coordinate 176 | squaredDistances = sum(differences.^2, 2); 177 | 178 | % Calculate the total path length 179 | totalLength = sum(sqrt(squaredDistances)); 180 | end 181 | 182 | %% ------------------------------Function--------------------------------- 183 | 184 | function UBar = calc_ubar(X, Y, Z, xd, yd, zd, Obj, rho0, sigma0, C, sf) 185 | 186 | dist = sqrt((X - xd)^2 + (Y - yd)^2 + (Z - zd)^2); 187 | 188 | u = -[C*(X - xd)/dist, C*(Y - yd)/dist, C*(Z - zd)/dist]'; 189 | 190 | %% Components 191 | numObj = size(Obj,2); 192 | 193 | Mm = zeros(3); 194 | sum_w = 0; 195 | 196 | for j = 1:numObj 197 | 198 | % Reading Gamma for each object 199 | Gamma = Obj(j).Gamma; 200 | 201 | % Unit normal vector and Unit tangential vector 202 | n = Obj(j).n; 203 | t = Obj(j).t; 204 | 205 | % Object Distance from UAV 206 | x0 = Obj(j).origin(1); 207 | y0 = Obj(j).origin(2); 208 | z0 = Obj(j).origin(3); 209 | 210 | % Calculate parameters 211 | % ---- Can also optimize the rho0, sigma0 here for each object 212 | dist_obj = sqrt((X - x0)^2 + (Y - y0)^2 + (Z - z0)^2); 213 | rho = rho0 * exp(1 - 1/(dist_obj * dist)); 214 | sigma = sigma0 * exp(1 - 1/(dist_obj * dist)); 215 | 216 | % Modular Matrix (Perturbation Matrix 217 | ntu = n' * u; 218 | if ntu < 0 || sf == 1 219 | M = eye(3) - n*n'/(abs(Gamma)^(1/rho)*(n')*n)... 220 | + t*n'/(abs(Gamma)^(1/sigma)*norm(t)*norm(n)); % tao is removed for now 221 | elseif ntu >= 0 && sf == 0 222 | M = eye(3); 223 | end 224 | 225 | % Weight 226 | w = 1; 227 | for i = 1:numObj 228 | if i == j 229 | continue 230 | else 231 | w = w * (Obj(i).Gamma - 1)/... 232 | ((Obj(j).Gamma - 1) + (Obj(i).Gamma - 1)); 233 | end 234 | end 235 | sum_w = sum_w + w; 236 | 237 | % Saving to Field 238 | Obj(j).n = n; 239 | Obj(j).t = t; 240 | Obj(j).dist = dist_obj; 241 | Obj(j).rho = rho; 242 | Obj(j).sigma = sigma; 243 | Obj(j).M = M; 244 | Obj(j).w = w; 245 | 246 | end 247 | 248 | for j = 1:numObj 249 | Obj(j).w_tilde = Obj(j).w/sum_w; 250 | Mm = Mm + Obj(j).w_tilde * Obj(j).M; 251 | end 252 | 253 | UBar = Mm*u; 254 | end 255 | 256 | function Obj = create_scene(num, Obj, X, Y, Z, rt) 257 | switch num 258 | case 0 % Single object 259 | Obj(1) = create_cone(100, 5, 0, 50, 80, Obj(1)); 260 | 261 | case 1 % single(complex) object 262 | Obj(1) = create_cylinder(100, 5, 0, 25, 200, Obj(1)); 263 | Obj(2) = create_pipe(60, 20, 60, 80, 5, Obj(2)); 264 | Obj(3) = create_pipe(130, -30, 30, 100, 5, Obj(3)); 265 | 266 | case 2 % 2 objects 267 | Obj(1) = create_cylinder(60, 5, 0, 30, 50, Obj(1)); 268 | Obj(2) = create_sphere(120, -10, 0, 50, Obj(2)); 269 | 270 | case 3 % 3 objects 271 | Obj(1) = create_cylinder(60, 5, 0, 30, 50, Obj(1)); 272 | Obj(2) = create_sphere(120, -10, 0, 50, Obj(2)); 273 | Obj(3) = create_pipe(168, 0, 0, 25, 80, Obj(3)); 274 | 275 | case 12 % 12 objects 276 | Obj(1) = create_cylinder(100, 5, 0, 30, 50, Obj(1)); 277 | Obj(2) = create_pipe(140, 20, 0, 40,10, Obj(2)); 278 | Obj(3) = create_pipe(20, 20, 0, 24, 40, Obj(3)); 279 | Obj(4) = create_pipe(55, -20, 0, 28, 50, Obj(4)); 280 | Obj(5) = create_sphere(53, -60, 0, 50, Obj(5)); 281 | Obj(6) = create_pipe(150, -80, 0, 40, 50, Obj(6)); 282 | Obj(7) = create_cone(100, -35, 0, 50,45, Obj(7)); 283 | Obj(8) = create_cone(170, 2, 0, 20,50, Obj(8)); 284 | Obj(9) = create_cone(60, 35, 0, 50,30, Obj(9)); 285 | Obj(10) = create_cylinder(110, 70, 0, 60, 50, Obj(10)); 286 | Obj(11) = create_pipe(170, 60, 0, 40, 27, Obj(11)); 287 | Obj(12) = create_cone(150, -30, 0, 32, 45, Obj(12)); 288 | case 7 % 7 objects 289 | Obj(1) = create_cone(60,8, 0, 70, 50, Obj(1)); 290 | Obj(2) = create_cone(100,-24, 0, 89, 100, Obj(2)); 291 | Obj(3) = create_cone(160,40, -4, 100, 30, Obj(3)); 292 | Obj(4) = create_cone(100,100, -10, 150, 100, Obj(4)); 293 | Obj(5) = create_cone(180,-70, -10, 150, 20, Obj(5)); 294 | Obj(6) = create_cone(75,-75, -10, 150, 40, Obj(6)); 295 | Obj(7) = create_cylinder(170, -6, 0, 34, 100, Obj(7)); 296 | case 41 297 | Oy = -80 + 2*single(rt); 298 | Obj(1) = create_cylinder(100, Oy, 0, 50, 80, Obj(1)); 299 | case 42 300 | Oy1 = 0 + 50*sin(0.2*single(rt)); 301 | Oy2 = -20 - 20*sin(0.5*single(rt)); 302 | Oz2 = 40 + 20*cos(0.5*single(rt)); 303 | Obj(1) = create_cylinder(60, 5, 0, 30, 50, Obj(1)); 304 | Obj(2) = create_cylinder(110, -10, 0, 25, 80,Obj(2)); 305 | Obj(3) = create_cylinder(80, Oy1, 0, 20, 60, Obj(3)); 306 | Obj(4) = create_sphere(160, Oy2, Oz2, 30, Obj(4)); 307 | 308 | case 69 %pp 309 | Obj(1) = create_cylinder(100, 5, 0, 30, 80, Obj(1)); 310 | Obj(2) = create_sphere(100, 30, 0, 40, Obj(2)); 311 | Obj(3) = create_sphere(100, -20, 0, 40, Obj(3)); 312 | Obj(4) = create_sphere(100, 5, 80, 30, Obj(4)); 313 | end 314 | 315 | function Obj = create_sphere(x0, y0, z0, D, Obj) 316 | 317 | a = D/2; b = D/2; c = D/2; % Object's axis length 318 | p = 1; q = 1; r = 1; % Index parameters 319 | 320 | % Object Shape Equation 321 | Gamma = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 322 | 323 | % Differential 324 | [dGdx, dGdy, dGdz] = calc_dG(); 325 | 326 | % n and t 327 | n = [dGdx; dGdy; dGdz]; 328 | t = [dGdy; -dGdx; 0]; 329 | 330 | 331 | % Save to Field 332 | Obj.origin(rt,:) = [x0, y0, z0]; 333 | Obj.Gamma = Gamma; 334 | Obj.n = n; 335 | Obj.t = t; 336 | Obj.a = a; 337 | Obj.b = b; 338 | Obj.c = c; 339 | Obj.p = p; 340 | Obj.q = q; 341 | Obj.r = r; 342 | function [dGdx, dGdy, dGdz] = calc_dG() 343 | dGdx = (2*p*((X - x0)/a).^(2*p - 1))/a; 344 | dGdy = (2*q*((Y - y0)/b).^(2*q - 1))/b; 345 | dGdz = (2*r*((Z - z0)/c).^(2*r - 1))/c; 346 | end 347 | end 348 | 349 | function Obj = create_cylinder(x0, y0, z0, D, h, Obj) 350 | 351 | a = D/2; b = D/2; c = h; % Object's axis length 352 | p = 1; q = 1; r = 5; % Index parameters 353 | 354 | % Object Shape Equation 355 | Gamma = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 356 | 357 | % Differential 358 | [dGdx, dGdy, dGdz] = calc_dG(); 359 | 360 | % n and t 361 | n = [dGdx; dGdy; dGdz]; 362 | t = [dGdy; -dGdx; 0]; 363 | 364 | % Save to Field 365 | Obj.origin(rt,:) = [x0, y0, z0]; 366 | Obj.Gamma = Gamma; 367 | Obj.n = n; 368 | Obj.t = t; 369 | Obj.a = a; 370 | Obj.b = b; 371 | Obj.c = c; 372 | Obj.p = p; 373 | Obj.q = q; 374 | Obj.r = r; 375 | function [dGdx, dGdy, dGdz] = calc_dG() 376 | dGdx = (2*p*((X - x0)/a).^(2*p - 1))/a; 377 | dGdy = (2*q*((Y - y0)/b).^(2*q - 1))/b; 378 | dGdz = (2*r*((Z - z0)/c).^(2*r - 1))/c; 379 | end 380 | end 381 | 382 | function Obj = create_cone(x0, y0, z0, D, h, Obj) 383 | 384 | a = D/2; b = D/2; c = h; % Object's axis length 385 | p = 1; q = 1; r = 0.5; % Index parameters 386 | 387 | % Object Shape Equation 388 | Gamma = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 389 | 390 | % Differential 391 | [dGdx, dGdy, dGdz] = calc_dG(); 392 | 393 | % n and t 394 | n = [dGdx; dGdy; dGdz]; 395 | t = [dGdy; -dGdx; 0]; 396 | 397 | % Save to Field 398 | Obj.origin(rt,:) = [x0, y0, z0]; 399 | Obj.Gamma = Gamma; 400 | Obj.n = n; 401 | Obj.t = t; 402 | Obj.a = a; 403 | Obj.b = b; 404 | Obj.c = c; 405 | Obj.p = p; 406 | Obj.q = q; 407 | Obj.r = r; 408 | function [dGdx, dGdy, dGdz] = calc_dG() 409 | dGdx = (2*p*((X - x0)/a).^(2*p - 1))/a; 410 | dGdy = (2*q*((Y - y0)/b).^(2*q - 1))/b; 411 | dGdz = (2*r*((Z - z0)/c).^(2*r - 1))/c; 412 | end 413 | end 414 | 415 | function Obj = create_pipe(x0, y0, z0, D, h, Obj) 416 | 417 | a = D/2; b = D/2; c = h; % Object's axis length 418 | p = 2; q = 2; r = 2; % Index parameters 419 | 420 | % Object Shape Equation 421 | Gamma = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 422 | 423 | % Differential 424 | [dGdx, dGdy, dGdz] = calc_dG(); 425 | 426 | % n and t 427 | n = [dGdx; dGdy; dGdz]; 428 | t = [dGdy; -dGdx; 0]; 429 | 430 | % Save to Field 431 | Obj.origin(rt,:) = [x0, y0, z0]; 432 | Obj.Gamma = Gamma; 433 | Obj.n = n; 434 | Obj.t = t; 435 | Obj.a = a; 436 | Obj.b = b; 437 | Obj.c = c; 438 | Obj.p = p; 439 | Obj.q = q; 440 | Obj.r = r; 441 | function [dGdx, dGdy, dGdz] = calc_dG() 442 | dGdx = (2*p*((X - x0)/a).^(2*p - 1))/a; 443 | dGdy = (2*q*((Y - y0)/b).^(2*q - 1))/b; 444 | dGdz = (2*r*((Z - z0)/c).^(2*r - 1))/c; 445 | end 446 | end 447 | 448 | 449 | end 450 | function plot_multi(rt, Paths, Xini, Yini, Zini, destin) 451 | figure(69) 452 | plot3(Paths{1,rt}(1,:), Paths{1,rt}(2,:), Paths{1,rt}(3,:),'b', 'LineWidth', 1.5) 453 | hold on, grid on, grid minor, axis equal 454 | plot3(Paths{2,rt}(1,:), Paths{2,rt}(2,:), Paths{2,rt}(3,:),'b', 'LineWidth', 1.5) 455 | plot3(Paths{3,rt}(1,:), Paths{3,rt}(2,:), Paths{3,rt}(3,:),'b', 'LineWidth', 1.5) 456 | plot3(Paths{4,rt}(1,:), Paths{4,rt}(2,:), Paths{4,rt}(3,:),'b', 'LineWidth', 1.5) 457 | plot3(Paths{5,rt}(1,:), Paths{5,rt}(2,:), Paths{5,rt}(3,:),'b', 'LineWidth', 1.5) 458 | plot3(Paths{6,rt}(1,:), Paths{6,rt}(2,:), Paths{6,rt}(3,:),'b', 'LineWidth', 1.5) 459 | plot3(Paths{7,rt}(1,:), Paths{7,rt}(2,:), Paths{7,rt}(3,:),'b', 'LineWidth', 1.5) 460 | plot3(Paths{8,rt}(1,:), Paths{8,rt}(2,:), Paths{8,rt}(3,:),'b', 'LineWidth', 1.5) 461 | plot3(Paths{9,rt}(1,:), Paths{9,rt}(2,:), Paths{9,rt}(3,:),'b', 'LineWidth', 1.5) 462 | scatter3(Xini, Yini, Zini, 'filled', 'r') 463 | scatter3(destin(1,1),destin(1,2),destin(1,3), 'xr', 'xr', 'sizedata', 100) 464 | scatter3(destin(2,1),destin(2,2),destin(2,3), 'xr', 'xr', 'sizedata', 100) 465 | scatter3(destin(3,1),destin(3,2),destin(3,3), 'xr', 'xr', 'sizedata', 100) 466 | scatter3(destin(4,1),destin(4,2),destin(4,3), 'xr', 'xr', 'sizedata', 100) 467 | scatter3(destin(5,1),destin(5,2),destin(5,3), 'xr', 'xr', 'sizedata', 100) 468 | scatter3(destin(6,1),destin(6,2),destin(6,3), 'xr', 'xr', 'sizedata', 100) 469 | scatter3(destin(7,1),destin(7,2),destin(7,3), 'xr', 'xr', 'sizedata', 100) 470 | scatter3(destin(8,1),destin(8,2),destin(8,3), 'xr', 'xr', 'sizedata', 100) 471 | scatter3(destin(9,1),destin(9,2),destin(9,3), 'xr', 'xr', 'sizedata', 100) 472 | title(num2str(rt,'time = %4.1f s')) 473 | xlim([0 200]) 474 | ylim([-100 100]) 475 | zlim([0 100]) 476 | end 477 | 478 | function plot_single(rt, Paths, Xini, Yini, Zini, destin) 479 | figure(69) 480 | plot3(Paths{1,rt}(1,:), Paths{1,rt}(2,:), Paths{1,rt}(3,:),'b', 'LineWidth', 1.5) 481 | hold on, grid on, grid minor, axis equal 482 | scatter3(Xini, Yini, Zini, 'filled', 'r', 'xr', 'sizedata', 100) 483 | scatter3(destin(1,1),destin(1,2),destin(1,3), 'xr', 'xr', 'sizedata', 100) 484 | title(num2str(rt,'time = %4.1f s')) 485 | xlim([0 200]) 486 | ylim([-100 100]) 487 | zlim([0 100]) 488 | end 489 | 490 | -------------------------------------------------------------------------------- /path_distance_objective.m: -------------------------------------------------------------------------------- 1 | function total_distance = path_distance_objective(rho0, sigma0) 2 | 3 | % Set-up Parameters 4 | tsim = uint16(400); % [s] simulation time for the path 5 | rtsim = 1; % [s] (50) time for the whole scenario 6 | dt = 0.1; % [s] simulation time step 7 | C = 30; % [m/s] UAV cruising speed 8 | targetThresh = 2.5; % [m] allowed error for final target distance 9 | scene = uint8(2); % Scenario selection 10 | % 1) 1 cone, 2) 1 complex object 11 | % 7) non-urban 12) urban environment 12 | 13 | % Starting location 14 | Xini = 0; 15 | Yini = 0; 16 | Zini = 0; 17 | 18 | % Target Destination 19 | Xfinal = 200; 20 | Yfinal = 0; 21 | Zfinal = 50; 22 | 23 | % Structure Pre-allocation 24 | switch scene 25 | case 0, numObj = 1; 26 | case 1, numObj = 1; 27 | case 2, numObj = 2; 28 | case 3, numObj = 3; 29 | case 7, numObj = 7; 30 | case 12, numObj = 12; 31 | case 41, numObj = 1; 32 | case 42, numObj = 4; 33 | case 69, numObj = 4; 34 | end 35 | 36 | Object(numObj) = struct('origin',zeros(rtsim,3),'Gamma',0,'dGdx',0,'dGdy',0,'dGdz',0,'a',0,'b',0,'c',0,'p',0,'q',0,'r',0); 37 | 38 | % Pre-allocation 39 | Wp = zeros(3, tsim+1); 40 | 41 | for rt = 1:rtsim 42 | Wp(:,1,rt) = [Xini; Yini; Zini]; % can change this to current uav pos 43 | total_distance = 0; 44 | xd = Xfinal; 45 | yd = Yfinal; 46 | zd = Zfinal; 47 | 48 | for t = 1:tsim 49 | xx = Wp(1,t); 50 | yy = Wp(2,t); 51 | zz = Wp(3,t); 52 | 53 | Object = create_scene(scene, Object, xx, yy, zz, rt); 54 | [UBar, n, u] = calc_ubar(xx, yy, zz, xd, yd, zd, Object, rho0, sigma0, C); 55 | 56 | ntu = n'*u; 57 | if norm([xx yy zz] - [xd yd zd]) < targetThresh 58 | Wp = Wp(:,1:t); 59 | break 60 | else 61 | if ntu < 0 || sf == 1 62 | Wp(:,t+1) = Wp(:,t) + UBar * dt; 63 | elseif ntu >= 0 && sf == 0 64 | Wp(:,t+1) = Wp(:,t) + u * dt; 65 | end 66 | end 67 | 68 | diff_segment = Wp(:,t+1) - Wp(:,t); 69 | total_distance = total_distance + sqrt(sum(diff_segment.^2)); 70 | 71 | end 72 | total_distance 73 | end 74 | 75 | end 76 | 77 | function [UBar, n, u] = calc_ubar(X, Y, Z, xd, yd, zd, Obj, rho0, sigma0, C) 78 | 79 | dist = sqrt((X - xd)^2 + (Y - yd)^2 + (Z - zd)^2); 80 | 81 | u = -[C*(X - xd)/dist, C*(Y - yd)/dist, C*(Z - zd)/dist]'; 82 | 83 | %% Components 84 | numObj = size(Obj,2); 85 | 86 | Mm = zeros(3); 87 | sum_w = 0; 88 | 89 | for j = 1:numObj 90 | dGdx = Obj(j).dGdx; 91 | dGdy = Obj(j).dGdy; 92 | dGdz = Obj(j).dGdz; 93 | % Reading Gamma for each object 94 | Gamma = Obj(j).Gamma; 95 | 96 | % Unit normal vector 97 | n = [dGdx; dGdy; dGdz]; 98 | 99 | % Unit tangential vector 100 | t = [dGdy; -dGdx; 0]; 101 | 102 | % Object Distance from UAV 103 | x0 = Obj(j).origin(1); 104 | y0 = Obj(j).origin(2); 105 | z0 = Obj(j).origin(3); 106 | 107 | % Calculate parameters 108 | dist_obj = sqrt((X - x0)^2 + (Y - y0)^2 + (Z - z0)^2); 109 | rho = rho0 * exp(1 - 1/(dist_obj * dist)); 110 | sigma = sigma0 * exp(1 - 1/(dist_obj * dist)); 111 | 112 | % Modular Matrix (Perturbation Matrix 113 | M = eye(3) - n*n'/(abs(Gamma)^(1/rho)*(n')*n)... 114 | + t*n'/(abs(Gamma)^(1/sigma)*norm(t)*norm(n)); % tao is removed for now 115 | 116 | % Weight 117 | w = 1; 118 | for i = 1:numObj 119 | if i == j 120 | continue 121 | else 122 | w = w * (Obj(i).Gamma - 1)/... 123 | ((Obj(j).Gamma - 1) + (Obj(i).Gamma - 1)); 124 | end 125 | end 126 | sum_w = sum_w + w; 127 | 128 | % Saving to Field 129 | Obj(j).n = n; 130 | Obj(j).t = t; 131 | Obj(j).dist = dist_obj; 132 | Obj(j).rho = rho; 133 | Obj(j).sigma = sigma; 134 | Obj(j).M = M; 135 | Obj(j).w = w; 136 | 137 | end 138 | 139 | for j = 1:numObj 140 | Obj(j).w_tilde = Obj(j).w/sum_w; 141 | Mm = Mm + Obj(j).w_tilde * Obj(j).M; 142 | end 143 | 144 | UBar = Mm*u; 145 | 146 | end 147 | 148 | function Obj = create_scene(num, Obj, X, Y, Z, rt) 149 | switch num 150 | case 0 % Single object 151 | Obj(1) = create_cone(100, 5, 0, 50, 80, Obj(1)); 152 | 153 | case 1 % single(complex) object 154 | Obj(1) = create_cylinder(100, 5, 0, 25, 200, Obj(1)); 155 | Obj(2) = create_pipe(60, 20, 60, 80, 5, Obj(2)); 156 | Obj(3) = create_pipe(130, -30, 30, 100, 5, Obj(3)); 157 | 158 | case 2 % 2 objects 159 | Obj(1) = create_cylinder(60, 5, 0, 30, 50, Obj(1)); 160 | Obj(2) = create_sphere(120, -10, 0, 50, Obj(2)); 161 | 162 | case 3 % 3 objects 163 | Obj(1) = create_cylinder(60, 5, 0, 30, 50, Obj(1)); 164 | Obj(2) = create_sphere(120, -10, 0, 50, Obj(2)); 165 | Obj(3) = create_pipe(168, 0, 0, 25, 80, Obj(3)); 166 | 167 | case 12 % 12 objects 168 | Obj(1) = create_cylinder(100, 5, 0, 30, 50, Obj(1)); 169 | Obj(2) = create_pipe(140, 20, 0, 40,10, Obj(2)); 170 | Obj(3) = create_pipe(20, 20, 0, 24, 40, Obj(3)); 171 | Obj(4) = create_pipe(55, -20, 0, 28, 50, Obj(4)); 172 | Obj(5) = create_sphere(53, -60, 0, 50, Obj(5)); 173 | Obj(6) = create_pipe(150, -80, 0, 40, 50, Obj(6)); 174 | Obj(7) = create_cone(100, -35, 0, 50,45, Obj(7)); 175 | Obj(8) = create_cone(170, 2, 0, 20,50, Obj(8)); 176 | Obj(9) = create_cone(60, 35, 0, 50,30, Obj(9)); 177 | Obj(10) = create_cylinder(110, 70, 0, 60, 50, Obj(10)); 178 | Obj(11) = create_pipe(170, 60, 0, 40, 27, Obj(11)); 179 | Obj(12) = create_cone(150, -30, 0, 32, 45, Obj(12)); 180 | case 7 % 7 objects 181 | Obj(1) = create_cone(60,8, 0, 70, 50, Obj(1)); 182 | Obj(2) = create_cone(100,-24, 0, 89, 100, Obj(2)); 183 | Obj(3) = create_cone(160,40, -4, 100, 30, Obj(3)); 184 | Obj(4) = create_cone(100,100, -10, 150, 100, Obj(4)); 185 | Obj(5) = create_cone(180,-70, -10, 150, 20, Obj(5)); 186 | Obj(6) = create_cone(75,-75, -10, 150, 40, Obj(6)); 187 | Obj(7) = create_cylinder(170, -6, 0, 34, 100, Obj(7)); 188 | case 41 189 | Oy = -80 + 2*single(rt); 190 | Obj(1) = create_cylinder(100, Oy, 0, 50, 80, Obj(1)); 191 | case 42 192 | Oy1 = 0 + 50*sin(0.2*single(rt)); 193 | Oy2 = -20 - 20*sin(0.5*single(rt)); 194 | Oz2 = 40 + 20*cos(0.5*single(rt)); 195 | Obj(1) = create_cylinder(60, 5, 0, 30, 50, Obj(1)); 196 | Obj(2) = create_cylinder(110, -10, 0, 25, 80,Obj(2)); 197 | Obj(3) = create_cylinder(80, Oy1, 0, 20, 60, Obj(3)); 198 | Obj(4) = create_sphere(160, Oy2, Oz2, 30, Obj(4)); 199 | 200 | case 69 %pp 201 | Obj(1) = create_cylinder(100, 5, 0, 30, 80, Obj(1)); 202 | Obj(2) = create_sphere(100, 30, 0, 40, Obj(2)); 203 | Obj(3) = create_sphere(100, -20, 0, 40, Obj(3)); 204 | Obj(4) = create_sphere(100, 5, 80, 30, Obj(4)); 205 | end 206 | 207 | function Obj = create_sphere(x0, y0, z0, D, Obj) 208 | 209 | a = D/2; b = D/2; c = D/2; % Object's axis length 210 | p = 1; q = 1; r = 1; % Index parameters 211 | 212 | % Object Shape Equation 213 | Gamma = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 214 | 215 | % Differential 216 | [dGdx, dGdy, dGdz] = calc_dG(); 217 | 218 | % Save to Field 219 | Obj.origin(rt,:) = [x0, y0, z0]; 220 | Obj.Gamma = Gamma; 221 | Obj.dGdx = dGdx; 222 | Obj.dGdy = dGdy; 223 | Obj.dGdz = dGdz; 224 | Obj.a = a; 225 | Obj.b = b; 226 | Obj.c = c; 227 | Obj.p = p; 228 | Obj.q = q; 229 | Obj.r = r; 230 | function [dGdx, dGdy, dGdz] = calc_dG() 231 | dGdx = (2*p*((X - x0)/a).^(2*p - 1))/a; 232 | dGdy = (2*q*((Y - y0)/b).^(2*q - 1))/b; 233 | dGdz = (2*r*((Z - z0)/c).^(2*r - 1))/c; 234 | end 235 | end 236 | 237 | function Obj = create_cylinder(x0, y0, z0, D, h, Obj) 238 | 239 | a = D/2; b = D/2; c = h; % Object's axis length 240 | p = 1; q = 1; r = 5; % Index parameters 241 | 242 | % Object Shape Equation 243 | Gamma = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 244 | 245 | % Differential 246 | [dGdx, dGdy, dGdz] = calc_dG(); 247 | 248 | % Save to Field 249 | Obj.origin(rt,:) = [x0, y0, z0]; 250 | Obj.Gamma = Gamma; 251 | Obj.dGdx = dGdx; 252 | Obj.dGdy = dGdy; 253 | Obj.dGdz = dGdz; 254 | Obj.a = a; 255 | Obj.b = b; 256 | Obj.c = c; 257 | Obj.p = p; 258 | Obj.q = q; 259 | Obj.r = r; 260 | function [dGdx, dGdy, dGdz] = calc_dG() 261 | dGdx = (2*p*((X - x0)/a).^(2*p - 1))/a; 262 | dGdy = (2*q*((Y - y0)/b).^(2*q - 1))/b; 263 | dGdz = (2*r*((Z - z0)/c).^(2*r - 1))/c; 264 | end 265 | end 266 | 267 | function Obj = create_cone(x0, y0, z0, D, h, Obj) 268 | 269 | a = D/2; b = D/2; c = h; % Object's axis length 270 | p = 1; q = 1; r = 0.5; % Index parameters 271 | 272 | % Object Shape Equation 273 | Gamma = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 274 | 275 | % Differential 276 | [dGdx, dGdy, dGdz] = calc_dG(); 277 | 278 | % Save to Field 279 | Obj.origin(rt,:) = [x0, y0, z0]; 280 | Obj.Gamma = Gamma; 281 | Obj.dGdx = dGdx; 282 | Obj.dGdy = dGdy; 283 | Obj.dGdz = dGdz; 284 | Obj.a = a; 285 | Obj.b = b; 286 | Obj.c = c; 287 | Obj.p = p; 288 | Obj.q = q; 289 | Obj.r = r; 290 | function [dGdx, dGdy, dGdz] = calc_dG() 291 | dGdx = (2*p*((X - x0)/a).^(2*p - 1))/a; 292 | dGdy = (2*q*((Y - y0)/b).^(2*q - 1))/b; 293 | dGdz = (2*r*((Z - z0)/c).^(2*r - 1))/c; 294 | end 295 | end 296 | 297 | function Obj = create_pipe(x0, y0, z0, D, h, Obj) 298 | 299 | a = D/2; b = D/2; c = h; % Object's axis length 300 | p = 2; q = 2; r = 2; % Index parameters 301 | 302 | % Object Shape Equation 303 | Gamma = ((X - x0) / a).^(2*p) + ((Y - y0) / b).^(2*q) + ((Z - z0) / c).^(2*r); 304 | 305 | % Differential 306 | [dGdx, dGdy, dGdz] = calc_dG(); 307 | 308 | % Save to Field 309 | Obj.origin(rt,:) = [x0, y0, z0]; 310 | Obj.Gamma = Gamma; 311 | Obj.dGdx = dGdx; 312 | Obj.dGdy = dGdy; 313 | Obj.dGdz = dGdz; 314 | Obj.a = a; 315 | Obj.b = b; 316 | Obj.c = c; 317 | Obj.p = p; 318 | Obj.q = q; 319 | Obj.r = r; 320 | function [dGdx, dGdy, dGdz] = calc_dG() 321 | dGdx = (2*p*((X - x0)/a).^(2*p - 1))/a; 322 | dGdy = (2*q*((Y - y0)/b).^(2*q - 1))/b; 323 | dGdz = (2*r*((Z - z0)/c).^(2*r - 1))/c; 324 | end 325 | end 326 | 327 | 328 | end -------------------------------------------------------------------------------- /plotting_everything.m: -------------------------------------------------------------------------------- 1 | % UAV Arrow 2 | % Destination 3 | pltDestin = scatter3(destin(1,1),destin(1,2),destin(1,3), 'xr', 'xr', 'sizedata', 150, 'LineWidth', 1.5); 4 | 5 | hold on, grid on, axis equal 6 | 7 | % if animation 8 | pltArrow = quiver3(traj{rt}(1,1), traj{rt}(2,1), traj{rt}(3,1),... 9 | traj{rt}(1,end)-traj{rt}(1,1), traj{rt}(2,end)-traj{rt}(2,1),... 10 | traj{rt}(3,end)-traj{rt}(3,1), 'ok','filled', 'LineWidth', 1.5, 'MaxHeadSize',100,'AutoScaleFactor', 2,... 11 | 'Alignment','tail', 'MarkerSize', 12, 'MarkerFaceColor','w','ShowArrowHead','on'); 12 | % end 13 | 14 | 15 | 16 | % IFDS Path, if available 17 | if ~isempty(Paths{rt}) 18 | pltPath = PlotPath(rt, Paths, Xini, Yini, Zini, destin, multiTarget); 19 | end 20 | 21 | % Trail of the UAV trajectory 22 | if rt>1 23 | prevTraj = [traj{1:rt-1}]; 24 | pltTraj = plot3(prevTraj(1,:), prevTraj(2,:), prevTraj(3,:), 'k', 'LineWidth', 1.2); 25 | end 26 | 27 | 28 | 29 | % Obstacle 30 | [Gamma, Gamma_star] = PlotObject(Object, delta_g, rt, rtsim, X, Y, Z, Gamma, Gamma_star); 31 | xlabel('X [m]'); ylabel('Y [m]'); zlabel('Z [m]'); camlight 32 | 33 | % Constraint Matrix 34 | % imagesc(0:200, -100:100, weatherMat(:,:,rt), 'AlphaData',1) 35 | 36 | 37 | 38 | set(gca, 'LineWidth', 2, 'FontSize', fontSize-8) 39 | hold off 40 | % colormap turbo 41 | clim([0 1]) 42 | 43 | %% Functions 44 | 45 | 46 | -------------------------------------------------------------------------------- /real-time_analysis/realtime_analysis.m: -------------------------------------------------------------------------------- 1 | clc, clear, close all 2 | lw = 2; 3 | 4 | t4 = load("time_test4.mat"); 5 | t5 = load("time_test5.mat"); 6 | t10 = load("time_test10.mat"); 7 | t11 = load("time_test11.mat"); 8 | t12 = load("time_test12.mat"); 9 | t13 = load("time_test13.mat"); 10 | t14 = load("time_test14.mat"); 11 | t15 = load("time_test15.mat"); 12 | t16 = load("time_test16.mat"); 13 | 14 | t12sp = load("time_test12sp.mat"); 15 | 16 | 17 | 18 | 19 | 20 | t4 = t4.timer; 21 | t5 = t5.timer; 22 | t10 = t10.timer; 23 | t11 = t11.timer; 24 | t12 = t12.timer; 25 | t13 = t13.timer; 26 | t14 = t14.timer; 27 | t15 = t15.timer; 28 | t16 = t16.timer; 29 | t12sp = t12sp.timer; 30 | 31 | figure 32 | plot(t12,'o-','LineWidth', lw), hold on 33 | plot(t12sp,'o-','LineWidth', lw), hold on 34 | plot(t13,'o-','LineWidth', lw) 35 | plot(t14,'o-','LineWidth', lw) 36 | grid on 37 | legend("No objects (scenario no.0)", "1 Object (scenario no.1)", "3 Objects (scenario no.2)",... 38 | "12 Objects (scenario no.3)") 39 | xlabel("Elapsed simulation time (s)") 40 | ylabel("Computed time (s)") 41 | title("Various scenarios with dynamic environmental constraints") 42 | set(gca, 'FontSize', 24, 'LineWidth', 2) 43 | 44 | 45 | figure 46 | plot(t4,'o-','LineWidth', lw), hold on 47 | plot(t10,'o-','LineWidth', lw) 48 | plot(t15,'o-','LineWidth', lw) 49 | grid on 50 | legend("No environmental constraints", "Static environmental constraints",... 51 | "Dynamic environmental constraints") 52 | xlabel("Elapsed simulation time (s)") 53 | ylabel("Computed time (s)") 54 | title("Scenario no.4 : 1 static obstacle + 2 dynamic obstacles") 55 | set(gca, 'FontSize', 24, 'LineWidth', 2) 56 | 57 | 58 | figure 59 | plot(t5,'o-','LineWidth', lw), hold on 60 | plot(t11,'o-','LineWidth', lw) 61 | plot(t16,'o-','LineWidth', lw) 62 | grid on 63 | legend("No environmental constraints", "Static environmental constraints",... 64 | "Dynamic environmental constraints") 65 | xlabel("Elapsed simulation time (s)") 66 | ylabel("Computed time (s)") 67 | title("Scenario no.5 : 2 static obstacles + 2 dynamic obstacles") 68 | set(gca, 'FontSize', 24, 'LineWidth', 2) 69 | 70 | xspace = 0:1:49; 71 | figure 72 | stem(xspace, t4*1000,'b','LineWidth', lw+0.7), hold on 73 | stem(xspace, t5*1000, 'r', 'LineWidth', lw) 74 | grid on, grid minor 75 | legend("Example 1 (1 static + 2 dynamic obstacles)", "Example 2 (2 static + 2 dynamic obstacles") 76 | xlim([0,25]) 77 | xlabel("Elapsed simulation time (s)") 78 | ylabel("Computed time (ms)") 79 | % title("Computing time of the IFDS with safeguarding for avoiding dynamic and static obstacles") 80 | set(gca, 'FontSize', 24, 'LineWidth', 2) 81 | 82 | -------------------------------------------------------------------------------- /real-time_analysis/time_test10.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/komxun/IFDS-Algorithm/9906efe91a2b0ad577fa04da683390ffecc82094/real-time_analysis/time_test10.mat -------------------------------------------------------------------------------- /real-time_analysis/time_test11.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/komxun/IFDS-Algorithm/9906efe91a2b0ad577fa04da683390ffecc82094/real-time_analysis/time_test11.mat -------------------------------------------------------------------------------- /real-time_analysis/time_test12.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/komxun/IFDS-Algorithm/9906efe91a2b0ad577fa04da683390ffecc82094/real-time_analysis/time_test12.mat -------------------------------------------------------------------------------- /real-time_analysis/time_test12sp.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/komxun/IFDS-Algorithm/9906efe91a2b0ad577fa04da683390ffecc82094/real-time_analysis/time_test12sp.mat -------------------------------------------------------------------------------- /real-time_analysis/time_test13.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/komxun/IFDS-Algorithm/9906efe91a2b0ad577fa04da683390ffecc82094/real-time_analysis/time_test13.mat -------------------------------------------------------------------------------- /real-time_analysis/time_test14.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/komxun/IFDS-Algorithm/9906efe91a2b0ad577fa04da683390ffecc82094/real-time_analysis/time_test14.mat -------------------------------------------------------------------------------- /real-time_analysis/time_test15.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/komxun/IFDS-Algorithm/9906efe91a2b0ad577fa04da683390ffecc82094/real-time_analysis/time_test15.mat -------------------------------------------------------------------------------- /real-time_analysis/time_test16.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/komxun/IFDS-Algorithm/9906efe91a2b0ad577fa04da683390ffecc82094/real-time_analysis/time_test16.mat -------------------------------------------------------------------------------- /real-time_analysis/time_test4.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/komxun/IFDS-Algorithm/9906efe91a2b0ad577fa04da683390ffecc82094/real-time_analysis/time_test4.mat -------------------------------------------------------------------------------- /real-time_analysis/time_test5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/komxun/IFDS-Algorithm/9906efe91a2b0ad577fa04da683390ffecc82094/real-time_analysis/time_test5.mat -------------------------------------------------------------------------------- /time_journal_dyna_1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/komxun/IFDS-Algorithm/9906efe91a2b0ad577fa04da683390ffecc82094/time_journal_dyna_1.mat -------------------------------------------------------------------------------- /time_journal_dyna_2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/komxun/IFDS-Algorithm/9906efe91a2b0ad577fa04da683390ffecc82094/time_journal_dyna_2.mat -------------------------------------------------------------------------------- /time_optim_journal_dyna_2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/komxun/IFDS-Algorithm/9906efe91a2b0ad577fa04da683390ffecc82094/time_optim_journal_dyna_2.mat --------------------------------------------------------------------------------