├── PF-toolbox ├── Bernouli.avi ├── GTF_Simulink_PlotAUV.m ├── M1.mat ├── MPC_setup.m ├── NLP_gamma.m ├── PF_ToolwithMedusa.slx ├── PF_ToolwithMedusa_Fix.slx ├── PF_ToolwithMedusa_Second.slx ├── PF_ToolwithMedusa_test.slx ├── PF_ToolwithMedusa_test_sfun.mexa64 ├── PFcontrollers.m ├── PFtools.m ├── README.md ├── RK4_integrator.m ├── Testpath.m ├── Trajectory_tracking.m ├── alg_convert.m ├── all_methods.avi ├── all_methods.fig ├── all_methods.mat ├── all_methods.png ├── animation_1vehicles.m ├── arrow3.m ├── bernoulli.avi ├── bernoulli.fig ├── bernoulli.png ├── compute_time_1.mat ├── compute_time_2.mat ├── compute_time_3.mat ├── compute_time_4.mat ├── compute_time_5.mat ├── compute_time_6.mat ├── compute_time_7.mat ├── heart.avi ├── method1.mat ├── method2.mat ├── path_eq.m ├── path_state.m ├── path_state_function.m ├── paths.m ├── paths_information.m ├── plot_animation_1vehicle.m ├── plot_arrow.m ├── plot_compute_time.m ├── plot_results.m ├── resample_1vehicles.m ├── save_to_base.m ├── sin.avi ├── sin.png ├── software_architecture.m ├── swarp.m ├── test.m └── vehicle_models.m ├── README.md └── sin.avi /PF-toolbox/Bernouli.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/Bernouli.avi -------------------------------------------------------------------------------- /PF-toolbox/GTF_Simulink_PlotAUV.m: -------------------------------------------------------------------------------- 1 | function GTF_Simulink_PlotAUV(pos, orient, scale_num, time, fuselage_colour,alpha) 2 | % ========================================================================= 3 | % Go To Formation - Plot AUV. 4 | % ========================================================================= 5 | % Plots a 3D version of a torpedo-shaped AUV. Uses Bruce R. Land's 6 | % Hierarchical Matlab Renderer. 7 | % http://www.nbb.cornell.edu/neurobio/land/PROJECTS/Hierarchy/ 8 | % 9 | % Code by: Andreas J. Haeusler 10 | % Last Revision: October 2009 11 | % 12 | % 13 | % 2009-10-07 clean new implementation 14 | %========================================================================== 15 | 16 | %build the fuselage 17 | tmpsphere = UnitSphere(2); 18 | tmpsphere.facecolor = fuselage_colour; 19 | % nose=translate(scale(tmpsphere,3,1,1),-4.5,0,1.01); 20 | nose=translate(scale(tmpsphere,3,1,1),-4.5,0,1.01); 21 | tailcap=translate(scale(tmpsphere,2,1,1),4.5,0,1.01); 22 | cyl1=UnitCylinder(4); 23 | cyl1=translate(rotateY(scale(cyl1,1,1,5),90),0,0,1.01); 24 | cyl1.facecolor=fuselage_colour; 25 | fuselage = combine(cyl1, nose, tailcap); 26 | 27 | % fins and rudders 28 | fins=translate(scale(UnitSphere(2),.75,3,.2),5,0,1); 29 | fins.facecolor = 'blue'; 30 | rudders=translate(rotateX(scale(UnitSphere(2),.75,3,.2),90),5,0,1); 31 | rudders.facecolor = 'blue'; 32 | foils = combine(fins, rudders); 33 | 34 | %motors and propellers 35 | motor = scale(UnitSphere(2),1.5,.5,.5); 36 | motor.facecolor = 'black'; 37 | motors = translate(motor, 5.5,0,1); 38 | prop=rotateX(scale(UnitSphere(1.5),.1,2,.2), time*90); 39 | prop.facecolor='black'; 40 | prop = translate(prop, 6.5, 0, 1); 41 | motorprop = combine(motors, prop); 42 | 43 | auv = combine(fuselage, foils, motorprop); 44 | % the following order must be kept as it is 45 | % first rotate, then scale, then translate 46 | auv = rotateX(auv, orient(1)); 47 | auv = rotateY(auv, orient(2)); 48 | auv = rotateZ(auv, orient(3) + 180); 49 | auv = scale(auv, scale_num, scale_num, scale_num); 50 | auv = translate(auv, pos(1), pos(2), pos(3)); 51 | 52 | % clf 53 | renderpatch(auv,alpha,fuselage_colour); 54 | light('position',[0,0,10]) ; 55 | daspect([1 1 1]) 56 | 57 | end 58 | 59 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 60 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 61 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 62 | %%%%%%%%%%%%%%%%%% The following code is copied from %%%%%%%%%%%%%%%%%%%%%% 63 | %%%%%%%%%%%%%%%%%%% Hierarchical Matlab Renderer %%%%%%%%%%%%%%%%%%%%%%%%% 64 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 65 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 66 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 67 | 68 | function count = renderwire(objIn,alpha) 69 | %hierarchical render function for structs and cell arrays 70 | %as a wire frame image. 71 | %Takes either a cell array or a single struct as input. 72 | %For each struct, can set: 73 | % maps facecolor to edgecolor 74 | % edgecolor: default='cyan' 75 | % visible: default='on' 76 | 77 | if (iscell(objIn)) %a list of structs 78 | 79 | for i=1:length(objIn) 80 | 81 | obj=patch(objIn{i},'FaceAlpha', alpha); 82 | 83 | if (isfield(objIn{i},'facecolor')) 84 | ecolor=objIn{i}.facecolor; 85 | else 86 | ecolor=[0,1,1]; 87 | end 88 | 89 | if (isfield(objIn{i},'edgecolor')) 90 | ecolor=objIn{i}.edgecolor; 91 | end 92 | 93 | if (isfield(objIn{i},'visible')) 94 | vis=objIn{i}.visible; 95 | else 96 | vis='on'; 97 | end 98 | 99 | set(obj, 'FaceColor', 'none', ... 100 | 'EdgeColor', ecolor, ... 101 | 'Visible',vis); 102 | end 103 | count=i; 104 | 105 | elseif (isstruct(objIn)) %must be a single struct 106 | obj=patch(objIn,'FaceAlpha', alpha); 107 | 108 | if (isfield(objIn,'facecolor')) 109 | ecolor=objIn.facecolor; 110 | else 111 | ecolor=[0,1,1]; 112 | end 113 | 114 | if (isfield(objIn,'edgecolor')) 115 | ecolor=objIn.edgecolor; 116 | end 117 | 118 | 119 | if (isfield(objIn,'visible')) 120 | vis=objIn.visible; 121 | else 122 | vis='on'; 123 | end 124 | 125 | set(obj, 'FaceColor', 'none', ... 126 | 'EdgeColor', ecolor, ... 127 | 'Visible',vis); 128 | count=1; 129 | end %if 130 | end 131 | 132 | function count = renderpatch(objIn,alpha,fuselage_colour) 133 | %hierarchical render function for structs and cell arrays 134 | %Takes either a cell array or a single struct as input. 135 | %For each struct, can set: 136 | % facecolor: default=cyan 137 | % edgecolor: default='none' 138 | % ambientstrength: default=.6 139 | % specularstrength: default=.2 140 | % specularexponent: default=10 141 | % facelighting: default='phong' 142 | % diffusestrength: default=.5 143 | % visible: default='on' 144 | 145 | if (iscell(objIn)) %a list of structs 146 | 147 | for i=1:length(objIn) 148 | 149 | obj=patch(objIn{i},'FaceAlpha', alpha); 150 | 151 | if (isfield(objIn{i},'facecolor')) 152 | fcolor=objIn{i}.facecolor; 153 | else 154 | fcolor=[0,1,1]; 155 | end 156 | 157 | if (isfield(objIn{i},'edgecolor')) 158 | ecolor=objIn{i}.edgecolor; 159 | else 160 | % ecolor='none'; 161 | ecolor=fuselage_colour; 162 | 163 | end 164 | 165 | if (isfield(objIn{i},'ambientstrength')) 166 | ambstr=objIn{i}.ambientstrength; 167 | else 168 | ambstr=.6; 169 | end 170 | 171 | if (isfield(objIn{i},'specularstrength')) 172 | spcstr=objIn{i}.specularstrength; 173 | else 174 | spcstr=.2; 175 | end 176 | 177 | if (isfield(objIn{i},'specularexponent')) 178 | spcexp=objIn{i}.specularexponent; 179 | else 180 | spcexp=10; 181 | end 182 | 183 | if (isfield(objIn{i},'facelighting')) 184 | facelight=objIn{i}.facelighting; 185 | else 186 | facelight='phong'; 187 | end 188 | 189 | if (isfield(objIn{i},'diffusestrength')) 190 | difstr=objIn{i}.diffusestrength; 191 | else 192 | difstr=.5; 193 | end 194 | 195 | if (isfield(objIn{i},'visible')) 196 | vis=objIn{i}.visible; 197 | else 198 | vis='on'; 199 | end 200 | 201 | 202 | set(obj, 'FaceColor', fcolor, ... 203 | 'EdgeColor', ecolor, ... 204 | 'AmbientStrength',ambstr,... 205 | 'SpecularStrength',spcstr, ... 206 | 'SpecularExponent', spcexp, ... 207 | 'FaceLighting', facelight, ... 208 | 'DiffuseStrength', difstr, ... 209 | 'Visible',vis); 210 | end 211 | count=i; 212 | 213 | elseif (isstruct(objIn)) %must be a single struct 214 | obj=patch(objIn,'FaceAlpha', alpha); 215 | 216 | if (isfield(objIn,'facecolor')) 217 | fcolor=objIn.facecolor; 218 | else 219 | fcolor=[0,1,1]; 220 | end 221 | 222 | if (isfield(objIn,'edgecolor')) 223 | ecolor=objIn.edgecolor; 224 | else 225 | ecolor='none'; 226 | end 227 | 228 | if (isfield(objIn,'ambientstrength')) 229 | ambstr=objIn.ambientstrength; 230 | else 231 | ambstr=.6; 232 | end 233 | 234 | if (isfield(objIn,'specularstrength')) 235 | spcstr=objIn.specularstrength; 236 | else 237 | spcstr=.2; 238 | end 239 | 240 | if (isfield(objIn,'specularexponent')) 241 | spcexp=objIn.specularexponent; 242 | else 243 | spcexp=10; 244 | end 245 | 246 | if (isfield(objIn,'facelighting')) 247 | facelight=objIn.facelighting; 248 | else 249 | facelight='phong'; 250 | end 251 | 252 | if (isfield(objIn,'diffusestrength')) 253 | difstr=objIn.diffusestrength; 254 | else 255 | difstr=.5; 256 | end 257 | 258 | if (isfield(objIn,'visible')) 259 | vis=objIn.visible; 260 | else 261 | vis='on'; 262 | end 263 | 264 | set(obj, 'FaceColor', fcolor, ... 265 | 'EdgeColor', ecolor, ... 266 | 'AmbientStrength',ambstr,... 267 | 'SpecularStrength',spcstr, ... 268 | 'SpecularExponent', spcexp, ... 269 | 'FaceLighting', facelight, ... 270 | 'DiffuseStrength', difstr, ... 271 | 'Visible',vis); 272 | count=1; 273 | end %if 274 | end 275 | 276 | function objOut = combine(varargin) 277 | 278 | %Takes a list of opjects (structs and cell arrays) and 279 | %returns a cell array 280 | 281 | num=length(varargin); 282 | 283 | if (num==0) 284 | error('must have at least one input object'); 285 | end 286 | 287 | objOut={}; 288 | 289 | for i=1:num 290 | if (iscell(varargin{i})) %a list of structs 291 | objOut=[objOut, varargin{i}]; 292 | elseif (isstruct(varargin{i})) %must be a single struct 293 | objOut=[objOut, {varargin{i}}]; 294 | else 295 | error('input must be s struct or cell array') 296 | end %if (iscell(varargin(i))) 297 | end %for 298 | end 299 | 300 | function objOut = rotateX(objIn,a) 301 | %hierarchical rotate function for structs and cell arrays 302 | a=a/57.29; %degrees to radians 303 | if (iscell(objIn)) %a list of structs 304 | for i=1:length(objIn) 305 | objOut{i}=objIn{i}; 306 | V=objOut{i}.vertices; 307 | V=[V(:,1), ... 308 | cos(a)*V(:,2)-sin(a)*V(:,3), ... 309 | sin(a)*V(:,2)+cos(a)*V(:,3)]; 310 | objOut{i}.vertices=V; 311 | end 312 | elseif (isstruct(objIn)) %must be a single struct 313 | V=objIn.vertices; 314 | V=[V(:,1), ... 315 | cos(a)*V(:,2)-sin(a)*V(:,3), ... 316 | sin(a)*V(:,2)+cos(a)*V(:,3)]; 317 | objOut=objIn; 318 | objOut.vertices=V; 319 | else 320 | error('input must be s struct or cell array') 321 | end %if 322 | end 323 | 324 | function objOut = rotateY(objIn,a) 325 | %hierarchical rotate function for structs and cell arrays 326 | a=a/57.29; %degrees to radians 327 | if (iscell(objIn)) %a list of structs 328 | for i=1:length(objIn) 329 | objOut{i}=objIn{i}; 330 | V=objOut{i}.vertices; 331 | 332 | V=[cos(a)*V(:,1)+sin(a)*V(:,3), ... 333 | V(:,2), ... 334 | -sin(a)*V(:,1)+cos(a)*V(:,3)]; 335 | 336 | objOut{i}.vertices=V; 337 | end 338 | elseif (isstruct(objIn)) %must be a single struct 339 | V=objIn.vertices; 340 | 341 | V=[cos(a)*V(:,1)+sin(a)*V(:,3), ... 342 | V(:,2), ... 343 | -sin(a)*V(:,1)+cos(a)*V(:,3)]; 344 | 345 | objOut=objIn; 346 | objOut.vertices=V; 347 | else 348 | error('input must be s struct or cell array') 349 | end %if 350 | end 351 | 352 | function objOut = rotateZ(objIn,a) 353 | %hierarchical rotate function for structs and cell arrays 354 | a=a/57.29; %degrees to radians 355 | if (iscell(objIn)) %a list of structs 356 | for i=1:length(objIn) 357 | objOut{i}=objIn{i}; 358 | V=objOut{i}.vertices; 359 | 360 | V=[cos(a)*V(:,1)-sin(a)*V(:,2), ... 361 | sin(a)*V(:,1)+cos(a)*V(:,2), ... 362 | V(:,3)]; 363 | 364 | objOut{i}.vertices=V; 365 | end 366 | elseif (isstruct(objIn)) %must be a single struct 367 | V=objIn.vertices; 368 | 369 | V=[cos(a)*V(:,1)-sin(a)*V(:,2), ... 370 | sin(a)*V(:,1)+cos(a)*V(:,2), ... 371 | V(:,3)]; 372 | 373 | objOut=objIn; 374 | objOut.vertices=V; 375 | else 376 | error('input must be s struct or cell array') 377 | end %if 378 | end 379 | 380 | function objOut = scale(objIn,x,y,z) 381 | %hierarchical scale function for structs and cell arrays 382 | 383 | if (iscell(objIn)) %a list of structs 384 | for i=1:length(objIn) 385 | objOut{i}=objIn{i}; 386 | V=objIn{i}.vertices; 387 | V=[V(:,1)*x, V(:,2)*y, V(:,3)*z]; 388 | objOut{i}.vertices=V; 389 | end 390 | elseif (isstruct(objIn)) %must be a single struct 391 | V=objIn.vertices; 392 | V=[V(:,1)*x, V(:,2)*y, V(:,3)*z]; 393 | objOut=objIn; 394 | objOut.vertices=V; 395 | else 396 | error('input must be s struct or cell array') 397 | end %if 398 | end 399 | 400 | function objOut = translate(objIn,x,y,z) 401 | %hierarchical translate function for structs and cell arrays 402 | %Input is: 403 | % an struct consisting of a vertex and face array 404 | % or an cell array of structs 405 | % an x,y,z translation 406 | if (iscell(objIn)) %a list of structs 407 | for i=1:length(objIn) 408 | objOut{i}=objIn{i}; 409 | V=objOut{i}.vertices; 410 | V=[V(:,1)+x, V(:,2)+y, V(:,3)+z]; 411 | objOut{i}.vertices=V; 412 | end 413 | elseif (isstruct(objIn)) %must be a single struct 414 | V=objIn.vertices; 415 | V=[V(:,1)+x, V(:,2)+y, V(:,3)+z]; 416 | objOut=objIn; 417 | objOut.vertices=V; 418 | else 419 | error('input must be s struct or cell array') 420 | end %if 421 | end 422 | 423 | function cube=UnitCube 424 | %unit cube in a format consistent with hieracrhical 425 | %modeler 426 | 427 | %Define a cube 428 | cube.vertices=[ 0 0 0; 1 0 0; 1 1 0; 0 1 0; ... 429 | 0 0 1; 1 0 1; 1 1 1; 0 1 1;] ; 430 | cube.faces=[ 1 2 6 5; 2 3 7 6; 3 4 8 7; 4 1 5 8; ... 431 | 1 2 3 4; 5 6 7 8; ] ; 432 | 433 | cube.vertices = cube.vertices * 2 - 1; 434 | end 435 | 436 | function cylinder=UnitCylinder(res) 437 | coder.extrinsic('isosurface'); 438 | %unit sphere in a format consistent with hieracrhical 439 | %modeler 440 | %The input paramenter is related to the sphere resolution. 441 | %Range 1-10. Higher number is better approximation 442 | %1=> 4-sided tube 443 | %1.5=> 8-sided tube 444 | %2=> 48 faces 445 | %3=> 80 faces 446 | %5=>136 faces 447 | %10=>272 faces 448 | 449 | %range check 450 | if (res>10) 451 | res=10; 452 | elseif (res<1) 453 | res=1; 454 | end 455 | 456 | res=1/res; 457 | [x,y,z]=meshgrid(-1-res:res:1+res, ... 458 | -1-res:res:1+res, -1:1:1); 459 | w=sqrt(x.^2+y.^2); 460 | cylinder=isosurface(x,y,z,w,1); 461 | 462 | end 463 | 464 | function sphere=UnitSphere(res) 465 | coder.extrinsic('isosurface'); 466 | %unit sphere in a format consistent with hieracrhical 467 | %modeler 468 | %The input paramenter is related to the sphere resolution. 469 | %Range 1-10. Higher number is better approximation 470 | %1=>octahedron 471 | %1.5=> 44 faces 472 | %2=> 100 faces 473 | %2.5 => 188 faces 474 | %3=> 296 faces 475 | %5=> 900 faces 476 | %10=>3600 faces 477 | 478 | %range check 479 | if (res>10) 480 | res=10; 481 | elseif (res<1) 482 | res=1; 483 | end 484 | 485 | res=1/res; 486 | [x,y,z]=meshgrid(-1-res:res:1+res, ... 487 | -1-res:res:1+res, -1-res:res:1+res); 488 | w=sqrt(x.^2+y.^2+z.^2); 489 | sphere=isosurface(x,y,z,w,1); 490 | 491 | end 492 | 493 | function square=UnitSquare 494 | %unit square in the x-y plane 495 | %in a format consistent with hieracrhical 496 | %modeler 497 | 498 | %Define a square 499 | square.vertices= ... 500 | [-1, -1, 0; 501 | -1, 1, 0; 502 | 1, 1 ,0; 503 | 1, -1, 0]; 504 | square.faces=[1 2 3 4]; 505 | end 506 | 507 | function surface=UnitSurface(res) 508 | %unit flat surface in a format consistent with hieracrhical 509 | %modeler 510 | %The input paramenter is related to the sphere resolution. 511 | %Range 1-10. Higher number is better approximation 512 | %1=> 8 triangular faces 513 | %2=> 32 faces 514 | %5=>200 faces 515 | %10=>800 faces 516 | %20=>3200 faces 517 | %50=>20000 faces 518 | 519 | %range check 520 | if (res>100) 521 | res=100; 522 | elseif (res<1) 523 | res=1; 524 | end 525 | 526 | res=1/res; 527 | [x,y,z]=meshgrid(-1:res:1, ... 528 | -1:res:1, -1:1:1); 529 | w=z; 530 | surface=isosurface(x,y,z,w,0); 531 | 532 | end 533 | 534 | function torus=UnitTorus(radius, res) 535 | %unit torus in a format consistent with hieracrhical 536 | %modeler 537 | %The first parameter is the radius of the cross-section 538 | %The second input paramenter is related to the resolution. 539 | %Range 1-10. Higher number is better approximation 540 | %Res input of less than 3 makes a very rough torus 541 | %3=> 384 faces 542 | %5=>1230 faces 543 | %10=>5100 faces 544 | 545 | %range check 546 | if (res>10) 547 | res=10; 548 | elseif (res<1) 549 | res=1; 550 | end 551 | 552 | res=1/res; 553 | [x,y,z]=meshgrid(-1-radius-res:res:1+radius+res, ... 554 | -1-radius-res:res:1+radius+res, -radius-res:res:radius+res); 555 | 556 | w=(1-sqrt(x.^2+y.^2)).^2 +z.^2 - radius^2; 557 | 558 | torus=isosurface(x,y,z,w,0); 559 | 560 | end -------------------------------------------------------------------------------- /PF-toolbox/M1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/M1.mat -------------------------------------------------------------------------------- /PF-toolbox/MPC_setup.m: -------------------------------------------------------------------------------- 1 | function MPC_PFC=MPC_setup(Ts,d_pd,dd_pd,l_bound,u_bound,vd,controller) 2 | import casadi.* 3 | N = 10; % number of control intervals 4 | gamma=SX.sym('gamma'); 5 | d_pd = feval(d_pd,gamma); 6 | dd_pd = feval(dd_pd,gamma); 7 | hg=norm(d_pd); 8 | kappa=(d_pd(1)*dd_pd(2)-d_pd(2)*dd_pd(1))/hg^3; 9 | 10 | if (strcmp(controller,'Method 5')) 11 | MPC_PFC=NLP_Method5(Ts,d_pd,dd_pd,hg,kappa,gamma,l_bound,u_bound,vd,N); 12 | end 13 | if (strcmp(controller,'Method 7')) 14 | MPC_PFC=NLP_Method7(Ts,d_pd,dd_pd,hg,kappa,gamma,l_bound,u_bound,vd,N); 15 | end 16 | end 17 | 18 | function MPC_PFC = NLP_Method5(Ts,d_pd,dd_pd,hg,kappa,gamma,l_bound,u_bound,vd,N) 19 | import casadi.* 20 | T = Ts*N; % Time horizon 21 | % system state 22 | s1 = SX.sym('s1'); 23 | y1 = SX.sym('y1'); 24 | psie=SX.sym('psie'); 25 | x = [s1;y1;psie;gamma]; 26 | nx=length(x); 27 | % Input 28 | r = SX.sym('r'); 29 | v_g=SX.sym('v_g'); 30 | u=[r;v_g]; 31 | nu=length(u); 32 | %% path following system 33 | 34 | xdot = [vd*hg*cos(psie)-v_g*hg*(1-kappa*y1);... 35 | vd*hg*sin(psie)-kappa*hg*s1*v_g; ... 36 | r-kappa*hg*v_g;... 37 | v_g]; 38 | % subject to constraint 39 | rmax=u_bound(2);rmin=l_bound(2); 40 | vmax=u_bound(3);vmin=l_bound(3); 41 | umax=[rmax;vmax]; 42 | umin=[rmin;vmin]; 43 | 44 | %% Objective term 45 | Q=diag([1 5 .1]); 46 | R=diag([1 1]); 47 | ua=[vd*cos(psie)-v_g ; 48 | r-kappa*hg*v_g]; 49 | L = [s1 y1 psie]*Q*[s1;y1;psie]+ ua'*R*ua; 50 | 51 | %% Continuous time dynamics 52 | f = Function('f', {x, u}, {xdot, L}); 53 | 54 | %% Formulate discrete time dynamics 55 | 56 | 57 | % Fixed step Runge-Kutta 4 integrator 58 | M = 4; % RK4 steps per interval 59 | DT = T/N/M; 60 | f = Function('f', {x, u}, {xdot, L}); 61 | X0 = MX.sym('X0', nx); 62 | U = MX.sym('U',nu); 63 | X = X0; 64 | Q = 0; 65 | for j=1:M 66 | [m1, m1_q] = f(X, U); 67 | [m2, m2_q] = f(X + DT/2 * m1, U); 68 | [m3, m3_q] = f(X + DT/2 * m2, U); 69 | [m4, m4_q] = f(X + DT * m3, U); 70 | X=X+DT/6*(m1 +2*m2 +2*m3 +m4); 71 | Q = Q + DT/6*(m1_q + 2*m2_q + 2*m3_q + m4_q); 72 | end 73 | F = Function('F', {X0, U}, {X, Q}, {'x0','p'}, {'xf', 'qf'}); 74 | 75 | %% Evaluate the model at a test point 76 | 77 | %% Start with an empty NLP 78 | w={}; 79 | w0 = []; 80 | lbw = []; 81 | ubw = []; 82 | J = 0; 83 | g={}; 84 | lbg = []; 85 | ubg = []; 86 | 87 | %% Formulate the NLP 88 | Xk = X0; 89 | xmax=[inf;inf;inf;inf]; 90 | xmin=[-inf;-inf;-inf;-inf]; 91 | uzero=[0;0]; 92 | xzero=[0;0;0;0]; 93 | 94 | % "Lift" initial conditions 95 | X0 = MX.sym('X0', 4); 96 | w = {w{:}, X0}; 97 | lbw = [lbw; xzero]; 98 | ubw = [ubw; xzero]; 99 | w0 = [w0; xzero]; 100 | 101 | % Formulate the NLP 102 | 103 | Xk = X0; 104 | for k=0:N-1 105 | % New NLP variable for the control 106 | Uk = MX.sym(['U_' num2str(k)],nu); 107 | w = {w{:}, Uk}; 108 | lbw = [lbw; umin]; 109 | ubw = [ubw; umax]; 110 | w0 = [w0; uzero]; 111 | 112 | % Integrate till the end of the interval 113 | Fk = F('x0', Xk, 'p', Uk); 114 | Xk_end = Fk.xf; 115 | J=J+Fk.qf; 116 | 117 | % New NLP variable for state at end of interval 118 | Xk = MX.sym(['X_' num2str(k+1)], nx); 119 | w = {w{:}, Xk}; 120 | lbw = [lbw; xmin]; 121 | ubw = [ubw; xmax]; 122 | w0 = [w0; xzero]; 123 | 124 | % Add equality constraint 125 | g = {g{:}, Xk_end-Xk}; 126 | lbg = [lbg; xzero]; 127 | ubg = [ubg; xzero]; 128 | 129 | % Add inequality constraint 130 | end 131 | %% contractive inequality constraint 132 | % 133 | %% Create an NLP solver 134 | prob = struct('f', J, 'x', vertcat(w{:}), 'g', vertcat(g{:})); 135 | options = struct('ipopt',struct('print_level',0),'print_time',false); 136 | MPC_PFC.nlp = nlpsol('solver', 'ipopt', prob, options); 137 | MPC_PFC.w0=w0; 138 | MPC_PFC.lbw=lbw; 139 | MPC_PFC.ubw=ubw; 140 | MPC_PFC.lbg=lbg; 141 | MPC_PFC.ubg=ubg; 142 | % w0(1:4)=x0; 143 | % % Initialize guess 144 | % u0=[0;0]; 145 | % for i=1:N 146 | % Fk = F('x0',x0,'p',u0); 147 | % x0=full(Fk.xf); 148 | % w0(6*i+1:6*i+4)=x0; 149 | % w0(6*i-1:6*i)=u0; 150 | % end 151 | end 152 | function MPC_PFC = NLP_Method7(Ts,d_pd,dd_pd,hg,kappa,gamma,l_bound,u_bound,vd,N) 153 | import casadi.* 154 | T = Ts*N; % Time horizon 155 | % state of the path following system 156 | eB = SX.sym('eB',2); 157 | psi=SX.sym('psi'); 158 | x = [eB;psi;gamma]; 159 | nx=length(x); 160 | % Input 161 | u_v= SX.sym('u_v'); 162 | r = SX.sym('r'); 163 | v_g=SX.sym('v_g'); 164 | u=[u_v;r;v_g]; 165 | nu=length(u); 166 | 167 | %% Model equations - Path following error 168 | % path folllowing equaiton 169 | S=[0 -r; 170 | r 0]; 171 | 172 | delta=-0.2; 173 | Delta= [1 0; 174 | 0 -delta]; 175 | RIB=[ cos(psi) sin(psi) 176 | -sin(psi) cos(psi)]; 177 | 178 | xdot = [S*eB+Delta*[u_v;r]-RIB*d_pd*v_g;... 179 | r; ... 180 | v_g]; 181 | % subject to constraint 182 | umax=u_bound; 183 | umin=l_bound; 184 | %% Objective term 185 | Q = diag([.5 .2]); 186 | R = diag([10 10]); 187 | ub = Delta*[u_v;r]-RIB*d_pd*v_g; 188 | L = eB'*Q*eB+ub'*R*ub+1*(v_g-vd)^2; 189 | %% Continuous time dynamics 190 | f = Function('f', {x, u}, {xdot, L}); 191 | 192 | %% Formulate discrete time dynamics 193 | 194 | 195 | % Fixed step Runge-Kutta 4 integrator 196 | M = 4; % RK4 steps per interval 197 | DT = T/N/M; 198 | f = Function('f', {x, u}, {xdot, L}); 199 | X0 = MX.sym('X0', nx); 200 | U = MX.sym('U',nu); 201 | X = X0; 202 | Q = 0; 203 | for j=1:M 204 | [m1, m1_q] = f(X, U); 205 | [m2, m2_q] = f(X + DT/2 * m1, U); 206 | [m3, m3_q] = f(X + DT/2 * m2, U); 207 | [m4, m4_q] = f(X + DT * m3, U); 208 | X=X+DT/6*(m1 +2*m2 +2*m3 +m4); 209 | Q = Q + DT/6*(m1_q + 2*m2_q + 2*m3_q + m4_q); 210 | end 211 | F = Function('F', {X0, U}, {X, Q}, {'x0','p'}, {'xf', 'qf'}); 212 | 213 | %% Evaluate the model at a test point 214 | 215 | %% Start with an empty NLP 216 | w={}; 217 | w0 = []; 218 | lbw = []; 219 | ubw = []; 220 | J = 0; 221 | g={}; 222 | lbg = []; 223 | ubg = []; 224 | 225 | %% Formulate the NLP 226 | Xk = X0; 227 | xmax=[inf;inf;inf;inf]; 228 | xmin=[-inf;-inf;-inf;-inf]; 229 | uzero=[0;0;0]; 230 | xzero=[0;0;0;0]; 231 | 232 | % "Lift" initial conditions 233 | X0 = MX.sym('X0', 4); 234 | w = {w{:}, X0}; 235 | lbw = [lbw; xzero]; 236 | ubw = [ubw; xzero]; 237 | w0 = [w0; xzero]; 238 | 239 | % Formulate the NLP 240 | Xk = X0; 241 | for k=0:N-1 242 | % New NLP variable for the control 243 | Uk = MX.sym(['U_' num2str(k)],nu); 244 | w = {w{:}, Uk}; 245 | lbw = [lbw; umin]; 246 | ubw = [ubw; umax]; 247 | w0 = [w0; uzero]; 248 | 249 | 250 | % Integrate till the end of the interval 251 | Fk = F('x0', Xk, 'p', Uk); 252 | Xk_end = Fk.xf; 253 | J=J+Fk.qf; 254 | 255 | % New NLP variable for state at end of interval 256 | Xk = MX.sym(['X_' num2str(k+1)], nx); 257 | w = {w{:}, Xk}; 258 | lbw = [lbw; xmin]; 259 | ubw = [ubw; xmax]; 260 | w0 = [w0; xzero]; 261 | 262 | % Add equality constraint 263 | g = {g{:}, Xk_end-Xk}; 264 | lbg = [lbg; xzero]; 265 | ubg = [ubg; xzero]; 266 | 267 | % Add inequality constraint 268 | end 269 | %% contractive inequality constraint 270 | % 271 | %% Create an NLP solver 272 | prob = struct('f', J, 'x', vertcat(w{:}), 'g', vertcat(g{:})); 273 | options = struct('ipopt',struct('print_level',0),'print_time',false); 274 | MPC_PFC.nlp = nlpsol('solver', 'ipopt', prob, options); 275 | MPC_PFC.w0=w0; 276 | MPC_PFC.lbw=lbw; 277 | MPC_PFC.ubw=ubw; 278 | MPC_PFC.lbg=lbg; 279 | MPC_PFC.ubg=ubg; 280 | % w0(1:4)=x0; 281 | % % Initialize guess 282 | % u0=[0;0]; 283 | % for i=1:N 284 | % Fk = F('x0',x0,'p',u0); 285 | % x0=full(Fk.xf); 286 | % w0(6*i+1:6*i+4)=x0; 287 | % w0(6*i-1:6*i)=u0; 288 | % end 289 | end 290 | -------------------------------------------------------------------------------- /PF-toolbox/NLP_gamma.m: -------------------------------------------------------------------------------- 1 | function [solver,w0,lbw,ubw,lbg,ubg,nx,nu,k3]=NLP_gamma(Np,Tp,Ts,path_data) 2 | 3 | import casadi.* 4 | 5 | s1 = SX.sym('s1'); 6 | y1 = SX.sym('y1'); 7 | psie = SX.sym('psie'); 8 | gamma = SX.sym('gamma'); 9 | x = [s1;y1;psie;gamma]; 10 | nx=length(x); 11 | % input of PF error system 12 | r = SX.sym('r'); 13 | u_g=SX.sym('u_g'); 14 | u=[r;u_g]; 15 | nu=length(u); 16 | %% PF error system 17 | % --------------------------------------------- 18 | % pd=[xd;yd]=[a*sin(omega*gamma); gamma]; 19 | % cg=(x'y''-y'x'')/(x'x'+y'y')^(3/2); 20 | % 21 | % --------------------------------------------- 22 | % circle 23 | % a=10; 24 | % omega=0.01; 25 | % h_g=a*omega; 26 | % h_g_max=a*omega; 27 | % cg=1/a; 28 | % cg_max=1/a; 29 | % sin path 30 | a=path_data(1); 31 | omega=path_data(2); 32 | d=path_data(3); 33 | h_g=sqrt(1+a^2*omega^2*cos(omega*gamma+d)^2); 34 | h_g_max=sqrt(1+a^2*omega^2); 35 | cg=a*omega^2*sin(omega*gamma+d)/(h_g^3); 36 | cg_max=a*omega^2; 37 | 38 | vd=0.5; 39 | vd_max=0.5; 40 | ds_max=1; 41 | ds_min=-1; 42 | % Input constraints 43 | rmax=0.3;rmin=-0.3; 44 | dgmax=ds_max/h_g_max;dgmin=ds_min/h_g_max; 45 | umax=[rmax;dgmax]; 46 | umin=[rmin;dgmin]; 47 | % PF error dynamics equation 48 | xdot = [vd*cos(psie)*h_g-h_g*u_g*(1-cg*y1);... 49 | vd*h_g*sin(psie)-cg*s1*h_g*u_g; ... 50 | r-cg*h_g*u_g;... 51 | u_g]; 52 | 53 | %% => gains for nonlinear controller 54 | k1=(ds_max-vd_max); 55 | k2=0.1*(rmax-dgmax*cg_max*h_g_max); 56 | k3=0.9*(rmax-dgmax*cg_max*h_g_max)/(0.5*vd_max); 57 | 58 | %% Objective term 59 | Q=diag([1 1 1 1 10]); 60 | L = [s1 y1 psie vd*cos(psie)*h_g-h_g*u_g r-cg*h_g*u_g]*Q*[s1;y1;psie;vd*cos(psie)*h_g-h_g*u_g;r-cg*h_g*u_g]; 61 | %% Continuous time dynamics 62 | f = Function('f', {x, u}, {xdot, L}); 63 | 64 | %% Formulate discrete time dynamics 65 | % Fixed step Runge-Kutta 4 integrator 66 | M = 4; % RK4 steps per interval 67 | DT = Tp/Np/M; 68 | f = Function('f', {x, u}, {xdot, L}); 69 | X0 = MX.sym('X0', nx); 70 | U = MX.sym('U',nu); 71 | X = X0; 72 | Q = 0; 73 | for j=1:M 74 | [m1, m1_q] = f(X, U); 75 | [m2, m2_q] = f(X + DT/2 * m1, U); 76 | [m3, m3_q] = f(X + DT/2 * m2, U); 77 | [m4, m4_q] = f(X + DT * m3, U); 78 | X=X+DT/6*(m1 +2*m2 +2*m3 +m4); 79 | Q = Q + DT/6*(m1_q + 2*m2_q + 2*m3_q + m4_q); 80 | end 81 | F = Function('F', {X0, U}, {X, Q}, {'x0','p'}, {'xf', 'qf'}); 82 | 83 | %% Start with an empty NLP 84 | w={}; 85 | w0 = []; 86 | lbw = []; 87 | ubw = []; 88 | J = 0; 89 | g={}; 90 | lbg = []; 91 | ubg = []; 92 | 93 | %% Formulate the NLP 94 | Xk = X0; 95 | xmax=[inf;inf;inf;inf]; 96 | xmin=[-inf;-inf;-inf;-inf]; 97 | uzero=[0;0]; 98 | xzero=[0;0;0;0]; 99 | 100 | % "Lift" initial conditions 101 | X0 = MX.sym('X0', nx); 102 | w = {w{:}, X0}; 103 | lbw = [lbw; xzero]; 104 | ubw = [ubw; xzero]; 105 | w0 = [w0; xzero]; 106 | 107 | % Formulate the NLP 108 | 109 | Xk = X0; 110 | V0_mpc=0.5*k3*log(1+Xk(1)^2 + Xk(2)^2) + 0.5*Xk(3)^2; 111 | dV_non=-k1*k3*Xk(1)*tanh(Xk(1))/(1+Xk(1)^2+Xk(2)^2)-k2*Xk(3)*tanh(Xk(3)); % derivative of lyapunov function 112 | for k=0:Np-1 113 | % New NLP variable for the control 114 | Uk = MX.sym(['U_' num2str(k)],nu); 115 | w = {w{:}, Uk}; 116 | lbw = [lbw; umin]; 117 | ubw = [ubw; umax]; 118 | w0 = [w0; uzero]; 119 | % Integrate till the end of the interval 120 | Fk = F('x0', Xk, 'p', Uk); 121 | Xk_end = Fk.xf; 122 | J=J+Fk.qf; 123 | 124 | % New NLP variable for state at end of interval 125 | Xk = MX.sym(['X_' num2str(k+1)], nx); 126 | w = {w{:}, Xk}; 127 | lbw = [lbw; xmin]; 128 | ubw = [ubw; xmax]; 129 | w0 = [w0; xzero]; 130 | 131 | % Add equality constraint 132 | g = {g{:}, Xk_end-Xk}; 133 | lbg = [lbg; xzero]; 134 | ubg = [ubg; xzero]; 135 | 136 | % Add inequality constraint 137 | 138 | if k==0 139 | V1_mpc=0.5*k3*log(1+Xk(1)^2 + Xk(2)^2) + 0.5*Xk(3)^2; 140 | dV_mpc=(V1_mpc-V0_mpc)/Ts; 141 | end 142 | end 143 | %% contractive inequality constraint 144 | g = {g{:}, dV_mpc-dV_non}; 145 | lbg = [lbg; -inf]; 146 | ubg = [ubg; 0]; 147 | % 148 | %% Create an NLP solver 149 | prob = struct('f', J, 'x', vertcat(w{:}), 'g', vertcat(g{:})); 150 | options = struct('ipopt',struct('print_level',0),'print_time',false); 151 | solver = nlpsol('solver', 'ipopt', prob, options); 152 | end -------------------------------------------------------------------------------- /PF-toolbox/PF_ToolwithMedusa.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/PF_ToolwithMedusa.slx -------------------------------------------------------------------------------- /PF-toolbox/PF_ToolwithMedusa_Fix.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/PF_ToolwithMedusa_Fix.slx -------------------------------------------------------------------------------- /PF-toolbox/PF_ToolwithMedusa_Second.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/PF_ToolwithMedusa_Second.slx -------------------------------------------------------------------------------- /PF-toolbox/PF_ToolwithMedusa_test.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/PF_ToolwithMedusa_test.slx -------------------------------------------------------------------------------- /PF-toolbox/PF_ToolwithMedusa_test_sfun.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/PF_ToolwithMedusa_test_sfun.mexa64 -------------------------------------------------------------------------------- /PF-toolbox/PFcontrollers.m: -------------------------------------------------------------------------------- 1 | function [upf,exc_time] = PFcontrollers(x_robot,x_path,upf,controller,vd,Ts,MPC_PF,l_bound,u_bound) 2 | 3 | p=x_robot(1:2); 4 | psi=x_robot(3); 5 | 6 | pd=x_path(1:2); 7 | pd_gamma=x_path(3:4); 8 | hg=sqrt(pd_gamma(1)^2+pd_gamma(2)^2); 9 | v_robot=upf(1);r_robot=upf(2);v_gamma=upf(3); 10 | psiP=x_path(6); 11 | kappa=x_path(5); 12 | gamma=x_path(end); 13 | %% Compute PF error 14 | if strcmp(controller,'Method 1') 15 | 16 | tic 17 | 18 | upf=Method1(p,psi,pd,psiP,kappa,v_robot,hg,vd); 19 | 20 | exc_time = toc; 21 | 22 | upf(2)=sat(upf(2),l_bound(2),u_bound(2)); 23 | 24 | 25 | 26 | elseif strcmp(controller,'Method 2') 27 | tic 28 | 29 | upf=Method2(p,psi,pd,psiP,kappa,v_gamma,v_robot,hg,vd); 30 | 31 | exc_time = toc; 32 | 33 | upf(2:3)=sat(upf(2:3),l_bound(2:3),u_bound(2:3)); 34 | 35 | 36 | 37 | 38 | elseif strcmp(controller,'Method 3') 39 | tic 40 | upf=Method3(p,psi,pd,psiP,kappa,v_robot,hg,vd); 41 | exc_time = toc; 42 | 43 | elseif strcmp(controller,'Method 4') 44 | tic 45 | upf=Method4(p,psi,pd,psiP,kappa,v_robot,hg,vd); 46 | exc_time = toc; 47 | 48 | upf(3)=sat(upf(3),l_bound(3),u_bound(3)); 49 | elseif strcmp(controller,'Method 5') 50 | tic 51 | upf=Method5(p,psi,pd,psiP,kappa,v_robot,hg,vd,gamma,MPC_PF); 52 | exc_time = toc; 53 | 54 | elseif strcmp(controller,'Method 6') 55 | tic; 56 | upf=Method6(p,psi,pd,pd_gamma,psiP,kappa,v_robot,hg,vd,Ts); 57 | 58 | exc_time = toc; 59 | 60 | upf=sat(upf,l_bound,u_bound); 61 | elseif strcmp(controller,'Method 7') 62 | tic 63 | upf= Method7(p,psi,pd,pd_gamma,psiP,vd,Ts,gamma,MPC_PF); 64 | exc_time = toc; 65 | end 66 | 67 | 68 | end 69 | function upf= Method1(p,psi,pd,psiP,kappa,v_robot,hg,vd) 70 | 71 | % T=x_path(9:10); 72 | % N=x_path(11:12); 73 | RI_F=[cos(psiP) sin(psiP); % From {I} to {F} 74 | -sin(psiP) cos(psiP)]; 75 | % RI_F=[T N]'; % From {I} to {F]; 76 | 77 | e_P=RI_F*(p-pd); 78 | % e_P1=RI_F1*(p-pd); 79 | s1=e_P(1); 80 | y1=e_P(2); 81 | psie=psi-psiP; 82 | 83 | [k1,k2,k3,k_delta,theta,Delta_h]=ConPara1(); 84 | 85 | delta=-theta*tanh(k_delta*y1); 86 | ydot=v_robot*sin(psie); 87 | delta_dot=-theta*k_delta*(1-(tanh(k_delta*y1))^2)*ydot; 88 | psi_tilde=psie-delta; 89 | % Controller 90 | %u_d=.5; 91 | u_d=hg*vd; 92 | u=u_d; 93 | uP=u*cos(psie)/(1-kappa*y1); 94 | v_gamma=uP/hg; 95 | if psie==delta 96 | r=delta_dot-k5*y1*v_robot+kappa*v_gamma*hg; 97 | else 98 | r=kappa*uP + delta_dot - k1*psi_tilde - k2*y1*u*(sin(psie) - sin(delta))/psi_tilde; 99 | end 100 | upf=[u;r;v_gamma]; 101 | end 102 | function upf= Method2(p,psi,pd,psiP,kappa,v_gamma,v_robot,hg,vd) 103 | 104 | % T=x_path(9:10); 105 | % N=x_path(11:12); 106 | RI_F=[cos(psiP) sin(psiP); % From {I} to {F} 107 | -sin(psiP) cos(psiP)]; 108 | % RI_F=[T N]'; % From {I} to {F]; 109 | 110 | e_P=RI_F*(p-pd); 111 | % e_P1=RI_F1*(p-pd); 112 | s1=e_P(1); 113 | y1=e_P(2); 114 | psie=psi-psiP; 115 | 116 | [k1,k2,k3,k_delta,theta,Delta_h]=ConPara1(); 117 | 118 | delta=-theta*tanh(k_delta*y1); 119 | ydot=v_robot*sin(psie)-hg*kappa*v_gamma*s1; 120 | delta_dot=-theta*k_delta*(1-(tanh(k_delta*y1))^2)*ydot; 121 | psi_tilde=psie-delta; 122 | % Controller 123 | %u_d=.5; 124 | u_d=hg*vd; 125 | u=u_d; 126 | uP=(u*cos(psie)+k3*s1); 127 | v_gamma=uP/hg; 128 | if psie==delta 129 | r=delta_dot-k5*y1*v_robot+kappa*v_gamma*hg; 130 | else 131 | r=kappa*uP + delta_dot - k1*psi_tilde - k2*y1*u*(sin(psie) - sin(delta))/psi_tilde; 132 | end 133 | upf=[u;r;v_gamma]; 134 | end 135 | function upf= Method3(p,psi,pd,psiP,kappa,v_robot,hg,vd) 136 | 137 | % T=x_path(9:10); 138 | % N=x_path(11:12); 139 | RI_F=[cos(psiP) sin(psiP); % From {I} to {F} 140 | -sin(psiP) cos(psiP)]; 141 | % RI_F=[T N]'; % From {I} to {F]; 142 | 143 | e_P=RI_F*(p-pd); 144 | % e_P1=RI_F1*(p-pd); 145 | s1=e_P(1); 146 | y1=e_P(2); 147 | 148 | [k1,k2,k3,k_delta,theta,Delta_h]=ConPara1(); 149 | % Controller 150 | %u_d=.5; 151 | u_d=hg*vd; 152 | u=u_d; 153 | v_gamma=0; 154 | psi_los=psiP+atan(-y1/Delta_h); 155 | upf=[u;psi_los;v_gamma]; 156 | end 157 | function upf= Method4(p,psi,pd,psiP,kappa,v_robot,hg,vd) 158 | 159 | % T=x_path(9:10); 160 | % N=x_path(11:12); 161 | RI_F=[cos(psiP) sin(psiP); % From {I} to {F} 162 | -sin(psiP) cos(psiP)]; 163 | % RI_F=[T N]'; % From {I} to {F]; 164 | 165 | e_P=RI_F*(p-pd); 166 | % e_P1=RI_F1*(p-pd); 167 | s1=e_P(1); 168 | y1=e_P(2); 169 | psie=psi-psiP; 170 | [k1,k2,k3,k_delta,theta,Delta_h]=ConPara1(); 171 | % Controller 172 | u_d=hg*vd; 173 | u=u_d; 174 | uP=u*cos(psie)+k3*s1; 175 | v_gamma=uP/hg; 176 | psi_los=psiP+atan(-y1/Delta_h); 177 | upf=[u;psi_los;v_gamma]; 178 | end 179 | function upf= Method5(p,psi,pd,psiP,kappa,v_robot,hg,vd,gamma,MPC_PF) 180 | persistent w0 lbw ubw lbg ubg; 181 | if isempty(w0) 182 | w0=MPC_PF.w0; 183 | lbw=MPC_PF.lbw; ubw=MPC_PF.ubw; 184 | lbg=MPC_PF.lbg; ubg=MPC_PF.ubg; 185 | end 186 | 187 | % T=x_path(9:10); 188 | % N=x_path(11:12); 189 | RI_F=[cos(psiP) sin(psiP); % From {I} to {F} 190 | -sin(psiP) cos(psiP)]; 191 | % RI_F=[T N]'; % From {I} to {F]; 192 | 193 | e_P=RI_F*(p-pd); 194 | % e_P1=RI_F1*(p-pd); 195 | s1=e_P(1); 196 | y1=e_P(2); 197 | psie=psi-psiP; 198 | 199 | u_d=hg*vd; 200 | u=u_d; 201 | 202 | x0=[s1;y1;psie;gamma]; 203 | nx=length(x0); 204 | w0(1:nx)=x0; 205 | lbw(1:nx)=x0; 206 | ubw(1:nx)=x0; 207 | sol = MPC_PF.nlp('x0', w0, 'lbx', lbw, 'ubx', ubw,... 208 | 'lbg', lbg, 'ubg', ubg); 209 | w_opt = full(sol.x); 210 | u_mpc=w_opt(5:6); 211 | r_d=u_mpc(1); 212 | v_gamma=u_mpc(2); 213 | w0=w_opt; 214 | upf=[u;r_d;v_gamma]; 215 | end 216 | function upf= Method6(p,psi,pd,pd_gamma,psiP,kappa,v_robot,hg,vd,Ts) 217 | %%% Tunning parameters (This can be done as initialization) 218 | persistent gamma_dot_old; 219 | if isempty(gamma_dot_old) 220 | gamma_dot_old=0; 221 | end 222 | [delta,Delta_inv,epsilon,Kk,kz]=ConPara2(); 223 | %%%----------------------------- 224 | RB_I=[cos(psi) -sin(psi); 225 | sin(psi) cos(psi)]; 226 | e_pos=RB_I'*(p-pd)-epsilon; 227 | e_gamma=gamma_dot_old-vd; 228 | 229 | %%% PF control law 230 | ud=Delta_inv*(-tanh(Kk*e_pos)+RB_I'*pd_gamma*vd); 231 | gamma_ddot=-kz*e_gamma+e_pos'*RB_I'*pd_gamma; 232 | %%% 233 | gamma_ddot=sat(gamma_ddot, -0.005, 0.005); 234 | gamma_dot=gamma_dot_old+Ts*gamma_ddot; 235 | gamma_dot=sat(gamma_dot, -0.005, 0.2); 236 | gamma_dot_old=gamma_dot; 237 | upf=[ud;gamma_dot]; 238 | end 239 | function upf= Method7(p,psi,pd,pd_gamma,psiP,vd,Ts,gamma,MPC_PF) 240 | %%% Tunning parameters (This can be done as initialization) 241 | persistent w0 lbw ubw lbg ubg; 242 | if isempty(w0) 243 | w0=MPC_PF.w0; 244 | lbw=MPC_PF.lbw; ubw=MPC_PF.ubw; 245 | lbg=MPC_PF.lbg; ubg=MPC_PF.ubg; 246 | end 247 | 248 | [delta,Delta_inv,epsilon,Kk,kz]=ConPara2(); 249 | 250 | RB_I=[cos(psi) -sin(psi); 251 | sin(psi) cos(psi)]; 252 | eB=RB_I'*(p-pd)-epsilon; 253 | 254 | x0=[eB;psi;gamma]; 255 | nx=length(x0); 256 | w0(1:nx)=x0; 257 | lbw(1:nx)=x0; 258 | ubw(1:nx)=x0; 259 | sol = MPC_PF.nlp('x0', w0, 'lbx', lbw, 'ubx', ubw,... 260 | 'lbg', lbg, 'ubg', ubg); 261 | w_opt = full(sol.x); 262 | upf=w_opt(5:7); 263 | w0=w_opt; 264 | end 265 | function [k1,k2,k3,k_delta,theta,Delta_h]=ConPara1() 266 | % Control parameters for Method 1 to Method 5 267 | k1=1; 268 | k2=1; 269 | k3=0.1; 270 | theta=0.1*pi/4; 271 | k_delta=2; 272 | Delta_h=5; 273 | end 274 | function [delta,Delta_inv,epsilon,Kk,kz]=ConPara2() 275 | % Control parameters for Method 6 and Method 7 276 | delta=-0.2; 277 | Delta= [1 0; 278 | 0 -delta]; 279 | Delta_inv=inv(Delta); 280 | epsilon=[delta; 0]; 281 | kx=.1; ky=0.05; 282 | Kk=[kx 0; 283 | 0 ky]; 284 | kz=1; 285 | end 286 | function y=sat(u,l_bound,u_bound) 287 | y=max(min(u,u_bound),l_bound); 288 | end 289 | -------------------------------------------------------------------------------- /PF-toolbox/PFtools.m: -------------------------------------------------------------------------------- 1 | % ================================================================================ 2 | % This is path following toolbox for testing path following algorithms in 2D cases 3 | % ================================================================================ 4 | % Details of the algorithms are described in the paper entittled: 5 | % 6 | % A review of path following control strategies for autonomous robotic vehicles: theory, simulations, and experiments 7 | % 8 | % Authors: Nguyen Hung, Francisco Rego, Joao Quintas, Joao Cruz, Marcelo Jacinto, David Souto, André Potes, Luis Sebastião and António Pascoal 9 | 10 | % The code was written by: Nguyen Hung 11 | % See more the work: https://nt-hung.github.io/ 12 | % 13 | % 14 | %================================================================================= 15 | 16 | function PFtools 17 | clear all; 18 | close all; 19 | %% Initialization 20 | T = 500; % Simulation time 21 | Ts = 0.2; % Sampling time 22 | N=T/Ts; 23 | t=0; 24 | % Initialize vehicle_position and orientation 25 | p0 = [25;-15]; psi0 = pi/2; 26 | x_robot0 = [p0;psi0]; % Robot state 27 | % Set the path 28 | gamma0 = 0.1; % 29 | path_type = 'Bernoulli'; % path types includes : {Sin, circle, polynominal,Bernoulli, Lawnmover, Heart} 30 | [pd,d_pd,dd_pd,vd] = path_eq(path_type); % 31 | % Set PF controller 32 | controller = 'Method 1'; % Controller {Method 1-Method 7} 33 | % Setup constraint for the vehicle input (speed and heading rate) 34 | umin = 0; umax = 1; 35 | rmin = -0.2; rmax = 0.2; 36 | vmin = -1; vmax = 1; 37 | l_bound = [umin;rmin;vmin]; u_bound = [umax;rmax;vmax]; 38 | if strcmp(controller,'Method 5')||strcmp(controller,'Method 7') 39 | MPC_PF = MPC_setup(Ts,d_pd,dd_pd,l_bound,u_bound,vd,controller); % Initilize an MPC object 40 | else 41 | MPC_PF=[]; 42 | end 43 | %% Start main Loop ========================================================= 44 | x_robot = x_robot0; 45 | x_path = []; 46 | upf = [0;0;0]; % upf={u,r,v_gamma} 47 | gamma = gamma0; 48 | compute_time = []; 49 | path_compute_time = []; 50 | for i = 1:N 51 | t = [t;i*Ts]; 52 | % Step 1: Get the state of the path 53 | p = x_robot(1:2,end); 54 | tic 55 | x_path(:,end+1) = path_state(pd,d_pd,dd_pd,controller,gamma(end),p); 56 | path_compute_time(i+1) = toc; 57 | % Step 2: Compute path following controller 58 | [upfi exc_time] = PFcontrollers(x_robot(:,end),x_path(:,end),upf(:,end),controller,vd,Ts,MPC_PF,l_bound,u_bound); 59 | compute_time(i+1) = exc_time + path_compute_time(end) ; 60 | u_robot = upfi(1:2); 61 | % Step 3: Update the state of the vehicle 62 | if (strcmp(controller,'Method 3'))||(strcmp(controller,'Method 4')) 63 | model_type = 'Type II'; 64 | else 65 | model_type = 'Type I'; 66 | end 67 | x_robot(:,end+1) = vehicle_models(x_robot(:,end),u_robot,model_type,Ts); 68 | u_gamma = upfi(3); 69 | upf = [upf upfi]; 70 | % Step 4: Update the path parameter 71 | if (strcmp(controller,'Method 1'))||(strcmp(controller,'Method 3')) 72 | gamma(end+1) = x_path(end,end); 73 | else 74 | gamma(end+1) = gamma(end)+Ts*u_gamma; 75 | end 76 | end 77 | %% Save Data to Workspace 78 | x_path = x_path'; 79 | x_robot = x_robot'; 80 | upf = upf'; 81 | save_to_base(1); 82 | %% Run animation 83 | pd = x_path(:,1:2); 84 | p = x_robot(:,1:2); 85 | animation_1vehicles 86 | % plot_results; 87 | end 88 | % End main loop =========================================================== 89 | 90 | 91 | -------------------------------------------------------------------------------- /PF-toolbox/README.md: -------------------------------------------------------------------------------- 1 | # path-following-Matlab 2 | ## Description 3 | This repo contains matlab codes that implement the path following methods studied in the paper entitled: 4 | 5 | A review of path following control strategies for autonomous robotic vehicles: theory, simulations, and experiments 6 | Link: https://nt-hung.github.io/publications 7 | 8 | ## Prerequisite 9 | - Matlab2016a or newer 10 | - Casadi - a tool to formulate optimal control problems [only need for MPC methods (Method 5 and 7)] 11 | 12 | ## Installation 13 | - Chose a folder that you want to locate your project, then run 14 | - clone/download the project 15 | - run PFtools.m and see the result. 16 | 17 | If you want to test different type of the path or controller, change parameters in variable "path_type" and "controller" in PFtools.m 18 | 19 | ## Citation 20 | Nguyen Hung, Francisco Rego, Joao Quintas, Joao Cruz, Marcelo Jacinto, David Souto, André Potes, Luis Sebastião and António Pascoal, "A review of path following control strategies for autonomous robotic vehicles: theory, simulations, and experiments", arXiv preprint arXiv:2204.07319, 2022, doi:10.48550/ARXIV.2204.07319 -------------------------------------------------------------------------------- /PF-toolbox/RK4_integrator.m: -------------------------------------------------------------------------------- 1 | function [ output ] = RK4_integrator( ode_fun, input) 2 | x0 = input.x; 3 | u0 = input.u; 4 | Ts = input.Ts; 5 | nSteps = input.nSteps; 6 | % h = Ts/nSteps; 7 | % output.value = rk4_step(ode_fun,x0,u0,h); 8 | 9 | 10 | nx = length(x0); 11 | nu = length(u0); 12 | h = Ts/nSteps; 13 | STEP = 1e-100; 14 | 15 | compute_sensitivities = ~isfield(input,'sens') || input.sens; 16 | 17 | xEnd = x0; 18 | A = eye(nx); 19 | B = zeros(nx,nu); 20 | for i = 1:nSteps 21 | x0 = xEnd; 22 | xEnd = rk4_step(ode_fun,x0,u0,h); 23 | if compute_sensitivities 24 | sensX = zeros(nx,nx); sensU = zeros(nx,nu); 25 | for j = 1:nx 26 | % imaginary trick for states 27 | xTemp1 = x0; xTemp1(j) = xTemp1(j) + STEP*sqrt(-1); 28 | xTemp1 = rk4_step(ode_fun,xTemp1,u0,h); 29 | 30 | sensX(:,j) = imag(xTemp1)./STEP; 31 | end 32 | for j = 1:nu 33 | % imaginary trick for controls 34 | uTemp1 = u0; uTemp1(j) = uTemp1(j) + STEP*sqrt(-1); 35 | xTemp1 = rk4_step(ode_fun,x0,uTemp1,h); 36 | 37 | sensU(:,j) = imag(xTemp1)./STEP; 38 | end 39 | % propagate sensitivities 40 | A = sensX*A; 41 | B = sensX*B + sensU; 42 | end 43 | end 44 | output.value = xEnd; 45 | if compute_sensitivities 46 | output.sensX = A; 47 | output.sensU = B; 48 | end 49 | end 50 | 51 | 52 | 53 | function x_next = rk4_step(ode_fun,x,u,h) 54 | k1 = ode_fun(0,x,u); 55 | k2 = ode_fun(0,x+h/2.*k1,u); 56 | k3 = ode_fun(0,x+h/2.*k2,u); 57 | k4 = ode_fun(0,x+h.*k3,u); 58 | x_next = x + h/6.*(k1+2*k2+2*k3+k4); 59 | end -------------------------------------------------------------------------------- /PF-toolbox/Testpath.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | gamma=0:1:61; 3 | % a=[0.0000; 0; 0; 1; 1; 0]; 4 | % b=[0.000; 0; 0; 0; 1; 0]; 5 | 6 | %a=[0;0;0;0;1;0]; 7 | c=238; 8 | b=[0;0; -3.255259096425168e-04; 0.263729636233269;-72.550756644896480; 6.505073497062662e+03]; 9 | x=[]; y=[]; psid=[]; 10 | for i=1:length(gamma) 11 | z=gamma(i)+c; 12 | x=[x; z]; 13 | y=[y; b(3)*z^3+b(4)*z^2+b(5)*z+b(6)]; 14 | %psid=[psid;atan2(dphi'*b(1:5),dphi'*a(1:5))]; 15 | end 16 | plot(x,y); 17 | %figure 18 | %plot(gamma,psid); 19 | 20 | % Gamma = 0:5:61; 21 | % c = 238; 22 | % for i = 1:length(Gamma) 23 | % x = Gamma(i) + c; 24 | % y = ((-0.000325*(Gamma (i)+c)^3)+(-0.2637* (Gamma (i)+c)^2)+ (72.558 * (Gamma (i)+c))-6505); 25 | % end 26 | % plot (x,y) 27 | 28 | -------------------------------------------------------------------------------- /PF-toolbox/Trajectory_tracking.m: -------------------------------------------------------------------------------- 1 | % ========================================================================= 2 | % Trajectory tracking for 2D cases 3 | % ========================================================================= 4 | % This is trajectory tracking controller described in the thesis of Vanni-Vanni 5 | 6 | % Coded by: Nguyen T. Hung 7 | % Contact: nt-hung.github.io 8 | 9 | 10 | % Desired trajectory: pd = [t; 10*sin(0.01*t); 11 | % 12 | %========================================================================== 13 | 14 | function TT_Controller 15 | clear all; 16 | close all; 17 | %% Initialization 18 | T = 200; % Simulation time 19 | Ts = 0.1; % Sampling time 20 | N = T/Ts; 21 | time = []; 22 | % Initialize vehicle_position and orientation 23 | p0=[15;-5]; psi0=pi/2; 24 | x_vehicle0 = [p0;psi0]; % vehicle state % Controller {Method 1-Method 7} 25 | % Setup constraint for the vehicle input (speed and heading rate) 26 | umin=0; umax=2; % limit on the vehicle's speed 27 | rmin=-0.5; rmax=0.5; % limit on the vehicle's heading rate 28 | l_bound=[umin;rmin]; u_bound=[umax;rmax]; 29 | % Start main Loop ========================================================= 30 | x_vehicle = x_vehicle0; 31 | x_traj = []; 32 | x_traj_dot = []; 33 | u_vehicle = []; % upf={u,r} 34 | for i = 0:N 35 | t = i*Ts; 36 | time(end+1) = i*Ts; 37 | % Step 1: update the desired trajectory 38 | x_traj(:,end+1) = [t; 10*sin(0.05*t)]; % desired trajectory 39 | x_traj_dot(:,end+1) = [1; 0.5*cos(0.05*t)]; % derivative of trajectory 40 | % Step 2: Compute trajectory tracking controller 41 | u_vehicle(:,end+1) = TT_controllers(x_vehicle(:,end),x_traj(:,end),x_traj_dot(:,end),l_bound,u_bound); 42 | % Step 3: Update the state of the vehicle 43 | [time y]=ode45(@(t,y) vehicle_model_2D(t,y,u_vehicle(:,end)), [0, Ts], x_vehicle(:,end)); 44 | x_vehicle(:,end+1) = y(end,:)'; 45 | end 46 | %% Save Data to Workspace 47 | x_traj = x_traj'; 48 | x_vehicle = x_vehicle'; 49 | u_vehicle = u_vehicle'; 50 | % save_to_base(1); 51 | %% Plot 52 | plot(x_vehicle(:,1),x_vehicle(:,2)); 53 | hold on; 54 | plot(x_traj(:,1),x_traj(:,2)); 55 | legend('vehicle trajectory', 'desired trajectory'); 56 | end 57 | % End main loop =========================================================== 58 | 59 | %% Vehicle_model_2D_Type1 dynamics 60 | function dx = vehicle_model_2D(t,x,u) 61 | % in this model, input includes the vehicle speed and heading rate 62 | psi=x(3); 63 | ur=u(1); 64 | r=u(2); 65 | dx=[ur*cos(psi);ur*sin(psi);r]; 66 | end 67 | function u_TT= TT_controllers(x_vehicle,x_traj,x_traj_dot,l_bound,u_bound) 68 | delta=-0.5; 69 | Delta= [1 0; 70 | 0 -delta]; 71 | Delta_inv=inv(Delta); 72 | epsilon=[delta; 0]; 73 | kx=.2; ky=0.05; 74 | Kk=[kx 0; 75 | 0 ky]; 76 | p = x_vehicle(1:2); 77 | psi = x_vehicle(3); 78 | pd = x_traj; 79 | d_pd = x_traj_dot; 80 | 81 | RB_I=[cos(psi) -sin(psi); 82 | sin(psi) cos(psi)]; 83 | e_pos=RB_I'*(p-pd)-epsilon; 84 | % TT control law 85 | u_TT = Delta_inv*(-Kk*e_pos+RB_I'*d_pd); 86 | u_TT = max(min(u_TT,u_bound),l_bound); 87 | end -------------------------------------------------------------------------------- /PF-toolbox/alg_convert.m: -------------------------------------------------------------------------------- 1 | function alg_out= alg_convert(alg_new, alg_old, alg_out_old) 2 | alg_e = alg_new - alg_old; 3 | if (alg_e > 3 * pi / 2) 4 | alg_out = alg_out_old - 2 * pi + alg_e; 5 | elseif (alg_e < -3 * pi / 2) 6 | alg_out = alg_out_old + 2 * pi + alg_e; 7 | else 8 | alg_out = alg_out_old + alg_e; 9 | end 10 | end 11 | -------------------------------------------------------------------------------- /PF-toolbox/all_methods.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/all_methods.avi -------------------------------------------------------------------------------- /PF-toolbox/all_methods.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/all_methods.fig -------------------------------------------------------------------------------- /PF-toolbox/all_methods.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/all_methods.mat -------------------------------------------------------------------------------- /PF-toolbox/all_methods.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/all_methods.png -------------------------------------------------------------------------------- /PF-toolbox/animation_1vehicles.m: -------------------------------------------------------------------------------- 1 | %% Video 2 | vid = 0; 3 | v = VideoWriter('sin'); 4 | v.FrameRate=5; 5 | open(v); 6 | 7 | fig1=figure(1); 8 | %fig2=figure(2); 9 | %colors 10 | 11 | AUV_COL= [0 0 0]; 12 | 13 | 14 | AUV_COL11= [0.8 0.8 0]; 15 | AUV_COL22= [0.1 0.1 0]; 16 | AUV_COL33= [0.9 0.2 0]; 17 | 18 | 19 | % yaw2_22=90-yaw22*180/pi; 20 | % yaw3_33=90-yaw33*180/pi; 21 | 22 | % Com1_old=COM1.Data(1); 23 | % Com2_old=COM2.Data(1); 24 | % Com3_old=COM3.Data(1); 25 | set (fig1, 'Units', 'normalized', 'Position', [0,0,1,1]); 26 | pd=x_path(:,1:2); 27 | p=x_robot(:,1:2); 28 | yaw=x_robot(:,3)*180/pi; 29 | %yaw=90-yaw; 30 | 31 | for i=1:10:500%data.i, 32 | 33 | hold off 34 | figure(fig1); 35 | % path black 36 | h1 = plot(pd(1:end,2),pd(1:end,1),'--','LineWidth',1,'Color','b'); 37 | hold on; 38 | % plot(pd22(1:i,2),pd22(1:i,1),'-','LineWidth',0.2,'Color',[10,10,0]/255); 39 | % plot(pd33(1:i,2),pd33(1:i,1),'-','LineWidth',0.2,'Color',[200,10,10]/255); 40 | 41 | h2 = plot(pd(i,2),pd(i,1),'r*','MarkerSize',20, 'LineWidth', 2); 42 | % T=x_path(i,9:10); 43 | % N=x_path(i,11:12); 44 | % arrow3([pd(i,2),pd(i,1)],[pd(i,2)+5*T(2),pd(i,1)+5*T(1)]); 45 | % arrow3([pd(i,2),pd(i,1)],[pd(i,2)+5*N(2),pd(i,1)+5*N(1)]); 46 | h3 = plot(p(1:i,2),p(1:i,1),'-','LineWidth',1.5,'Color',[10,10,0]/255); 47 | 48 | % plot(p22(1:i,2),p22(1:i,1),'-','LineWidth',0.2,'Color',[10,10,0]/255); 49 | % plot(p33(1:i,2),p33(1:i,1),'-','LineWidth',0.2,'Color',[200,10,10]/255); 50 | hold on; 51 | %view(45,20) 52 | % path myellow 53 | Scale=.2; 54 | 55 | GTF_Simulink_PlotAUV([p(i,2),p(i,1),0], [0,0,90-yaw(i)], Scale, 0,AUV_COL,1); 56 | 57 | % GTF_Simulink_PlotAUV([p22(i,2),p22(i,1),0], [0,0,yaw2_22(i)], Scale, 0,AUV_COL22,1); 58 | % GTF_Simulink_PlotAUV([p33(i,2),p33(i,1),0], [0,0,yaw3_33(i)], Scale, 0,AUV_COL33,1); 59 | led = legend([h1,h2,h3],'Desired path', 'Reference point to track','Vehicle trajectory'); 60 | led.FontSize = 12; 61 | if (i==100) 62 | hello=0; 63 | end 64 | 65 | Scale=.25*8; 66 | grid on; %axis equal; 67 | % axis([-15 15 -15 15 ]) 68 | str ={'Path following method:', controller}; 69 | title1 = title(str); 70 | title1.FontSize = 12; 71 | xlabel('Y[m]') 72 | ylabel('X[m]') 73 | % axis([-15, 15 -22 28]); 74 | % axis([-15, 60 0 28]); 75 | if(vid==1) 76 | drawnow; 77 | currFrame = getframe(fig1); 78 | writeVideo(v, currFrame); 79 | else 80 | pause(0); 81 | drawnow; 82 | end 83 | hold off; 84 | 85 | % pause(0.5); 86 | 87 | % figure(fig2) 88 | % subplot(3,1,1);hold on; 89 | % if COM1.Data(i)~=Com1_old 90 | % stem(COM1.Time(i),1,'Color',[0.8,0.9,0]); 91 | % title('transmistion signal on vehicle 1'); 92 | % Com1_old=COM1.Data(i); 93 | % end 94 | % subplot(3,1,2);hold on; 95 | % if COM2.Data(i)~=Com2_old 96 | % stem(COM2.Time(i),1,'k') 97 | % title('transmistion signal on vehicle 2'); 98 | % Com2_old=COM2.Data(i); 99 | % end 100 | % subplot(3,1,3);hold on; 101 | % if COM3.Data(i)~=Com3_old 102 | % stem(COM3.Time(i),1,'r') 103 | % title('transmistion signal on vehicle 3') 104 | % Com3_old=COM3.Data(i); 105 | % end 106 | end 107 | 108 | if(vid==1) 109 | close(v); 110 | % close(fig); 111 | end -------------------------------------------------------------------------------- /PF-toolbox/arrow3.m: -------------------------------------------------------------------------------- 1 | function hn=arrow3(p1,p2,s,w,h,ip,alpha,beta) 2 | % ARROW3 (R13) 3 | % ARROW3(P1,P2) draws lines from P1 to P2 with directional arrowheads. 4 | % P1 and P2 are either nx2 or nx3 matrices. Each row of P1 is an 5 | % initial point, and each row of P2 is a terminal point. 6 | % 7 | % ARROW3(P1,P2,S,W,H,IP,ALPHA,BETA) can be used to specify properties 8 | % of the line, initial point marker, and arrowhead. S is a character 9 | % string made with one element from any or all of the following 3 10 | % columns: 11 | % 12 | % Color Switches LineStyle LineWidth 13 | % ------------------ ------------------- -------------------- 14 | % k blacK (default) - solid (default) 0.5 points (default) 15 | % y Yellow : dotted 0 no lines 16 | % m Magenta -. dashdot / LineWidthOrder 17 | % c Cyan -- dashed 18 | % r Red * LineStyleOrder _______ __ 19 | % g Green ^ | 20 | % b Blue / \ | 21 | % w White Arrowhead / \ Height 22 | % a Asparagus / \ | 23 | % d Dark gray / \ | 24 | % e Evergreen /___ ___\ __|__ 25 | % f Firebrick | | | | 26 | % h Hot pink |-- Width --| 27 | % i Indigo | | | | 28 | % j Jade | | 29 | % l Light gray | | 30 | % n Nutbrown | | 31 | % p Pear | | 32 | % q kumQuat Line -->| |<--LineWidth 33 | % s Sky blue | | 34 | % t Tawny | | 35 | % u bUrgundy | | 36 | % v Violet | | 37 | % z aZure | | 38 | % x random Initial / \ 39 | % o colorOrder Point -->( )<--IP 40 | % | magnitude Marker \_ _/ 41 | % 42 | % ------------------------------------------------------------- 43 | % Color Equivalencies 44 | % ------------------------------------------------------------- 45 | % ColorOrder Arrow3 | Simulink Arrow3 46 | % ---------- ---------- | ---------- ---------- 47 | % Color1 Blue | LightBlue aZure 48 | % Color2 Evergreen | DarkGreen Asparagus 49 | % Color3 Red | Orange kumQuat 50 | % Color4 Sky blue | Gray Light gray 51 | % Color5 Violet | 52 | % Color6 Pear | 53 | % Color7 Dark gray | 54 | % ------------------------------------------------------------- 55 | % 56 | % The components of S may be specified in any order. Invalid 57 | % characters in S will be ignored and replaced by default settings. 58 | % 59 | % Prefixing the color code with '_' produces a darker shade, e.g. 60 | % '_t' is dark tawny; prefixing the color code with '^' produces a 61 | % lighter shade, e.g. '^q' is light kumquat. The relative brightness 62 | % of light and dark color shades is controlled by the scalar parameter 63 | % BETA. Color code prefixes do not affect black (k), white (w), or 64 | % the special color switches (xo|). 65 | % 66 | % ColorOrder may be achieved in two fashions: The user may either 67 | % set the ColorOrder property (using RGB triples) or define the 68 | % global variable ColorOrder (using a string of valid color codes). 69 | % If the color switch is specified with 'o', and the global variable 70 | % ColorOrder is a string of color codes (color switches less 'xo|', 71 | % optionally prefixed with '_' or '^'), then the ColorOrder property 72 | % will be set to the sequence of colors indicated by the ColorOrder 73 | % variable. The color sequence 'bersvpd' matches the default 74 | % ColorOrder property. If the color switch is specified with 'o', and 75 | % the global variable ColorOrder is empty or invalid, then the current 76 | % ColorOrder property will be used. Note that the ColorOrder variable 77 | % takes precedence over the ColorOrder property. 78 | % 79 | % The magnitude color switch is used to visualize vector magnitudes 80 | % in conjunction with a colorbar. If the color switch is specified 81 | % with '|', colors are linearly interpolated from the current ColorMap 82 | % according to the length of the associated line. This option sets 83 | % CLim to [MinM,MaxM], where MinM and MaxM are the minimum and maximum 84 | % magnitudes, respectively. 85 | % 86 | % The current LineStyleOrder property will be used if LineStyle is 87 | % specified with '*'. MATLAB cycles through the line styles defined 88 | % by the LineStyleOrder property only after using all colors defined 89 | % by the ColorOrder property. If however, the global variable 90 | % LineWidthOrder is defined, and LineWidth is specified with '/', 91 | % then each line will be drawn with sequential color, linestyle, and 92 | % linewidth. 93 | % 94 | % W (default = 1) is a vector of arrowhead widths; use W = 0 for no 95 | % arrowheads. H (default = 3W) is a vector of arrowhead heights. If 96 | % vector IP is neither empty nor negative, initial point markers will 97 | % be plotted with diameter IP; for default diameter W, use IP = 0. 98 | % The units of W, H and IP are 1/72 of the PlotBox diagonal. 99 | % 100 | % ALPHA (default = 1) is a vector of FaceAlpha values ranging between 101 | % 0 (clear) and 1 (opaque). FaceAlpha is a surface (arrowhead and 102 | % initial point marker) property and does not affect lines. FaceAlpha 103 | % is not supported for 2D rendering. 104 | % 105 | % BETA (default = 0.4) is a scalar that controls the relative 106 | % brightness of light and dark color shades, ranging between 0 (no 107 | % contrast) and 1 (maximum contrast). 108 | % 109 | % Plotting lines with a single color, linestyle, and linewidth is 110 | % faster than plotting lines with multiple colors and/or linestyles. 111 | % Plotting lines with multiple linewidths is slower still. ARROW3 112 | % chooses renderers that produce the best screen images; exported 113 | % or printed plots may benefit from different choices. 114 | % 115 | % ARROW3(P1,P2,S,W,H,'cone',...) will plot cones with bases centered 116 | % on P1 in the direction given by P2. In this instance, P2 is 117 | % interpreted as a direction vector instead of a terminal point. 118 | % Neither initial point markers nor lines are plotted with the 'cone' 119 | % option. 120 | % 121 | % HN = ARROW3(P1,P2,...) returns a vector of handles to line and 122 | % surface objects created by ARROW3. 123 | % 124 | % ARROW3 COLORS will plot a table of named colors with default 125 | % brightness. ARROW3('colors',BETA) will plot a table of named 126 | % colors with brightness BETA. 127 | % 128 | % ARROW3 attempts to preserve the appearance of existing axes. In 129 | % particular, ARROW3 will not change XYZLim, View, or CameraViewAngle. 130 | % ARROW3 does not, however, support stretch-to-fill scaling. AXIS 131 | % NORMAL will restore the current axis box to full size and remove any 132 | % restrictions on the scaling of units, but will likely result in 133 | % distorted arrowheads and initial point markers. See 134 | % (arrow3_messes_up_my_plots.html). 135 | % 136 | % If a particular aspect ratio or variable limit is required, use 137 | % DASPECT, PBASPECT, AXIS, or XYZLIM commands before calling ARROW3. 138 | % Changing limits or aspect ratios after calling ARROW3 may alter the 139 | % appearance of arrowheads and initial point markers. ARROW3 sets 140 | % XYZCLimMode to manual for all plots, sets DataAspectRatioMode to 141 | % manual for linear plots, and sets PlotBoxAspectRatioMode to manual 142 | % for log plots and 3D plots. CameraViewAngleMode is also set to 143 | % manual for 3D plots. 144 | % 145 | % ARROW3 UPDATE will restore the appearance of arrowheads and 146 | % initial point markers that have become corrupted by changes to 147 | % limits or aspect ratios. ARROW3('update',SF) will redraw initial 148 | % point markers and arrowheads with scale factor SF. If SF has one 149 | % element, SF scales W, H and IP. If SF has two elements, SF(1) 150 | % scales W and IP, and SF(2) scales H. If SF has three elements, 151 | % SF(1) scales W, SF(2) scales H, and SF(3) scales IP. All sizes are 152 | % relative to the current PlotBox diagonal. 153 | % 154 | % ARROW3 UPDATE COLORS will update the magnitude coloring of 155 | % arrowheads, initial point markers, and lines to conform to the 156 | % current ColorMap. 157 | % 158 | % HN = ARROW3('update',...) returns a vector of handles to updated 159 | % objects. 160 | % 161 | % EXAMPLES: 162 | % 163 | % % 2D vectors 164 | % arrow3([0 0],[1 3]) 165 | % arrow3([0 0],[1 2],'-.e') 166 | % arrow3([0 0],[10 10],'--x2',1) 167 | % arrow3(zeros(10,2),50*rand(10,2),'x',1,3) 168 | % arrow3(zeros(10,2),[10*rand(10,1),500*rand(10,1)],'u') 169 | % arrow3(10*rand(10,2),50*rand(10,2),'x',1,[],1) 170 | % 171 | % % 3D vectors 172 | % arrow3([0 0 0],[1 1 1]) 173 | % arrow3(zeros(20,3),50*rand(20,3),'--x1.5',2) 174 | % arrow3(zeros(100,3),50*rand(100,3),'x',1,3) 175 | % arrow3(zeros(10,3),[10*rand(10,1),500*rand(10,1),50*rand(10,1)],'a') 176 | % arrow3(10*rand(10,3),50*rand(10,3),'x',[],[],0) 177 | % 178 | % % 3D animation 179 | % t=(0:pi/40:8*pi)'; u=cos(t); v=sin(t); 180 | % plot3(20*t,u,v) 181 | % axis([0,600,-1.5,1.5,-1.5,1.5]) 182 | % grid on, view(35,25) 183 | % hold on 184 | % pbaspect([1.8,1.4,1]) 185 | % arrow3(zeros(3),diag([500,1.5,1.5]),'l',0.7,[],0) 186 | % p=[20*t,u,v]; inc=4:1:length(t); 187 | % p2=p(inc,:); p1=p(inc-1,:); 188 | % hn=arrow3(p1(1,:),p2(1,:),'0_b',0.7); 189 | % for i=2:1:length(p1) 190 | % delete(hn) 191 | % hn=arrow3(p1(i,:),p2(i,:),'0_b',0.7); 192 | % pause(0.01) 193 | % end 194 | % hold off 195 | % 196 | % % Cone plot 197 | % t=(pi/8:pi/8:2*pi)'; p1=[cos(t) sin(t) t]; p2=repmat([0 0 1],16,1); 198 | % arrow3(p1,p2,'x',2,4,'cone'), hold on 199 | % plot3(p1(:,1),p1(:,2),p1(:,3)), hold off 200 | % pause % change cone size 201 | % arrow3('update',[1,2]) 202 | % 203 | % % Just for fun 204 | % arrow3(zeros(100,3),50*rand(100,3),'x',8,4,[],0.95) 205 | % light('position',[-10 -10 -10],'style','local') 206 | % light('position',[60,60,60]), lighting gouraud 207 | % 208 | % % ColorOrder variable, color code prefixes, and Beta 209 | % global ColorOrder, ColorOrder='^ui^e_hq^v'; 210 | % theta=[0:pi/22:pi/2]'; 211 | % arrow3(zeros(12,2),[cos(theta),sin(theta)],'1.5o',1.5,[],[],[],0.5) 212 | % 213 | % % ColorOrder property, LineStyleOrder, and LineWidthOrder 214 | % global ColorOrder, ColorOrder=[]; 215 | % set(gca,'ColorOrder',[1,0,0;0,0,1;0.25,0.75,0.25;0,0,0]) 216 | % set(gca,'LineStyleOrder',{'-','--','-.',':'}) 217 | % global LineWidthOrder, LineWidthOrder=[1,2,4,8]; 218 | % w=[1,2,3,4]; h=[4,6,4,2]; 219 | % arrow3(zeros(4,2),[10*rand(4,1),500*rand(4,1)],'o*/',w,h,0) 220 | % 221 | % % Magnitude coloring 222 | % colormap spring 223 | % arrow3(20*randn(20,3),50*randn(20,3),'|',[],[],0) 224 | % set(gca,'color',0.7*[1,1,1]) 225 | % set(gcf,'color',0.5*[1,1,1]), grid on, colorbar 226 | % pause % change the ColorMap and update colors 227 | % colormap hot 228 | % arrow3('update','colors') 229 | % 230 | % % LogLog plot 231 | % set(gca,'xscale','log','yscale','log'); 232 | % axis([1e2,1e8,1e-2,1e-1]); hold on 233 | % p1=repmat([1e3,2e-2],15,1); 234 | % q1=[1e7,1e6,1e5,1e4,1e3,1e7,1e7,1e7,1e7,1e7,1e7,1e6,1e5,1e4,1e3]; 235 | % q2=1e-2*[9,9,9,9,9,7,5,4,3,2,1,1,1,1,1]; p2=[q1',q2']; 236 | % global ColorOrder, ColorOrder=[]; 237 | % set(gca,'ColorOrder',rand(15,3)) 238 | % arrow3(p1,p2,'o'), grid on, hold off 239 | % 240 | % % SemiLogX plot 241 | % set(gca,'xscale','log','yscale','linear'); 242 | % axis([1e2,1e8,1e-2,1e-1]); hold on 243 | % p1=repmat([1e3,0.05],15,1); 244 | % q1=[1e7,1e6,1e5,1e4,1e3,1e7,1e7,1e7,1e7,1e7,1e7,1e6,1e5,1e4,1e3]; 245 | % q2=1e-2*[9,9,9,9,9,7,5,4,3,2,1,1,1,1,1]; p2=[q1',q2']; 246 | % arrow3(p1,p2,'x'), grid on, hold off 247 | % 248 | % % SemiLogY plot 249 | % set(gca,'xscale','linear','yscale','log'); 250 | % axis([2,8,1e-2,1e-1]); hold on 251 | % p1=repmat([3,2e-2],17,1); 252 | % q1=[7,6,5,4,3,7,7,7,7,7,7,7,7,6,5,4,3]; 253 | % q2=1e-2*[9,9,9,9,9,8,7,6,5,4,3,2,1,1,1,1,1]; p2=[q1',q2']; 254 | % set(gca,'LineStyleOrder',{'-','--','-.',':'}) 255 | % arrow3(p1,p2,'*',1,[],0), grid on, hold off 256 | % 257 | % % Color tables 258 | % arrow3('colors') % default color table 259 | % arrow3('colors',0.3) % low contrast color table 260 | % arrow3('colors',0.5) % high contrast color table 261 | % 262 | % % Update initial point markers and arrowheads 263 | % % relative to the current PlotBox diagonal 264 | % arrow3('update') % redraw same size 265 | % arrow3('update',2) % redraw double size 266 | % arrow3('update',0.5) % redraw half size 267 | % arrow3('update',[0.5,2,1]) % redraw W half size, 268 | % % H double size, and 269 | % % IP same size 270 | % 271 | % See also (arrow3_examples.html), (arrow3_messes_up_my_plots.html). 272 | 273 | % Copyright(c)2002-2019 Version 5.17 274 | % Tom Davis (tdavis@metzgerwillard.com) 275 | % Jeff Chang 276 | 277 | % Revision History: 278 | % 279 | % 11/02/19 - Do not restore preexisting ColorOrder property. (TD) 280 | % 08/28/18 - Use explicit EdgeColor. (TD) 281 | % 01/15/13 - Use AppData instead of UserData. (TD) 282 | % 07/27/11 - Added animation example. (TD) 283 | % 05/13/09 - Corrected spelling errors (TD) 284 | % 03/16/08 - Updated contact information (TD) 285 | % 10/23/07 - Corrected zero magnitude exclusion (TD) 286 | % 09/08/07 - Added cone plot option; removed adaptive grid 287 | % spacing; corrected scale factor; removed "nearly" 288 | % tight limits (TD) 289 | % 07/24/07 - Ignore zero-magnitude input (TD) 290 | % 07/08/07 - Modified named colors to match named Simulink 291 | % colors; added light and dark shades for basic 292 | % colors (ymcrgb) (TD) 293 | % 07/01/07 - Modified named colors to match default ColorOrder 294 | % colors (TD) 295 | % 06/24/07 - Error checking for empty P1, P2 (TD) 296 | % 06/17/07 - Trim colors,W,H,IP,ALPHA to LENGTH(P1) (TD) 297 | % 05/27/07 - Magnitude coloring and documentation revision (TD) 298 | % 03/10/07 - Improved code metrics (TD) 299 | % 02/21/07 - Preserve existing axis appearance; 300 | % use relative sizes for W, H, and IP; 301 | % removed version checking; minor bug fixes (TD) 302 | % 01/09/04 - Replaced calls to LINSPACE, INTERP1, and 303 | % COLORMAP (TD) 304 | % 12/17/03 - Semilog examples, CAXIS support, magnitude 305 | % coloring, and color updating; use CData instead 306 | % of FaceColor; minor bug fixes (TD) 307 | % 07/17/03 - Changed 2D rendering from OpenGL to ZBuffer; 308 | % defined HN for COLORS and UPDATE options (TD) 309 | % 02/27/03 - Replaced calls to RANDPERM, VIEW, REPMAT, SPHERE, 310 | % and CYLINDER; added ZBuffer for log plots, RESET 311 | % for CLA and CLF, and ABS for W and H (TD) 312 | % 02/01/03 - Added UPDATE scale factor and MATLAB version 313 | % checking, replaced call to CROSS (TD) 314 | % 12/26/02 - Added UserData and UPDATE option (TD) 315 | % 11/16/02 - Added more named colors, color code prefix, 316 | % global ColorOrder, ALPHA , and BETA (TD) 317 | % 10/12/02 - Added global LineWidthOrder, 318 | % vectorized W, H and IP (TD) 319 | % 10/05/02 - Changed CLF to CLA for subplot support, 320 | % added ColorOrder and LineStyleOrder support (TD) 321 | % 04/27/02 - Minor log plot revisions (TD) 322 | % 03/26/02 - Added log plot support (TD) 323 | % 03/24/02 - Adaptive grid spacing control to trade off 324 | % appearance vs. speed based on size of matrix (JC) 325 | % 03/16/02 - Added "axis tight" for improved appearance (JC) 326 | % 03/12/02 - Added initial point marker (TD) 327 | % 03/03/02 - Added aspect ratio support (TD) 328 | % 03/02/02 - Enhanced program's user friendliness (JC) 329 | % (lump Color, LineStyle, and LineWidth together) 330 | % 03/01/02 - Replaced call to ROTATE (TD) 331 | % 02/28/02 - Modified line plotting, 332 | % added linewidth and linestyle (TD) 333 | % 02/27/02 - Minor enhancements on 3D appearance (JC) 334 | % 02/26/02 - Minor enhancements for speed (TD&JC) 335 | % 02/26/02 - Optimize PLOT3 and SURF for speed (TD) 336 | % 02/25/02 - Return handler, error handling, color effect, 337 | % generalize for 2D/3D vectors (JC) 338 | % 02/24/02 - Optimize PLOT3 and SURF for speed (TD) 339 | % 02/23/02 - First release (JC&TD) 340 | 341 | %------------------------------------------------------------------------- 342 | % Error Checking 343 | global LineWidthOrder ColorOrder 344 | if nargin<8 || isempty(beta), beta=0.4; end 345 | beta=abs(beta(1)); if nargout, hn=[]; end 346 | if strcmpi(p1,'colors') % plot color table 347 | if nargin>1, beta=abs(p2(1)); end 348 | LocalColorTable(1,beta); return 349 | end 350 | fig=gcf; ax=gca; 351 | if strcmpi(p1,'update'), ad=getappdata(ax,'Arrow3'); % update 352 | LocalLogCheck(ax); 353 | if size(ad,2)<13, error('Invalid AppData'), end 354 | setappdata(ax,'Arrow3',[]); sf=[1,1,1]; flag=0; 355 | if nargin>1 356 | if strcmpi(p2,'colors'), flag=1; % update colors 357 | elseif ~isempty(p2) % update surfaces 358 | sf=p2(1)*sf; n=length(p2(:)); 359 | if n>1, sf(2)=p2(2); if n>2, sf(3)=p2(3); end, end 360 | end 361 | end 362 | H=LocalUpdate(fig,ax,ad,sf,flag); if nargout, hn=H; end, return 363 | end 364 | InputError=['Invalid input, type HELP ',upper(mfilename),... 365 | ' for usage examples']; 366 | if nargin<2, error(InputError), end 367 | [r1,c1]=size(p1); [r2,c2]=size(p2); 368 | if c1<2 || c1>3 || r1*r2==0, error(InputError), end 369 | if r1~=r2, error('P1 and P2 must have same number of rows'), end 370 | if c1~=c2, error('P1 and P2 must have same number of columns'), end 371 | p=sum(abs(p2-p1),2)~=0; cone=0; 372 | if nargin>5 && ~isempty(ip) && strcmpi(ip,'cone') % cone plot 373 | cone=1; p=sum(p2,2)~=0; 374 | if ~any(p), error('P2 cannot equal 0'), end 375 | set(ax,'tag','Arrow3ConePlot'); 376 | elseif ~any(p), error('P1 cannot equal P2') 377 | end 378 | if ~all(p) 379 | warning('Arrow3:ZeroMagnitude','Zero magnitude ignored') 380 | p1=p1(p,:); p2=p2(p,:); [r1,c1]=size(p1); 381 | end 382 | n=r1; Zeros=zeros(n,1); 383 | if c1==2, p1=[p1,Zeros]; p2=[p2,Zeros]; 384 | elseif ~any([p1(:,3);p2(:,3)]), c1=2; end 385 | L=get(ax,'LineStyleOrder'); C=get(ax,'ColorOrder'); 386 | ST=get(ax,'DefaultSurfaceTag'); LT=get(ax,'DefaultLineTag'); 387 | EC=get(ax,'DefaultSurfaceEdgeColor'); 388 | if strcmp(get(ax,'nextplot'),'add') && strcmp(get(fig,'nextplot'),'add') 389 | Xr=get(ax,'xlim'); Yr=get(ax,'ylim'); Zr=get(ax,'zlim'); 390 | [xs,ys,xys]=LocalLogCheck(ax); restore=1; 391 | if xys, mode='auto'; 392 | if any([p1(:,3);p2(:,3)]), error('3D log plot not supported'), end 393 | if (xs && ~all([p1(:,1);p2(:,1)]>0)) || ... 394 | (ys && ~all([p1(:,2);p2(:,2)]>0)) 395 | error('Nonpositive log data not supported') 396 | end 397 | else mode='manual'; 398 | if strcmp(get(ax,'WarpToFill'),'on') 399 | warning('Arrow3:WarpToFill',['Stretch-to-fill scaling not ',... 400 | 'supported;\nuse DASPECT or PBASPECT before calling ARROW3.']); 401 | end 402 | end 403 | set(ax,'XLimMode',mode,'YLimMode',mode,'ZLimMode',mode,... 404 | 'CLimMode','manual'); 405 | else restore=0; cla reset; xys=0; set(fig,'nextplot','add'); 406 | if c1==2, azel=[0,90]; else azel=[-37.5,30]; end 407 | setappdata(ax,'Arrow3',[]); 408 | set(ax,'nextplot','add','View',azel); 409 | end 410 | 411 | %------------------------------------------------------------------------- 412 | % Style Control 413 | [vc,cn]=LocalColorTable(0); prefix=''; OneColor=0; 414 | if nargin<3, [c,ls,lw]=LocalValidateCLSW;% default color, linestyle/width 415 | else 416 | [c,ls,lw]=LocalValidateCLSW(s); 417 | if length(c)>1, if sum('_^'==c(1)), prefix=c(1); end, c=c(2); end 418 | if c=='x' % random named color (less white) 419 | [ignore,i]=sort(rand(1,23)); c=cn(i,:); % #ok 420 | elseif c=='o' % ColorOrder 421 | if length(ColorOrder) 422 | [c,failed]=LocalColorMap(lower(ColorOrder),vc,cn,beta); 423 | if failed, ColorOrderWarning=['Invalid ColorOrder ',... 424 | 'variable, current ColorOrder property will be used']; 425 | warning('Arrow3:ColorOrder',ColorOrderWarning) 426 | else C=c; 427 | end 428 | end, c=C; 429 | elseif c=='|', map=get(fig,'colormap'); % magnitude coloring 430 | M=(p1-p2); M=sqrt(sum(M.*M,2)); minM=min(M); maxM=max(M); 431 | if maxM-minM<1, minM=0; end 432 | set(ax,'clim',[minM,maxM]); c=LocalInterp(minM,maxM,map,M); 433 | elseif ~sum(vc==c), c='k'; ColorWarning=['Invalid color switch, ',... 434 | 'default color (black) will be used']; 435 | warning('Arrow3:Color',ColorWarning) 436 | end 437 | end 438 | if length(c)==1 % single color 439 | c=LocalColorMap([prefix,c],vc,cn,beta); OneColor=1; 440 | end 441 | set(ax,'ColorOrder',c); c=LocalRepmat(c,[ceil(n/size(c,1)),1]); 442 | if ls~='*', set(ax,'LineStyleOrder',ls); end % LineStyleOrder 443 | if lw=='/' && ~length(LineWidthOrder) % LineWidthOrder 444 | LineWidthOrder=0.5; LineWidthOrderWarning=['Undefined ',... 445 | 'LineWidthOrder, default width (0.5) will be used']; 446 | warning('Arrow3:LineWidthOrder',LineWidthOrderWarning) 447 | else 448 | LineWidthOrder=lw; % 02/09/2020 449 | end 450 | if nargin<4 || isempty(w), w=1; end % width 451 | w=LocalRepmat(abs(w(:)),[ceil(n/length(w)),1]); 452 | if nargin<5 || isempty(h), h=3*w; end % height 453 | h=LocalRepmat(abs(h(:)),[ceil(n/length(h)),1]); 454 | if nargin>5 && ~isempty(ip) && ~cone % ip 455 | ip=LocalRepmat(ip(:),[ceil(n/length(ip)),1]); 456 | i=find(ip==0); ip(i)=w(i); 457 | else ip=-ones(n,1); 458 | end 459 | if nargin<7 || isempty(alpha), alpha=1; end 460 | a=LocalRepmat(alpha(:),[ceil(n/length(alpha)),1]); % FaceAlpha 461 | 462 | %------------------------------------------------------------------------- 463 | % Log Plot 464 | if xys 465 | units=get(ax,'units'); set(ax,'units','points'); 466 | pos=get(ax,'position'); set(ax,'units',units); 467 | if strcmp(get(ax,'PlotBoxAspectRatioMode'),'auto') 468 | set(ax,'PlotBoxAspectRatio',[pos(3),pos(4),1]); 469 | end 470 | par=get(ax,'PlotBoxAspectRatio'); 471 | set(ax,'DataAspectRatio',[par(2),par(1),par(3)]); 472 | % map coordinates onto unit square 473 | q=[p1;p2]; xr=Xr; yr=Yr; 474 | if xs, xr=log10(xr); q(:,1)=log10(q(:,1)); end 475 | if ys, yr=log10(yr); q(:,2)=log10(q(:,2)); end 476 | q=q-LocalRepmat([xr(1),yr(1),0],[2*n,1]); 477 | dx=xr(2)-xr(1); dy=yr(2)-yr(1); 478 | q=q*diag([1/dx,1/dy,1]); 479 | q1=q(1:n,:); q2=q(n+1:end,:); 480 | else xs=0; ys=0; dx=0; dy=0; xr=0; yr=0; 481 | end 482 | 483 | %------------------------------------------------------------------------- 484 | % Line 485 | if ~cone 486 | set(ax,'DefaultLineTag','arrow3'); 487 | % if length(lw)==1 488 | % if lw>0 489 | % if OneColor && ls(end)~='*' && n>1 % single color, linestyle/width 490 | % P=zeros(3*n,3); i=1:n; 491 | % P(3*i-2,:)=p1(i,:); P(3*i-1,:)=p2(i,:); P(3*i,1)=NaN; 492 | % H1=plot3(P(:,1),P(:,2),P(:,3),'LineWidth',lw); 493 | % else % single linewidth 494 | % H1=plot3([p1(:,1),p2(:,1)]',[p1(:,2),p2(:,2)]',... 495 | % [p1(:,3),p2(:,3)]','LineWidth',lw); 496 | % end 497 | % else H1=[]; 498 | % end 499 | if lw==0 % 02/09/2020 500 | H1=[]; 501 | else 502 | LS=get(ax,'LineStyleOrder'); 503 | ls=LocalRepmat(cellstr(LS),[ceil(n/size(LS,1)),1]); 504 | lw=LocalRepmat(LineWidthOrder(:),[ceil(n/length(LineWidthOrder)),1]); 505 | % lsSize=size(ls), lwSize=size(lw), cSize=size(c) 506 | H1=Zeros; 507 | for i=1:n 508 | H1(i)=plot3([p1(i,1),p2(i,1)],[p1(i,2),p2(i,2)],... 509 | [p1(i,3),p2(i,3)],ls{i},'Color',c(i,:),'LineWidth',lw(i)); 510 | end 511 | end 512 | else % cone plot 513 | P=zeros(3*n,3); i=1:n; 514 | P(3*i-2,:)=p1(i,:); P(3*i-1,:)=p1(i,:); P(3*i,1)=NaN; 515 | H1=plot3(P(:,1),P(:,2),P(:,3)); 516 | end 517 | 518 | %------------------------------------------------------------------------- 519 | % Scale 520 | if ~restore, axis tight, end 521 | ar=get(ax,'DataAspectRatio'); ar=sqrt(3)*ar/norm(ar); 522 | set(ax,'DataAspectRatioMode','manual'); 523 | if xys, sf=1; 524 | else xr=get(ax,'xlim'); yr=get(ax,'ylim'); zr=get(ax,'zlim'); 525 | sf=norm(diff([xr;yr;zr],1,2)./ar')/72; 526 | end 527 | 528 | %------------------------------------------------------------------------- 529 | % AppData 530 | c=c(1:n,:); w=w(1:n); h=h(1:n); ip=ip(1:n); a=a(1:n); 531 | setappdata(ax,'Arrow3',[getappdata(ax,'Arrow3');p1,p2,c,w,h,ip,a]); 532 | 533 | %------------------------------------------------------------------------- 534 | % Arrowhead 535 | whip=sf*[w,h,ip]; 536 | if xys, whip=whip*sqrt(2)/72; p1=q1; p2=q2; end 537 | w=whip(:,1); h=whip(:,2); ip=whip(:,3); 538 | if cone % cone plot 539 | delete(H1), H1=[]; 540 | p2=p2./LocalRepmat(sqrt(sum(p2.*p2,2)),[1,3]); 541 | p2=p1+p2.*LocalRepmat(ar,[n,1]).*LocalRepmat(h,[1,3]); 542 | end 543 | W=(p1-p2)./LocalRepmat(ar,[n,1]); 544 | W=W./LocalRepmat(sqrt(sum(W.*W,2)),[1,3]); % new z direction 545 | U=[-W(:,2),W(:,1),Zeros]; 546 | N=sqrt(sum(U.*U,2)); i=find(N0) 574 | theta=(-m:2:m)/m*pi; phi=(-m:2:m)'/m*pi/2; cosphi=cos(phi); 575 | x=cosphi*cos(theta); y=cosphi*sin(theta); z=sin(phi)*Ones; 576 | G=surface(x*ar(1)/2,y*ar(2)/2,z*ar(3)/2); 577 | set(G,'EdgeColor','none'); 578 | X=get(G,'XData'); Y=get(G,'YData'); Z=get(G,'ZData'); 579 | H3=zeros(n,1); 580 | for i=1:n % translate 581 | if ip(i)>0 582 | H3(i)=copyobj(G,ax); 583 | x=p1(i,1)+X*ip(i); y=p1(i,2)+Y*ip(i); z=p1(i,3)+Z*ip(i); 584 | LocalSetSurface(xys,xs,ys,dx,dy,xr,yr,... 585 | x,y,z,a(i),c(i,:),H3(i),m+1,m+1); 586 | end 587 | end, delete(G); 588 | else H3=[]; 589 | end 590 | 591 | %------------------------------------------------------------------------- 592 | % Finish 593 | if restore, xr=Xr; yr=Yr; zr=Zr; 594 | if xys, set(ax,'DataAspectRatioMode','auto'); end 595 | else 596 | axis tight 597 | xr=get(ax,'xlim'); yr=get(ax,'ylim'); zr=get(ax,'zlim'); 598 | set(ax,'nextplot','replace'); 599 | end 600 | azel=get(ax,'view'); 601 | if abs(azel(2))==90, renderer='ZBuffer'; else renderer='OpenGL'; c1=3; end 602 | set(fig,'Renderer',renderer); 603 | % set(ax,'LineStyleOrder',L,'DefaultLineTag',LT,... 604 | set(ax,'LineStyleOrder',L,'ColorOrder',C,'DefaultLineTag',LT,... 605 | 'DefaultSurfaceTag',ST,'DefaultSurfaceEdgeColor',EC,... 606 | 'xlim',xr,'ylim',yr,'zlim',zr,'clim',get(ax,'CLim')); 607 | if c1==3, set(ax,'CameraViewAngle',get(ax,'CameraViewAngle'),... 608 | 'PlotBoxAspectRatio',get(ax,'PlotBoxAspectRatio')); 609 | end 610 | if nargout, hn=[H1(:);H2(:);H3(:)]; end 611 | 612 | %------------------------------------------------------------------------- 613 | % Local Functions 614 | %------------------------------------------------------------------------- 615 | % Update 616 | function H=LocalUpdate(fig,ax,ad,sf,flag) 617 | global ColorOrder 618 | p1=ad(:,1:3); p2=ad(:,4:6); c=ad(:,7:9); a=ad(:,13); 619 | w=sf(1)*ad(:,10); h=sf(2)*ad(:,11); ip=sf(3)*ad(:,12); 620 | H=get(ax,'children'); tag=get(H,'tag'); type=get(H,'type'); 621 | delete(H(strcmp(tag,'arrow3') & strcmp(type,'surface'))); 622 | set(fig,'nextplot','add'); set(ax,'nextplot','add'); H1=[]; 623 | if flag, map=get(fig,'colormap'); % update colors 624 | M=(p1-p2); M=sqrt(sum(M.*M,2)); minM=min(M); maxM=max(M); 625 | H1=H(strcmp(tag,'arrow3') & strcmp(type,'line')); 626 | MagnitudeWarning=['Cannot perform magnitude coloring on lines ',... 627 | 'that\nwere drawn with a single color, linestyle, and linewidth']; 628 | if length(H1)>1 629 | for i=1:length(H1) % update line colors 630 | x=get(H1(i),'xdata'); y=get(H1(i),'ydata'); z=get(H1(i),'zdata'); 631 | if length(x)>2 % multiple lines 632 | warning('Arrow3:Magnitude',MagnitudeWarning), continue 633 | end 634 | m=sqrt((x(1)-x(2))^2+(y(1)-y(2))^2+(z(1)-z(2))^2); 635 | c=LocalInterp(minM,maxM,map,m); set(H1(i),'color',c); 636 | end 637 | elseif length(H1)==1 638 | warning('Arrow3:Magnitude',MagnitudeWarning) 639 | end 640 | c=LocalInterp(minM,maxM,map,M); 641 | end 642 | set(ax,'ColorOrder',c); % update surfaces 643 | ColorOrder=[]; 644 | if strcmp(get(ax,'tag'),'Arrow3ConePlot') 645 | H=arrow3(p1,p2,'o' ,w,h,'cone',a); % update cones 646 | else H=arrow3(p1,p2,'o0',w,h, ip,a); 647 | end, H=[H1(:);H(:)]; 648 | set(ax,'nextplot','replace'); 649 | 650 | %------------------------------------------------------------------------- 651 | % SetSurface 652 | function LocalSetSurface(xys,xs,ys,dx,dy,xr,yr,x,y,z,a,c,H,n,m) 653 | if xys 654 | x=x*dx+xr(1); y=y*dy+yr(1); 655 | if xs, x=10.^x; end 656 | if ys, y=10.^y; end 657 | end 658 | cd=zeros(n,m,3); cd(:,:,1)=c(1); cd(:,:,2)=c(2); cd(:,:,3)=c(3); 659 | set(H,'XData',x,'YData',y,'ZData',z,'CData',cd,'FaceAlpha',a); 660 | 661 | %------------------------------------------------------------------------- 662 | % ColorTable 663 | function [vc,cn]=LocalColorTable(n,beta) 664 | vc='kymcrgbadefhijlnpqstuvzw'; % valid color codes 665 | % k y m c 666 | cn=[0.00,0.00,0.00; 1.00,1.00,0.00; 1.00,0.00,1.00; 0.00,1.00,1.00; 667 | % r g b a 668 | 1.00,0.00,0.00; 0.00,1.00,0.00; 0.00,0.00,1.00; 0.42,0.59,0.24; 669 | % d e f h 670 | 0.25,0.25,0.25; 0.00,0.50,0.00; 0.70,0.13,0.13; 1.00,0.41,0.71; 671 | % i j l n 672 | 0.29,0.00,0.51; 0.00,0.66,0.42; 0.50,0.50,0.50; 0.50,0.20,0.00; 673 | % p q s t 674 | 0.75,0.75,0.00; 1.00,0.50,0.00; 0.00,0.75,0.75; 0.80,0.34,0.00; 675 | % u v z w 676 | 0.50,0.00,0.13; 0.75,0.00,0.75; 0.38,0.74,0.99; 1.00,1.00,1.00]; 677 | 678 | % Named Simulink Colors (zaql) 679 | % LightBlue = 0.38 0.74 0.99 = aZure 680 | % DarkGreen = 0.42 0.59 0.24 = Asparagus 681 | % Orange = 1.00 0.50 0.00 = kumQuat 682 | % Gray = 0.50 0.50 0.50 = Light gray 683 | % 684 | % Default ColorOrder Property Colors (bersvpd) 685 | % Color1 = 0.00 0.00 1.00 = Blue 686 | % Color2 = 0.00 0.50 0.00 = Evergreen 687 | % Color3 = 1.00 0.00 0.00 = Red 688 | % Color4 = 0.00 0.75 0.75 = Sky blue 689 | % Color5 = 0.75 0.00 0.75 = Violet 690 | % Color6 = 0.75 0.75 0.00 = Pear 691 | % Color7 = 0.25 0.25 0.25 = Dark gray 692 | 693 | if n, clf reset % plot color table 694 | name={'blacK','Yellow','Magenta','Cyan',... 695 | 'Red','Green','Blue','Asparagus',... 696 | 'Dark gray','Evergreen','Firebrick','Hot pink',... 697 | 'Indigo','Jade','Light gray','Nutbrown',... 698 | 'Pear','kumQuat','Sky blue','Tawny',... 699 | 'bUrgundy','Violet','aZure','White'}; 700 | c=['yptn';'gjae';'czsb';'hmvi';'qrfu';'wldk']; 701 | set(gcf,'DefaultAxesXTick',[],'DefaultAxesYTick',[],... 702 | 'DefaultAxesXTickLabel',[],'DefaultAxesYTickLabel',[],... 703 | 'DefaultAxesXLim',[0,0.75],'DefaultAxesYLim',[0,0.75],... 704 | 'DefaultRectangleEdgeColor','none'); 705 | for i=1:24, subplot(4,6,i); box on 706 | j=find(vc==c(i)); title(name{j}); 707 | dark=LocalBrighten(cn(j,:),-beta); 708 | light=LocalBrighten(cn(j,:),beta); 709 | rectangle('Position',[0,0.00,0.75,0.25],'FaceColor',dark); 710 | rectangle('Position',[0,0.25,0.75,0.25],'FaceColor',cn(j,:)); 711 | rectangle('Position',[0,0.50,0.75,0.25],'FaceColor',light); 712 | rectangle('Position',[0,0.00,0.75,0.75],'EdgeColor','k'); 713 | if rem(i,6)==1 714 | set(gca,'YTickLabel',{'dark','normal','light'},... 715 | 'YTick',[0.125,0.375,0.625]); 716 | if i==19 717 | text(0,-0.25,['{\bf\itARROW3} Named Color Table ',... 718 | '( \beta = ',num2str(beta),' )']); 719 | end 720 | end 721 | end 722 | end 723 | 724 | %------------------------------------------------------------------------- 725 | % ColorMap 726 | function [C,failed]=LocalColorMap(c,vc,cn,beta) 727 | n=length(c); failed=0; C=zeros(n,3); i=1; j=1; 728 | while 1 729 | if ~sum([vc,'_^']==c(i)), failed=1; break, end 730 | if sum('_^'==c(i)) 731 | if i+1>n, failed=1; break, end 732 | if ~sum(vc==c(i+1)), failed=1; break, end 733 | cc=cn(vc==c(i+1),:); gamma=beta; 734 | if c(i)=='_', gamma=-beta; end 735 | C(j,:)=LocalBrighten(cc,gamma); i=i+2; 736 | else C(j,:)=cn(vc==c(i),:); i=i+1; 737 | end 738 | if i>n, break, end, j=j+1; 739 | end 740 | if n>j, C(j+1:n,:)=[]; end 741 | 742 | %------------------------------------------------------------------------- 743 | % Brighten 744 | function C=LocalBrighten(c,beta) 745 | if sum([c==0,c==1])==3 && sum(c==0)<3 && sum(c==1)<3 746 | if beta<0 747 | C=(1+beta)*c; 748 | else 749 | C=c; C(C==0)=beta; 750 | end 751 | else 752 | C=c.^((1-min(1-sqrt(eps),abs(beta)))^sign(beta)); 753 | end 754 | 755 | %------------------------------------------------------------------------- 756 | % Repmat 757 | function B=LocalRepmat(A,siz) 758 | if length(A)==1, B(prod(siz))=A; B(:)=A; B=reshape(B,siz); 759 | else [m,n]=size(A); mind=(1:m)'; nind=(1:n)'; 760 | mind=mind(:,ones(1,siz(1))); nind=nind(:,ones(1,siz(2))); 761 | B=A(mind,nind); 762 | end 763 | 764 | %------------------------------------------------------------------------- 765 | % Interp 766 | function v=LocalInterp(xmin,xmax,y,u) 767 | [m,n]=size(y); h=(xmax-xmin)/(m-1); p=length(u); v=zeros(p,n); 768 | k=min(max(1+floor((u-xmin)/h),1),m-1); s=(u-xmin)/h-k+1; 769 | for j=1:n, v(:,j)=y(k,j)+s.*(y(k+1,j)-y(k,j)); end 770 | v(v<0)=0; v(v>1)=1; 771 | 772 | %------------------------------------------------------------------------- 773 | % Check for supported log scales 774 | function [xs,ys,xys]=LocalLogCheck(ax) 775 | xs=strcmp(get(ax,'xscale'),'log'); 776 | ys=strcmp(get(ax,'yscale'),'log'); 777 | zs=strcmp(get(ax,'zscale'),'log'); 778 | if zs, error('Z log scale not supported'), end 779 | xys=xs+ys; 780 | if xys, azel=get(ax,'view'); 781 | if abs(azel(2))~=90, error('3D log plot not supported'), end 782 | end 783 | 784 | %------------------------------------------------------------------------- 785 | % Generate valid value for color, linestyle and linewidth 786 | function [c,ls,lw]=LocalValidateCLSW(s) 787 | if nargin<1, c='k'; ls='-'; lw=0.5; 788 | else 789 | % identify linestyle 790 | if findstr(s,'--'), ls='--'; s=strrep(s,'--',''); 791 | elseif findstr(s,'-.'), ls='-.'; s=strrep(s,'-.',''); 792 | elseif findstr(s,'-'), ls='-'; s=strrep(s,'-',''); 793 | elseif findstr(s,':'), ls=':'; s=strrep(s,':',''); 794 | elseif findstr(s,'*'), ls='*'; s=strrep(s,'*',''); 795 | else ls='-'; 796 | end 797 | 798 | % identify linewidth 799 | tmp=double(s); 800 | tmp=find(tmp>45 & tmp<58); 801 | if length(tmp) 802 | if any(s(tmp)=='/'), lw='/'; else lw=str2double(s(tmp)); end 803 | s(tmp)=''; 804 | else lw=0.5; 805 | end 806 | 807 | % identify color 808 | if length(s), s=lower(s); 809 | if length(s)>1, c=s(1:2); 810 | else c=s(1); end 811 | else c='k'; 812 | end 813 | end -------------------------------------------------------------------------------- /PF-toolbox/bernoulli.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/bernoulli.avi -------------------------------------------------------------------------------- /PF-toolbox/bernoulli.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/bernoulli.fig -------------------------------------------------------------------------------- /PF-toolbox/bernoulli.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/bernoulli.png -------------------------------------------------------------------------------- /PF-toolbox/compute_time_1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/compute_time_1.mat -------------------------------------------------------------------------------- /PF-toolbox/compute_time_2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/compute_time_2.mat -------------------------------------------------------------------------------- /PF-toolbox/compute_time_3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/compute_time_3.mat -------------------------------------------------------------------------------- /PF-toolbox/compute_time_4.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/compute_time_4.mat -------------------------------------------------------------------------------- /PF-toolbox/compute_time_5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/compute_time_5.mat -------------------------------------------------------------------------------- /PF-toolbox/compute_time_6.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/compute_time_6.mat -------------------------------------------------------------------------------- /PF-toolbox/compute_time_7.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/compute_time_7.mat -------------------------------------------------------------------------------- /PF-toolbox/heart.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/heart.avi -------------------------------------------------------------------------------- /PF-toolbox/method1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/method1.mat -------------------------------------------------------------------------------- /PF-toolbox/method2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/PF-toolbox/method2.mat -------------------------------------------------------------------------------- /PF-toolbox/path_eq.m: -------------------------------------------------------------------------------- 1 | %% Define path equation function 2 | % ========================================================================== 3 | % Inputs: pathtype 4 | % Outputs: 5 | % pd: path equation 6 | % d_pd: partial derivative of pd respect to gamma 7 | % dd_pd: partial derivative of d_pd respecto gamma 8 | % ========================================================================= 9 | function [pd,d_pd,dd_pd,vd]=path_eq(pathtype) 10 | syms t; % We use t instead of for the simplicity in presentation 11 | if strcmp(pathtype,'line'); 12 | vd = 0.02; % Desired speed profile for dot_gamma 13 | a = 30; x0 = 0; y0 = 0; 14 | pd=[x0+a*t+1e-10*cos(t); 15 | y0+0]; 16 | end 17 | if strcmp(pathtype,'circle'); 18 | vd = 0.05; 19 | a = 10; x0 = 0; y0 = 0; 20 | pd=[x0+a*cos(t); 21 | y0+a*sin(t)]; 22 | end 23 | if strcmp(pathtype,'sin'); 24 | vd = 0.2; 25 | a = 10;omega = 0.05; phi = 0; d = 10; 26 | pd = [a*sin(omega*t+phi)+d; 27 | t]; 28 | end 29 | if strcmp(pathtype,'polynominal'); 30 | vd = 0.005; 31 | a=[0;0;-1.797;9.905;10.891;1]; b=[0;0;-23.539;37.078;-3.539;1]; c=0; 32 | x_offset = 20; 33 | y_offset = -10; 34 | z=t+c; 35 | phi=[z^5;z^4;z^3;z^2;z;1]; 36 | pd=[a'*phi + x_offset; 37 | b'*phi + y_offset]; 38 | end 39 | if strcmp(pathtype,'Bernoulli'); 40 | vd = 0.02; 41 | a = 20; 42 | z = 1+sin(t)^2; 43 | pd = [a*cos(t)/z; 44 | a*sin(t)*cos(t)/z]; 45 | end 46 | if strcmp(pathtype,'Heart'); 47 | vd = 0.02; 48 | x_offset = 20; 49 | y_offset = -20; 50 | pd = [16*sin(t)^3 + x_offset ; 51 | 13*cos(t)-5*cos(2*t)-2*cos(3*t)-cos(4*t) + y_offset]; 52 | end 53 | d_pd = diff(pd) ; % partial derivative of pd respect to t 54 | dd_pd = diff(d_pd); % partial 55 | pd = matlabFunction(pd); % convert to matlab handle function 56 | d_pd = matlabFunction(d_pd); 57 | dd_pd = matlabFunction(dd_pd); 58 | end -------------------------------------------------------------------------------- /PF-toolbox/path_state.m: -------------------------------------------------------------------------------- 1 | %% Path function 2 | % Input: type of path, gamma, t 3 | % Output: x_path= [pd,pd_gamma,psi_P,kappa] 4 | % pd: The position of the reference point 5 | % pd_gamma: partial derivative of pd_gamma 6 | % psiP: Angle of the tangent of the path 7 | % kammpa: curvature of the path 8 | % ========================================================================= 9 | function x_path=path_state(pd,d_pd,dd_pd,controller,gamma,p) 10 | persistent psiP_old psiP_out_old 11 | if (strcmp(controller,'Method 1')) || (strcmp(controller,'Method 3')) 12 | % find the point on the path closet to the vehicle 13 | f=@(t)(pd(t)-p)'*d_pd(t); 14 | gamma_opt = fzero(f,gamma); 15 | else 16 | gamma_opt=gamma; % just use the last gamma 17 | end 18 | 19 | %% Compute the first and second derivative of pd with the optimal gamma 20 | t=gamma_opt(1); 21 | pd=pd(t); 22 | pd_gamma=d_pd(t); 23 | b=dd_pd(t); 24 | kappa=(pd_gamma(1)*b(2)-pd_gamma(2)*b(1))/norm(pd_gamma)^3; % curvature of the path 25 | 26 | %% Open the space of the angle to R 27 | psiP_new = atan2(pd_gamma(2), pd_gamma(1)); 28 | if isempty(psiP_old) 29 | psiP_old = psiP_new; 30 | psiP_out_old = psiP_new; 31 | psiP_out = psiP_new; 32 | else 33 | psiP_out = alg_convert(psiP_new,psiP_old,psiP_out_old); 34 | end 35 | psiP_old = psiP_new; 36 | psiP_out_old = psiP_out; 37 | 38 | %% 39 | x_path=[pd;pd_gamma;kappa;psiP_out;gamma_opt]; 40 | 41 | end -------------------------------------------------------------------------------- /PF-toolbox/path_state_function.m: -------------------------------------------------------------------------------- 1 | 2 | function x_path=path_state(path_type,gamma) 3 | %% Path function 4 | % Input: pathtype: circle, line, sine, polynominnal 5 | % gamma 6 | % Output: x_path= [pd,pd_gamma,psi_P,kappa] 7 | % pd: the position of the reference point on the path 8 | % d_pd: partial derivative of pd respect to gamma 9 | % psiP: Angle of the tangent of the path at gamma 10 | % kammpa: curvature of the path at gamma 11 | % vd: desired speed profile along the path 12 | % ========================================================================= 13 | persistent psiP_old psiP_out_old % memorized variable 14 | if path_type=='circle' 15 | % path equation, for each type of path we have each 16 | % equation here, the below is an example for for circle path (arc), 17 | % I will give you another equation for other type paths later 18 | a=10; % radius of the circle 19 | pd=a*[cos(omega*gamma); 20 | sin(omega*gamma)]; 21 | end 22 | % compute the first partial derivative of pd respect to gamma 23 | d_pd=a*[-omega*sin(omega*gamma); 24 | omega*cos(omega*gamma)]; 25 | % compute the second partial derivative of pd respect to gamma 26 | dd_pd=a*[-omega*sin(omega*gamma); 27 | omega*cos(omega*gamma)]; 28 | % compute the curvature of the path at gamma 29 | kappa=(d_pd(1)*dd_pd(2)-d_pd(2)*dd_pd(1))/norm(d_pd)^3; 30 | % compute desired speed profile along the path for gamma 31 | vd=0.01; % this is just an example, it can be a constant or a function of gamma 32 | % compute the angle of the tangent of the path makes with x_I (North) 33 | psiP_new = atan2(pd_gamma(2), pd_gamma(1)); 34 | % Open the space of this angle to real - this is VERY important for method 1 to Method 6 35 | % We need to apply this algoirtm for the vehicle heading as well 36 | if isempty(psiP_old) 37 | % in the first interation 38 | psiP_old = psiP_new; 39 | psiP_out_old = psiP_new; 40 | psiP = psiP_new; 41 | else 42 | psiP = alg_convert(psiP_new,psiP_old,psiP_out_old); 43 | end 44 | psiP_old = psiP_new; 45 | psiP_out_old = psiP; 46 | 47 | %% Return 48 | x_path=[pd;d_pd;kappa;psiP;gamma;vd]; 49 | 50 | % x_path will be the input of path following controllers. 51 | % In console it is enough to plot pd - the reference point on the path to 52 | % to compare with the position of the vehicle, 53 | % to see if the path following method work well or not 54 | end 55 | 56 | 57 | function alg_out= alg_convert(alg_new, alg_old, alg_out_old) 58 | % This algorithm is used to open the space of yaw angle and path agle 59 | % to real - this is VERY important for method 1 to Method 6 60 | % ========================================================================= 61 | alg_e = alg_new - alg_old; 62 | if (alg_e > 3 * pi / 2) 63 | alg_out = alg_out_old - 2 * pi + alg_e; 64 | elseif (alg_e < -3 * pi / 2) 65 | alg_out = alg_out_old + 2 * pi + alg_e; 66 | else 67 | alg_out = alg_out_old + alg_e; 68 | end 69 | end 70 | -------------------------------------------------------------------------------- /PF-toolbox/paths.m: -------------------------------------------------------------------------------- 1 | %% Path function 2 | % Input: type of path, gamma, t 3 | % Output: x_path= [pd,pd_gamma,psi_P,kappa] 4 | % pd: The position of the reference point 5 | % pd_gamma: partial derivative of pd_gamma 6 | % psiP: Angle of the tangent of the path 7 | % kammpa: curvature of the path 8 | 9 | function x_path=paths(pathtype,gamma,t) 10 | persistent psiP_old psiP_out_old 11 | if strcmp(pathtype,'Circle'); 12 | [pd,pd_gamma,kappa]=circle(gamma,t); 13 | end 14 | if strcmp(pathtype,'Sin'); 15 | [pd,pd_gamma,kappa]=sinpath(gamma,t); 16 | end 17 | if strcmp(pathtype,'Polynominal'); 18 | [pd,pd_gamma,kappa]=polynominal(gamma,t); 19 | end 20 | if strcmp(pathtype,'Bernoulli'); 21 | [pd,pd_gamma,kappa]=Bernoulli(gamma,t); 22 | end 23 | if strcmp(pathtype,'Lawnmover'); 24 | [pd,pd_gamma,kappa]=lawnmover(gamma,t); 25 | end 26 | if strcmp(pathtype,'Heart'); 27 | [pd,pd_gamma,kappa]=heart(gamma,t); 28 | end 29 | %% Open the space of the angle to R 30 | psiP_new = atan2(pd_gamma(2), pd_gamma(1)); 31 | if isempty(psiP_old) 32 | psiP_old = psiP_new; 33 | psiP_out_old = psiP_new; 34 | psiP_out = psiP_new; 35 | else 36 | psiP_out = alg_convert(psiP_new,psiP_old,psiP_out_old); 37 | end 38 | psiP_old = psiP_new; 39 | psiP_out_old = psiP_out; 40 | 41 | x_path=[pd;pd_gamma;kappa;psiP_out]; 42 | end 43 | 44 | %% ************************************************************************ 45 | function [pd,pd_gamma,kappa]=sinpath(gamma,t) 46 | %% Path equation 47 | % x=a*sin(omega*gamma+phi)+d; 48 | % y=gamma; 49 | a=10;omega=0.05;phi=0;d=10; 50 | pd=[a*sin(omega*gamma+phi)+d; 51 | gamma]; 52 | pd_gamma=[a*omega*cos(omega*gamma+phi); 53 | 1]; 54 | kappa=a*omega^2*sin(omega*gamma+phi)/(norm(pd_gamma)^3); 55 | end 56 | %% ************************************************************************ 57 | function [pd,pd_gamma,kappa]=circle(gamma,t) 58 | %% Path equation 59 | % x=x0+a*cos(gamma); 60 | % y=y0+a*sin(gamma); 61 | a=10; x0=0; y0=0; 62 | %% Clockwise 63 | pd=[x0+a*cos(gamma); 64 | y0+a*sin(gamma)]; 65 | pd_gamma=[-a*sin(gamma); 66 | a*cos(gamma)]; 67 | kappa=1/a; 68 | 69 | % T=[-sin(gamma);cos(gamma)]; % tangent vector; 70 | % N=[-cos(gamma);-sin(gamma)]; % normal vector; 71 | % N=[-T(2);T(1)]; 72 | %% counter clockwise 73 | % pd=[x0+a*cos(gamma);y0-a*sin(gamma)]; 74 | % pd_gamma=[-a*sin(gamma);-a*cos(gamma)]; 75 | % hg=a; 76 | % kappa=1/a; 77 | % T=[-sin(gamma); -cos(gamma)]; % tangent vector; 78 | % % N=[-cos(gamma); sin(gamma)]; % normal vector; 79 | % N=[-T(2);T(1)]; 80 | % if t==0 81 | % psiP0=atan2(pd_gamma(2),pd_gamma(1)); 82 | % end 83 | % if t==0 84 | % x_path=[pd;psiP0;pd_gamma;hg;kappa;gamma;T;N]; 85 | % else 86 | % x_path=[pd;psiP;pd_gamma;hg;kappa;gamma;T;N]; 87 | % end 88 | end 89 | %% ************************************************************************ 90 | function [pd,pd_gamma,kappa]=polynominal(gamma,t) 91 | %% Path equation 92 | % a=[a1;a2;a3;a4;a5;a6]; b=[b1;b2;b3;b4;b5;b6]; 93 | % phi=[(gamma+c)^5;(gamma+c)^4;(gamma+c)^3;(gamma+c)^2;(gamma+c);1] 94 | % x=a'*phi; 95 | % y=b'*phi; 96 | % Path from yogang 97 | % a=[0;0;0;0;1;0]; 98 | % b=[0;0;-3.255e-04; 0.263;-72.550; 6.505e+03]; % parameters of polynominal path 99 | % c=0; 100 | % Path from Bahareh 101 | % a=pathpar(:,1);b=pathpar(:,2); 102 | % a=[-25.0647738972264;70.4704087877383;-76.2420352989336;32.7607709906985;7.04154149110126; 1]; 103 | % b=[-25.8120636399255;60.5162465481219;-26.4811860051878;-16.2546057813239;7.05800506702106; 1]; % parameters of polynominal path 104 | a=[0;0;-1.797;9.905;10.891;1]; b=[0;0;-23.539;37.078;-3.539;1]; c=0; 105 | z=gamma+c; 106 | phi=[z^5;z^4;z^3;z^2;z;1]; 107 | dphi=[5*z^4;4*z^3;3*z^2;2*z;1]; 108 | ddphi=[20*z^3;12*z^2;6*z;2]; 109 | pd=[a'*phi; 110 | b'*phi]; 111 | xdot_gamma=a(1:5)'*dphi; ydot_gamma=b(1:5)'*dphi; 112 | xddot_gamma=a(1:4)'*ddphi; yddot_gamma=b(1:4)'*ddphi; 113 | pd_gamma=[xdot_gamma; 114 | ydot_gamma]; 115 | kappa=(xdot_gamma*yddot_gamma-ydot_gamma*xddot_gamma)/(norm(pd_gamma)^3); 116 | 117 | end 118 | %% ************************************************************************ 119 | function [pd,pd_gamma,kappa]=Bernoulli(gamma,t) 120 | %% Path equation 121 | % x=a*cos(gamma)/(1+(sin(gamma))^2); 122 | % y=a*sin(gamma)*cos(gamma)/(1+sin(gamma)^2); 123 | a=20; 124 | z=1+sin(gamma)^2; 125 | xd=a*cos(gamma)/z; 126 | yd=a*sin(gamma)*cos(gamma)/z; 127 | xdot_gamma =(a*sin(gamma)*(sin(gamma)^2 - 3))/z^2; 128 | ydot_gamma=-(a*(3*sin(gamma)^2 - 1))/(sin(gamma)^2 + 1)^2; 129 | pd=[xd; 130 | yd]; 131 | pd_gamma=[xdot_gamma; 132 | ydot_gamma]; 133 | num=3*sqrt(2)*cos(gamma); 134 | den=a*sqrt(3-cos(2*gamma)); 135 | kappa=num/den; 136 | end 137 | %% ************************************************************************ 138 | function [pd,pd_gamma,kappa]=lawnmover(gamma,t) 139 | % 140 | a=30; R1=10; R2=10; d=0; 141 | 142 | if gamma<=1 143 | xd=a*gamma; yd=d; 144 | pd=[xd;yd]; 145 | pd_gamma=[a;0]; 146 | kappa=0; 147 | elseif (1250 && (yaw1(i)<=360) 109 | % yaw1(i)=yaw1(i)-360; 110 | % end 111 | % if yaw2(i)>250 && (yaw2(i)<=360) 112 | % yaw2(i)=yaw2(i)-360; 113 | % end 114 | % % if yaw3(i)>250 && (yaw3(i)<=360) 115 | % % yaw3(i)=yaw3(i)-360; 116 | % % end 117 | % end 118 | fig2=figure(2) ; 119 | % subplot(2,1,1); 120 | % plot(t,yaw1,'k'); 121 | % hold on 122 | % plot(t,yaw2,'r'); 123 | % % plot(t,yaw3(k:n),'color',ycol); 124 | % grid; 125 | % title('Heading'); 126 | % ylabel('yaw[degree]'); 127 | % xlabel('t[second]'); 128 | % legend('show') 129 | % legend('MedBlack','MedRed');legend('Location','north');legend('boxoff'); 130 | % tmin=t(1)-1; tmax=t(end)+1; 131 | % limit=[tmin tmax -10 370]; 132 | % axis(limit); 133 | % 134 | % subplot(2,1,2); 135 | plot(t,V1,'b'); 136 | hold on; 137 | plot(t,V2,'r'); 138 | title('Path following error'); 139 | ylabel('||\textbf{e})(t)||','Interpreter','latex'); 140 | xlabel('$$t[second]$$','Interpreter','latex'); 141 | % legend('show') 142 | legend('MPCPF','NPF'); 143 | grid on; 144 | %bbbbblegend('Location','northeast');legend('boxoff'); 145 | tmin=t(1)-1; tmax=t(end)+1; 146 | limit=[tmin tmax -inf inf]; 147 | axis(limit); 148 | grid; 149 | 150 | 151 | 152 | %% Patch vehicle to the mission 153 | resample_3vehicles; 154 | animation_3vehicles_LMH; % animation_3vehicles_LMH; for circle 155 | 156 | % str = ' $$ v_{cx}=0m/s$$ \\ $$v_{cy}=0m/s$$' ; 157 | % text(-45,30, str,'Interpreter','latex') 158 | % 159 | dim = [0.2 0.5 0.3 0.3]; 160 | str = {'$$v_{cx}=0.2m/s$$','$$v_{cy}=0.2m/s$$'}; 161 | annotation('textbox',dim,'String',str,'Interpreter','latex','FitBoxToText','on'); 162 | 163 | %% Plot evolution of rabit 164 | fig3= figure(3); 165 | subplot(2,1,1); 166 | plot(t,ug1,'b');hold on; plot(t,ug2,'r'); 167 | tmin=t(1)-1; tmax=t(end)+1; 168 | limit=[tmin tmax -1 1]; 169 | axis(limit); 170 | title('Speed of the rabit'); 171 | % ylabel('sdot[m/s]'); 172 | ylabel('$$\dot{s}[m/s]$$','Interpreter','latex'); 173 | xlabel('$$t[second]$$','Interpreter','latex'); 174 | % legend('show') 175 | legend('MPCPF','NPF'); 176 | % legend('Location','northeast');legend('boxoff'); 177 | plot(t,ug_max,'k--');plot(t,ug_min,'k--'); 178 | grid on; 179 | 180 | subplot(2,1,2); 181 | plot(t,rd1,'b');hold on; plot(t,rd2,'r'); 182 | tmin=t(1)-1; tmax=t(end)+1; 183 | limit=[tmin tmax -.4 .4]; 184 | axis(limit); 185 | title('Heading rate'); 186 | ylabel('$$r_{d}[rad/s]$$','Interpreter','latex'); 187 | xlabel('$$t[second]$$','Interpreter','latex'); 188 | % legend('show') 189 | legend('MPCPF','NPF'); 190 | % legend('Location','northeast');legend('boxoff'); 191 | plot(t,rd_max,'k--');plot(t,rd_min,'k--'); 192 | grid on; 193 | % 194 | % saveas(fig1,'mission_circlecurrent.png'); 195 | % saveas(fig2,'heading_circlecurrent.png'); 196 | % saveas(fig3,'ug_circlecurrent.png'); 197 | % 198 | % 199 | % 200 | % 201 | % 202 | % 203 | % 204 | % 205 | % 206 | % 207 | % saveas(fig1,'mission_circlecurrent.png'); 208 | % saveas(fig2,'heading_circlecurrent.png'); 209 | % saveas(fig3,'ug_circlecurrent.png'); 210 | 211 | -------------------------------------------------------------------------------- /PF-toolbox/plot_arrow.m: -------------------------------------------------------------------------------- 1 | function handles = plot_arrow( x1,y1,x2,y2,varargin ) 2 | % 3 | % plot_arrow - plots an arrow to the current plot 4 | % 5 | % format: handles = plot_arrow( x1,y1,x2,y2 [,options...] ) 6 | % 7 | % input: x1,y1 - starting point 8 | % x2,y2 - end point 9 | % options - come as pairs of "property","value" as defined for "line" and "patch" 10 | % controls, see matlab help for listing of these properties. 11 | % note that not all properties where added, one might add them at the end of this file. 12 | % 13 | % additional options are: 14 | % 'headwidth': relative to complete arrow size, default value is 0.07 15 | % 'headheight': relative to complete arrow size, default value is 0.15 16 | % (encoded are maximal values if pixels, for the case that the arrow is very long) 17 | % 18 | % output: handles - handles of the graphical elements building the arrow 19 | % 20 | % Example: plot_arrow( -1,-1,15,12,'linewidth',2,'color',[0.5 0.5 0.5],'facecolor',[0.5 0.5 0.5] ); 21 | % plot_arrow( 0,0,5,4,'linewidth',2,'headwidth',0.25,'headheight',0.33 ); 22 | % plot_arrow; % will launch demo 23 | 24 | % ============================================= 25 | % for debug - demo - can be erased 26 | % ============================================= 27 | if (nargin==0) 28 | figure; 29 | axis; 30 | set( gca,'nextplot','add' ); 31 | for x = 0:0.3:2*pi 32 | color = [rand rand rand]; 33 | h = plot_arrow( 1,1,50*rand*cos(x),50*rand*sin(x),... 34 | 'color',color,'facecolor',color,'edgecolor',color ); 35 | set( h,'linewidth',2 ); 36 | end 37 | hold off; 38 | return 39 | end 40 | % ============================================= 41 | % end of for debug 42 | % ============================================= 43 | 44 | 45 | % ============================================= 46 | % constants (can be edited) 47 | % ============================================= 48 | alpha = 0.15; % head length 49 | beta = 0.07; % head width 50 | max_length = 22; 51 | max_width = 10; 52 | 53 | % ============================================= 54 | % check if head properties are given 55 | % ============================================= 56 | % if ratio is always fixed, this section can be removed! 57 | if ~isempty( varargin ) 58 | for c = 1:floor(length(varargin)/2) 59 | try 60 | switch lower(varargin{c*2-1}) 61 | % head properties - do nothing, since handled above already 62 | case 'headheight',alpha = max( min( varargin{c*2},1 ),0.01 ); 63 | case 'headwidth', beta = max( min( varargin{c*2},1 ),0.01 ); 64 | end 65 | catch 66 | fprintf( 'unrecognized property or value for: %s\n',varargin{c*2-1} ); 67 | end 68 | end 69 | end 70 | 71 | % ============================================= 72 | % calculate the arrow head coordinates 73 | % ============================================= 74 | den = x2 - x1 + eps; % make sure no devision by zero occurs 75 | teta = atan( (y2-y1)/den ) + pi*(x2 3 * pi / 2) 63 | alg_out = alg_out_old - 2 * pi + alg_e; 64 | elseif (alg_e < -3 * pi / 2) 65 | alg_out = alg_out_old + 2 * pi + alg_e; 66 | else 67 | alg_out = alg_out_old + alg_e; 68 | end 69 | end 70 | -------------------------------------------------------------------------------- /PF-toolbox/swarp.m: -------------------------------------------------------------------------------- 1 | a1=zz(1:3,1);a1([1 2 3])=a1([3 2 1]); 2 | a2=zz(1:3,2);a2([1 2 3])=a2([3 2 1]); 3 | a3=zz(1:3,2);a3([1 2 3])=a3([3 2 1]); 4 | 5 | b1=zz(4:6,1);b1([1 2 3])=b1([3 2 1]); 6 | b2=zz(4:6,2);b2([1 2 3])=b2([3 2 1]); 7 | b3=zz(4:6,2);b3([1 2 3])=b3([3 2 1]); -------------------------------------------------------------------------------- /PF-toolbox/test.m: -------------------------------------------------------------------------------- 1 | function test 2 | syms t; 3 | xd=@(t) t; 4 | yd=@(t)10*sin(t); 5 | p=[pi/2;-15]; 6 | pd=@(t)[xd(t);yd(t)]; 7 | %pd 8 | dist=@(t)norm(pd(t)-p); 9 | x0=1; 10 | A=[-1; 11 | 1]; 12 | b=[10 20*pi]; 13 | S = fmincon(dist,x0,A,b); 14 | 15 | xd_opt=xd(S); 16 | yd_opt=yd(S); 17 | x=0:0.1:2*pi; 18 | xd_out=xd(x); 19 | yd_out=yd(x); 20 | hold on; 21 | plot(xd_out,yd_out); 22 | plot(xd_opt,yd_opt,'*'); 23 | plot(p(1),p(2),'o') 24 | end -------------------------------------------------------------------------------- /PF-toolbox/vehicle_models.m: -------------------------------------------------------------------------------- 1 | %% Vehicle kinematics_models 2 | function x_robot_next = vehicle_models(x_robot,u_robot,model_type, Ts) 3 | if strcmp(model_type,'Type I') 4 | [time y] = ode45(@(t,y) vehicle_model_2D_Type1(t,y,u_robot), [0, Ts], x_robot); 5 | x_robot_next = y(end,:)'; 6 | elseif strcmp(model_type,'Type II') 7 | [time y] = ode45(@(t,y) vehicle_model_2D_Type2(t,y,u_robot), [0, Ts], x_robot(1:2)); 8 | x_robot_next = [y(end,:)';u_robot(2)]; 9 | else 10 | display('Model type is not compatible'); 11 | return; 12 | x_robot_next = zeros(length(x_robot)); 13 | end 14 | end 15 | function dx = vehicle_model_2D_Type1(t,x,u) 16 | % In this model, input includes the vehicle speed and heading rate. 17 | % it is used for testing Methods 1,2,5,6,7 18 | psi = x(3); 19 | ur = u(1); 20 | r = u(2); 21 | dx = [ur*cos(psi);ur*sin(psi);r]; 22 | end 23 | function dx = vehicle_model_2D_Type2(t,x,u) 24 | % In this model, input includes the vehicle speed and heading. 25 | % it is used for testing Methods 3,4 26 | psi = u(2); 27 | ur = u(1); 28 | dx = [ur*cos(psi);ur*sin(psi)]; 29 | end 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # path-following-Matlab 2 | ## Description 3 | This repo contains Matlab codes that implement and animate the path following methods studied in the paper entitled: 4 | 5 | Nguyen Hung et al. A review of path following control strategies for autonomous robotic vehicles: theory, simulations, and experiments, Journal of Field Robotics, 2022. 6 | 7 | Link: https://nt-hung.github.io/files/pdf/research/JFR2022_preprint.pdf 8 | 9 | ## Prerequisite 10 | - Matlab2016a or newer 11 | - Casadi - a tool to formulate optimal control problems [only need for MPC methods (Method 5 and 7)] 12 | 13 | ## Installation 14 | - Chose a folder that you want to locate your project, then 15 | - clone/download the project 16 | - run PFtools.m and see the results. 17 | 18 | If you want to test different type of the path or controller, just change parameters in variables "path_type" and "controller" in PFtools.m 19 | 20 | ## Relevant ROS package 21 | 22 | The path following methods were also implemented in the followng ROS package: https://github.com/dsor-isr/Paper-PathFollowingSurvey 23 | 24 | ## References 25 | If you find the code useful and would like to cite, please kindly reference to the following paper: 26 | 27 | Hung N., Rego F., Quintas J., Cruz J., Jacinto M., Souto D. et al. (2022) A review of path following control strategies for autonomous 28 | robotic vehicles: Theory, simulations, and experiments. Journal of Field Robotics, 1–33. 29 | https://doi.org/10.1002/rob.22142 30 | 31 | ![alt text](https://github.com/hungrepo/path-following-Matlab/blob/master/PF-toolbox/all_methods.png?raw=true) 32 | 33 | -------------------------------------------------------------------------------- /sin.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hungrepo/path-following-Matlab/07a6a7d1134875c4940780d5d798d8693a4c87dd/sin.avi --------------------------------------------------------------------------------