├── README.md ├── formation-code ├── CalAlluavs.m ├── Formation_control_main │ ├── 5jia.mat │ ├── 5jia1.mat │ ├── 5jia2.mat │ ├── 5jia3.mat │ ├── 5jia4.mat │ ├── A5pre.m │ ├── MPC_p2p │ │ ├── 2agent.fig │ │ ├── agentQP.m │ │ ├── agentQPCollision.m │ │ ├── changeDestination.m │ │ ├── changeMatrix.m │ │ ├── detectCollision.m │ │ ├── forwardState.m │ │ ├── getA0.m │ │ ├── getAbieaCollision.m │ │ ├── getLambda.m │ │ ├── getLambda_v.m │ │ ├── getPAB.m │ │ ├── getUstar.m │ │ ├── getdelta.m │ │ ├── judgeArrived.m │ │ ├── main.m │ │ └── test.m │ ├── consensus │ │ ├── formation_consensus.m │ │ └── formation_consensus_p.m │ ├── fixedwing.m │ ├── matlab1.mat │ └── transmitStates.m ├── New_mavsim_chap12.slxc ├── ShowAlluavs.m ├── data │ ├── 5jia.mat │ ├── 5jia1.mat │ ├── 5jia2.mat │ ├── 5jia3.mat │ ├── 5jia31.mat │ ├── x1.mat │ ├── x2.mat │ ├── x3.mat │ ├── x4.mat │ └── x5.mat ├── main.m ├── mavsim_show.slxc ├── mavsim_trim.slxc ├── slprj │ └── sim │ │ └── varcache │ │ ├── New_mavsim_chap12 │ │ ├── checksumOfCache.mat │ │ ├── tmwinternal │ │ │ └── simulink_cache.xml │ │ └── varInfo.mat │ │ ├── mavsim_show │ │ ├── checksumOfCache.mat │ │ ├── tmwinternal │ │ │ └── simulink_cache.xml │ │ └── varInfo.mat │ │ └── mavsim_trim │ │ ├── checksumOfCache.mat │ │ ├── tmwinternal │ │ └── simulink_cache.xml │ │ └── varInfo.mat ├── uavA1 │ ├── New_mavsim_chap12.slx │ ├── New_mavsim_chap12.slxc │ ├── airdata.m │ ├── autopilot.m │ ├── batcam.png │ ├── compute_gains.m │ ├── compute_ss_model.m │ ├── compute_tf_model.m │ ├── compute_trim.m │ ├── createWorld.m │ ├── drawEnvironment.m │ ├── drawEnvironment1.m │ ├── drawPathError.m │ ├── dubinsParameters.m │ ├── dubinsRRT │ ├── dubinsRRTcoverage │ ├── estimate_states.m │ ├── forces_moments.m │ ├── getWpp.m │ ├── gps.m │ ├── guidance_model1.m │ ├── kestrel.jpg │ ├── m123.m │ ├── main.m │ ├── mav_dynamics.m │ ├── mavsim_chap12_model.slx.r2015b │ ├── mavsim_show.slxc │ ├── mavsim_show123.slx │ ├── mavsim_show123.slxc │ ├── mavsim_trim.slx │ ├── mavsim_trim.slx.autosave │ ├── mavsim_trim.slx.r2015b │ ├── mavsim_trim.slxc │ ├── param_chap1.m │ ├── path_follow1.m │ ├── path_manager1.m │ ├── path_manager_dubins.m │ ├── path_manager_fillet.m │ ├── path_manager_line.m │ ├── path_planner1.m │ ├── planCover.m │ ├── planCoverRRTDubins.m │ ├── planRRT.m │ ├── planRRTDubins.m │ ├── plotxyz.m │ ├── predator.jpg │ ├── readme.txt │ ├── sensors.m │ ├── slprj │ │ └── sim │ │ │ └── varcache │ │ │ ├── New_mavsim_chap12 │ │ │ ├── checksumOfCache.mat │ │ │ ├── tmwinternal │ │ │ │ └── simulink_cache.xml │ │ │ └── varInfo.mat │ │ │ ├── mavsim_chap12 │ │ │ ├── checksumOfCache.mat │ │ │ ├── tmwinternal │ │ │ │ └── simulink_cache.xml │ │ │ └── varInfo.mat │ │ │ ├── mavsim_chap12_model │ │ │ ├── checksumOfCache.mat │ │ │ ├── tmwinternal │ │ │ │ └── simulink_cache.xml │ │ │ └── varInfo.mat │ │ │ ├── mavsim_show │ │ │ ├── checksumOfCache.mat │ │ │ ├── tmwinternal │ │ │ │ └── simulink_cache.xml │ │ │ └── varInfo.mat │ │ │ ├── mavsim_show123 │ │ │ ├── checksumOfCache.mat │ │ │ ├── tmwinternal │ │ │ │ └── simulink_cache.xml │ │ │ └── varInfo.mat │ │ │ └── mavsim_trim │ │ │ ├── checksumOfCache.mat │ │ │ ├── tmwinternal │ │ │ └── simulink_cache.xml │ │ │ └── varInfo.mat │ ├── totxt.m │ ├── true_states.m │ └── tv.jpg ├── uavShow │ ├── New_mavsim_chap12.slx │ ├── New_mavsim_chap12.slxc │ ├── airdata.m │ ├── autopilot.m │ ├── batcam.png │ ├── compute_gains.m │ ├── compute_ss_model.m │ ├── compute_tf_model.m │ ├── compute_trim.m │ ├── createWorld.m │ ├── drawEnvironment.m │ ├── drawEnvironment1.m │ ├── drawEnvironment5.m │ ├── drawPathError.m │ ├── dubinsParameters.m │ ├── dubinsRRT │ ├── dubinsRRTcoverage │ ├── estimate_states.m │ ├── forces_moments.m │ ├── getWpp.m │ ├── gps.m │ ├── guidance_model1.m │ ├── kestrel.jpg │ ├── m123.m │ ├── main.m │ ├── mav_dynamics.m │ ├── mavsim_chap12_model.slx.r2015b │ ├── mavsim_show.slx │ ├── mavsim_show.slxc │ ├── mavsim_show123.slxc │ ├── mavsim_trim.slx │ ├── mavsim_trim.slx.autosave │ ├── mavsim_trim.slx.r2015b │ ├── mavsim_trim.slxc │ ├── param_chap1.m │ ├── path_follow1.m │ ├── path_manager1.m │ ├── path_manager_dubins.m │ ├── path_manager_fillet.m │ ├── path_manager_line.m │ ├── path_planner1.m │ ├── planCover.m │ ├── planCoverRRTDubins.m │ ├── planRRT.m │ ├── planRRTDubins.m │ ├── plotxyz.m │ ├── predator.jpg │ ├── readme.txt │ ├── sensors.m │ ├── slprj │ │ └── sim │ │ │ └── varcache │ │ │ ├── New_mavsim_chap12 │ │ │ ├── checksumOfCache.mat │ │ │ ├── tmwinternal │ │ │ │ └── simulink_cache.xml │ │ │ └── varInfo.mat │ │ │ ├── mavsim_chap12 │ │ │ ├── checksumOfCache.mat │ │ │ ├── tmwinternal │ │ │ │ └── simulink_cache.xml │ │ │ └── varInfo.mat │ │ │ ├── mavsim_chap12_model │ │ │ ├── checksumOfCache.mat │ │ │ ├── tmwinternal │ │ │ │ └── simulink_cache.xml │ │ │ └── varInfo.mat │ │ │ ├── mavsim_show │ │ │ ├── checksumOfCache.mat │ │ │ ├── tmwinternal │ │ │ │ └── simulink_cache.xml │ │ │ └── varInfo.mat │ │ │ ├── mavsim_show123 │ │ │ ├── checksumOfCache.mat │ │ │ ├── tmwinternal │ │ │ │ └── simulink_cache.xml │ │ │ └── varInfo.mat │ │ │ └── mavsim_trim │ │ │ ├── checksumOfCache.mat │ │ │ ├── tmwinternal │ │ │ └── simulink_cache.xml │ │ │ └── varInfo.mat │ ├── totxt.m │ ├── true_states.m │ └── tv.jpg ├── uavW.mat ├── x1.mat ├── x2.mat ├── x3.mat ├── x4.mat └── x5.mat └── gif ├── 1.gif ├── 2.gif ├── 3.gif └── 4.gif /README.md: -------------------------------------------------------------------------------- 1 | # Consensus-based-DMPC 2 | A leader-follow formation algorithm for fixed-wing UAVs. 3 | 4 | Simulation code for the paper in matlab. 5 | 6 | 7 | ## Paper 8 | 9 | [Consensus-based Control of Multiple Fixed-wing UAVs Using Distributed Model Predictive Control](https://ieeexplore.ieee.org/document/9336925) 10 | 11 | If you find this code useful in your work, please consider citing: 12 | 13 | H. Liu, J. Hu, C. Zhao, X. Hou, Z. Xu and Q. Pan, "Consensus-based Control of Multiple Fixed-Wing UAVs Using Distributed Model Predictive Control," 2020 7th International Conference on Information, Cybernetics, and Computational Social Systems (ICCSS), Guangzhou, China, 2020, pp. 858-863. 14 | 15 | ## introduction 16 | 17 | In this paper, a consensus-based distributed model predictive control algorithm for fixed-wing UAVs is proposed. 18 | 19 | There is a new framework to combine consensus algorithm 20 | of formation control with DMPC. By using this, all fixed 21 | wing UAVs could achieve desired formaiton simultaneously 22 | with collision avoidance. And the formation will not be 23 | changed in the next time period. The algorithm can be 24 | separated into two parts. The former part is consensus 25 | algorithm which all followers compute its trajectories by 26 | using states errors compared to leader. The latter part 27 | is DMPC algorithm which each agent using its tracking 28 | trajectory and information from neighbors to get optimal 29 | input of next movement. 30 | 31 | ## example 32 | 33 | ### Matlab show 34 | 35 | ![avatar](gif/2.gif) 36 | 37 | ![avatar](gif/1.gif) 38 | 39 | 40 | 41 | ### UE4 42 | We also tried the Unreal enignee 4 for better performance. 43 | 44 | ![avatar](gif/3.gif) 45 | 46 | ![avatar](gif/4.gif) 47 | 48 | ## How to run 49 | 50 | Just run the file "main". 51 | 52 | "main" is a conbination of the "CalAlluavs" and "ShowAlluavs" 53 | 54 | Several uavs data are provided in folder "data", you can change some code in "main" to use them by following the rules. 55 | 56 | If you want to compute your own data, "A5pre" is provided in the folder "Formation_control_main". 57 | 58 | ## Other-links 59 | 60 | [DMPC-for-multi-agent](https://github.com/chengji253/DMPC-for-multi-agent) 61 | 62 | 63 | [DMPC-cpp](https://github.com/chengji253/DMPC-Cpp) 64 | -------------------------------------------------------------------------------- /formation-code/CalAlluavs.m: -------------------------------------------------------------------------------- 1 | close all;clear; 2 | addpath 'data' 3 | addpath 'uavA1' 4 | % addpath 'uavShow' 5 | 6 | %---------------- 7 | uavW = 1; 8 | 9 | save('uavW.mat','uavW'); 10 | sim('New_mavsim_chap12'); 11 | 12 | ii = 1; 13 | eval(['x' num2str(ii) '= x;']) 14 | eval(['path' num2str(ii) '= path;']) 15 | eval(['waypoints' num2str(ii) '= waypoints;']) 16 | 17 | save('x1.mat','x1','path1','waypoints1'); 18 | %---------------- 19 | clear; 20 | uavW = 2; 21 | uavi = uavW; 22 | save('uavW.mat','uavW'); 23 | sim('New_mavsim_chap12'); 24 | 25 | ii = 2; 26 | eval(['x' num2str(ii) '= x;']) 27 | eval(['path' num2str(ii) '= path;']) 28 | eval(['waypoints' num2str(ii) '= waypoints;']) 29 | save('x2.mat','x2','path2','waypoints2'); 30 | %---------------- 31 | clear; 32 | uavW = 3; 33 | save('uavW.mat','uavW'); 34 | sim('New_mavsim_chap12'); 35 | 36 | ii = 3; 37 | eval(['x' num2str(ii) '= x;']) 38 | eval(['path' num2str(ii) '= path;']) 39 | eval(['waypoints' num2str(ii) '= waypoints;']) 40 | save('x3.mat','x3','path3','waypoints3'); 41 | %---------------- 42 | clear; 43 | uavW = 4; 44 | 45 | save('uavW.mat','uavW'); 46 | sim('New_mavsim_chap12'); 47 | 48 | ii = 4; 49 | eval(['x' num2str(ii) '= x;']) 50 | eval(['path' num2str(ii) '= path;']) 51 | eval(['waypoints' num2str(ii) '= waypoints;']) 52 | save('x4.mat','x4','path4','waypoints4'); 53 | %---------------- 54 | clear; 55 | uavW = 5; 56 | save('uavW.mat','uavW'); 57 | sim('New_mavsim_chap12'); 58 | 59 | ii = 5; 60 | eval(['x' num2str(ii) '= x;']) 61 | eval(['path' num2str(ii) '= path;']) 62 | eval(['waypoints' num2str(ii) '= waypoints;']) 63 | save('x5.mat','x5','path5','waypoints5'); 64 | %---------------- 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /formation-code/Formation_control_main/5jia.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/Formation_control_main/5jia.mat -------------------------------------------------------------------------------- /formation-code/Formation_control_main/5jia1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/Formation_control_main/5jia1.mat -------------------------------------------------------------------------------- /formation-code/Formation_control_main/5jia2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/Formation_control_main/5jia2.mat -------------------------------------------------------------------------------- /formation-code/Formation_control_main/5jia3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/Formation_control_main/5jia3.mat -------------------------------------------------------------------------------- /formation-code/Formation_control_main/5jia4.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/Formation_control_main/5jia4.mat -------------------------------------------------------------------------------- /formation-code/Formation_control_main/MPC_p2p/2agent.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/Formation_control_main/MPC_p2p/2agent.fig -------------------------------------------------------------------------------- /formation-code/Formation_control_main/MPC_p2p/agentQP.m: -------------------------------------------------------------------------------- 1 | function [u] = agentQP(ui,K,lambda,Q,delta,S,R,Pd,A0,X0,Aieq,bieq) 2 | 3 | Ustar = getUstar(ui,K); 4 | 5 | %---Quadratic Programming 6 | 7 | H = (lambda'*Q*lambda)+(delta'*S*delta)+R; 8 | f = - (Pd'*Q*lambda-(A0*X0)'*Q*lambda)-(Ustar'*S*delta); 9 | 10 | u = quadprog(H,f,Aieq,bieq); 11 | -------------------------------------------------------------------------------- /formation-code/Formation_control_main/MPC_p2p/agentQPCollision.m: -------------------------------------------------------------------------------- 1 | function [u] = agentQPCollision(ui,K,lambda,Q,delta,S,R,Pd,A0,X0,P,P1,kci,epsilon,r_min) 2 | 3 | Ustar = getUstar(ui,K); 4 | 5 | %---Quadratic Programming 6 | C = zeros(3,3*K); 7 | C(1,3*kci-2) = 1; C(2,3*kci-1) = 1; C(3,3*kci) = 1; 8 | 9 | Perror = P(kci:kci+2)-P1(kci:kci+2); 10 | k1 = 1000000001000/(norm(Perror-r_min)); 11 | H = (lambda'*Q*lambda)+(delta'*S*delta)+R +k1*(lambda'*lambda); 12 | f = - (Pd'*Q*lambda-(A0*X0)'*Q*lambda)-(Ustar'*S*delta) ... 13 | +k1*(X0'*A0'*C'*C*lambda-P1(kci:kci+2)'*C*lambda); 14 | % + k2*0.5*C*lambda 15 | [Aieq,bieq] = getAbieaCollision(P,P1,kci,r_min,K,lambda,A0,X0,epsilon); 16 | 17 | u = quadprog(H,f,Aieq,bieq); 18 | 19 | 20 | -------------------------------------------------------------------------------- /formation-code/Formation_control_main/MPC_p2p/changeDestination.m: -------------------------------------------------------------------------------- 1 | function [Pd] = changeDestination(Pd,p_x,p_y,p_z,t,K) 2 | 3 | if size(p_x,2)>1 4 | for i = 1:K 5 | Pd(3*i-2) = p_x(t); 6 | Pd(3*i-1) = p_y(t); 7 | Pd(3*i) = p_z(t); 8 | t = t+1; 9 | end 10 | end 11 | 12 | if size(p_x,2) == 1 13 | for i = 1:K 14 | Pd(3*i-2) = p_x; 15 | Pd(3*i-1) = p_y; 16 | Pd(3*i) = p_z; 17 | end 18 | end 19 | 20 | % 21 | % for i = 1:K 22 | % Pd(3*i-2) = p_x(agenti,t); 23 | % Pd(3*i-1) = p_y(agenti,t); 24 | % Pd(3*i) = p_z(agenti,t); 25 | % t = t+1; 26 | % end 27 | 28 | 29 | -------------------------------------------------------------------------------- /formation-code/Formation_control_main/MPC_p2p/changeMatrix.m: -------------------------------------------------------------------------------- 1 | function [P1] = changeMatrix(P) 2 | 3 | k1 = size(P,1); 4 | P1 = []; 5 | for i = 1:3:k1 6 | P1 = [ P1;P(i) P(i+1) P(i+2) ]; 7 | end 8 | -------------------------------------------------------------------------------- /formation-code/Formation_control_main/MPC_p2p/detectCollision.m: -------------------------------------------------------------------------------- 1 | function [D,kci] = detectCollision(P,P1,K,r_min) 2 | 3 | kci = 1; 4 | D = 0; 5 | for kci = 1:K 6 | if( norm(P(3*kci-2:3*kci)-P1(3*kci-2:3*kci))0 9 | C = [C eye(3)]; 10 | t = t-1; 11 | end 12 | lambda_v( 3*(i-1)+1:3*i, 1:3*i) = C ; 13 | 14 | end 15 | 16 | -------------------------------------------------------------------------------- /formation-code/Formation_control_main/MPC_p2p/getPAB.m: -------------------------------------------------------------------------------- 1 | function [PAB] = getPAB(i,phi,A,B) 2 | 3 | PAB = []; 4 | c = phi*A^(i-1)*B; 5 | while i>0 6 | PAB = [PAB c]; 7 | i = i-1; 8 | c = phi*A^(i-1)*B; 9 | end 10 | 11 | 12 | -------------------------------------------------------------------------------- /formation-code/Formation_control_main/MPC_p2p/getUstar.m: -------------------------------------------------------------------------------- 1 | function [Ustar] = getUstar(ui,K) 2 | 3 | Ustar = [ui']; 4 | c = zeros(3,1); 5 | 6 | for i = 1:K-1 7 | Ustar = [Ustar c']; 8 | end 9 | Ustar = Ustar'; 10 | 11 | 12 | -------------------------------------------------------------------------------- /formation-code/Formation_control_main/MPC_p2p/getdelta.m: -------------------------------------------------------------------------------- 1 | function [delta] = getdelta(K) 2 | 3 | delta = eye(3*K); 4 | 5 | for i = 1:K-1 6 | delta(3*i+1:(i+1)*3 ,3*(i-1)+1:3*i) = -1*eye(3); 7 | end 8 | -------------------------------------------------------------------------------- /formation-code/Formation_control_main/MPC_p2p/judgeArrived.m: -------------------------------------------------------------------------------- 1 | function [J] = judgeArrived(Pd,Pd1,X0,X1) 2 | 3 | if norm(Pd(1:3)-X0(1:3))<=0.3 || norm(Pd1(1:3)-X1(1:3))<=0.3 4 | J = 1; 5 | else 6 | J = 0; 7 | end 8 | 9 | 10 | -------------------------------------------------------------------------------- /formation-code/Formation_control_main/MPC_p2p/main.m: -------------------------------------------------------------------------------- 1 | close all;clc; 2 | K = 15; 3 | h = 0.2; 4 | U = zeros(3*K,1); % ax0,ay0,az0,......,ax9,ay9,az9 5 | x = zeros(6,1); 6 | 7 | A = eye(6); 8 | A(1:3,4:6) = h*eye(3); 9 | B = zeros(6,3); 10 | B(1:3,1:3) = 0.5*(h^2)*eye(3); 11 | B(4:6,1:3) = h*eye(3); 12 | 13 | phi = zeros(3,6); 14 | phi(1:3,1:3) = eye(3); 15 | lambda = getLambda(K , phi,A,B); 16 | delta = getdelta(K); 17 | lambda_v = getLambda_v(K); 18 | %X0 = zeros(6,1); %initial state 19 | %X0 = [0;0;0;-1;-1;-3]; 20 | 21 | A0 = getA0(K ,phi,A); 22 | epsilon = 0.05; 23 | r_min = 3; 24 | 25 | % agent 0 26 | ui = zeros(3,1); %initial input 27 | X0 = [0;0;0;0;0;0]; %initial state 28 | Pd = ones(3*K,1)*50; %destination 29 | %Pd = zeros(3*K,1); 30 | %Pd(4:3*K) = ones(27,1)*10; 31 | 32 | %agent 1 33 | ui1 = zeros(3,1); 34 | X1 = [50;50;50;0;0;0]; 35 | Pd1 = zeros(3*K,1)*10; 36 | %Pd1(3*K-2:3*K) = [0;0;0]; 37 | 38 | %weight matrix 39 | Q = eye(3*K); 40 | %Q(1:3*K-3,1:3*K-3) = zeros(3*K-3,3*K-3); % whether more aggressive 41 | %Q(1:3*K-6,1:3*K-6) = zeros(3*K-6,3*K-6); 42 | R = eye(3*K); 43 | S = eye(3*K); 44 | 45 | % U constraints 46 | Aieq = [-1*eye(3*K);eye(3*K)]; 47 | bieq = ones(6*K,1); 48 | 49 | 50 | % 0:0.2:8 40 step 51 | %Jei = U'*(lambda'*Q*lambda)*U - 2*(Pd'*Q*lambda-(A0*X0)'*Q*lambda)*U; 52 | 53 | %Jui = U'*R*U; 54 | 55 | %Jdelta = U'*(delta'*S*delta)*U-2*(Ustar*S*delta)*U; 56 | 57 | %J = Jei+Jui+Jdelta; 58 | 59 | Xplot = [X0(1:3)]; 60 | Xplot1 = [X1(1:3)]; 61 | D = 0; 62 | timesteps = 200; 63 | Uplot = []; 64 | Uplot1 = []; 65 | 66 | 67 | 68 | 69 | figure 70 | 71 | % ---------------main loop----------------- 72 | for t = 1:timesteps 73 | 74 | %if D == 1 75 | % u = agentQPCollision(ui,K,lambda,Q,delta,S,R,Pd,A0,X0,P,P1,kci,epsilon,r_min); 76 | 77 | %u1 = agentQPCollision(ui1,K,lambda,Q,delta,S,R,Pd1,A0,X1,P1,P,kci,epsilon,r_min); 78 | 79 | %elseif D == 0 80 | 81 | u = agentQP(ui,K,lambda,Q,delta,S,R,Pd,A0,X0,Aieq,bieq); 82 | 83 | u1 = agentQP(ui1,K,lambda,Q,delta,S,R,Pd1,A0,X1,Aieq,bieq); 84 | 85 | % end 86 | 87 | %if ~isempty(u) ~= 0 88 | ui = u(1:3); % MPC:using the first optimization input 89 | %end 90 | X0 = forwardState(X0,ui,A,B); 91 | Xplot = [Xplot X0(1:3)]; 92 | Uplot = [Uplot ;ui]; 93 | 94 | %if ~isempty(u1) ~= 0 95 | ui1 = u1(1:3); 96 | %end 97 | 98 | X1 = forwardState(X1,ui1,A,B); 99 | Xplot1 = [Xplot1 X1(1:3)]; 100 | Uplot1 = [Uplot1 ;ui1]; 101 | 102 | P = A0*X0 + lambda*u; 103 | P1 = A0*X1 + lambda*u1; 104 | 105 | 106 | [D,kci] = detectCollision(P,P1,K,r_min); 107 | 108 | 109 | J = judgeArrived(Pd,Pd1,X0,X1); 110 | if J==1 111 | break; 112 | end 113 | 114 | Pplot = reshape(P,[K,3]); 115 | 116 | scatter3(X0(1),X0(2),X0(3),'g^'); hold on;drawnow 117 | scatter3(X1(1),X1(2),X1(3),'r^'); hold on;drawnow 118 | 119 | % Pplot = changeMatrix(P); 120 | % P1plot = changeMatrix(P1); 121 | % plot3(Pplot(:,1),Pplot(:,2),Pplot(:,3),'b' ) 122 | % plot3(P1plot(:,1),P1plot(:,2),P1plot(:,3),'m' ) 123 | 124 | end % end for 125 | %----------------end loop---------------- 126 | 127 | 128 | 129 | figure(2) 130 | plot(Uplot(:,1)); 131 | % figure(3) 132 | % plot(Uplot(:,2)); 133 | % figure(4) 134 | % plot(Uplot(:,3)); 135 | %plot3(Xplot(1,:) , Xplot(2,:) , Xplot(3,:),'r.'); hold on; 136 | %plot3(Xplot1(1,:) , Xplot1(2,:) , Xplot1(3,:),'g'); 137 | % 138 | % fps = 40; 139 | % myVideo = VideoWriter('test2.avi'); 140 | % myVideo.FrameRate = fps; 141 | % open(myVideo); 142 | % figure 143 | % for k = 1:size(Xplot(1,:),2) 144 | % plot3(Xplot(1,k),Xplot(1,k),Xplot(1,k),'r.'),hold on 145 | % plot3(Xplot1(1,k),Xplot1(1,k),Xplot1(1,k),'b.'); 146 | % axis([0,max(Xplot(1,:)),0,max(Xplot(2,:)),0,max(Xplot(3,:))]),drawnow 147 | % frame = getframe(gcf); 148 | % im = frame2im(frame); 149 | % writeVideo(myVideo,im); 150 | % 151 | 152 | 153 | 154 | % 155 | % end 156 | % close(myVideo); -------------------------------------------------------------------------------- /formation-code/Formation_control_main/MPC_p2p/test.m: -------------------------------------------------------------------------------- 1 | % nci = 5; 2 | % K = 10; 3 | % epsilon = -3; 4 | % U = [ones(30,1)*2]; 5 | % E = [ones(nci,1)*epsilon]; 6 | % H_eps = [zeros(3*K,3*K) zeros(3*K,nci);zeros(nci,3*K) eye(nci)]; 7 | % f_eps = [zeros(3*K,1)' ones(nci,1)']'; 8 | % U_aug = [U;E]; 9 | % 10 | % E1 = U_aug'*H_eps*U_aug-f_eps'*U_aug; 11 | % E2 = E'*E-epsilon*nci; 12 | fps = 40; 13 | myVideo = VideoWriter('test2.avi'); 14 | myVideo.FrameRate = fps; 15 | open(myVideo); 16 | figure 17 | for k = 1:size(Xplot(1,:),2) 18 | plot3(Xplot(1,k),Xplot(1,k),Xplot(1,k),'r.'),hold on 19 | plot3(Xplot1(1,k),Xplot1(1,k),Xplot1(1,k),'b.'); 20 | axis([0,max(Xplot(1,:)),0,max(Xplot(2,:)),0,max(Xplot(3,:))]),drawnow 21 | frame = getframe(gcf); 22 | im = frame2im(frame); 23 | writeVideo(myVideo,im); 24 | end 25 | close(myVideo); -------------------------------------------------------------------------------- /formation-code/Formation_control_main/consensus/formation_consensus.m: -------------------------------------------------------------------------------- 1 | function [p_x,p_y,p_z,V_x,V_y,V_z]= formation_consensus() 2 | 3 | N=10; % 1 leader + 9 followers 4 | countmax=1000; 5 | dt=0.02; 6 | rd=5; 7 | SafeRegion =3; 8 | Kca=150; 9 | bata=2; 10 | alpha=1; 11 | 12 | A=[0 1 1 1 1 1 1 1 1 1; % a(ij) 13 | 1 0 1 1 1 1 1 1 1 1; 14 | 1 1 0 1 1 1 1 1 1 1; 15 | 1 1 1 0 1 1 1 1 1 1; 16 | 1 1 1 1 0 1 1 1 1 1; 17 | 1 1 1 1 1 0 1 1 1 1; 18 | 1 1 1 1 1 1 0 1 1 1; 19 | 1 1 1 1 1 1 1 0 1 1; 20 | 1 1 1 1 1 1 1 1 0 1; 21 | 1 1 1 1 1 1 1 1 1 0] ; 22 | % deta_x=[0 5 10 15 20 25 30 35 40 45]; % 一字型 23 | % deta_y=[0 0 0 0 0 0 0 0 0 0]; 24 | % deta_z=[0 0 0 0 0 0 0 0 0 0]; 25 | 26 | % deta_x=[0 -30 -60 -90 -45 0 45 90 60 30]; % 三角形 27 | % deta_y=[0 -30 -60 -90 -90 -90 -90 -90 -60 -30]; 28 | % deta_z=[0 0 0 0 0 0 0 0 0 0]; 29 | 30 | deta_x=[0 -30 -60 -90 -45 0 45 90 60 30]; % 三角形 31 | deta_y=[0 -51.9 -103.8 -155.7 -155.7 -155.7 -155.7 -155.7 -103.8 -51.9]; 32 | deta_z=[0 0 0 0 0 0 0 0 0 0]; 33 | 34 | h_g=120; 35 | %% 初始化 位置、速度、加速度 36 | p_x(:,1)=[-25;-70;-50;-20;-10;10;-10;20;20;10]; 37 | p_y(:,1)=[9;-1.2;4;1;10;1.5;2;8;4;10]; 38 | p_z(:,1)=[150;140;100;160;175;145;180;168;135;165]; 39 | 40 | % p_x(:,1)=[-25;-70;-50;-20;-10;10;-10;20;20;10]; 41 | % p_y(:,1)=[9;-1.2;4;1;10;1.5;2;8;4;10]; 42 | % p_z(:,1)=[-2.1;-1;1;-1.5;2.5;2;3;11;3;-2]; 43 | 44 | V_x(:,1)=[20;10;10;10;8;12;13;9;12;10]; 45 | V_y(:,1)=[20;9;11;10;12;9;10;12;10;9]; 46 | V_z(:,1)=[20;12;10;10;10;11;9;10;9;10]; 47 | 48 | u_x(:,1)=[0;0;0;0;0;0;0;0;0;0]; 49 | u_y(:,1)=[0;0;0;0;0;0;0;0;0;0]; 50 | u_z(:,1)=[0;0;0;0;0;0;0;0;0;0]; 51 | k=0; 52 | k_p=0.01; 53 | 54 | %% 开始循环 55 | for count=1:countmax 56 | k=k+1; 57 | % u_x(1,k)=0; %leader 匀速 58 | % u_y(1,k)=0; 59 | % u_z(1,k+1)=0; 60 | V_x(1,k+1)=V_x(1,k)+dt*u_x(1,k); 61 | V_y(1,k+1)=V_y(1,k)+dt*u_y(1,k); 62 | % u_z(1,k+1)=V_z(1,k)+k_p*(h_g-p_z(1,k)); %二阶高度保持 63 | % V_z(1,k+1)=V_z(1,k)+dt*u_z(1,k); 64 | p_x(1,k+1)=p_x(1,k)+dt*V_x(1,k); 65 | p_y(1,k+1)=p_y(1,k)+dt*V_y(1,k); 66 | % p_z(1,k+1)=p_z(1,k)+dt*V_z(1,k); 67 | p_z(1,k+1)=p_z(1,k)+k_p*(h_g-p_z(1,k)); 68 | % theta(1,k+1)=atan(V_z(1,k+1)/V_x(1,k+1))*180/pi; 69 | % psi(1,k+1)=atan(V_y(1,k+1)/V_x(1,k+1))*180/pi; 70 | % V(1,k+1)=sqrt((V_y(1,k+1))^2+(V_y(1,k+1))^2+(V_z(1,k+1))^2); 71 | for i=2:N 72 | for j=2:N %领航跟随法一致性协议 73 | u_x(i,k+1)=u_x(1,k)-alpha*(((p_x(i,k)-deta_x(i))-p_x(1,k))+bata*(V_x(i,k)-V_x(1,k)))-A(i,j)*((p_x(i,k)-p_x(j,k))-(deta_x(i)-deta_x(j))+bata*(V_x(i,k)-V_x(j,k))); 74 | u_y(i,k+1)=u_y(1,k)-alpha*(((p_y(i,k)-deta_y(i))-p_y(1,k))+bata*(V_y(i,k)-V_y(1,k)))-A(i,j)*((p_y(i,k)-p_y(j,k))-(deta_y(i)-deta_y(j))+bata*(V_y(i,k)-V_y(j,k))); 75 | u_z(i,k+1)=u_z(1,k)-alpha*(((p_z(i,k)-deta_z(i))-p_z(1,k))+bata*(V_z(i,k)-V_z(1,k)))-A(i,j)*((p_z(i,k)-p_z(j,k))-(deta_z(i)-deta_z(j))+bata*(V_z(i,k)-V_z(j,k))); 76 | % h_ddot(i,k+1)=h_ddot(1,k)-alpha*(((h(i,k)-deta_z(i))-h(1,k))+bata*(h_dot(i,k)-h_dot(1,k)))-A(i,j)*((h(i,k)-h(j,k))-(deta_z(i)-deta_z(j))); 77 | end 78 | end 79 | for i=2:N 80 | V_x(i,k+1)=V_x(i,k)+dt*u_x(i,k); %v=vo+at 9followers 81 | V_y(i,k+1)=V_y(i,k)+dt*u_y(i,k); 82 | V_z(i,k+1)=V_z(i,k)+dt*u_z(i,k); 83 | V(i,k+1)=sqrt((V_x(i,k+1))^2+(V_y(i,k+1))^2+(V_z(i,k+1))^2); 84 | theta(i,k+1)=atan(V_z(i,k+1)/V_x(i,k+1))*180/pi; 85 | psi(i,k+1)=atan(V_y(i,k+1)/V_x(i,k+1))*180/pi; 86 | end 87 | for i=2:N 88 | p_x(i,k+1)=p_x(i,k)+dt*V_x(i,k); %x=xo+vt 89 | p_y(i,k+1)=p_y(i,k)+dt*V_y(i,k); 90 | p_z(i,k+1)=p_z(i,k)+dt*V_z(i,k); 91 | % p_z(i,k+1)=120; 92 | end 93 | end 94 | 95 | 96 | 97 | 98 | -------------------------------------------------------------------------------- /formation-code/Formation_control_main/consensus/formation_consensus_p.m: -------------------------------------------------------------------------------- 1 | function [p_x,p_y,p_z,V_x,V_y,V_z,u_x,u_y,u_z]= formation_consensus_p(p_x,p_y,p_z,V_x,V_y,V_z,u_x,u_y,u_z,dt) 2 | % consensus algorithm step bu step 3 | k_p=0.01; 4 | bata=2; 5 | deta_x=[0 -30 -60 -90 -45 0 45 90 60 30]; % 三角形 6 | deta_y=[0 -51.9 -103.8 -155.7 -155.7 -155.7 -155.7 -155.7 -103.8 -51.9]; 7 | deta_z=[0 0 0 0 0 0 0 0 0 0]; 8 | N=10; 9 | h_g=120; 10 | alpha=1; 11 | 12 | A=[0 1 1 1 1 1 1 1 1 1; % a(ij) 13 | 1 0 1 1 1 1 1 1 1 1; 14 | 1 1 0 1 1 1 1 1 1 1; 15 | 1 1 1 0 1 1 1 1 1 1; 16 | 1 1 1 1 0 1 1 1 1 1; 17 | 1 1 1 1 1 0 1 1 1 1; 18 | 1 1 1 1 1 1 0 1 1 1; 19 | 1 1 1 1 1 1 1 0 1 1; 20 | 1 1 1 1 1 1 1 1 0 1; 21 | 1 1 1 1 1 1 1 1 1 0] ; 22 | 23 | %forward leader's state 24 | V_x(1)=V_x(1)+dt*u_x(1); 25 | V_y(1)=V_y(1)+dt*u_y(1); 26 | 27 | p_x(1)=p_x(1)+dt*V_x(1); 28 | p_y(1)=p_y(1)+dt*V_y(1); 29 | p_z(1)=p_z(1)+k_p*(h_g-p_z(1)); 30 | %compute followers' accleration and forward their states 31 | for i=2:N 32 | for j=2:N %领航跟随法一致性协议 33 | u_x(i)=u_x(1)-alpha*(((p_x(i)-deta_x(i))-p_x(1))+bata*(V_x(i)-V_x(1)))-A(i,j)*((p_x(i)-p_x(j))-(deta_x(i)-deta_x(j))+bata*(V_x(i)-V_x(j))); 34 | u_y(i)=u_y(1)-alpha*(((p_y(i)-deta_y(i))-p_y(1))+bata*(V_y(i)-V_y(1)))-A(i,j)*((p_y(i)-p_y(j))-(deta_y(i)-deta_y(j))+bata*(V_y(i)-V_y(j))); 35 | u_z(i)=u_z(1)-alpha*(((p_z(i)-deta_z(i))-p_z(1))+bata*(V_z(i)-V_z(1)))-A(i,j)*((p_z(i)-p_z(j))-(deta_z(i)-deta_z(j))+bata*(V_z(i)-V_z(j))); 36 | end 37 | end 38 | for i=2:N 39 | V_x(i)=V_x(i)+dt*u_x(i); %v=vo+at 9followers 40 | V_y(i)=V_y(i)+dt*u_y(i); 41 | V_z(i)=V_z(i)+dt*u_z(i); 42 | V(i)=sqrt((V_x(i))^2+(V_y(i))^2+(V_z(i))^2); 43 | theta(i)=atan(V_z(i)/V_x(i))*180/pi; 44 | psi(i)=atan(V_y(i)/V_x(i))*180/pi; 45 | end 46 | for i=2:N 47 | p_x(i)=p_x(i)+dt*V_x(i); %x=xo+vt 48 | p_y(i)=p_y(i)+dt*V_y(i); 49 | p_z(i)=p_z(i)+dt*V_z(i); 50 | end -------------------------------------------------------------------------------- /formation-code/Formation_control_main/fixedwing.m: -------------------------------------------------------------------------------- 1 | function [XallStates,Xfw_States] = fixedwing(XallStates,h,Xfw_States) 2 | % states now 3 | desired_pos = XallStates; 4 | x_now = Xfw_States(1); 5 | y_now = Xfw_States(2); 6 | z_now = Xfw_States(3); 7 | v_now = Xfw_States(4); 8 | theta_now = Xfw_States(5); 9 | phi_now = Xfw_States(6); 10 | 11 | %dynamic limits 12 | v_min = 35; 13 | v_max = 45; 14 | theta_min = -pi/6; 15 | theta_max = pi/6; 16 | a_max = 1; 17 | a_min = -1; 18 | w_theta_max = pi/36; 19 | w_theta_min = -pi/36; 20 | w_phi_max = pi/36; 21 | w_phi_min = -pi/36; 22 | error_mer = 10000000000000; 23 | w_theta_part = pi/360; 24 | w_phi_part = pi/360; 25 | 26 | 27 | a_record = 0; 28 | w1_record = 0; 29 | w2_record = 0; 30 | 31 | 32 | 33 | for a = a_min : 0.01:a_max 34 | for w1 = w_theta_min: w_theta_part :w_theta_max 35 | for w2 = w_phi_min: w_phi_part :w_phi_max 36 | v = v_now; 37 | theta = theta_now; 38 | phi = phi_now; 39 | x = x_now; 40 | y = y_now; 41 | z = z_now; 42 | 43 | v = v + a*h; 44 | theta = theta + w1*h; 45 | phi = phi + w2*h; 46 | if(abs(theta)<= theta_max && v_min<=v && v<=v_max) 47 | %if(abs(theta)<= theta_max) 48 | x = x + h*v*cos(theta)*cos(phi); 49 | y = y + h*v*cos(theta)*sin(phi); 50 | z = z + h*v*sin(theta); 51 | else 52 | continue; 53 | end 54 | 55 | error = norm([x-desired_pos(1) ;y-desired_pos(2) ;z-desired_pos(3)] ); 56 | if(error<=error_mer) %record the least error and all inputs 57 | error_mer = error; 58 | a_record = a; 59 | w1_record = w1; 60 | w2_record = w2; 61 | end 62 | 63 | end 64 | end 65 | end 66 | 67 | 68 | 69 | Xfw_States(4) = v_now + a_record*h; 70 | Xfw_States(5) = theta_now + w1_record*h; 71 | Xfw_States(6) = phi_now + w2_record*h; 72 | Xfw_States(1) = x_now + h*Xfw_States(4) *cos( Xfw_States(5))*cos(Xfw_States(6)); 73 | Xfw_States(2) = y_now + h*Xfw_States(4) *cos( Xfw_States(5))*sin(Xfw_States(6)); 74 | Xfw_States(3) = z_now + h*Xfw_States(4) *sin( Xfw_States(5)); 75 | 76 | XallStates(1) = Xfw_States(1); 77 | XallStates(2) = Xfw_States(2); 78 | XallStates(3) = Xfw_States(3); 79 | XallStates(4) = Xfw_States(4)*cos(Xfw_States(5))*cos(Xfw_States(6)); 80 | XallStates(5) = Xfw_States(4)*cos(Xfw_States(5))*sin(Xfw_States(6)); 81 | XallStates(6) = Xfw_States(4)*sin(Xfw_States(5)); 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /formation-code/Formation_control_main/matlab1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/Formation_control_main/matlab1.mat -------------------------------------------------------------------------------- /formation-code/Formation_control_main/transmitStates.m: -------------------------------------------------------------------------------- 1 | function [p_x,p_y,p_z,V_x,V_y,V_z] = transmitStates(XallStates,p_x,p_y,p_z,V_x,V_y,V_z,N) 2 | 3 | for i = 1:N 4 | 5 | p_x(i) = XallStates(1,i); 6 | p_y(i) = XallStates(2,i); 7 | p_z(i) = XallStates(3,i); 8 | V_x(i) = XallStates(4,i); 9 | V_y(i) = XallStates(5,i); 10 | V_z(i) = XallStates(6,i); 11 | end 12 | -------------------------------------------------------------------------------- /formation-code/New_mavsim_chap12.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/New_mavsim_chap12.slxc -------------------------------------------------------------------------------- /formation-code/ShowAlluavs.m: -------------------------------------------------------------------------------- 1 | close all;clear; 2 | load 'x1' 3 | load 'x2' 4 | load 'x3' 5 | load 'x4' 6 | load 'x5' 7 | 8 | addpath 'uavShow' 9 | 10 | sim('mavsim_show'); -------------------------------------------------------------------------------- /formation-code/data/5jia.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/data/5jia.mat -------------------------------------------------------------------------------- /formation-code/data/5jia1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/data/5jia1.mat -------------------------------------------------------------------------------- /formation-code/data/5jia2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/data/5jia2.mat -------------------------------------------------------------------------------- /formation-code/data/5jia3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/data/5jia3.mat -------------------------------------------------------------------------------- /formation-code/data/5jia31.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/data/5jia31.mat -------------------------------------------------------------------------------- /formation-code/data/x1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/data/x1.mat -------------------------------------------------------------------------------- /formation-code/data/x2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/data/x2.mat -------------------------------------------------------------------------------- /formation-code/data/x3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/data/x3.mat -------------------------------------------------------------------------------- /formation-code/data/x4.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/data/x4.mat -------------------------------------------------------------------------------- /formation-code/data/x5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/data/x5.mat -------------------------------------------------------------------------------- /formation-code/main.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/main.m -------------------------------------------------------------------------------- /formation-code/mavsim_show.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/mavsim_show.slxc -------------------------------------------------------------------------------- /formation-code/mavsim_trim.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/mavsim_trim.slxc -------------------------------------------------------------------------------- /formation-code/slprj/sim/varcache/New_mavsim_chap12/checksumOfCache.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/slprj/sim/varcache/New_mavsim_chap12/checksumOfCache.mat -------------------------------------------------------------------------------- /formation-code/slprj/sim/varcache/New_mavsim_chap12/tmwinternal/simulink_cache.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | QGRk0yjko+PPIPqua/sPcw== 5 | 6 | -------------------------------------------------------------------------------- /formation-code/slprj/sim/varcache/New_mavsim_chap12/varInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/slprj/sim/varcache/New_mavsim_chap12/varInfo.mat -------------------------------------------------------------------------------- /formation-code/slprj/sim/varcache/mavsim_show/checksumOfCache.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/slprj/sim/varcache/mavsim_show/checksumOfCache.mat -------------------------------------------------------------------------------- /formation-code/slprj/sim/varcache/mavsim_show/tmwinternal/simulink_cache.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 2JSeazkqLXxq9Y8e+N0qug== 5 | 6 | -------------------------------------------------------------------------------- /formation-code/slprj/sim/varcache/mavsim_show/varInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/slprj/sim/varcache/mavsim_show/varInfo.mat -------------------------------------------------------------------------------- /formation-code/slprj/sim/varcache/mavsim_trim/checksumOfCache.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/slprj/sim/varcache/mavsim_trim/checksumOfCache.mat -------------------------------------------------------------------------------- /formation-code/slprj/sim/varcache/mavsim_trim/tmwinternal/simulink_cache.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Q9nQuUBprOfNSHQIvphdTA== 5 | 6 | -------------------------------------------------------------------------------- /formation-code/slprj/sim/varcache/mavsim_trim/varInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/slprj/sim/varcache/mavsim_trim/varInfo.mat -------------------------------------------------------------------------------- /formation-code/uavA1/New_mavsim_chap12.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/New_mavsim_chap12.slx -------------------------------------------------------------------------------- /formation-code/uavA1/New_mavsim_chap12.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/New_mavsim_chap12.slxc -------------------------------------------------------------------------------- /formation-code/uavA1/airdata.m: -------------------------------------------------------------------------------- 1 | function out=airdata(uu) 2 | % 3 | % Fake air data - this will be replaced with real air data in Chapter 4 4 | % 5 | % modified 12/11/2009 - RB 6 | 7 | % process inputs to function 8 | pn = uu(1); % North position (meters) 9 | pe = uu(2); % East position (meters) 10 | h = -uu(3); % altitude (meters) 11 | u = uu(4); % body velocity along x-axis (meters/s) 12 | v = uu(5); % body velocity along y-axis (meters/s) 13 | w = uu(6); % body velocity along z-axis (meters/s) 14 | phi = 180/pi*uu(7); % roll angle (degrees) 15 | theta = 180/pi*uu(8); % pitch angle (degrees) 16 | psi = 180/pi*uu(9); % yaw angle (degrees) 17 | p = 180/pi*uu(10); % body angular rate along x-axis (degrees/s) 18 | q = 180/pi*uu(11); % body angular rate along y-axis (degrees/s) 19 | r = 180/pi*uu(12); % body angular rate along z-axis (degrees/s) 20 | 21 | Va = sqrt(u^2+v^2+w^2); 22 | alpha = atan2(w,u); 23 | beta = asin(v); 24 | wn = 0; 25 | we = 0; 26 | wd = 0; 27 | 28 | out = [Va; alpha; beta; wn; we; wd]; 29 | -------------------------------------------------------------------------------- /formation-code/uavA1/batcam.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/batcam.png -------------------------------------------------------------------------------- /formation-code/uavA1/compute_gains.m: -------------------------------------------------------------------------------- 1 | % some variables needed 2 | u = P.x_trim(4); 3 | v = P.x_trim(5); 4 | w = P.x_trim(6); 5 | Va_trim = sqrt(u^2 + v^2 + w^2); 6 | theta_trim = P.x_trim(8); 7 | alpha_trim = atan(w/u); 8 | delta_e_trim = P.u_trim(1); 9 | delta_t_trim = P.u_trim(4); 10 | 11 | % Roll attitude hold 12 | P.delta_a_max = 45 * pi / 180; 13 | P.phi_max = 15 * pi / 180; 14 | P.roll_max = 45 * pi / 180; 15 | a_phi1 = -0.5 * P.rho * Va_trim^2 * P.S_wing * P.b * P.C_p_p * P.b / (2 * Va_trim); 16 | a_phi2 = 0.5 * P.rho * Va_trim^2 * P.S_wing * P.b * P.C_p_delta_a; 17 | P.kp_phi = P.delta_a_max / P.phi_max * sign(a_phi2); 18 | omega_phi = sqrt(abs(a_phi2) * P.delta_a_max / P.phi_max); 19 | zeta_phi = 1.2; % design parameter 20 | P.kd_phi = (2 * zeta_phi * omega_phi - a_phi1) / a_phi2; 21 | P.ki_phi = 0.2; % design parameter 22 | 23 | % Course hold 24 | W_chi = 8; % design parameter 25 | omega_chi = 1 / W_chi * omega_phi; 26 | zeta_chi = 0.707; % design parameter 27 | Vg = P.Va0; 28 | P.kp_chi = 2 * zeta_chi * omega_chi * Vg / P.gravity; 29 | P.ki_chi = omega_chi^2 * Vg / P.gravity; 30 | 31 | % Pitch attitude hold 32 | P.delta_e_max = 45 * pi / 180; 33 | P.e_theta_max = 10 * pi / 180; 34 | a_theta1 = -P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_q * P.c / (2 * P.Jy * 2 * Va_trim); 35 | a_theta2 = -P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_alpha / (2 * P.Jy); 36 | a_theta3 = P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_delta_e / (2 * P.Jy); 37 | P.kp_theta = P.delta_e_max / P.e_theta_max * sign(a_theta3); 38 | omega_theta = sqrt(a_theta2 + P.delta_e_max / P.e_theta_max * abs(a_theta3)); 39 | zeta_theta = 0.707; % design parameter 40 | P.kd_theta = (2 * zeta_theta * omega_theta - a_theta1) / a_theta3; 41 | P.theta_max = 20 * pi / 180; 42 | P.pitch_max = 40 * pi / 180; 43 | 44 | % Airspeed hold using Throttle 45 | a_V1 = P.rho * Va_trim * P.S_wing / P.mass ... 46 | * (P.C_D_0 + P.C_D_alpha * alpha_trim + P.C_L_delta_e * delta_e_trim) ... 47 | + P.rho * P.S_prop / P.mass * P.C_prop * Va_trim; 48 | a_V2 = P.rho * P.S_prop / P.mass * P.C_prop * P.k_motor^2 * delta_t_trim; 49 | a_V3 = P.gravity * cos(theta_trim - alpha_trim); 50 | omega_v = 5; % design parameter 51 | zeta_v = 0.707; % design parameter 52 | P.delta_t_max = 1; 53 | P.delta_t_min = 0; 54 | P.ki_v = omega_v^2 / a_V2; 55 | P.kp_v = (2 * zeta_v * omega_v - a_V1) / a_V2; 56 | 57 | % Airspeed hold using Pitch 58 | W_v2 = 7; % design parameter 59 | zeta_v2 = 0.707; % design parameter 60 | omega_v2 = 1 / W_v2 * omega_theta; 61 | K_theta_dc = P.kp_theta * a_theta3 / (a_theta2 + P.kp_theta * a_theta3); 62 | P.ki_v2 = -omega_v2^2/(K_theta_dc * P.gravity); 63 | P.kp_v2 = (a_V1 - 2 * zeta_v2 * omega_v2) / (K_theta_dc * P.gravity); 64 | 65 | % Altitude hold using Pitch 66 | W_h = 10; % design parameter 67 | omega_h = 1 / W_h * omega_theta; 68 | Va = P.Va0; 69 | zeta_h = 1.2; % design parameter 70 | P.h_max = 1000; 71 | P.h_min = 0; 72 | P.ki_h = omega_h^2 / (K_theta_dc * Va); 73 | P.kp_h = 2 * zeta_h * omega_h / (K_theta_dc * Va); 74 | 75 | -------------------------------------------------------------------------------- /formation-code/uavA1/compute_ss_model.m: -------------------------------------------------------------------------------- 1 | function [A_lon,B_lon,A_lat,B_lat] = compute_ss_model(filename,x_trim,u_trim) 2 | % x_trim is the trimmed state, 3 | % u_trim is the trimmed input 4 | 5 | % add stuff here 6 | [A,B,C,D] = linmod(filename, x_trim, u_trim); 7 | 8 | E1 = [... 9 | 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0;... 10 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0;... 11 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1;... 12 | 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0;... 13 | 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0;... 14 | ]; 15 | E2 = [... 16 | 0, 1, 0, 0;... 17 | 0, 0, 1, 0;... 18 | ]; 19 | A_lat = E1 * A * E1'; 20 | B_lat = E1 * B * E2'; 21 | 22 | E3 = [... 23 | 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0;... 24 | 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0;... 25 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0;... 26 | 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0;... 27 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0;... 28 | ]; 29 | E4 = [... 30 | 1, 0, 0, 0;... 31 | 0, 0, 0, 1;... 32 | ]; 33 | 34 | A_lon = E3 * A * E3'; 35 | B_lon = E3 * B * E4'; -------------------------------------------------------------------------------- /formation-code/uavA1/compute_tf_model.m: -------------------------------------------------------------------------------- 1 | function [T_phi_delta_a,T_chi_phi,T_theta_delta_e,T_h_theta,T_h_Va,T_Va_delta_t,T_Va_theta,T_v_delta_r]... 2 | = compute_tf_model(x_trim,u_trim,P) 3 | % x_trim is the trimmed state, 4 | % u_trim is the trimmed input 5 | 6 | % add stuff here 7 | 8 | u = x_trim(4); 9 | v = x_trim(5); 10 | w = x_trim(6); 11 | Va_trim = sqrt(u^2 + v^2 + w^2); 12 | theta_trim = x_trim(8); 13 | alpha_trim = atan(w/u); 14 | delta_e_trim = u_trim(1); 15 | delta_t_trim = u_trim(4); 16 | 17 | a_phi1 = -0.5 * P.rho * Va_trim^2 * P.S_wing * P.b * P.C_p_p * P.b / (2 * Va_trim); 18 | a_phi2 = 0.5 * P.rho * Va_trim^2 * P.S_wing * P.b * P.C_p_delta_a; 19 | a_theta1 = -P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_q * P.c / (2 * P.Jy * 2 * Va_trim); 20 | a_theta2 = -P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_alpha / (2 * P.Jy); 21 | a_theta3 = P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_delta_e / (2 * P.Jy); 22 | a_V1 = P.rho * Va_trim * P.S_wing / P.mass ... 23 | * (P.C_D_0 + P.C_D_alpha * alpha_trim + P.C_L_delta_e * delta_e_trim) ... 24 | + P.rho * P.S_prop / P.mass * P.C_prop * Va_trim; 25 | a_V2 = P.rho * P.S_prop / P.mass * P.C_prop * P.k_motor^2 * delta_t_trim; 26 | a_V3 = P.gravity * cos(theta_trim - alpha_trim); 27 | a_beta1 = -P.rho * Va_trim * P.S_wing * P.C_Y_beta / (2 * P.mass); 28 | a_beta2 = P.rho * Va_trim * P.S_wing * P.C_Y_delta_r / (2 * P.mass); 29 | 30 | % define transfer functions 31 | T_phi_delta_a = tf([a_phi2],[1,a_phi1,0]); 32 | T_chi_phi = tf([P.gravity/Va_trim],[1,0]); 33 | T_theta_delta_e = tf(a_theta3,[1,a_theta1,a_theta2]); 34 | T_h_theta = tf([Va_trim],[1,0]); 35 | T_h_Va = tf([theta_trim],[1,0]); 36 | T_Va_delta_t = tf([a_V2],[1,a_V1]); 37 | T_Va_theta = tf([-a_V3],[1,a_V1]); 38 | T_v_delta_r = tf([Va_trim*a_beta2],[1,a_beta1]); 39 | 40 | -------------------------------------------------------------------------------- /formation-code/uavA1/compute_trim.m: -------------------------------------------------------------------------------- 1 | function [x_trim,u_trim] = compute_trim(filename, Va, gamma, R) 2 | % Va is the desired airspeed (m/s) 3 | % gamma is the desired flight path angle (radians) 4 | % R is the desired radius (m) - use (+) for right handed orbit, 5 | % (-) for left handed orbit 6 | 7 | 8 | % add stuff here 9 | x0 = [0; 0; 0; Va; 0; 0; 0; gamma; 0; 0; 0; 0]; 10 | u0 = [0; 0; 0; 1]; 11 | y0 = [Va; 0; 0]; 12 | ix = []; 13 | iu = []; 14 | iy = [1, 3]; 15 | dx0 = [0; 0; -Va*sin(gamma); 0; 0; 0; 0; 0; Va*cos(gamma)/R; 0; 0; 0]; 16 | idx = [3; 4; 5; 6; 7; 8; 9; 10; 11; 12]; 17 | 18 | % compute trim conditions 19 | [x_trim,u_trim,y_trim,dx_trim] = trim(filename,x0,u0,y0,ix,iu,iy,dx0,idx); 20 | 21 | % check to make sure that the linearization worked (should be small) 22 | norm(dx_trim(3:end)-dx0(3:end)); 23 | 24 | -------------------------------------------------------------------------------- /formation-code/uavA1/createWorld.m: -------------------------------------------------------------------------------- 1 | %% createWorld 2 | %% - create random 3D city 3 | %% - city_width: the city is of size (city_width)x(city_width) 4 | %% - building_height: the maximum height of the buildings 5 | %% - num_blocks: the number of (square) blocks in the city 6 | %% - street_width: the percent of block that is street (0-1) 7 | %% 8 | %% Last Modified - 4/12/2010 - R. Beard 9 | 10 | function map = createWorld(city_width, building_height, num_blocks, street_width) 11 | 12 | map.width = city_width; % the city is of size (width)x(width) 13 | map.MaxHeight = building_height; % maximum height of buildings 14 | map.NumBlocks = num_blocks; % number of blocks in city 15 | map.StreetWidth = street_width; % percent of block that is street. 16 | 17 | % computed parameters 18 | map.BuildingWidth = map.width/map.NumBlocks*(1-map.StreetWidth); 19 | map.StreetWidth = map.width/map.NumBlocks*map.StreetWidth; 20 | map.heights = map.MaxHeight*rand(map.NumBlocks,map.NumBlocks); 21 | for i=1:map.NumBlocks, 22 | map.buildings_n(i) = [... 23 | 0.5*map.width/map.NumBlocks*(2*(i-1)+1),... 24 | ]; 25 | end 26 | map.buildings_e = map.buildings_n; 27 | 28 | end 29 | 30 | -------------------------------------------------------------------------------- /formation-code/uavA1/drawPathError.m: -------------------------------------------------------------------------------- 1 | function drawPathError(uu) 2 | 3 | % process inputs to function 4 | pn = uu(1); % inertial North position 5 | pe = uu(2); % inertial East position 6 | pd = uu(3); % inertial Down position 7 | u = uu(4); % body frame velocities 8 | v = uu(5); 9 | w = uu(6); 10 | phi = uu(7); % roll angle 11 | theta = uu(8); % pitch angle 12 | psi = uu(9); % yaw angle 13 | p = uu(10); % roll rate 14 | q = uu(11); % pitch rate 15 | r = uu(12); % yaw rate 16 | t = uu(13); % time 17 | NN = 13; 18 | flag_path = uu(1+NN); % path flag 19 | r_path = [uu(3+NN); uu(4+NN); uu(5+NN)]; 20 | q_path = [uu(6+NN); uu(7+NN); uu(8+NN)]; 21 | c_orbit = [uu(9+NN); uu(10+NN); uu(11+NN)]; 22 | rho_orbit = uu(12+NN); 23 | lam_orbit = uu(13+NN); 24 | 25 | 26 | % define persistent variables 27 | persistent aircraft_handle; % figure handle for MAV 28 | persistent Vertices 29 | persistent Faces 30 | persistent facecolors 31 | 32 | % first time function is called, initialize plot and persistent vars 33 | if t==0, 34 | 35 | figure(4), clf 36 | S = 1000; 37 | switch flag_path, 38 | case 1, 39 | XX = [r_path(1), r_path(1)+S*q_path(1)]; 40 | YY = [r_path(2), r_path(2)+S*q_path(2)]; 41 | ZZ = [r_path(3), r_path(3)+S*q_path(3)]; 42 | case 2, 43 | N = 100; 44 | th = [0:2*pi/N:2*pi]; 45 | XX = c_orbit(1) + rho_orbit*cos(th); 46 | YY = c_orbit(2) + rho_orbit*sin(th); 47 | ZZ = c_orbit(3)*ones(size(th)); 48 | end 49 | plot3(YY,XX,-ZZ,'r') 50 | hold on 51 | [Vertices,Faces,facecolors] = defineAircraftBody; 52 | aircraft_handle = drawBody(Vertices,Faces,facecolors,... 53 | pn,pe,pd,phi,theta,psi,... 54 | [], 'normal'); 55 | title('UAV') 56 | xlabel('East') 57 | ylabel('North') 58 | zlabel('-Down') 59 | view(0,90) % set the view angle for figure 60 | axis([-S,S,-S,S,-.1, S]); 61 | grid on 62 | 63 | 64 | % at every other time step, redraw MAV 65 | else 66 | drawBody(Vertices,Faces,facecolors,... 67 | pn,pe,pd,phi,theta,psi,... 68 | aircraft_handle); 69 | end 70 | % figure(1), plot3(pe,pn,-pd,'.k'); 71 | end 72 | 73 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 74 | function handle = drawBody(V,F,colors,... 75 | pn, pe, pd, phi, theta, psi,... 76 | handle, mode) 77 | V = rotate(V', phi, theta, psi)'; % rotate rigid body 78 | V = translate(V', pn, pe, pd)'; % translate after rotation 79 | 80 | % transform vertices from NED to XYZ (for matlab rendering) 81 | R = [... 82 | 0, 1, 0;... 83 | 1, 0, 0;... 84 | 0, 0, -1;... 85 | ]; 86 | V = V*R; 87 | 88 | if isempty(handle), 89 | handle = patch('Vertices', V, 'Faces', F,... 90 | 'FaceVertexCData',colors,... 91 | 'FaceColor','flat',... 92 | 'EraseMode', mode); 93 | else 94 | set(handle,'Vertices',V,'Faces',F); 95 | drawnow 96 | end 97 | 98 | end 99 | 100 | 101 | %%%%%%%%%%%%%%%%%%%%%%% 102 | function XYZ=rotate(XYZ,phi,theta,psi) 103 | % define rotation matrix 104 | R_roll = [... 105 | 1, 0, 0;... 106 | 0, cos(phi), -sin(phi);... 107 | 0, sin(phi), cos(phi)]; 108 | R_pitch = [... 109 | cos(theta), 0, sin(theta);... 110 | 0, 1, 0;... 111 | -sin(theta), 0, cos(theta)]; 112 | R_yaw = [... 113 | cos(psi), -sin(psi), 0;... 114 | sin(psi), cos(psi), 0;... 115 | 0, 0, 1]; 116 | 117 | % rotate vertices 118 | XYZ = R_yaw*R_pitch*R_roll*XYZ; 119 | 120 | end 121 | 122 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 123 | % translate vertices by pn, pe, pd 124 | function XYZ = translate(XYZ,pn,pe,pd) 125 | 126 | XYZ = XYZ + repmat([pn;pe;pd],1,size(XYZ,2)); 127 | 128 | end 129 | 130 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 131 | % define aircraft vertices and faces 132 | function [V,F,colors] = defineAircraftBody 133 | 134 | % parameters for drawing aircraft 135 | % scale size 136 | size = 2; 137 | fuse_l1 = 7; 138 | fuse_l2 = 4; 139 | fuse_l3 = 15; 140 | fuse_w = 2; 141 | wing_l = 6; 142 | wing_w = 20; 143 | tail_l = 3; 144 | tail_h = 3; 145 | tailwing_w = 10; 146 | tailwing_l = 3; 147 | % colors 148 | red = [1, 0, 0]; 149 | green = [0, 1, 0]; 150 | blue = [0, 0, 1]; 151 | yellow = [1,1,0]; 152 | magenta = [0, 1, 1]; 153 | 154 | 155 | % define vertices and faces for aircraft 156 | V = [... 157 | fuse_l1, 0, 0;... % point 1 158 | fuse_l2, -fuse_w/2, -fuse_w/2;... % point 2 159 | fuse_l2, fuse_w/2, -fuse_w/2;... % point 3 160 | fuse_l2, fuse_w/2, fuse_w/2;... % point 4 161 | fuse_l2, -fuse_w/2, fuse_w/2;... % point 5 162 | -fuse_l3, 0, 0;... % point 6 163 | 0, wing_w/2, 0;... % point 7 164 | -wing_l, wing_w/2, 0;... % point 8 165 | -wing_l, -wing_w/2, 0;... % point 9 166 | 0, -wing_w/2, 0;... % point 10 167 | -fuse_l3+tailwing_l, tailwing_w/2, 0;... % point 11 168 | -fuse_l3, tailwing_w/2, 0;... % point 12 169 | -fuse_l3, -tailwing_w/2, 0;... % point 13 170 | -fuse_l3+tailwing_l, -tailwing_w/2, 0;... % point 14 171 | -fuse_l3+tailwing_l, 0, 0;... % point 15 172 | -fuse_l3+tailwing_l, 0, -tail_h;... % point 16 173 | -fuse_l3, 0, -tail_h;... % point 17 174 | ]; 175 | 176 | F = [... 177 | 1, 2, 3, 1;... % nose-top 178 | 1, 3, 4, 1;... % nose-left 179 | 1, 4, 5, 1;... % nose-bottom 180 | 1, 5, 2, 1;... % nose-right 181 | 2, 3, 6, 2;... % fuselage-top 182 | 3, 6, 4, 3;... % fuselage-left 183 | 4, 6, 5, 4;... % fuselage-bottom 184 | 2, 5, 6, 2;... % fuselage-right 185 | 7, 8, 9, 10;... % wing 186 | 11, 12, 13, 14;... % tailwing 187 | 6, 15, 17, 17;... % tail 188 | 189 | ]; 190 | 191 | colors = [... 192 | yellow;... % nose-top 193 | yellow;... % nose-left 194 | yellow;... % nose-bottom 195 | yellow;... % nose-right 196 | blue;... % fuselage-top 197 | blue;... % fuselage-left 198 | red;... % fuselage-bottom 199 | blue;... % fuselage-right 200 | green;... % wing 201 | green;... % tailwing 202 | blue;... % tail 203 | ]; 204 | 205 | V = size*V; % rescale vertices 206 | 207 | end 208 | -------------------------------------------------------------------------------- /formation-code/uavA1/dubinsParameters.m: -------------------------------------------------------------------------------- 1 | % dubinsParameters 2 | % - Find Dubin's parameters between two configurations 3 | % 4 | % input is: 5 | % start_node - [wn_s, we_s, wd_s, chi_s, 0, 0] 6 | % end_node - [wn_e, wn_e, wd_e, chi_e, 0, 0] 7 | % R - minimum turn radius 8 | % 9 | % output is: 10 | % dubinspath - a matlab structure with the following fields 11 | % dubinspath.ps - the start position in re^3 12 | % dubinspath.chis - the start course angle 13 | % dubinspath.pe - the end position in re^3 14 | % dubinspath.chie - the end course angle 15 | % dubinspath.R - turn radius 16 | % dubinspath.L - length of the Dubins path 17 | % dubinspath.cs - center of the start circle 18 | % dubinspath.lams - direction of the start circle 19 | % dubinspath.ce - center of the end circle 20 | % dubinspath.lame - direction of the end circle 21 | % dubinspath.w1 - vector in re^3 defining half plane H1 22 | % dubinspath.q1 - unit vector in re^3 along straight line path 23 | % dubinspath.w2 - vector in re^3 defining position of half plane H2 24 | % dubinspath.w3 - vector in re^3 defining position of half plane H3 25 | % dubinspath.q3 - unit vector defining direction of half plane H3 26 | % 27 | 28 | function dubinspath = dubinsParameters(start_node, end_node, R) 29 | 30 | ell = norm(start_node(1:2)-end_node(1:2)); 31 | if ell<2*R, 32 | disp('The distance between nodes must be larger than 2R.'); 33 | dubinspath = []; 34 | else 35 | 36 | ps = start_node(1:3); 37 | chis = start_node(4); 38 | pe = end_node(1:3); 39 | chie = end_node(4); 40 | 41 | crs = ps' + R*rotz(pi/2)*[cos(chis); sin(chis); 0]; 42 | cls = ps' + R*rotz(-pi/2)*[cos(chis); sin(chis); 0]; 43 | cre = pe' + R*rotz(pi/2)*[cos(chie); sin(chie); 0]; 44 | cle = pe' + R*rotz(-pi/2)*[cos(chie); sin(chie); 0]; 45 | 46 | % compute L1 47 | connecting_line = cre-crs; 48 | north_unit = [1; 0; 0]; 49 | % theta = acos(dot(connecting_line,north_unit)/norm(connecting_line)); 50 | theta = atan2(connecting_line(2), connecting_line(1)); 51 | L1 = norm(crs-cre) + R*mod(2*pi + mod(theta-pi/2, 2*pi) - mod(chis-pi/2, 2*pi), 2*pi) ... 52 | + R*mod(2*pi + mod(chie-pi/2, 2*pi) - mod(theta-pi/2, 2*pi), 2*pi); 53 | % compute L2 54 | connecting_line = cle-crs; 55 | ell = norm(connecting_line); 56 | % theta = acos(dot(connecting_line,north_unit)/norm(connecting_line)); 57 | theta = atan2(connecting_line(2), connecting_line(1)); 58 | theta2 = theta - pi/2 + asin(2*R/ell); 59 | if isreal(theta2)==0, 60 | L2 = 9999; 61 | else 62 | L2 = sqrt(ell^2-4*R^2) + R*mod(2*pi + mod(theta2, 2*pi) - mod(chis-pi/2, 2*pi), 2*pi) ... 63 | + R*mod(2*pi + mod(theta2+pi, 2*pi) - mod(chie+pi/2, 2*pi), 2*pi); 64 | end 65 | % compute L3 66 | connecting_line = cre-cls; 67 | ell = norm(connecting_line); 68 | % theta = acos(dot(connecting_line,north_unit)/norm(connecting_line)); 69 | theta = atan2(connecting_line(2), connecting_line(1)); 70 | theta2 = acos(2*R/ell); 71 | if isreal(theta2)==0, 72 | L3 = 9999; 73 | else 74 | L3 = sqrt(ell^2-4*R^2) + R*mod(2*pi + mod(chis+pi/2, 2*pi) - mod(theta+theta2, 2*pi), 2*pi) ... 75 | + R*mod(2*pi + mod(chie-pi/2, 2*pi) - mod(theta+theta2-pi, 2*pi), 2*pi); 76 | end 77 | % compute L4 78 | connecting_line = cle-cls; 79 | % theta = acos(dot(connecting_line,north_unit)/norm(connecting_line)); 80 | theta = atan2(connecting_line(2), connecting_line(1)); 81 | L4 = norm(cls-cle) + R*mod(2*pi + mod(chis+pi/2, 2*pi) - mod(theta+pi/2, 2*pi), 2*pi) ... 82 | + R*mod(2*pi + mod(theta+pi/2, 2*pi) - mod(chie+pi/2, 2*pi), 2*pi); 83 | % L is the minimum distance 84 | [L,idx] = min([L1,L2,L3,L4]); 85 | e1 = [1; 0; 0]; % north unit vector 86 | switch(idx), 87 | case 1, 88 | cs = crs; 89 | lams = 1; 90 | ce = cre; 91 | lame = 1; 92 | q1 = (ce-cs)/norm(ce-cs); 93 | w1 = cs + R*rotz(-pi/2)*q1; 94 | w2 = ce + R*rotz(-pi/2)*q1; 95 | case 2, 96 | cs = crs; 97 | lams = 1; 98 | ce = cle; 99 | lame = -1; 100 | ell = norm(ce-cs); 101 | connecting_line = ce - cs; 102 | theta = atan2(connecting_line(2), connecting_line(1)); 103 | % theta = acos(dot(ce-cs,e1)/ell); 104 | theta2 = theta - pi/2 + asin(2*R/ell); 105 | q1 = rotz(theta2+pi/2)*e1; 106 | w1 = cs + R*rotz(theta2)*e1; 107 | w2 = ce + R*rotz(theta2+pi)*e1; 108 | case 3, 109 | cs = cls; 110 | lams = -1; 111 | ce = cre; 112 | lame = 1; 113 | ell = norm(ce-cs); 114 | connecting_line = ce - cs; 115 | % theta = acos(dot(ce-cs,e1)/ell); 116 | theta = atan2(connecting_line(2), connecting_line(1)); 117 | theta2 = acos(2*R/ell); 118 | q1 = rotz(theta+theta2-pi/2)*e1; 119 | w1 = cs + R*rotz(theta+theta2)*e1; 120 | w2 = ce + R*rotz(theta+theta2-pi)*e1; 121 | case 4, 122 | cs = cls; 123 | lams = -1; 124 | ce = cle; 125 | lame = -1; 126 | q1 = (ce-cs)/norm(ce-cs); 127 | w1 = cs + R*rotz(pi/2)*q1; 128 | w2 = ce + R*rotz(pi/2)*q1; 129 | end 130 | w3 = pe'; 131 | q3 = rotz(chie)*e1; 132 | 133 | % assign path variables 134 | dubinspath.ps = ps; 135 | dubinspath.chis = chis; 136 | dubinspath.pe = pe; 137 | dubinspath.chie = chie; 138 | dubinspath.R = R; 139 | dubinspath.L = L; 140 | dubinspath.cs = cs; 141 | dubinspath.lams = lams; 142 | dubinspath.ce = ce; 143 | dubinspath.lame = lame; 144 | dubinspath.w1 = w1; 145 | dubinspath.q1 = q1; 146 | dubinspath.w2 = w2; 147 | dubinspath.w3 = w3; 148 | dubinspath.q3 = q3; 149 | end 150 | end 151 | 152 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 153 | %% rotz(theta) 154 | %% rotation matrix about the z axis. 155 | function R = rotz(theta) 156 | R = [... 157 | cos(theta), -sin(theta), 0;... 158 | sin(theta), cos(theta), 0;... 159 | 0, 0, 1;... 160 | ]; 161 | end 162 | -------------------------------------------------------------------------------- /formation-code/uavA1/dubinsRRT: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/dubinsRRT -------------------------------------------------------------------------------- /formation-code/uavA1/dubinsRRTcoverage: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/dubinsRRTcoverage -------------------------------------------------------------------------------- /formation-code/uavA1/forces_moments.m: -------------------------------------------------------------------------------- 1 | % forces_moments.m 2 | % Computes the forces and moments acting on the airframe. 3 | % 4 | % Output is 5 | % F - forces 6 | % M - moments 7 | % Va - airspeed 8 | % alpha - angle of attack 9 | % beta - sideslip angle 10 | % wind - wind vector in the inertial frame 11 | % 12 | 13 | function out = forces_moments(x, delta, wind, P) 14 | 15 | % relabel the inputs 16 | pn = x(1); 17 | pe = x(2); 18 | pd = x(3); 19 | u = x(4); 20 | v = x(5); 21 | w = x(6); 22 | phi = x(7); 23 | theta = x(8); 24 | psi = x(9); 25 | p = x(10); 26 | q = x(11); 27 | r = x(12); 28 | delta_e = delta(1); 29 | delta_a = delta(2); 30 | delta_r = delta(3); 31 | delta_t = delta(4); 32 | w_ns = wind(1); % steady wind - North 33 | w_es = wind(2); % steady wind - East 34 | w_ds = wind(3); % steady wind - Down 35 | u_wg = wind(4); % gust along body x-axis 36 | v_wg = wind(5); % gust along body y-axis 37 | w_wg = wind(6); % gust along body z-axis 38 | 39 | % rotation matrix from vehicle to body 40 | sp = sin(phi); 41 | cp = cos(phi); 42 | st = sin(theta); 43 | ct = cos(theta); 44 | ss = sin(psi); 45 | cs = cos(psi); 46 | 47 | rotation_to_body = [ct*cs ct*ss -st; 48 | sp*st*cs-cp*ss sp*st*ss+cp*cs sp*ct; 49 | cp*st*cs+sp*ss cp*st*ss-sp*cs cp*ct 50 | ]; 51 | 52 | V_wind_body = rotation_to_body * [w_ns; w_es; w_ds] + [u_wg; v_wg; w_wg]; 53 | V_airspeed_body = [u-V_wind_body(1); v-V_wind_body(2); w-V_wind_body(3)]; 54 | 55 | % compute wind data in NED 56 | w_n = 0; 57 | w_e = 0; 58 | w_d = 0; 59 | 60 | % compute air data 61 | u_r = V_airspeed_body(1); 62 | v_r = V_airspeed_body(2); 63 | w_r = V_airspeed_body(3); 64 | Va = sqrt(u_r^2 + v_r^2 + w_r^2); % air speed 65 | alpha = atan(w_r / u_r); % angle of attack 66 | beta = asin(v_r / (Va)); % side slip 67 | 68 | % coefficients 69 | sigma = (1 + exp(-P.M * (alpha - P.alpha0)) + exp(P.M * (alpha + P.alpha0))) ... 70 | / ((1 + exp(-P.M * (alpha - P.alpha0))) * (1 + exp(P.M * (alpha + P.alpha0)))); 71 | C_L = (1 - sigma) * (P.C_L_0 + P.C_L_alpha * alpha) + sigma * (2 * sign(alpha) * (sin(alpha))^2 * cos(alpha)); 72 | C_D = P.C_D_p + (P.C_L_0 + P.C_L_alpha * alpha)^2 / (pi * P.e * P.b^2 / P.S_wing); 73 | 74 | C_X = -C_D * cos(alpha) + C_L * sin(alpha); 75 | C_X_q = -P.C_D_q * cos(alpha) + P.C_L_q * sin(alpha); 76 | C_X_delta_e = -P.C_D_delta_e * cos(alpha) + P.C_L_delta_e * sin(alpha); 77 | C_Z = -C_D * sin(alpha) - C_L * cos(alpha); 78 | C_Z_q = -P.C_D_q * sin(alpha) - P.C_L_q * cos(alpha); 79 | C_Z_delta_e = -P.C_D_delta_e * sin(alpha) - P.C_L_delta_e * cos(alpha); 80 | 81 | % compute external forces and torques on aircraft 82 | Force(1) = -P.mass * P.gravity * sin(theta) ... 83 | + 0.5 * P.rho * Va^2 * P.S_wing ... 84 | * (C_X + C_X_q * P.c * q / (2 * Va) + C_X_delta_e * delta_e) ... 85 | + 0.5 * P.rho * P.S_prop * P.C_prop * ((P.k_motor * delta_t)^2 - Va^2); 86 | 87 | Force(2) = P.mass * P.gravity * cos(theta) * sin(phi) ... 88 | + 0.5 * P.rho * Va^2 * P.S_wing ... 89 | * (P.C_Y_0 + P.C_Y_beta * beta + P.C_Y_p * P.b * p / (2 * Va) ... 90 | + P.C_Y_r * P.b * r / (2 * Va) + P.C_Y_delta_a * delta_a + P.C_Y_delta_r * delta_r); 91 | 92 | Force(3) = P.mass * P.gravity * cos(theta) * cos(phi) ... 93 | + 0.5 * P.rho * Va^2 * P.S_wing ... 94 | * (C_Z + C_Z_q * P.c * q / (2 * Va) + C_Z_delta_e * delta_e); 95 | 96 | Torque(1) = 0.5 * P.rho * Va^2 * P.S_wing ... 97 | * (P.b * (P.C_ell_0 + P.C_ell_beta * beta + P.C_ell_p * P.b * p / (2 * Va) ... 98 | + P.C_ell_r * P.b * r / (2 * Va) + P.C_ell_delta_a * delta_a + P.C_ell_delta_r * delta_r)) ... 99 | - P.k_T_P * (P.k_Omega * delta_t)^2; 100 | 101 | Torque(2) = 0.5 * P.rho * Va^2 * P.S_wing ... 102 | * (P.c * (P.C_m_0 + P.C_m_alpha * alpha ... 103 | + P.C_m_q * P.c * q / (2 * Va) + P.C_m_delta_e * delta_e)); 104 | Torque(3) = 0.5 * P.rho * Va^2 * P.S_wing ... 105 | * (P.b * (P.C_n_0 + P.C_n_beta * beta + P.C_n_p * P.b * p / (2 * Va) ... 106 | + P.C_n_r * P.b * r / (2 * Va) + P.C_n_delta_a * delta_a + P.C_n_delta_r * delta_r)); 107 | 108 | out = [Force'; Torque'; Va; alpha; beta; w_n; w_e; w_d]; 109 | end -------------------------------------------------------------------------------- /formation-code/uavA1/getWpp.m: -------------------------------------------------------------------------------- 1 | function [num_waypoints , wpp] = getWpp(P,uav) 2 | 3 | load '5jia3.mat' 4 | 5 | a1 = 2; 6 | a2 = 400; 7 | num_waypoints = a2/a1; 8 | % wpp = [... 9 | % 100, 0, -100, -9999, P.Va0;... 10 | % 200, 100, -100, -9999, 45;... 11 | % 100, 300, -100, -9999, P.Va0;... 12 | % 100, 400, -100, -9999, P.Va0;... 13 | wpp = []; 14 | i1 = uav; 15 | 16 | 17 | 18 | for i = 1:a1:a2 19 | x = []; 20 | x = Xplot2(i,6*i1-5:6*i1-3); 21 | x(3) = -x(3); 22 | x = [x -9999 Xplot2(i,6*i1-2)]; 23 | wpp = [wpp;x]; 24 | 25 | end 26 | 27 | % 28 | % if(uav == 2) 29 | % for i = 1:a1:a2 30 | % x = []; 31 | % x = Xplot2(i,7:9); 32 | % x(3) = -x(3); 33 | % x = [x -9999 Xplot2(i,10)]; 34 | % wpp = [wpp;x]; 35 | % end 36 | % end 37 | % 38 | % if(uav == 3) 39 | % for i = 1:a1:a2 40 | % x = []; 41 | % x = Xplot2(i,13:15); 42 | % x(3) = -x(3); 43 | % x = [x -9999 Xplot2(i,16)]; 44 | % wpp = [wpp;x]; 45 | % end 46 | % end 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /formation-code/uavA1/gps.m: -------------------------------------------------------------------------------- 1 | % gps.m 2 | % Compute the output of gps sensor 3 | % 4 | % Revised: 5 | % 3/5/2010 - RB 6 | % 5/14/2010 - RB 7 | 8 | function y = gps(uu, P) 9 | 10 | % relabel the inputs 11 | Va = uu(1); 12 | % alpha = uu(2); 13 | % beta = uu(3); 14 | wn = uu(4); 15 | we = uu(5); 16 | % wd = uu(6); 17 | pn = uu(7); 18 | pe = uu(8); 19 | pd = uu(9); 20 | % u = uu(10); 21 | % v = uu(11); 22 | % w = uu(12); 23 | % phi = uu(13); 24 | % theta = uu(14); 25 | psi = uu(15); 26 | % p = uu(16); 27 | % q = uu(17); 28 | % r = uu(18); 29 | t = uu(19); 30 | 31 | persistent v_n; 32 | persistent v_e; 33 | persistent v_h; 34 | 35 | if t==0 36 | v_n = 0; 37 | v_e = 0; 38 | v_h = 0; 39 | else 40 | v_n = exp(-P.k_gps * P.Ts_gps) * v_n + P.sigma_gps_n * randn; 41 | v_e = exp(-P.k_gps * P.Ts_gps) * v_e + P.sigma_gps_e * randn; 42 | v_h = exp(-P.k_gps * P.Ts_gps) * v_h + P.sigma_gps_altitude * randn; 43 | end 44 | 45 | % construct North, East, and altitude GPS measurements 46 | y_gps_n = pn + v_n; 47 | y_gps_e = pe + v_e; 48 | y_gps_h = -pd + v_h; 49 | 50 | % construct groundspeed and course measurements 51 | V_n = Va * cos(psi) + wn; 52 | V_e = Va * sin(psi) + we; 53 | V_g = sqrt(V_n^2 + V_e^2); 54 | 55 | sigma_course = P.sigma_gps_V_g / V_g; 56 | 57 | y_gps_Vg = sqrt(V_n^2 + V_e^2) + P.sigma_gps_V_g * randn; 58 | y_gps_course = atan2(V_e, V_n) + sigma_course * randn; 59 | 60 | % construct total output 61 | y = [... 62 | y_gps_n;... 63 | y_gps_e;... 64 | y_gps_h;... 65 | y_gps_Vg;... 66 | y_gps_course;... 67 | ]; 68 | 69 | end 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /formation-code/uavA1/kestrel.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/kestrel.jpg -------------------------------------------------------------------------------- /formation-code/uavA1/m123.m: -------------------------------------------------------------------------------- 1 | load '..\uavW.mat' 2 | i1 = uavW; 3 | clear uavW; 4 | -------------------------------------------------------------------------------- /formation-code/uavA1/main.m: -------------------------------------------------------------------------------- 1 | clear; close all; clc; 2 | 3 | mdl = 'New_mavsim_chap12'; 4 | sim(mdl); 5 | 6 | -------------------------------------------------------------------------------- /formation-code/uavA1/mav_dynamics.m: -------------------------------------------------------------------------------- 1 | function [sys,x0,str,ts,simStateCompliance] = mav_dynamics(t,x,u,flag,P) 2 | 3 | switch flag, 4 | 5 | %%%%%%%%%%%%%%%%%% 6 | % Initialization % 7 | %%%%%%%%%%%%%%%%%% 8 | case 0, 9 | [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes(P); 10 | 11 | %%%%%%%%%%%%%%% 12 | % Derivatives % 13 | %%%%%%%%%%%%%%% 14 | case 1, 15 | sys=mdlDerivatives(t,x,u,P); 16 | 17 | %%%%%%%%%% 18 | % Update % 19 | %%%%%%%%%% 20 | case 2, 21 | sys=mdlUpdate(t,x,u); 22 | 23 | %%%%%%%%%%% 24 | % Outputs % 25 | %%%%%%%%%%% 26 | case 3, 27 | sys=mdlOutputs(t,x,u); 28 | 29 | %%%%%%%%%%%%%%%%%%%%%%% 30 | % GetTimeOfNextVarHit % 31 | %%%%%%%%%%%%%%%%%%%%%%% 32 | case 4, 33 | sys=mdlGetTimeOfNextVarHit(t,x,u); 34 | 35 | %%%%%%%%%%%%% 36 | % Terminate % 37 | %%%%%%%%%%%%% 38 | case 9, 39 | sys=mdlTerminate(t,x,u); 40 | 41 | %%%%%%%%%%%%%%%%%%%% 42 | % Unexpected flags % 43 | %%%%%%%%%%%%%%%%%%%% 44 | otherwise 45 | DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag)); 46 | 47 | end 48 | 49 | % end sfuntmpl 50 | 51 | % 52 | %============================================================================= 53 | % mdlInitializeSizes 54 | % Return the sizes, initial conditions, and sample times for the S-function. 55 | %============================================================================= 56 | % 57 | function [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes(P) 58 | 59 | % 60 | % call simsizes for a sizes structure, fill it in and convert it to a 61 | % sizes array. 62 | % 63 | % Note that in this example, the values are hard coded. This is not a 64 | % recommended practice as the characteristics of the block are typically 65 | % defined by the S-function parameters. 66 | % 67 | sizes = simsizes; 68 | 69 | sizes.NumContStates = 12; 70 | sizes.NumDiscStates = 0; 71 | sizes.NumOutputs = 12; 72 | sizes.NumInputs = 6; 73 | sizes.DirFeedthrough = 0; 74 | sizes.NumSampleTimes = 1; % at least one sample time is needed 75 | 76 | sys = simsizes(sizes); 77 | 78 | % 79 | % initialize the initial conditions 80 | % 81 | x0 = [... 82 | P.pn0;... 83 | P.pe0;... 84 | P.pd0;... 85 | P.u0;... 86 | P.v0;... 87 | P.w0;... 88 | P.phi0;... 89 | P.theta0;... 90 | P.psi0;... 91 | P.p0;... 92 | P.q0;... 93 | P.r0;... 94 | ]; 95 | 96 | % 97 | % str is always an empty matrix 98 | % 99 | str = []; 100 | 101 | % 102 | % initialize the array of sample times 103 | % 104 | ts = [0 0]; 105 | 106 | % Specify the block simStateCompliance. The allowed values are: 107 | % 'UnknownSimState', < The default setting; warn and assume DefaultSimState 108 | % 'DefaultSimState', < Same sim state as a built-in block 109 | % 'HasNoSimState', < No sim state 110 | % 'DisallowSimState' < Error out when saving or restoring the model sim state 111 | simStateCompliance = 'UnknownSimState'; 112 | 113 | % end mdlInitializeSizes 114 | 115 | % 116 | %============================================================================= 117 | % mdlDerivatives 118 | % Return the derivatives for the continuous states. 119 | %============================================================================= 120 | % 121 | function sys=mdlDerivatives(t,x,uu, P) 122 | 123 | pn = x(1); 124 | pe = x(2); 125 | pe = x(3); 126 | u = x(4); 127 | v = x(5); 128 | w = x(6); 129 | phi = x(7); 130 | theta = x(8); 131 | psi = x(9); 132 | p = x(10); 133 | q = x(11); 134 | r = x(12); 135 | fx = uu(1); 136 | fy = uu(2); 137 | fz = uu(3); 138 | ell = uu(4); 139 | m = uu(5); 140 | n = uu(6); 141 | 142 | sp = sin(phi); 143 | cp = cos(phi); 144 | st = sin(theta); 145 | ct = cos(theta); 146 | ss = sin(psi); 147 | cs = cos(psi); 148 | tt = tan(theta); 149 | 150 | % translational kinematics 151 | rotation_position = [ct*cs sp*st*cs-cp*ss cp*st*cs+sp*ss; 152 | ct*ss sp*st*ss+cp*cs cp*st*ss-sp*cs; 153 | -st sp*ct cp*ct 154 | ]; 155 | position_dot = rotation_position*[u; v; w]; 156 | pndot = position_dot(1); 157 | pedot = position_dot(2); 158 | pddot = position_dot(3); 159 | 160 | % translational dynamics 161 | udot = r*v-q*w+fx/P.mass; 162 | vdot = p*w-r*u+fy/P.mass; 163 | wdot = q*u-p*v+fz/P.mass; 164 | 165 | % rotational kinematics 166 | rotation_angle = [1 sp*tt cp*tt; 167 | 0 cp -sp; 168 | 0 sp/ct cp/ct 169 | ]; 170 | angle_dot = rotation_angle*[p; q; r]; 171 | phidot = angle_dot(1); 172 | thetadot = angle_dot(2); 173 | psidot = angle_dot(3); 174 | 175 | % rorational dynamics 176 | pdot = P.r1*p*q-P.r2*q*r+P.r3*ell+P.r4*n; 177 | qdot = P.r5*p*r-P.r6*(p^2-r^2)+m/P.Jy; 178 | rdot = P.r7*p*q-P.r1*q*r+P.r4*ell+P.r8*n; 179 | 180 | sys = [pndot; pedot; pddot; udot; vdot; wdot; phidot; thetadot; psidot; pdot; qdot; rdot]; 181 | 182 | % end mdlDerivatives 183 | 184 | % 185 | %============================================================================= 186 | % mdlUpdate 187 | % Handle discrete state updates, sample time hits, and major time step 188 | % requirements. 189 | %============================================================================= 190 | % 191 | function sys=mdlUpdate(t,x,u) 192 | 193 | sys = []; 194 | 195 | % end mdlUpdate 196 | 197 | % 198 | %============================================================================= 199 | % mdlOutputs 200 | % Return the block outputs. 201 | %============================================================================= 202 | % 203 | function sys=mdlOutputs(t,x,u) 204 | 205 | sys = x; 206 | 207 | % end mdlOutputs 208 | 209 | % 210 | %============================================================================= 211 | % mdlGetTimeOfNextVarHit 212 | % Return the time of the next hit for this block. Note that the result is 213 | % absolute time. Note that this function is only used when you specify a 214 | % variable discrete-time sample time [-2 0] in the sample time array in 215 | % mdlInitializeSizes. 216 | %============================================================================= 217 | % 218 | function sys=mdlGetTimeOfNextVarHit(t,x,u) 219 | 220 | sampleTime = 1; % Example, set the next hit to be one second later. 221 | sys = t + sampleTime; 222 | 223 | % end mdlGetTimeOfNextVarHit 224 | 225 | % 226 | %============================================================================= 227 | % mdlTerminate 228 | % Perform any end of simulation tasks. 229 | %============================================================================= 230 | % 231 | function sys=mdlTerminate(t,x,u) 232 | 233 | sys = []; 234 | 235 | % end mdlTerminate 236 | -------------------------------------------------------------------------------- /formation-code/uavA1/mavsim_chap12_model.slx.r2015b: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/mavsim_chap12_model.slx.r2015b -------------------------------------------------------------------------------- /formation-code/uavA1/mavsim_show.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/mavsim_show.slxc -------------------------------------------------------------------------------- /formation-code/uavA1/mavsim_show123.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/mavsim_show123.slx -------------------------------------------------------------------------------- /formation-code/uavA1/mavsim_show123.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/mavsim_show123.slxc -------------------------------------------------------------------------------- /formation-code/uavA1/mavsim_trim.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/mavsim_trim.slx -------------------------------------------------------------------------------- /formation-code/uavA1/mavsim_trim.slx.autosave: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/mavsim_trim.slx.autosave -------------------------------------------------------------------------------- /formation-code/uavA1/mavsim_trim.slx.r2015b: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/mavsim_trim.slx.r2015b -------------------------------------------------------------------------------- /formation-code/uavA1/mavsim_trim.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/mavsim_trim.slxc -------------------------------------------------------------------------------- /formation-code/uavA1/param_chap1.m: -------------------------------------------------------------------------------- 1 | clear; close all; clc; 2 | load '5jia3.mat' 3 | % load 'x1' 4 | % load 'x2' 5 | % load 'x3' 6 | % load 'x4' 7 | % load 'x5' 8 | load 'uavW.mat' 9 | i = uavW; 10 | clear uavW; 11 | 12 | 13 | P.gravity = 9.8; 14 | 15 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 16 | % Params for Aersonade UAV 17 | %physical parameters of airframe 18 | P.mass = 13.5; 19 | P.Jx = 0.8244; 20 | P.Jy = 1.135; 21 | P.Jz = 1.759; 22 | P.Jxz = .1204; 23 | % aerodynamic coefficients 24 | P.S_wing = 0.55; 25 | P.b = 2.8956; 26 | P.c = 0.18994; 27 | P.S_prop = 0.2027; 28 | P.rho = 1.2682; 29 | P.k_motor = 80; 30 | P.k_T_P = 0; 31 | P.k_Omega = 0; 32 | P.e = 0.9; 33 | 34 | P.C_L_0 = 0.28; 35 | P.C_L_alpha = 3.45; 36 | P.C_L_q = 0.0; 37 | P.C_L_delta_e = -0.36; 38 | P.C_D_0 = 0.03; 39 | P.C_D_alpha = 0.30; 40 | P.C_D_p = 0.0437; 41 | P.C_D_q = 0.0; 42 | P.C_D_delta_e = 0.0; 43 | P.C_m_0 = -0.02338; 44 | P.C_m_alpha = -0.38; 45 | P.C_m_q = -3.6; 46 | P.C_m_delta_e = -0.5; 47 | P.C_Y_0 = 0.0; 48 | P.C_Y_beta = -0.98; 49 | P.C_Y_p = 0.0; 50 | P.C_Y_r = 0.0; 51 | P.C_Y_delta_a = 0.0; 52 | P.C_Y_delta_r = -0.17; 53 | P.C_ell_0 = 0.0; 54 | P.C_ell_beta = -0.12; 55 | P.C_ell_p = -0.26; 56 | P.C_ell_r = 0.14; 57 | P.C_ell_delta_a = 0.08; 58 | P.C_ell_delta_r = 0.105; 59 | P.C_n_0 = 0.0; 60 | P.C_n_beta = 0.25; 61 | P.C_n_p = 0.022; 62 | P.C_n_r = -0.35; 63 | P.C_n_delta_a = 0.06; 64 | P.C_n_delta_r = -0.032; 65 | P.C_prop = 1.0; 66 | P.M = 50; 67 | P.epsilon = 0.1592; 68 | P.alpha0 = 0.4712; 69 | 70 | % wind parameters 71 | P.wind_n = 0; 72 | P.wind_e = 0; 73 | P.wind_d = 0; 74 | P.L_u = 200; 75 | P.L_v = 200; 76 | P.L_w = 50; 77 | P.sigma_u = 1.06; 78 | P.sigma_v = 1.06; 79 | P.sigma_w = .7; 80 | 81 | % r1 - r8 82 | P.r = P.Jx*P.Jz-P.Jxz^2; 83 | P.r1 = P.Jxz*(P.Jx-P.Jy+P.Jz)/P.r; 84 | P.r2 = P.Jz*(P.Jz-P.Jy)+P.Jxz^2; 85 | P.r3 = P.Jz/P.r; 86 | P.r4 = P.Jxz/P.r; 87 | P.r5 = (P.Jz-P.Jx)/P.Jy; 88 | P.r6 = P.Jxz/P.Jy; 89 | P.r7 = ((P.Jx-P.Jy)*P.Jx+P.Jxz^2)/P.r; 90 | P.r8 = P.Jx/P.r; 91 | 92 | % C parameters on p.62 93 | P.C_p_0 = P.r3 * P.C_ell_0 + P.r4 * P.C_n_0; 94 | P.C_p_beta = P.r3 * P.C_ell_beta + P.r4 * P.C_n_beta; 95 | P.C_p_p = P.r3 * P.C_ell_p + P.r4 * P.C_n_p; 96 | P.C_p_r = P.r3 * P.C_ell_r + P.r4 * P.C_n_r; 97 | P.C_p_delta_a = P.r3 * P.C_ell_delta_a + P.r4 * P.C_n_delta_a; 98 | P.C_p_delta_r = P.r3 * P.C_ell_delta_r + P.r4 * P.C_n_delta_r; 99 | P.C_r_0 = P.r4 * P.C_ell_0 + P.r8 * P.C_n_0; 100 | P.C_r_beta = P.r4 * P.C_ell_beta + P.r8 * P.C_n_beta; 101 | P.C_r_p = P.r4 * P.C_ell_p + P.r8 * P.C_n_p; 102 | P.C_r_r = P.r4 * P.C_ell_r + P.r8 * P.C_n_r; 103 | P.C_r_delta_a = P.r4 * P.C_ell_delta_a + P.r8 * P.C_n_delta_a; 104 | P.C_r_delta_r = P.r4 * P.C_ell_delta_r + P.r8 * P.C_n_delta_r; 105 | 106 | 107 | % compute trim conditions using 'mavsim_chap5_trim.slx' 108 | % initial airspeed 109 | P.Va0 = 35; 110 | gamma = 0; %5*pi/180; % desired flight path angle (radians) 111 | R = inf; %150; % desired radius (m) - use (+) for right handed orbit, can't be 0 112 | 113 | % autopilot sample rate 114 | P.Ts = 0.01; 115 | P.tau = 0.5; 116 | 117 | % first cut at initial conditions 118 | 119 | P.pn0 = 0; % initial North position 120 | P.pe0 = 0; % initial East position 121 | P.pd0 = 0; % initial Down position (negative altitude) 122 | 123 | P.u0 = P.Va0; % initial velocity along body x-axis 124 | P.v0 = 0; % initial velocity along body y-axis 125 | P.w0 = 0; % initial velocity along body z-axis 126 | P.phi0 = 0; % initial roll angle 127 | P.theta0 = 0; % initial pitch angle 128 | P.psi0 = 0; % initial yaw angle 129 | P.p0 = 0; % initial body frame roll rate 130 | P.q0 = 0; % initial body frame pitch rate 131 | P.r0 = 0; % initial body frame yaw rate 132 | 133 | 134 | % run trim commands 135 | [x_trim, u_trim]=compute_trim('mavsim_trim',P.Va0,gamma,R); 136 | P.u_trim = u_trim; 137 | P.x_trim = x_trim; 138 | 139 | % set initial conditions to trim conditions 140 | % initial conditions 141 | P.pn0 = Xplot2(1,6*i-5); 142 | P.pe0 = Xplot2(1,6*i-4); 143 | P.pd0 = -Xplot2(1,6*i-3); 144 | 145 | % P.pn0 = 0; % initial North position 146 | % P.pe0 = 0; % initial East position 147 | % P.pd0 = 0; % initial Down position (negative altitude) 148 | 149 | 150 | P.u0 = x_trim(4); % initial velocity along body x-axis 151 | P.v0 = x_trim(5); % initial velocity along body y-axis 152 | P.w0 = x_trim(6); % initial velocity along body z-axis 153 | P.phi0 = x_trim(7); % initial roll angle 154 | P.theta0 = x_trim(8); % initial pitch angle 155 | P.psi0 = x_trim(9); % initial yaw angle 156 | P.p0 = x_trim(10); % initial body frame roll rate 157 | P.q0 = x_trim(11); % initial body frame pitch rate 158 | P.r0 = x_trim(12); % initial body frame yaw rate 159 | 160 | % compute different transfer functions 161 | [T_phi_delta_a,T_chi_phi,T_theta_delta_e,T_h_theta,T_h_Va,T_Va_delta_t,T_Va_theta,T_v_delta_r]... 162 | = compute_tf_model(x_trim,u_trim,P); 163 | 164 | % linearize the equations of motion around trim conditions 165 | [A_lon, B_lon, A_lat, B_lat] = compute_ss_model('mavsim_trim',x_trim,u_trim); 166 | 167 | eig_lon = eig(A_lon); 168 | eig_lat = eig(A_lat); 169 | 170 | compute_gains; 171 | 172 | % longitudinal state-machine parameters 173 | P.altitude_take_off_zone = 50; 174 | P.altitude_hold_zone = 10; 175 | 176 | % sensor parameters(chapter 7) 177 | P.sigma_gyro = 0.13; 178 | P.sigma_accel = 0.0025; 179 | P.beta_abs_pres = 0.125; 180 | P.sigma_abs_pres = 0.01; 181 | P.beta_diff_pres = 0.02; 182 | P.sigma_diff_pres = 0.002; 183 | 184 | P.Ts_gps = 1; 185 | P.k_gps = 1/1000; 186 | P.sigma_gps_n = 0.21; 187 | P.sigma_gps_e = 0.21; 188 | P.sigma_gps_altitude = 0.4; 189 | P.sigma_gps_V_g = 0.05; 190 | 191 | % filter parameters(chapter 8) 192 | P.alpha_lpf_gyro = 0.1; 193 | P.alpha_lpf_static_pres = 0.1; 194 | P.alpha_lpf_diff_pres = 0.1; 195 | 196 | P.bias_gyro_x = 0; % x-gyro bias 197 | P.bias_gyro_y = 0; % y-gyro bias 198 | P.bias_gyro_z = 0; % z-gyro bias 199 | 200 | % autopilot guidance model coefficients(chapter 9) 201 | wn_h = 1.5; 202 | zeta_h = 0.707; 203 | P.b_hdot = 2 * zeta_h * wn_h; 204 | P.b_h = wn_h^2; 205 | 206 | wn_chi = 0.7; 207 | zeta_chi = 0.707; 208 | P.b_chidot = 2 * zeta_chi * wn_chi; 209 | P.b_chi = wn_chi^2; 210 | 211 | P.b_Va = 10; 212 | P.gamma_max = 45*pi/180; 213 | 214 | % chapter 10 parameters 215 | P.k_path = 0.05; 216 | P.chi_inf = 40*pi/180; 217 | P.k_orbit = 0.01; 218 | 219 | % chapter 11 - path manager 220 | % number of waypoints in data structure 221 | P.size_waypoint_array = 200; 222 | P.R_min = P.Va0^2/P.gravity/tan(P.roll_max); 223 | 224 | % create random city map 225 | city_width = 2000; % the city is of size (width)x(width) 226 | building_height = 300; % maximum height of buildings 227 | %building_height = 1; % maximum height of buildings (for camera) 228 | num_blocks = 5; % number of blocks in city 229 | street_width = .8; % percent of block that is street. 230 | P.h0 = 100; 231 | P.pd0 = -P.h0; % initial height of MAV 232 | P.map = createWorld(city_width, building_height, num_blocks, street_width); 233 | -------------------------------------------------------------------------------- /formation-code/uavA1/path_follow1.m: -------------------------------------------------------------------------------- 1 | % path follow 2 | % - follow straight line path or orbit 3 | % 4 | % Modified: 5 | % 3/25/2010 - RB 6 | % 6/5/2010 - RB 7 | % 11/08/2010 - RB 8 | % 14/11/2014 - RWB 9 | % 10 | % input is: 11 | % flag - if flag==1, follow waypoint path 12 | % if flag==2, follow orbit 13 | % 14 | % Va^d - desired airspeed 15 | % r - inertial position of start of waypoint path 16 | % q - unit vector that defines inertial direction of waypoint path 17 | % c - center of orbit 18 | % rho - radius of orbit 19 | % lambda - direction of orbit (+1 for CW, -1 for CCW) 20 | % xhat - estimated MAV states (pn, pe, pd, Va, alpha, beta, phi, theta, chi, p, q, r, Vg, wn, we, psi) 21 | % 22 | % output is: 23 | % Va_c - airspeed command 24 | % h_c - altitude command 25 | % chi_c - heading command 26 | % phi_ff - feed forward roll command 27 | % 28 | function out = path_follow(in,P) 29 | 30 | NN = 0; 31 | flag = in(1+NN); 32 | Va_d = in(2+NN); 33 | r_path = [in(3+NN); in(4+NN); in(5+NN)]; 34 | q_path = [in(6+NN); in(7+NN); in(8+NN)]; 35 | c_orbit = [in(9+NN); in(10+NN); in(11+NN)]; 36 | rho_orbit = in(12+NN); 37 | lam_orbit = in(13+NN); 38 | NN = NN + 13; 39 | pn = in(1+NN); 40 | pe = in(2+NN); 41 | h = in(3+NN); 42 | Va = in(4+NN); 43 | % alpha = in(5+NN); 44 | % beta = in(6+NN); 45 | phi = in(7+NN); 46 | theta = in(8+NN); 47 | chi = in(9+NN); 48 | % p = in(10+NN); 49 | % q = in(11+NN); 50 | r = in(12+NN); 51 | % Vg = in(13+NN); 52 | % wn = in(14+NN); 53 | % we = in(15+NN); 54 | % psi = in(16+NN); 55 | NN = NN + 16; 56 | t = in(1+NN); 57 | 58 | switch flag, 59 | case 1, % follow straight line path specified by r and q 60 | 61 | e_p = [pn; pe; -h] - r_path; 62 | n = cross(q_path, [0; 0; 1]) / norm(cross(q_path, [0; 0; 1])); 63 | s = e_p - dot(e_p, n)*n; 64 | h_c = -r_path(3) - sqrt(s(1)^2 + s(2)^2) * q_path(3) / sqrt(q_path(1)^2+q_path(2)^2); 65 | chi_q = atan2(q_path(2), q_path(1)); 66 | while chi_q-chi < -pi 67 | chi_q = chi_q + 2*pi; 68 | end 69 | while chi_q-chi > pi 70 | chi_q = chi_q - 2*pi; 71 | end 72 | e_py = -sin(chi_q)*(pn-r_path(1)) + cos(chi_q)*(pe-r_path(2)); 73 | 74 | chi_c = chi_q - P.chi_inf*2/pi*atan(P.k_path*e_py); 75 | 76 | phi_ff = 0; 77 | 78 | case 2, % follow orbit specified by c, rho, lam 79 | h_c = -c_orbit(3); 80 | d = sqrt((pn-c_orbit(1))^2 + (pe-c_orbit(2))^2); 81 | barphi = atan2(pe-c_orbit(2), pn-c_orbit(1)); 82 | while barphi-chi < -pi 83 | barphi = barphi + 2*pi; 84 | end 85 | while barphi-chi > pi 86 | barphi = barphi - 2*pi; 87 | end 88 | 89 | chi_c = barphi + lam_orbit*(pi/2 + atan(P.k_orbit*(d-rho_orbit))); 90 | phi_ff = 0; 91 | end 92 | 93 | % command airspeed equal to desired airspeed 94 | Va_c = Va_d; 95 | 96 | % create output 97 | out = [Va_c; h_c; chi_c; phi_ff]; 98 | end 99 | 100 | 101 | -------------------------------------------------------------------------------- /formation-code/uavA1/path_manager1.m: -------------------------------------------------------------------------------- 1 | % path manager 2 | % 3 | % Modified: 4 | % - 3/25/2010 - RWB 5 | % - 4/7/2010 - RWB 6 | % 7 | % input is: 8 | % num_waypoints - number of waypoint configurations 9 | % waypoints - an array of dimension 5 by P.size_waypoint_array. 10 | % - the first num_waypoints rows define waypoint 11 | % configurations 12 | % - format for each waypoint configuration: 13 | % [wn, we, wd, chi_d, Va_d] 14 | % where the (wn, we, wd) is the NED position of the 15 | % waypoint, chi_d is the desired course at the waypoint, 16 | % and Va_d is the desired airspeed along the path. If 17 | % abs(chi_d)<2*pi then Dubins paths will be followed 18 | % between waypoint configurations. If abs(chi_d)>=2*pi 19 | % then straight-line paths (with fillets) are commanded. 20 | % 21 | % output is: 22 | % flag - if flag==1, follow waypoint path 23 | % if flag==2, follow orbit 24 | % Va_d - desired airspeed 25 | % r - inertial position of start of waypoint path 26 | % q - unit vector that defines inertial direction of waypoint path 27 | % c - center of orbit 28 | % rho - radius of orbit 29 | % lambda = direction of orbit (+1 for CW, -1 for CCW) 30 | % 31 | function out = path_manager1(in,P) 32 | 33 | persistent start_of_simulation 34 | 35 | t = in(end); 36 | if t==0 37 | start_of_simulation = 1; 38 | end 39 | 40 | NN = 0; 41 | num_waypoints = in(1+NN); 42 | if num_waypoints==0 % start of simulation 43 | flag = 1; % following straight line path 44 | Va_d = P.Va0; % desired airspeed along waypoint path 45 | NN = NN + 1 + 5*P.size_waypoint_array; 46 | pn = in(1+NN); 47 | pe = in(2+NN); 48 | h = in(3+NN); 49 | chi = in(9+NN); 50 | r = [pn; pe; -h]; 51 | q = [cos(chi); sin(chi); 0]; 52 | c = [0; 0; 0]; 53 | rho = 0; 54 | lambda = 0; 55 | state = in(1+NN:16+NN); 56 | flag_need_new_waypoints = 1; 57 | out = [flag; Va_d; r; q; c; rho; lambda; state; flag_need_new_waypoints]; 58 | else 59 | waypoints = reshape(in(2+NN:5*P.size_waypoint_array+1+NN),5,P.size_waypoint_array); 60 | 61 | if abs(waypoints(4,1))>=2*pi 62 | out = path_manager_line(in,P,start_of_simulation); % follows straight-lines and switches at waypoints 63 | % out = path_manager_fillet(in,P,start_of_simulation); % smooths through waypoints with fillets 64 | start_of_simulation=0; 65 | else 66 | out = path_manager_dubins(in,P,start_of_simulation); % follows Dubins paths between waypoint configurations 67 | start_of_simulation=0; 68 | end 69 | end 70 | 71 | end -------------------------------------------------------------------------------- /formation-code/uavA1/path_manager_fillet.m: -------------------------------------------------------------------------------- 1 | % path_manager_fillet 2 | % - follow lines between waypoints. Smooth transition with fillets 3 | % 4 | % 5 | % input is: 6 | % num_waypoints - number of waypoint configurations 7 | % waypoints - an array of dimension 5 by P.size_waypoint_array. 8 | % - the first num_waypoints rows define waypoint 9 | % configurations 10 | % - format for each waypoint configuration: 11 | % [wn, we, wd, dont_care, Va_d] 12 | % where the (wn, we, wd) is the NED position of the 13 | % waypoint, and Va_d is the desired airspeed along the 14 | % path. 15 | % 16 | % output is: 17 | % flag - if flag==1, follow waypoint path 18 | % if flag==2, follow orbit 19 | % 20 | % Va^d - desired airspeed 21 | % r - inertial position of start of waypoint path 22 | % q - unit vector that defines inertial direction of waypoint path 23 | % c - center of orbit 24 | % rho - radius of orbit 25 | % lambda = direction of orbit (+1 for CW, -1 for CCW) 26 | % 27 | function out = path_manager_fillet(in,P,start_of_simulation) 28 | 29 | NN = 0; 30 | num_waypoints = in(1+NN); 31 | waypoints = reshape(in(2+NN:5*P.size_waypoint_array+1+NN),5,P.size_waypoint_array); 32 | NN = NN + 1 + 5*P.size_waypoint_array; 33 | pn = in(1+NN); 34 | pe = in(2+NN); 35 | h = in(3+NN); 36 | % Va = in(4+NN); 37 | % alpha = in(5+NN); 38 | % beta = in(6+NN); 39 | % phi = in(7+NN); 40 | % theta = in(8+NN); 41 | % chi = in(9+NN); 42 | % p = in(10+NN); 43 | % q = in(11+NN); 44 | % r = in(12+NN); 45 | % Vg = in(13+NN); 46 | % wn = in(14+NN); 47 | % we = in(15+NN); 48 | % psi = in(16+NN); 49 | state = in(1+NN:16+NN); 50 | NN = NN + 16; 51 | t = in(1+NN); 52 | 53 | p = [pn; pe; -h]; 54 | 55 | 56 | persistent waypoints_old % stored copy of old waypoints 57 | persistent ptr_a % waypoint pointer 58 | persistent state_transition % state of transition state machine 59 | persistent flag_need_new_waypoints % flag that request new waypoints from path planner 60 | 61 | 62 | if start_of_simulation || isempty(waypoints_old), 63 | waypoints_old = zeros(5,P.size_waypoint_array); 64 | flag_need_new_waypoints = 0; 65 | 66 | end 67 | 68 | persistent ptr_b 69 | persistent ptr_c 70 | 71 | % if the waypoints have changed, update the waypoint pointer 72 | if min(min(waypoints==waypoints_old))==0, 73 | ptr_a = 1; 74 | ptr_b = 2; 75 | ptr_c = 3; 76 | waypoints_old = waypoints; 77 | state_transition = 1; 78 | flag_need_new_waypoints = 0; 79 | end 80 | 81 | % define current and next two waypoints 82 | r = waypoints(1:3,ptr_a); % previous waypoint position (a waypoint that MAV is flying from) 83 | w = waypoints(1:3,ptr_b); % current waypoint position (a waypoint that MAV is flying to) 84 | w_next = waypoints(1:3,ptr_c); % next waypoint position 85 | q_prev = (w-r)/norm(w-r); 86 | q_next = (w_next-w)/norm(w_next-w); 87 | barrho = acos(-q_prev'*q_next); 88 | 89 | % define transition state machine 90 | switch state_transition, 91 | case 1, % follow straight line from wpp_a to wpp_b 92 | flag = 1; % following straight line path 93 | Va_d = waypoints(5,ptr_a); % desired airspeed along waypoint path 94 | r = r; 95 | q = q_prev; 96 | c = [0; 0; 0]; 97 | rho = 1; 98 | lambda = 1; 99 | z = w - (P.R_min/tan(barrho/2))*q_prev; 100 | if (p-z)'*q_prev >= 0 101 | state_transition = 2; 102 | end 103 | 104 | case 2, % follow orbit from wpp_a-wpp_b to wpp_b-wpp_c 105 | flag = 2; % following orbit 106 | Va_d = waypoints(5,ptr_a); % desired airspeed along waypoint path 107 | r = r; 108 | q = q_prev; 109 | q_next = q_next; 110 | c = w - (P.R_min/sin(barrho/2))*(q_prev-q_next)/norm(q_prev-q_next); 111 | rho = P.R_min; 112 | lambda = sign(q_prev(1)*q_next(2) - q_prev(2)*q_next(1)); 113 | z = w + (P.R_min/tan(barrho/2))*q_next; 114 | if (p-z)'*q_next >= 0 115 | state_transition = 1; 116 | ptr_a = ptr_a + 1; 117 | ptr_b = ptr_b + 1; 118 | ptr_c = ptr_c + 1; 119 | end 120 | if ptr_c == num_waypoints+1 121 | ptr_c = 1; 122 | end 123 | if ptr_b == num_waypoints+1 124 | ptr_b = 1; 125 | end 126 | if ptr_a == num_waypoints+1 127 | ptr_a = 1; 128 | end 129 | end 130 | 131 | out = [flag; Va_d; r; q; c; rho; lambda; state; flag_need_new_waypoints]; 132 | 133 | end -------------------------------------------------------------------------------- /formation-code/uavA1/path_manager_line.m: -------------------------------------------------------------------------------- 1 | % path_manager_line 2 | % - follow lines between waypoints. 3 | % 4 | % input is: 5 | % num_waypoints - number of waypoint configurations 6 | % waypoints - an array of dimension 5 by P.size_waypoint_array. 7 | % - the first num_waypoints rows define waypoint 8 | % configurations 9 | % - format for each waypoint configuration: 10 | % [wn, we, wd, dont_care, Va_d] 11 | % where the (wn, we, wd) is the NED position of the 12 | % waypoint, and Va_d is the desired airspeed along the 13 | % path. 14 | % 15 | % output is: 16 | % flag - if flag==1, follow waypoint path 17 | % if flag==2, follow orbit 18 | % 19 | % Va^d - desired airspeed 20 | % r - inertial position of start of waypoint path 21 | % q - unit vector that defines inertial direction of waypoint path 22 | % c - center of orbit 23 | % rho - radius of orbit 24 | % lambda = direction of orbit (+1 for CW, -1 for CCW) 25 | % 26 | function out = path_manager_line(in,P,start_of_simulation) 27 | 28 | NN = 0; 29 | num_waypoints = in(1+NN); 30 | waypoints = reshape(in(2+NN:5*P.size_waypoint_array+1+NN),5,P.size_waypoint_array); 31 | NN = NN + 1 + 5*P.size_waypoint_array; 32 | pn = in(1+NN); 33 | pe = in(2+NN); 34 | h = in(3+NN); 35 | % Va = in(4+NN); 36 | % alpha = in(5+NN); 37 | % beta = in(6+NN); 38 | % phi = in(7+NN); 39 | % theta = in(8+NN); 40 | % chi = in(9+NN); 41 | % p = in(10+NN); 42 | % q = in(11+NN); 43 | % r = in(12+NN); 44 | % Vg = in(13+NN); 45 | % wn = in(14+NN); 46 | % we = in(15+NN); 47 | % psi = in(16+NN); 48 | state = in(1+NN:16+NN); 49 | NN = NN + 16; 50 | t = in(1+NN); 51 | 52 | 53 | p = [pn; pe; -h]; 54 | 55 | persistent waypoints_old % stored copy of old waypoints 56 | persistent ptr_a % waypoint pointer 57 | persistent flag_need_new_waypoints % flag that request new waypoints from path planner 58 | 59 | 60 | if start_of_simulation || isempty(waypoints_old), 61 | waypoints_old = zeros(5,P.size_waypoint_array); 62 | flag_need_new_waypoints = 0; 63 | 64 | end 65 | 66 | persistent ptr_b 67 | persistent ptr_c 68 | 69 | % if the waypoints have changed, update the waypoint pointer 70 | if min(min(waypoints==waypoints_old))==0, 71 | ptr_a = 1; 72 | ptr_b = 2; 73 | ptr_c = 3; 74 | waypoints_old = waypoints; 75 | flag_need_new_waypoints = 0; 76 | end 77 | 78 | % algorithm 5 in UAV book (p. 189) 79 | r = waypoints(1:3,ptr_a); % previous waypoint position (a waypoint that MAV is flying from) 80 | w = waypoints(1:3,ptr_b); % current waypoint position (a waypoint that MAV is flying to) 81 | w_next = waypoints(1:3,ptr_c); % next waypoint position 82 | q_prev = (w-r)/norm(w-r); 83 | q = (w_next-w)/norm(w_next-w); 84 | n = (q_prev+q)/norm(q_prev+q); 85 | 86 | % construct output for path follower 87 | flag = 1; % following straight line path 88 | Va_d = waypoints(5,ptr_a); % desired airspeed along waypoint path 89 | r = r; 90 | q = q_prev; 91 | c = [0; 0; 0]; 92 | rho = 1; 93 | lambda = 1; 94 | 95 | out = [flag; Va_d; r; q; c; rho; lambda; state; flag_need_new_waypoints]; 96 | 97 | % determine if next waypoint path has been reached, and rotate ptrs if 98 | % necessary 99 | if (p-w)'*n >= 0 100 | ptr_a = ptr_a + 1; 101 | ptr_b = ptr_b + 1; 102 | ptr_c = ptr_c + 1; 103 | end 104 | if ptr_c == num_waypoints+1 105 | ptr_c = 1; 106 | end 107 | if ptr_b == num_waypoints+1 108 | ptr_b = 1; 109 | end 110 | if ptr_a == num_waypoints+1 111 | ptr_a = 1; 112 | end 113 | 114 | end -------------------------------------------------------------------------------- /formation-code/uavA1/path_planner1.m: -------------------------------------------------------------------------------- 1 | % path planner 2 | % 3 | % Modified: 4 | % - 4/06/2010 - RWB 5 | % 6 | % output is a vector containing P.num_waypoints waypoints 7 | % 8 | % input is the map of the environment and desired goal location 9 | function out = path_planner1(in,P,map) 10 | load 'uavW.mat' 11 | i1 = uavW; 12 | clear uavW; 13 | 14 | NN = 0; 15 | pn = in(1+NN); 16 | pe = in(2+NN); 17 | h = in(3+NN); 18 | % Va = in(4+NN); 19 | % alpha = in(5+NN); 20 | % beta = in(6+NN); 21 | % phi = in(7+NN); 22 | % theta = in(8+NN); 23 | chi = in(9+NN); 24 | % p = in(10+NN); 25 | % q = in(11+NN); 26 | % r = in(12+NN); 27 | % Vg = in(13+NN); 28 | % wn = in(14+NN); 29 | % we = in(15+NN); 30 | % psi = in(16+NN); 31 | flag_new_waypoints = in(17+NN); 32 | NN = NN + 17; 33 | t = in(1+NN); 34 | 35 | 36 | persistent num_waypoints 37 | persistent wpp 38 | 39 | 40 | if (t==0) || flag_new_waypoints 41 | % format for each point is [pn, pe, pd, chi, Va^d] where the position 42 | % of the waypoint is (pn, pe, pd), the desired course at the waypoint 43 | % is chi, and the desired airspeed between waypoints is Va 44 | % if chi!=-9999, then Dubins paths will be used between waypoints. 45 | switch 1 46 | case 1 47 | 48 | [num_waypoints , wpp] = getWpp(P,i1); 49 | 50 | case 2 % Dubins 51 | num_waypoints = 4; 52 | wpp = [... 53 | 0, 0, -100, 0, P.Va0;... 54 | 300, 0, -100, 45*pi/180, P.Va0;... 55 | 0, 300, -100, 45*pi/180, P.Va0;... 56 | 300, 300, -100, -135*pi/180, P.Va0;... 57 | ]; 58 | case 3 % path through city using Dubin's paths 59 | % current configuration 60 | wpp_start = [pn, pe, -h, chi, P.Va0]; 61 | % desired end waypoint 62 | if norm([pn; pe; -h]-[map.width; map.width; -h])= downAtNE(map, X, Y), 109 | collision_flag = 1; 110 | end 111 | % check to see if outside of world 112 | if (X>map.width) | (X<0) | (Y>map.width) | (Y<0), 113 | collision_flag = 1; 114 | end 115 | end 116 | end 117 | 118 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 119 | %% downAtNE 120 | %% find the map down coordinate at a specified (n,e) location 121 | function down = downAtNE(map, n, e) 122 | 123 | [d_n,idx_n] = min(abs(n - map.buildings_n)); 124 | [d_e,idx_e] = min(abs(e - map.buildings_e)); 125 | 126 | if (d_n<=map.BuildingWidth) & (d_e<=map.BuildingWidth), 127 | down = -map.heights(idx_e,idx_n); 128 | else 129 | down = 0; 130 | end 131 | end 132 | 133 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 134 | %% findReturn 135 | %% compute the return value at a particular location 136 | function return_value = findReturn(pn,pe,return_map,map); 137 | 138 | [pn_max,pe_max] = size(return_map); 139 | fn = pn_max*pn/map.width; 140 | fn = min(pn_max,round(fn)); 141 | fn = max(1,fn); 142 | fe = pe_max*pe/map.width; 143 | fe = min(pe_max,round(fe)); 144 | fe = max(1,fe); 145 | return_value = return_map(fn,fe); 146 | end 147 | 148 | 149 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 150 | %% findMaxReturnPath 151 | %% find the maximum return path in the tree 152 | function path = findMaxReturnPath(tree) 153 | 154 | % find node with max return 155 | [tmp,idx] = max(tree(:,5)); 156 | 157 | % construct path with maximum return 158 | path = tree(idx,:); 159 | parent_node = tree(idx,6); 160 | %while parent_node>1, 161 | while parent_node>1, 162 | path = [tree(parent_node,:); path]; 163 | parent_node = tree(parent_node,6); 164 | end 165 | end 166 | 167 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 168 | %% updateReturnMap 169 | %% update the return map to indicate where MAV has been 170 | function new_return_map = updateReturnMap(path,return_map,map) 171 | 172 | new_return_map = return_map; 173 | for i=1:size(path,1), 174 | pn = path(i,1); 175 | pe = path(i,2); 176 | [pn_max,pe_max] = size(return_map); 177 | fn = pn_max*pn/map.width; 178 | fn = min(pn_max,round(fn)); 179 | fn = max(1,fn); 180 | fe = pe_max*pe/map.width; 181 | fe = min(pe_max,round(fe)); 182 | fe = max(1,fe); 183 | 184 | new_return_map(fn,fe) = return_map(fn,fe) - 50; 185 | end 186 | end 187 | 188 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 189 | %% plotReturnMap 190 | %% plot the return map 191 | function plotReturnMap(return_map) 192 | 193 | figure(2), clf 194 | mesh(return_map) 195 | end 196 | 197 | 198 | -------------------------------------------------------------------------------- /formation-code/uavA1/planCoverRRTDubins.m: -------------------------------------------------------------------------------- 1 | %% planRRT 2 | %% - basic coverage algorithm 3 | %% 4 | %% Last Modified - 11/15/2010 - R. Beard 5 | 6 | function path=planCoverRRTDubins(wpp_start, R, map) 7 | 8 | % desired down position is down position of start node 9 | pd = wpp_start(3); 10 | 11 | % specify start node from wpp_start 12 | start_node = [wpp_start(1), wpp_start(2), pd, 0, 0, 0]; 13 | % format is [N, E, D, chi, cost, parent] 14 | 15 | 16 | % return map 17 | returnMapSize = 30; % this is a critical parameter! 18 | return_map = 50*ones(returnMapSize,returnMapSize)+ rand(returnMapSize,returnMapSize); 19 | plotReturnMap(return_map), %pause 20 | 21 | % construct search path by doing N search cycles 22 | SEARCH_CYCLES = 100; % number of search cycles 23 | 24 | % look ahead tree parameters 25 | L = 2*R+1; % segment Length 26 | vartheta = pi/4; % search angle 27 | depth = 5; % depth of look ahead tree 28 | 29 | % initialize path and tree 30 | path = start_node; 31 | for i=1:SEARCH_CYCLES, 32 | tree = extendTree(path(end,:),L,vartheta,depth,map,return_map,pd); 33 | next_path = findMaxReturnPath(tree); 34 | path = [path; next_path(1,:)]; 35 | % update the return map 36 | return_map = updateReturnMap(next_path(1,:),return_map,map); 37 | plotReturnMap(return_map), %pause 38 | % set the end of the path as the root of the tree 39 | end 40 | 41 | % remove path segments where there is no turn 42 | path_=path; 43 | path = path(1,:); 44 | for i=2:size(path_,1), 45 | if path_(i,4)~=path_(i-1,4); 46 | path = [path; path_(i,:)]; 47 | end 48 | end 49 | 50 | % % specify that these are straight-line paths. 51 | % for i=1:size(path,1), 52 | % path(i,4)=-9999; 53 | % end 54 | 55 | end 56 | 57 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 58 | %% extendTree 59 | %% extend tree by randomly selecting point and growing tree toward that 60 | %% point 61 | function tree = extendTree(start_node,L,vartheta,depth,map,return_map,pd) 62 | 63 | tree_ = [start_node, 0]; % the last variable marks node as expanded 64 | 65 | % extend tree initially 66 | for d = 1:depth, 67 | newnodes = []; 68 | for j=1:size(tree_,1), 69 | if tree_(j,7)~=1, % process unexpanded nodes 70 | for i=1:3, 71 | if i==1, theta = tree_(j,4)-vartheta; 72 | elseif i==2, theta = tree_(j,4); 73 | elseif i==3, theta = tree_(j,4)+vartheta; 74 | end 75 | newnode_ = [... 76 | tree_(j,1)+L*cos(theta),... 77 | tree_(j,2)+L*sin(theta),... 78 | tree_(j,3),... 79 | theta,... 80 | 0,... 81 | j,... 82 | 0,... 83 | ]; 84 | if collision(tree_(j,:), newnode_, map)==0, 85 | newnode_(5) = tree_(j,5)... 86 | +findReturn(newnode_(1),newnode_(2),return_map,map); 87 | newnodes = [newnodes; newnode_]; 88 | end 89 | end 90 | tree_(j,7)=1; % mark as expanded 91 | end 92 | end 93 | tree_ = [tree_; newnodes]; 94 | end 95 | tree = tree_(:,1:6); 96 | end 97 | 98 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 99 | %% collision 100 | %% check to see if a node is in collsion with obstacles 101 | function collision_flag = collision(start_node, end_node, map) 102 | collision_flag = 0; 103 | sigma = 0:.1:1; % define where collisions will be checked 104 | for i=1:length(sigma), 105 | X = (1-sigma(i))*start_node(1) + sigma(i)*end_node(1); 106 | Y = (1-sigma(i))*start_node(2) + sigma(i)*end_node(2); 107 | Z = (1-sigma(i))*start_node(3) + sigma(i)*end_node(3); 108 | if Z >= downAtNE(map, X, Y), 109 | collision_flag = 1; 110 | end 111 | % check to see if outside of world 112 | if (X>map.width) | (X<0) | (Y>map.width) | (Y<0), 113 | collision_flag = 1; 114 | end 115 | end 116 | end 117 | 118 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 119 | %% downAtNE 120 | %% find the map down coordinate at a specified (n,e) location 121 | function down = downAtNE(map, n, e) 122 | 123 | [d_n,idx_n] = min(abs(n - map.buildings_n)); 124 | [d_e,idx_e] = min(abs(e - map.buildings_e)); 125 | 126 | if (d_n<=map.BuildingWidth) & (d_e<=map.BuildingWidth), 127 | down = -map.heights(idx_e,idx_n); 128 | else 129 | down = 0; 130 | end 131 | end 132 | 133 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 134 | %% findReturn 135 | %% compute the return value at a particular location 136 | function return_value = findReturn(pn,pe,return_map,map); 137 | 138 | [pn_max,pe_max] = size(return_map); 139 | fn = pn_max*pn/map.width; 140 | fn = min(pn_max,round(fn)); 141 | fn = max(1,fn); 142 | fe = pe_max*pe/map.width; 143 | fe = min(pe_max,round(fe)); 144 | fe = max(1,fe); 145 | return_value = return_map(fn,fe); 146 | end 147 | 148 | 149 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 150 | %% findMaxReturnPath 151 | %% find the maximum return path in the tree 152 | function path = findMaxReturnPath(tree) 153 | 154 | % find node with max return 155 | [tmp,idx] = max(tree(:,5)); 156 | 157 | % construct path with maximum return 158 | path = tree(idx,:); 159 | parent_node = tree(idx,6); 160 | %while parent_node>1, 161 | while parent_node>1, 162 | path = [tree(parent_node,:); path]; 163 | parent_node = tree(parent_node,6); 164 | end 165 | end 166 | 167 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 168 | %% updateReturnMap 169 | %% update the return map to indicate where MAV has been 170 | function new_return_map = updateReturnMap(path,return_map,map) 171 | 172 | new_return_map = return_map; 173 | for i=1:size(path,1), 174 | pn = path(i,1); 175 | pe = path(i,2); 176 | [pn_max,pe_max] = size(return_map); 177 | fn = pn_max*pn/map.width; 178 | fn = min(pn_max,round(fn)); 179 | fn = max(1,fn); 180 | fe = pe_max*pe/map.width; 181 | fe = min(pe_max,round(fe)); 182 | fe = max(1,fe); 183 | 184 | new_return_map(fn,fe) = return_map(fn,fe) - 50; 185 | end 186 | end 187 | 188 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 189 | %% plotReturnMap 190 | %% plot the return map 191 | function plotReturnMap(return_map) 192 | 193 | figure(2), clf 194 | mesh(return_map) 195 | end 196 | 197 | 198 | -------------------------------------------------------------------------------- /formation-code/uavA1/plotxyz.m: -------------------------------------------------------------------------------- 1 | load uav1u 2 | a1 = A; 3 | load uav2u 4 | a2 = A; 5 | load uav3u 6 | a3 = A; 7 | 8 | n1 = 3890; 9 | plot3(a1(1:n1,2), a1(1:n1,3),a1(1:n1,4)); hold on; 10 | plot3(a2(1:n1,2), a2(1:n1,3),a2(1:n1,4)); hold on; 11 | plot3(a3(1:n1,2), a3(1:n1,3),a3(1:n1,4)); hold on; 12 | axis equal -------------------------------------------------------------------------------- /formation-code/uavA1/predator.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/predator.jpg -------------------------------------------------------------------------------- /formation-code/uavA1/readme.txt: -------------------------------------------------------------------------------- 1 | 单机数据的生成 2 | 3 | 需要生成 path waypoints x 根据i序号给数据带上标记 4 | 改文件 param_chap1.m i = 2 5 | path_planner1.m i1 = 2 6 | 直接运行New mavsim chap12 7 | 工作空间会得到path waypoints x 8 | 根据相应的序号更改名字并保存 -------------------------------------------------------------------------------- /formation-code/uavA1/sensors.m: -------------------------------------------------------------------------------- 1 | % sensors.m 2 | % Compute the output of rate gyros, accelerometers, and pressure sensors 3 | % 4 | % Revised: 5 | % 3/5/2010 - RB 6 | % 5/14/2010 - RB 7 | 8 | function y = sensors(uu, P) 9 | 10 | % relabel the inputs 11 | % pn = uu(1); 12 | % pe = uu(2); 13 | pd = uu(3); 14 | % u = uu(4); 15 | % v = uu(5); 16 | % w = uu(6); 17 | phi = uu(7); 18 | theta = uu(8); 19 | % psi = uu(9); 20 | p = uu(10); 21 | q = uu(11); 22 | r = uu(12); 23 | F_x = uu(13); 24 | F_y = uu(14); 25 | F_z = uu(15); 26 | % M_l = uu(16); 27 | % M_m = uu(17); 28 | % M_n = uu(18); 29 | Va = uu(19); 30 | % alpha = uu(20); 31 | % beta = uu(21); 32 | % wn = uu(22); 33 | % we = uu(23); 34 | % wd = uu(24); 35 | 36 | % simulate rate gyros (units are rad/sec) 37 | y_gyro_x = p + P.sigma_gyro * randn; 38 | y_gyro_y = q + P.sigma_gyro * randn; 39 | y_gyro_z = r + P.sigma_gyro * randn; 40 | 41 | % simulate accelerometers (units of g) 42 | y_accel_x = F_x / P.mass / P.gravity + sin(theta) + P.sigma_accel * randn; 43 | y_accel_y = F_y / P.mass / P.gravity - cos(theta) * sin(phi) + P.sigma_accel * randn; 44 | y_accel_z = F_z / P.mass / P.gravity - cos(theta) * cos(phi) + P.sigma_accel * randn; 45 | 46 | % simulate pressure sensors 47 | y_static_pres = P.rho * P.gravity * (-pd) + P.beta_abs_pres + P.sigma_abs_pres * randn; 48 | y_diff_pres = P.rho * Va^2 / 2 + P.beta_diff_pres + P.sigma_diff_pres * randn; 49 | 50 | % construct total output 51 | y = [... 52 | y_gyro_x;... 53 | y_gyro_y;... 54 | y_gyro_z;... 55 | y_accel_x;... 56 | y_accel_y;... 57 | y_accel_z;... 58 | y_static_pres;... 59 | y_diff_pres;... 60 | ]; 61 | 62 | end 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /formation-code/uavA1/slprj/sim/varcache/New_mavsim_chap12/checksumOfCache.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/slprj/sim/varcache/New_mavsim_chap12/checksumOfCache.mat -------------------------------------------------------------------------------- /formation-code/uavA1/slprj/sim/varcache/New_mavsim_chap12/tmwinternal/simulink_cache.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | WuRqSDBRbmd5z/EQMMA63w== 5 | 6 | -------------------------------------------------------------------------------- /formation-code/uavA1/slprj/sim/varcache/New_mavsim_chap12/varInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/slprj/sim/varcache/New_mavsim_chap12/varInfo.mat -------------------------------------------------------------------------------- /formation-code/uavA1/slprj/sim/varcache/mavsim_chap12/checksumOfCache.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/slprj/sim/varcache/mavsim_chap12/checksumOfCache.mat -------------------------------------------------------------------------------- /formation-code/uavA1/slprj/sim/varcache/mavsim_chap12/tmwinternal/simulink_cache.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | CANcFgG0TkTtRz7fpypkCQ== 5 | 6 | -------------------------------------------------------------------------------- /formation-code/uavA1/slprj/sim/varcache/mavsim_chap12/varInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/slprj/sim/varcache/mavsim_chap12/varInfo.mat -------------------------------------------------------------------------------- /formation-code/uavA1/slprj/sim/varcache/mavsim_chap12_model/checksumOfCache.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/slprj/sim/varcache/mavsim_chap12_model/checksumOfCache.mat -------------------------------------------------------------------------------- /formation-code/uavA1/slprj/sim/varcache/mavsim_chap12_model/tmwinternal/simulink_cache.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | GnEJzjsQWqwygUunqqgD+A== 5 | 6 | -------------------------------------------------------------------------------- /formation-code/uavA1/slprj/sim/varcache/mavsim_chap12_model/varInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/slprj/sim/varcache/mavsim_chap12_model/varInfo.mat -------------------------------------------------------------------------------- /formation-code/uavA1/slprj/sim/varcache/mavsim_show/checksumOfCache.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/slprj/sim/varcache/mavsim_show/checksumOfCache.mat -------------------------------------------------------------------------------- /formation-code/uavA1/slprj/sim/varcache/mavsim_show/tmwinternal/simulink_cache.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | zl9QvmX7MVs6OT5+Femr9Q== 5 | 6 | -------------------------------------------------------------------------------- /formation-code/uavA1/slprj/sim/varcache/mavsim_show/varInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/slprj/sim/varcache/mavsim_show/varInfo.mat -------------------------------------------------------------------------------- /formation-code/uavA1/slprj/sim/varcache/mavsim_show123/checksumOfCache.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/slprj/sim/varcache/mavsim_show123/checksumOfCache.mat -------------------------------------------------------------------------------- /formation-code/uavA1/slprj/sim/varcache/mavsim_show123/tmwinternal/simulink_cache.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 4oMMJTJBzDXI87IW9fGOLw== 5 | 6 | -------------------------------------------------------------------------------- /formation-code/uavA1/slprj/sim/varcache/mavsim_show123/varInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/slprj/sim/varcache/mavsim_show123/varInfo.mat -------------------------------------------------------------------------------- /formation-code/uavA1/slprj/sim/varcache/mavsim_trim/checksumOfCache.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/slprj/sim/varcache/mavsim_trim/checksumOfCache.mat -------------------------------------------------------------------------------- /formation-code/uavA1/slprj/sim/varcache/mavsim_trim/tmwinternal/simulink_cache.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | +Dq/H/Qmlios84tVlwqmuQ== 5 | 6 | -------------------------------------------------------------------------------- /formation-code/uavA1/slprj/sim/varcache/mavsim_trim/varInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/slprj/sim/varcache/mavsim_trim/varInfo.mat -------------------------------------------------------------------------------- /formation-code/uavA1/totxt.m: -------------------------------------------------------------------------------- 1 | load uav3 2 | abs = states.data; 3 | t = 0; 4 | n = size(abs,1); 5 | A = []; 6 | for i = 1:n 7 | a1 = [t abs(i,1) abs(i,2) abs(i,3) abs(i,4) abs(i,8) abs(i,7) abs(i,16)]; 8 | A = [A;a1]; 9 | 10 | t = t+0.01; 11 | end 12 | 13 | n1 = n; 14 | plot3(A(1:n1,2), A(1:n1,3),A(1:n1,4)); 15 | axis equal -------------------------------------------------------------------------------- /formation-code/uavA1/true_states.m: -------------------------------------------------------------------------------- 1 | function xhat = estimate_states(uu,P) 2 | % 3 | % fake state estimation for mavsim 4 | % - this function will be replaced with a state estimator in a later 5 | % chapter. 6 | % 7 | % Outputs are: 8 | % pnhat - estimated North position, 9 | % pehat - estimated East position, 10 | % hhat - estimated altitude, 11 | % Vahat - estimated airspeed, 12 | % alphahat - estimated angle of attack 13 | % betahat - estimated sideslip angle 14 | % phihat - estimated roll angle, 15 | % thetahat - estimated pitch angel, 16 | % chihat - estimated course, 17 | % phat - estimated roll rate, 18 | % qhat - estimated pitch rate, 19 | % rhat - estimated yaw rate, 20 | % Vghat - estimated ground speed, 21 | % wnhat - estimate of North wind, 22 | % wehat - estimate of East wind 23 | % psihat - estimate of heading angle 24 | % bxhat - estimate of x-gyro bias 25 | % byhat - estimate of y-gyro bias 26 | % bzhat - estimate of z-gyro bias 27 | % 28 | % 29 | % Modification History: 30 | % 2/11/2010 - RWB 31 | % 5/14/2010 - RWB 32 | % 33 | 34 | % process inputs 35 | NN = 0; 36 | pn = uu(1+NN); % inertial North position 37 | pe = uu(2+NN); % inertial East position 38 | h = -uu(3+NN); % altitude 39 | % u = uu(4+NN); % inertial velocity along body x-axis 40 | % v = uu(5+NN); % inertial velocity along body y-axis 41 | % w = uu(6+NN); % inertial velocity along body z-axis 42 | phi = uu(7+NN); % roll angle 43 | theta = uu(8+NN); % pitch angle 44 | psi = uu(9+NN); % yaw angle 45 | p = uu(10+NN); % body frame roll rate 46 | q = uu(11+NN); % body frame pitch rate 47 | r = uu(12+NN); % body frame yaw rate 48 | NN = NN+12; 49 | Va = uu(1+NN); % airspeed 50 | alpha = uu(2+NN); % angle of attack 51 | beta = uu(3+NN); % sideslip angle 52 | wn = uu(4+NN); % wind North 53 | we = uu(5+NN); % wind East 54 | % wd = uu(6+NN); % wind down 55 | NN = NN+6; 56 | % t = uu(1+NN); % time 57 | 58 | % estimate states (using real state data) 59 | pnhat = pn; 60 | pehat = pe; 61 | hhat = h; 62 | Vahat = Va; 63 | alphahat = alpha; 64 | betahat = beta; 65 | phihat = phi; 66 | thetahat = theta; 67 | chihat = atan2(Va*sin(psi)+we, Va*cos(psi)+wn); 68 | phat = p; 69 | qhat = q; 70 | rhat = r; 71 | Vghat = sqrt((Va*cos(psi)+wn)^2 + (Va*sin(psi)+we)^2); 72 | wnhat = wn; 73 | wehat = we; 74 | psihat = psi; 75 | % bxhat = P.bias_gyro_x; 76 | % byhat = P.bias_gyro_y; 77 | % bzhat = P.bias_gyro_z; 78 | bxhat = 0; 79 | byhat = 0; 80 | bzhat = 0; 81 | 82 | xhat = [... 83 | pnhat;... 84 | pehat;... 85 | hhat;... 86 | Vahat;... 87 | alphahat;... 88 | betahat;... 89 | phihat;... 90 | thetahat;... 91 | chihat;... 92 | phat;... 93 | qhat;... 94 | rhat;... 95 | Vghat;... 96 | wnhat;... 97 | wehat;... 98 | psihat;... 99 | bxhat;... 100 | byhat;... 101 | bzhat;... 102 | ]; 103 | 104 | end -------------------------------------------------------------------------------- /formation-code/uavA1/tv.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavA1/tv.jpg -------------------------------------------------------------------------------- /formation-code/uavShow/New_mavsim_chap12.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/New_mavsim_chap12.slx -------------------------------------------------------------------------------- /formation-code/uavShow/New_mavsim_chap12.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/New_mavsim_chap12.slxc -------------------------------------------------------------------------------- /formation-code/uavShow/airdata.m: -------------------------------------------------------------------------------- 1 | function out=airdata(uu) 2 | % 3 | % Fake air data - this will be replaced with real air data in Chapter 4 4 | % 5 | % modified 12/11/2009 - RB 6 | 7 | % process inputs to function 8 | pn = uu(1); % North position (meters) 9 | pe = uu(2); % East position (meters) 10 | h = -uu(3); % altitude (meters) 11 | u = uu(4); % body velocity along x-axis (meters/s) 12 | v = uu(5); % body velocity along y-axis (meters/s) 13 | w = uu(6); % body velocity along z-axis (meters/s) 14 | phi = 180/pi*uu(7); % roll angle (degrees) 15 | theta = 180/pi*uu(8); % pitch angle (degrees) 16 | psi = 180/pi*uu(9); % yaw angle (degrees) 17 | p = 180/pi*uu(10); % body angular rate along x-axis (degrees/s) 18 | q = 180/pi*uu(11); % body angular rate along y-axis (degrees/s) 19 | r = 180/pi*uu(12); % body angular rate along z-axis (degrees/s) 20 | 21 | Va = sqrt(u^2+v^2+w^2); 22 | alpha = atan2(w,u); 23 | beta = asin(v); 24 | wn = 0; 25 | we = 0; 26 | wd = 0; 27 | 28 | out = [Va; alpha; beta; wn; we; wd]; 29 | -------------------------------------------------------------------------------- /formation-code/uavShow/batcam.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/batcam.png -------------------------------------------------------------------------------- /formation-code/uavShow/compute_gains.m: -------------------------------------------------------------------------------- 1 | % some variables needed 2 | u = P.x_trim(4); 3 | v = P.x_trim(5); 4 | w = P.x_trim(6); 5 | Va_trim = sqrt(u^2 + v^2 + w^2); 6 | theta_trim = P.x_trim(8); 7 | alpha_trim = atan(w/u); 8 | delta_e_trim = P.u_trim(1); 9 | delta_t_trim = P.u_trim(4); 10 | 11 | % Roll attitude hold 12 | P.delta_a_max = 45 * pi / 180; 13 | P.phi_max = 15 * pi / 180; 14 | P.roll_max = 45 * pi / 180; 15 | a_phi1 = -0.5 * P.rho * Va_trim^2 * P.S_wing * P.b * P.C_p_p * P.b / (2 * Va_trim); 16 | a_phi2 = 0.5 * P.rho * Va_trim^2 * P.S_wing * P.b * P.C_p_delta_a; 17 | P.kp_phi = P.delta_a_max / P.phi_max * sign(a_phi2); 18 | omega_phi = sqrt(abs(a_phi2) * P.delta_a_max / P.phi_max); 19 | zeta_phi = 1.2; % design parameter 20 | P.kd_phi = (2 * zeta_phi * omega_phi - a_phi1) / a_phi2; 21 | P.ki_phi = 0.2; % design parameter 22 | 23 | % Course hold 24 | W_chi = 8; % design parameter 25 | omega_chi = 1 / W_chi * omega_phi; 26 | zeta_chi = 0.707; % design parameter 27 | Vg = P.Va0; 28 | P.kp_chi = 2 * zeta_chi * omega_chi * Vg / P.gravity; 29 | P.ki_chi = omega_chi^2 * Vg / P.gravity; 30 | 31 | % Pitch attitude hold 32 | P.delta_e_max = 45 * pi / 180; 33 | P.e_theta_max = 10 * pi / 180; 34 | a_theta1 = -P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_q * P.c / (2 * P.Jy * 2 * Va_trim); 35 | a_theta2 = -P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_alpha / (2 * P.Jy); 36 | a_theta3 = P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_delta_e / (2 * P.Jy); 37 | P.kp_theta = P.delta_e_max / P.e_theta_max * sign(a_theta3); 38 | omega_theta = sqrt(a_theta2 + P.delta_e_max / P.e_theta_max * abs(a_theta3)); 39 | zeta_theta = 0.707; % design parameter 40 | P.kd_theta = (2 * zeta_theta * omega_theta - a_theta1) / a_theta3; 41 | P.theta_max = 20 * pi / 180; 42 | P.pitch_max = 40 * pi / 180; 43 | 44 | % Airspeed hold using Throttle 45 | a_V1 = P.rho * Va_trim * P.S_wing / P.mass ... 46 | * (P.C_D_0 + P.C_D_alpha * alpha_trim + P.C_L_delta_e * delta_e_trim) ... 47 | + P.rho * P.S_prop / P.mass * P.C_prop * Va_trim; 48 | a_V2 = P.rho * P.S_prop / P.mass * P.C_prop * P.k_motor^2 * delta_t_trim; 49 | a_V3 = P.gravity * cos(theta_trim - alpha_trim); 50 | omega_v = 5; % design parameter 51 | zeta_v = 0.707; % design parameter 52 | P.delta_t_max = 1; 53 | P.delta_t_min = 0; 54 | P.ki_v = omega_v^2 / a_V2; 55 | P.kp_v = (2 * zeta_v * omega_v - a_V1) / a_V2; 56 | 57 | % Airspeed hold using Pitch 58 | W_v2 = 7; % design parameter 59 | zeta_v2 = 0.707; % design parameter 60 | omega_v2 = 1 / W_v2 * omega_theta; 61 | K_theta_dc = P.kp_theta * a_theta3 / (a_theta2 + P.kp_theta * a_theta3); 62 | P.ki_v2 = -omega_v2^2/(K_theta_dc * P.gravity); 63 | P.kp_v2 = (a_V1 - 2 * zeta_v2 * omega_v2) / (K_theta_dc * P.gravity); 64 | 65 | % Altitude hold using Pitch 66 | W_h = 10; % design parameter 67 | omega_h = 1 / W_h * omega_theta; 68 | Va = P.Va0; 69 | zeta_h = 1.2; % design parameter 70 | P.h_max = 1000; 71 | P.h_min = 0; 72 | P.ki_h = omega_h^2 / (K_theta_dc * Va); 73 | P.kp_h = 2 * zeta_h * omega_h / (K_theta_dc * Va); 74 | 75 | -------------------------------------------------------------------------------- /formation-code/uavShow/compute_ss_model.m: -------------------------------------------------------------------------------- 1 | function [A_lon,B_lon,A_lat,B_lat] = compute_ss_model(filename,x_trim,u_trim) 2 | % x_trim is the trimmed state, 3 | % u_trim is the trimmed input 4 | 5 | % add stuff here 6 | [A,B,C,D] = linmod(filename, x_trim, u_trim); 7 | 8 | E1 = [... 9 | 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0;... 10 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0;... 11 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1;... 12 | 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0;... 13 | 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0;... 14 | ]; 15 | E2 = [... 16 | 0, 1, 0, 0;... 17 | 0, 0, 1, 0;... 18 | ]; 19 | A_lat = E1 * A * E1'; 20 | B_lat = E1 * B * E2'; 21 | 22 | E3 = [... 23 | 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0;... 24 | 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0;... 25 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0;... 26 | 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0;... 27 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0;... 28 | ]; 29 | E4 = [... 30 | 1, 0, 0, 0;... 31 | 0, 0, 0, 1;... 32 | ]; 33 | 34 | A_lon = E3 * A * E3'; 35 | B_lon = E3 * B * E4'; -------------------------------------------------------------------------------- /formation-code/uavShow/compute_tf_model.m: -------------------------------------------------------------------------------- 1 | function [T_phi_delta_a,T_chi_phi,T_theta_delta_e,T_h_theta,T_h_Va,T_Va_delta_t,T_Va_theta,T_v_delta_r]... 2 | = compute_tf_model(x_trim,u_trim,P) 3 | % x_trim is the trimmed state, 4 | % u_trim is the trimmed input 5 | 6 | % add stuff here 7 | 8 | u = x_trim(4); 9 | v = x_trim(5); 10 | w = x_trim(6); 11 | Va_trim = sqrt(u^2 + v^2 + w^2); 12 | theta_trim = x_trim(8); 13 | alpha_trim = atan(w/u); 14 | delta_e_trim = u_trim(1); 15 | delta_t_trim = u_trim(4); 16 | 17 | a_phi1 = -0.5 * P.rho * Va_trim^2 * P.S_wing * P.b * P.C_p_p * P.b / (2 * Va_trim); 18 | a_phi2 = 0.5 * P.rho * Va_trim^2 * P.S_wing * P.b * P.C_p_delta_a; 19 | a_theta1 = -P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_q * P.c / (2 * P.Jy * 2 * Va_trim); 20 | a_theta2 = -P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_alpha / (2 * P.Jy); 21 | a_theta3 = P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_delta_e / (2 * P.Jy); 22 | a_V1 = P.rho * Va_trim * P.S_wing / P.mass ... 23 | * (P.C_D_0 + P.C_D_alpha * alpha_trim + P.C_L_delta_e * delta_e_trim) ... 24 | + P.rho * P.S_prop / P.mass * P.C_prop * Va_trim; 25 | a_V2 = P.rho * P.S_prop / P.mass * P.C_prop * P.k_motor^2 * delta_t_trim; 26 | a_V3 = P.gravity * cos(theta_trim - alpha_trim); 27 | a_beta1 = -P.rho * Va_trim * P.S_wing * P.C_Y_beta / (2 * P.mass); 28 | a_beta2 = P.rho * Va_trim * P.S_wing * P.C_Y_delta_r / (2 * P.mass); 29 | 30 | % define transfer functions 31 | T_phi_delta_a = tf([a_phi2],[1,a_phi1,0]); 32 | T_chi_phi = tf([P.gravity/Va_trim],[1,0]); 33 | T_theta_delta_e = tf(a_theta3,[1,a_theta1,a_theta2]); 34 | T_h_theta = tf([Va_trim],[1,0]); 35 | T_h_Va = tf([theta_trim],[1,0]); 36 | T_Va_delta_t = tf([a_V2],[1,a_V1]); 37 | T_Va_theta = tf([-a_V3],[1,a_V1]); 38 | T_v_delta_r = tf([Va_trim*a_beta2],[1,a_beta1]); 39 | 40 | -------------------------------------------------------------------------------- /formation-code/uavShow/compute_trim.m: -------------------------------------------------------------------------------- 1 | function [x_trim,u_trim] = compute_trim(filename, Va, gamma, R) 2 | % Va is the desired airspeed (m/s) 3 | % gamma is the desired flight path angle (radians) 4 | % R is the desired radius (m) - use (+) for right handed orbit, 5 | % (-) for left handed orbit 6 | 7 | 8 | % add stuff here 9 | x0 = [0; 0; 0; Va; 0; 0; 0; gamma; 0; 0; 0; 0]; 10 | u0 = [0; 0; 0; 1]; 11 | y0 = [Va; 0; 0]; 12 | ix = []; 13 | iu = []; 14 | iy = [1, 3]; 15 | dx0 = [0; 0; -Va*sin(gamma); 0; 0; 0; 0; 0; Va*cos(gamma)/R; 0; 0; 0]; 16 | idx = [3; 4; 5; 6; 7; 8; 9; 10; 11; 12]; 17 | 18 | % compute trim conditions 19 | [x_trim,u_trim,y_trim,dx_trim] = trim(filename,x0,u0,y0,ix,iu,iy,dx0,idx); 20 | 21 | % check to make sure that the linearization worked (should be small) 22 | norm(dx_trim(3:end)-dx0(3:end)); 23 | 24 | -------------------------------------------------------------------------------- /formation-code/uavShow/createWorld.m: -------------------------------------------------------------------------------- 1 | %% createWorld 2 | %% - create random 3D city 3 | %% - city_width: the city is of size (city_width)x(city_width) 4 | %% - building_height: the maximum height of the buildings 5 | %% - num_blocks: the number of (square) blocks in the city 6 | %% - street_width: the percent of block that is street (0-1) 7 | %% 8 | %% Last Modified - 4/12/2010 - R. Beard 9 | 10 | function map = createWorld(city_width, building_height, num_blocks, street_width) 11 | 12 | map.width = city_width; % the city is of size (width)x(width) 13 | map.MaxHeight = building_height; % maximum height of buildings 14 | map.NumBlocks = num_blocks; % number of blocks in city 15 | map.StreetWidth = street_width; % percent of block that is street. 16 | 17 | % computed parameters 18 | map.BuildingWidth = map.width/map.NumBlocks*(1-map.StreetWidth); 19 | map.StreetWidth = map.width/map.NumBlocks*map.StreetWidth; 20 | map.heights = map.MaxHeight*rand(map.NumBlocks,map.NumBlocks); 21 | for i=1:map.NumBlocks, 22 | map.buildings_n(i) = [... 23 | 0.5*map.width/map.NumBlocks*(2*(i-1)+1),... 24 | ]; 25 | end 26 | map.buildings_e = map.buildings_n; 27 | 28 | end 29 | 30 | -------------------------------------------------------------------------------- /formation-code/uavShow/dubinsParameters.m: -------------------------------------------------------------------------------- 1 | % dubinsParameters 2 | % - Find Dubin's parameters between two configurations 3 | % 4 | % input is: 5 | % start_node - [wn_s, we_s, wd_s, chi_s, 0, 0] 6 | % end_node - [wn_e, wn_e, wd_e, chi_e, 0, 0] 7 | % R - minimum turn radius 8 | % 9 | % output is: 10 | % dubinspath - a matlab structure with the following fields 11 | % dubinspath.ps - the start position in re^3 12 | % dubinspath.chis - the start course angle 13 | % dubinspath.pe - the end position in re^3 14 | % dubinspath.chie - the end course angle 15 | % dubinspath.R - turn radius 16 | % dubinspath.L - length of the Dubins path 17 | % dubinspath.cs - center of the start circle 18 | % dubinspath.lams - direction of the start circle 19 | % dubinspath.ce - center of the end circle 20 | % dubinspath.lame - direction of the end circle 21 | % dubinspath.w1 - vector in re^3 defining half plane H1 22 | % dubinspath.q1 - unit vector in re^3 along straight line path 23 | % dubinspath.w2 - vector in re^3 defining position of half plane H2 24 | % dubinspath.w3 - vector in re^3 defining position of half plane H3 25 | % dubinspath.q3 - unit vector defining direction of half plane H3 26 | % 27 | 28 | function dubinspath = dubinsParameters(start_node, end_node, R) 29 | 30 | ell = norm(start_node(1:2)-end_node(1:2)); 31 | if ell<2*R, 32 | disp('The distance between nodes must be larger than 2R.'); 33 | dubinspath = []; 34 | else 35 | 36 | ps = start_node(1:3); 37 | chis = start_node(4); 38 | pe = end_node(1:3); 39 | chie = end_node(4); 40 | 41 | crs = ps' + R*rotz(pi/2)*[cos(chis); sin(chis); 0]; 42 | cls = ps' + R*rotz(-pi/2)*[cos(chis); sin(chis); 0]; 43 | cre = pe' + R*rotz(pi/2)*[cos(chie); sin(chie); 0]; 44 | cle = pe' + R*rotz(-pi/2)*[cos(chie); sin(chie); 0]; 45 | 46 | % compute L1 47 | connecting_line = cre-crs; 48 | north_unit = [1; 0; 0]; 49 | % theta = acos(dot(connecting_line,north_unit)/norm(connecting_line)); 50 | theta = atan2(connecting_line(2), connecting_line(1)); 51 | L1 = norm(crs-cre) + R*mod(2*pi + mod(theta-pi/2, 2*pi) - mod(chis-pi/2, 2*pi), 2*pi) ... 52 | + R*mod(2*pi + mod(chie-pi/2, 2*pi) - mod(theta-pi/2, 2*pi), 2*pi); 53 | % compute L2 54 | connecting_line = cle-crs; 55 | ell = norm(connecting_line); 56 | % theta = acos(dot(connecting_line,north_unit)/norm(connecting_line)); 57 | theta = atan2(connecting_line(2), connecting_line(1)); 58 | theta2 = theta - pi/2 + asin(2*R/ell); 59 | if isreal(theta2)==0, 60 | L2 = 9999; 61 | else 62 | L2 = sqrt(ell^2-4*R^2) + R*mod(2*pi + mod(theta2, 2*pi) - mod(chis-pi/2, 2*pi), 2*pi) ... 63 | + R*mod(2*pi + mod(theta2+pi, 2*pi) - mod(chie+pi/2, 2*pi), 2*pi); 64 | end 65 | % compute L3 66 | connecting_line = cre-cls; 67 | ell = norm(connecting_line); 68 | % theta = acos(dot(connecting_line,north_unit)/norm(connecting_line)); 69 | theta = atan2(connecting_line(2), connecting_line(1)); 70 | theta2 = acos(2*R/ell); 71 | if isreal(theta2)==0, 72 | L3 = 9999; 73 | else 74 | L3 = sqrt(ell^2-4*R^2) + R*mod(2*pi + mod(chis+pi/2, 2*pi) - mod(theta+theta2, 2*pi), 2*pi) ... 75 | + R*mod(2*pi + mod(chie-pi/2, 2*pi) - mod(theta+theta2-pi, 2*pi), 2*pi); 76 | end 77 | % compute L4 78 | connecting_line = cle-cls; 79 | % theta = acos(dot(connecting_line,north_unit)/norm(connecting_line)); 80 | theta = atan2(connecting_line(2), connecting_line(1)); 81 | L4 = norm(cls-cle) + R*mod(2*pi + mod(chis+pi/2, 2*pi) - mod(theta+pi/2, 2*pi), 2*pi) ... 82 | + R*mod(2*pi + mod(theta+pi/2, 2*pi) - mod(chie+pi/2, 2*pi), 2*pi); 83 | % L is the minimum distance 84 | [L,idx] = min([L1,L2,L3,L4]); 85 | e1 = [1; 0; 0]; % north unit vector 86 | switch(idx), 87 | case 1, 88 | cs = crs; 89 | lams = 1; 90 | ce = cre; 91 | lame = 1; 92 | q1 = (ce-cs)/norm(ce-cs); 93 | w1 = cs + R*rotz(-pi/2)*q1; 94 | w2 = ce + R*rotz(-pi/2)*q1; 95 | case 2, 96 | cs = crs; 97 | lams = 1; 98 | ce = cle; 99 | lame = -1; 100 | ell = norm(ce-cs); 101 | connecting_line = ce - cs; 102 | theta = atan2(connecting_line(2), connecting_line(1)); 103 | % theta = acos(dot(ce-cs,e1)/ell); 104 | theta2 = theta - pi/2 + asin(2*R/ell); 105 | q1 = rotz(theta2+pi/2)*e1; 106 | w1 = cs + R*rotz(theta2)*e1; 107 | w2 = ce + R*rotz(theta2+pi)*e1; 108 | case 3, 109 | cs = cls; 110 | lams = -1; 111 | ce = cre; 112 | lame = 1; 113 | ell = norm(ce-cs); 114 | connecting_line = ce - cs; 115 | % theta = acos(dot(ce-cs,e1)/ell); 116 | theta = atan2(connecting_line(2), connecting_line(1)); 117 | theta2 = acos(2*R/ell); 118 | q1 = rotz(theta+theta2-pi/2)*e1; 119 | w1 = cs + R*rotz(theta+theta2)*e1; 120 | w2 = ce + R*rotz(theta+theta2-pi)*e1; 121 | case 4, 122 | cs = cls; 123 | lams = -1; 124 | ce = cle; 125 | lame = -1; 126 | q1 = (ce-cs)/norm(ce-cs); 127 | w1 = cs + R*rotz(pi/2)*q1; 128 | w2 = ce + R*rotz(pi/2)*q1; 129 | end 130 | w3 = pe'; 131 | q3 = rotz(chie)*e1; 132 | 133 | % assign path variables 134 | dubinspath.ps = ps; 135 | dubinspath.chis = chis; 136 | dubinspath.pe = pe; 137 | dubinspath.chie = chie; 138 | dubinspath.R = R; 139 | dubinspath.L = L; 140 | dubinspath.cs = cs; 141 | dubinspath.lams = lams; 142 | dubinspath.ce = ce; 143 | dubinspath.lame = lame; 144 | dubinspath.w1 = w1; 145 | dubinspath.q1 = q1; 146 | dubinspath.w2 = w2; 147 | dubinspath.w3 = w3; 148 | dubinspath.q3 = q3; 149 | end 150 | end 151 | 152 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 153 | %% rotz(theta) 154 | %% rotation matrix about the z axis. 155 | function R = rotz(theta) 156 | R = [... 157 | cos(theta), -sin(theta), 0;... 158 | sin(theta), cos(theta), 0;... 159 | 0, 0, 1;... 160 | ]; 161 | end 162 | -------------------------------------------------------------------------------- /formation-code/uavShow/dubinsRRT: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/dubinsRRT -------------------------------------------------------------------------------- /formation-code/uavShow/dubinsRRTcoverage: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/dubinsRRTcoverage -------------------------------------------------------------------------------- /formation-code/uavShow/forces_moments.m: -------------------------------------------------------------------------------- 1 | % forces_moments.m 2 | % Computes the forces and moments acting on the airframe. 3 | % 4 | % Output is 5 | % F - forces 6 | % M - moments 7 | % Va - airspeed 8 | % alpha - angle of attack 9 | % beta - sideslip angle 10 | % wind - wind vector in the inertial frame 11 | % 12 | 13 | function out = forces_moments(x, delta, wind, P) 14 | 15 | % relabel the inputs 16 | pn = x(1); 17 | pe = x(2); 18 | pd = x(3); 19 | u = x(4); 20 | v = x(5); 21 | w = x(6); 22 | phi = x(7); 23 | theta = x(8); 24 | psi = x(9); 25 | p = x(10); 26 | q = x(11); 27 | r = x(12); 28 | delta_e = delta(1); 29 | delta_a = delta(2); 30 | delta_r = delta(3); 31 | delta_t = delta(4); 32 | w_ns = wind(1); % steady wind - North 33 | w_es = wind(2); % steady wind - East 34 | w_ds = wind(3); % steady wind - Down 35 | u_wg = wind(4); % gust along body x-axis 36 | v_wg = wind(5); % gust along body y-axis 37 | w_wg = wind(6); % gust along body z-axis 38 | 39 | % rotation matrix from vehicle to body 40 | sp = sin(phi); 41 | cp = cos(phi); 42 | st = sin(theta); 43 | ct = cos(theta); 44 | ss = sin(psi); 45 | cs = cos(psi); 46 | 47 | rotation_to_body = [ct*cs ct*ss -st; 48 | sp*st*cs-cp*ss sp*st*ss+cp*cs sp*ct; 49 | cp*st*cs+sp*ss cp*st*ss-sp*cs cp*ct 50 | ]; 51 | 52 | V_wind_body = rotation_to_body * [w_ns; w_es; w_ds] + [u_wg; v_wg; w_wg]; 53 | V_airspeed_body = [u-V_wind_body(1); v-V_wind_body(2); w-V_wind_body(3)]; 54 | 55 | % compute wind data in NED 56 | w_n = 0; 57 | w_e = 0; 58 | w_d = 0; 59 | 60 | % compute air data 61 | u_r = V_airspeed_body(1); 62 | v_r = V_airspeed_body(2); 63 | w_r = V_airspeed_body(3); 64 | Va = sqrt(u_r^2 + v_r^2 + w_r^2); % air speed 65 | alpha = atan(w_r / u_r); % angle of attack 66 | beta = asin(v_r / (Va)); % side slip 67 | 68 | % coefficients 69 | sigma = (1 + exp(-P.M * (alpha - P.alpha0)) + exp(P.M * (alpha + P.alpha0))) ... 70 | / ((1 + exp(-P.M * (alpha - P.alpha0))) * (1 + exp(P.M * (alpha + P.alpha0)))); 71 | C_L = (1 - sigma) * (P.C_L_0 + P.C_L_alpha * alpha) + sigma * (2 * sign(alpha) * (sin(alpha))^2 * cos(alpha)); 72 | C_D = P.C_D_p + (P.C_L_0 + P.C_L_alpha * alpha)^2 / (pi * P.e * P.b^2 / P.S_wing); 73 | 74 | C_X = -C_D * cos(alpha) + C_L * sin(alpha); 75 | C_X_q = -P.C_D_q * cos(alpha) + P.C_L_q * sin(alpha); 76 | C_X_delta_e = -P.C_D_delta_e * cos(alpha) + P.C_L_delta_e * sin(alpha); 77 | C_Z = -C_D * sin(alpha) - C_L * cos(alpha); 78 | C_Z_q = -P.C_D_q * sin(alpha) - P.C_L_q * cos(alpha); 79 | C_Z_delta_e = -P.C_D_delta_e * sin(alpha) - P.C_L_delta_e * cos(alpha); 80 | 81 | % compute external forces and torques on aircraft 82 | Force(1) = -P.mass * P.gravity * sin(theta) ... 83 | + 0.5 * P.rho * Va^2 * P.S_wing ... 84 | * (C_X + C_X_q * P.c * q / (2 * Va) + C_X_delta_e * delta_e) ... 85 | + 0.5 * P.rho * P.S_prop * P.C_prop * ((P.k_motor * delta_t)^2 - Va^2); 86 | 87 | Force(2) = P.mass * P.gravity * cos(theta) * sin(phi) ... 88 | + 0.5 * P.rho * Va^2 * P.S_wing ... 89 | * (P.C_Y_0 + P.C_Y_beta * beta + P.C_Y_p * P.b * p / (2 * Va) ... 90 | + P.C_Y_r * P.b * r / (2 * Va) + P.C_Y_delta_a * delta_a + P.C_Y_delta_r * delta_r); 91 | 92 | Force(3) = P.mass * P.gravity * cos(theta) * cos(phi) ... 93 | + 0.5 * P.rho * Va^2 * P.S_wing ... 94 | * (C_Z + C_Z_q * P.c * q / (2 * Va) + C_Z_delta_e * delta_e); 95 | 96 | Torque(1) = 0.5 * P.rho * Va^2 * P.S_wing ... 97 | * (P.b * (P.C_ell_0 + P.C_ell_beta * beta + P.C_ell_p * P.b * p / (2 * Va) ... 98 | + P.C_ell_r * P.b * r / (2 * Va) + P.C_ell_delta_a * delta_a + P.C_ell_delta_r * delta_r)) ... 99 | - P.k_T_P * (P.k_Omega * delta_t)^2; 100 | 101 | Torque(2) = 0.5 * P.rho * Va^2 * P.S_wing ... 102 | * (P.c * (P.C_m_0 + P.C_m_alpha * alpha ... 103 | + P.C_m_q * P.c * q / (2 * Va) + P.C_m_delta_e * delta_e)); 104 | Torque(3) = 0.5 * P.rho * Va^2 * P.S_wing ... 105 | * (P.b * (P.C_n_0 + P.C_n_beta * beta + P.C_n_p * P.b * p / (2 * Va) ... 106 | + P.C_n_r * P.b * r / (2 * Va) + P.C_n_delta_a * delta_a + P.C_n_delta_r * delta_r)); 107 | 108 | out = [Force'; Torque'; Va; alpha; beta; w_n; w_e; w_d]; 109 | end -------------------------------------------------------------------------------- /formation-code/uavShow/getWpp.m: -------------------------------------------------------------------------------- 1 | function [num_waypoints , wpp] = getWpp(P,uav) 2 | 3 | load '5jia1.mat' 4 | 5 | a1 = 2; 6 | a2 = 200; 7 | num_waypoints = a2/a1; 8 | % wpp = [... 9 | % 100, 0, -100, -9999, P.Va0;... 10 | % 200, 100, -100, -9999, 45;... 11 | % 100, 300, -100, -9999, P.Va0;... 12 | % 100, 400, -100, -9999, P.Va0;... 13 | wpp = []; 14 | i1 = uav; 15 | 16 | 17 | 18 | for i = 1:a1:a2 19 | x = []; 20 | x = Xplot2(i,6*i1-5:6*i1-3); 21 | x(3) = -x(3); 22 | x = [x -9999 Xplot2(i,6*i1-2)]; 23 | wpp = [wpp;x]; 24 | 25 | end 26 | 27 | % 28 | % if(uav == 2) 29 | % for i = 1:a1:a2 30 | % x = []; 31 | % x = Xplot2(i,7:9); 32 | % x(3) = -x(3); 33 | % x = [x -9999 Xplot2(i,10)]; 34 | % wpp = [wpp;x]; 35 | % end 36 | % end 37 | % 38 | % if(uav == 3) 39 | % for i = 1:a1:a2 40 | % x = []; 41 | % x = Xplot2(i,13:15); 42 | % x(3) = -x(3); 43 | % x = [x -9999 Xplot2(i,16)]; 44 | % wpp = [wpp;x]; 45 | % end 46 | % end 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /formation-code/uavShow/gps.m: -------------------------------------------------------------------------------- 1 | % gps.m 2 | % Compute the output of gps sensor 3 | % 4 | % Revised: 5 | % 3/5/2010 - RB 6 | % 5/14/2010 - RB 7 | 8 | function y = gps(uu, P) 9 | 10 | % relabel the inputs 11 | Va = uu(1); 12 | % alpha = uu(2); 13 | % beta = uu(3); 14 | wn = uu(4); 15 | we = uu(5); 16 | % wd = uu(6); 17 | pn = uu(7); 18 | pe = uu(8); 19 | pd = uu(9); 20 | % u = uu(10); 21 | % v = uu(11); 22 | % w = uu(12); 23 | % phi = uu(13); 24 | % theta = uu(14); 25 | psi = uu(15); 26 | % p = uu(16); 27 | % q = uu(17); 28 | % r = uu(18); 29 | t = uu(19); 30 | 31 | persistent v_n; 32 | persistent v_e; 33 | persistent v_h; 34 | 35 | if t==0 36 | v_n = 0; 37 | v_e = 0; 38 | v_h = 0; 39 | else 40 | v_n = exp(-P.k_gps * P.Ts_gps) * v_n + P.sigma_gps_n * randn; 41 | v_e = exp(-P.k_gps * P.Ts_gps) * v_e + P.sigma_gps_e * randn; 42 | v_h = exp(-P.k_gps * P.Ts_gps) * v_h + P.sigma_gps_altitude * randn; 43 | end 44 | 45 | % construct North, East, and altitude GPS measurements 46 | y_gps_n = pn + v_n; 47 | y_gps_e = pe + v_e; 48 | y_gps_h = -pd + v_h; 49 | 50 | % construct groundspeed and course measurements 51 | V_n = Va * cos(psi) + wn; 52 | V_e = Va * sin(psi) + we; 53 | V_g = sqrt(V_n^2 + V_e^2); 54 | 55 | sigma_course = P.sigma_gps_V_g / V_g; 56 | 57 | y_gps_Vg = sqrt(V_n^2 + V_e^2) + P.sigma_gps_V_g * randn; 58 | y_gps_course = atan2(V_e, V_n) + sigma_course * randn; 59 | 60 | % construct total output 61 | y = [... 62 | y_gps_n;... 63 | y_gps_e;... 64 | y_gps_h;... 65 | y_gps_Vg;... 66 | y_gps_course;... 67 | ]; 68 | 69 | end 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /formation-code/uavShow/kestrel.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/kestrel.jpg -------------------------------------------------------------------------------- /formation-code/uavShow/m123.m: -------------------------------------------------------------------------------- 1 | load '..\uavW.mat' 2 | i1 = uavW; 3 | clear uavW; 4 | -------------------------------------------------------------------------------- /formation-code/uavShow/main.m: -------------------------------------------------------------------------------- 1 | clear; close all; clc; 2 | 3 | mdl = 'New_mavsim_chap12'; 4 | sim(mdl); 5 | 6 | -------------------------------------------------------------------------------- /formation-code/uavShow/mav_dynamics.m: -------------------------------------------------------------------------------- 1 | function [sys,x0,str,ts,simStateCompliance] = mav_dynamics(t,x,u,flag,P) 2 | 3 | switch flag, 4 | 5 | %%%%%%%%%%%%%%%%%% 6 | % Initialization % 7 | %%%%%%%%%%%%%%%%%% 8 | case 0, 9 | [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes(P); 10 | 11 | %%%%%%%%%%%%%%% 12 | % Derivatives % 13 | %%%%%%%%%%%%%%% 14 | case 1, 15 | sys=mdlDerivatives(t,x,u,P); 16 | 17 | %%%%%%%%%% 18 | % Update % 19 | %%%%%%%%%% 20 | case 2, 21 | sys=mdlUpdate(t,x,u); 22 | 23 | %%%%%%%%%%% 24 | % Outputs % 25 | %%%%%%%%%%% 26 | case 3, 27 | sys=mdlOutputs(t,x,u); 28 | 29 | %%%%%%%%%%%%%%%%%%%%%%% 30 | % GetTimeOfNextVarHit % 31 | %%%%%%%%%%%%%%%%%%%%%%% 32 | case 4, 33 | sys=mdlGetTimeOfNextVarHit(t,x,u); 34 | 35 | %%%%%%%%%%%%% 36 | % Terminate % 37 | %%%%%%%%%%%%% 38 | case 9, 39 | sys=mdlTerminate(t,x,u); 40 | 41 | %%%%%%%%%%%%%%%%%%%% 42 | % Unexpected flags % 43 | %%%%%%%%%%%%%%%%%%%% 44 | otherwise 45 | DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag)); 46 | 47 | end 48 | 49 | % end sfuntmpl 50 | 51 | % 52 | %============================================================================= 53 | % mdlInitializeSizes 54 | % Return the sizes, initial conditions, and sample times for the S-function. 55 | %============================================================================= 56 | % 57 | function [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes(P) 58 | 59 | % 60 | % call simsizes for a sizes structure, fill it in and convert it to a 61 | % sizes array. 62 | % 63 | % Note that in this example, the values are hard coded. This is not a 64 | % recommended practice as the characteristics of the block are typically 65 | % defined by the S-function parameters. 66 | % 67 | sizes = simsizes; 68 | 69 | sizes.NumContStates = 12; 70 | sizes.NumDiscStates = 0; 71 | sizes.NumOutputs = 12; 72 | sizes.NumInputs = 6; 73 | sizes.DirFeedthrough = 0; 74 | sizes.NumSampleTimes = 1; % at least one sample time is needed 75 | 76 | sys = simsizes(sizes); 77 | 78 | % 79 | % initialize the initial conditions 80 | % 81 | x0 = [... 82 | P.pn0;... 83 | P.pe0;... 84 | P.pd0;... 85 | P.u0;... 86 | P.v0;... 87 | P.w0;... 88 | P.phi0;... 89 | P.theta0;... 90 | P.psi0;... 91 | P.p0;... 92 | P.q0;... 93 | P.r0;... 94 | ]; 95 | 96 | % 97 | % str is always an empty matrix 98 | % 99 | str = []; 100 | 101 | % 102 | % initialize the array of sample times 103 | % 104 | ts = [0 0]; 105 | 106 | % Specify the block simStateCompliance. The allowed values are: 107 | % 'UnknownSimState', < The default setting; warn and assume DefaultSimState 108 | % 'DefaultSimState', < Same sim state as a built-in block 109 | % 'HasNoSimState', < No sim state 110 | % 'DisallowSimState' < Error out when saving or restoring the model sim state 111 | simStateCompliance = 'UnknownSimState'; 112 | 113 | % end mdlInitializeSizes 114 | 115 | % 116 | %============================================================================= 117 | % mdlDerivatives 118 | % Return the derivatives for the continuous states. 119 | %============================================================================= 120 | % 121 | function sys=mdlDerivatives(t,x,uu, P) 122 | 123 | pn = x(1); 124 | pe = x(2); 125 | pe = x(3); 126 | u = x(4); 127 | v = x(5); 128 | w = x(6); 129 | phi = x(7); 130 | theta = x(8); 131 | psi = x(9); 132 | p = x(10); 133 | q = x(11); 134 | r = x(12); 135 | fx = uu(1); 136 | fy = uu(2); 137 | fz = uu(3); 138 | ell = uu(4); 139 | m = uu(5); 140 | n = uu(6); 141 | 142 | sp = sin(phi); 143 | cp = cos(phi); 144 | st = sin(theta); 145 | ct = cos(theta); 146 | ss = sin(psi); 147 | cs = cos(psi); 148 | tt = tan(theta); 149 | 150 | % translational kinematics 151 | rotation_position = [ct*cs sp*st*cs-cp*ss cp*st*cs+sp*ss; 152 | ct*ss sp*st*ss+cp*cs cp*st*ss-sp*cs; 153 | -st sp*ct cp*ct 154 | ]; 155 | position_dot = rotation_position*[u; v; w]; 156 | pndot = position_dot(1); 157 | pedot = position_dot(2); 158 | pddot = position_dot(3); 159 | 160 | % translational dynamics 161 | udot = r*v-q*w+fx/P.mass; 162 | vdot = p*w-r*u+fy/P.mass; 163 | wdot = q*u-p*v+fz/P.mass; 164 | 165 | % rotational kinematics 166 | rotation_angle = [1 sp*tt cp*tt; 167 | 0 cp -sp; 168 | 0 sp/ct cp/ct 169 | ]; 170 | angle_dot = rotation_angle*[p; q; r]; 171 | phidot = angle_dot(1); 172 | thetadot = angle_dot(2); 173 | psidot = angle_dot(3); 174 | 175 | % rorational dynamics 176 | pdot = P.r1*p*q-P.r2*q*r+P.r3*ell+P.r4*n; 177 | qdot = P.r5*p*r-P.r6*(p^2-r^2)+m/P.Jy; 178 | rdot = P.r7*p*q-P.r1*q*r+P.r4*ell+P.r8*n; 179 | 180 | sys = [pndot; pedot; pddot; udot; vdot; wdot; phidot; thetadot; psidot; pdot; qdot; rdot]; 181 | 182 | % end mdlDerivatives 183 | 184 | % 185 | %============================================================================= 186 | % mdlUpdate 187 | % Handle discrete state updates, sample time hits, and major time step 188 | % requirements. 189 | %============================================================================= 190 | % 191 | function sys=mdlUpdate(t,x,u) 192 | 193 | sys = []; 194 | 195 | % end mdlUpdate 196 | 197 | % 198 | %============================================================================= 199 | % mdlOutputs 200 | % Return the block outputs. 201 | %============================================================================= 202 | % 203 | function sys=mdlOutputs(t,x,u) 204 | 205 | sys = x; 206 | 207 | % end mdlOutputs 208 | 209 | % 210 | %============================================================================= 211 | % mdlGetTimeOfNextVarHit 212 | % Return the time of the next hit for this block. Note that the result is 213 | % absolute time. Note that this function is only used when you specify a 214 | % variable discrete-time sample time [-2 0] in the sample time array in 215 | % mdlInitializeSizes. 216 | %============================================================================= 217 | % 218 | function sys=mdlGetTimeOfNextVarHit(t,x,u) 219 | 220 | sampleTime = 1; % Example, set the next hit to be one second later. 221 | sys = t + sampleTime; 222 | 223 | % end mdlGetTimeOfNextVarHit 224 | 225 | % 226 | %============================================================================= 227 | % mdlTerminate 228 | % Perform any end of simulation tasks. 229 | %============================================================================= 230 | % 231 | function sys=mdlTerminate(t,x,u) 232 | 233 | sys = []; 234 | 235 | % end mdlTerminate 236 | -------------------------------------------------------------------------------- /formation-code/uavShow/mavsim_chap12_model.slx.r2015b: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/mavsim_chap12_model.slx.r2015b -------------------------------------------------------------------------------- /formation-code/uavShow/mavsim_show.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/mavsim_show.slx -------------------------------------------------------------------------------- /formation-code/uavShow/mavsim_show.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/mavsim_show.slxc -------------------------------------------------------------------------------- /formation-code/uavShow/mavsim_show123.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/mavsim_show123.slxc -------------------------------------------------------------------------------- /formation-code/uavShow/mavsim_trim.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/mavsim_trim.slx -------------------------------------------------------------------------------- /formation-code/uavShow/mavsim_trim.slx.autosave: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/mavsim_trim.slx.autosave -------------------------------------------------------------------------------- /formation-code/uavShow/mavsim_trim.slx.r2015b: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/mavsim_trim.slx.r2015b -------------------------------------------------------------------------------- /formation-code/uavShow/mavsim_trim.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/mavsim_trim.slxc -------------------------------------------------------------------------------- /formation-code/uavShow/param_chap1.m: -------------------------------------------------------------------------------- 1 | clear; close all; clc; 2 | load '5jia1.mat' 3 | load 'x1' 4 | load 'x2' 5 | load 'x3' 6 | load 'x4' 7 | load 'x5' 8 | load 'uavW.mat' 9 | i = uavW; 10 | clear uavW; 11 | 12 | 13 | P.gravity = 9.8; 14 | 15 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 16 | % Params for Aersonade UAV 17 | %physical parameters of airframe 18 | P.mass = 13.5; 19 | P.Jx = 0.8244; 20 | P.Jy = 1.135; 21 | P.Jz = 1.759; 22 | P.Jxz = .1204; 23 | % aerodynamic coefficients 24 | P.S_wing = 0.55; 25 | P.b = 2.8956; 26 | P.c = 0.18994; 27 | P.S_prop = 0.2027; 28 | P.rho = 1.2682; 29 | P.k_motor = 80; 30 | P.k_T_P = 0; 31 | P.k_Omega = 0; 32 | P.e = 0.9; 33 | 34 | P.C_L_0 = 0.28; 35 | P.C_L_alpha = 3.45; 36 | P.C_L_q = 0.0; 37 | P.C_L_delta_e = -0.36; 38 | P.C_D_0 = 0.03; 39 | P.C_D_alpha = 0.30; 40 | P.C_D_p = 0.0437; 41 | P.C_D_q = 0.0; 42 | P.C_D_delta_e = 0.0; 43 | P.C_m_0 = -0.02338; 44 | P.C_m_alpha = -0.38; 45 | P.C_m_q = -3.6; 46 | P.C_m_delta_e = -0.5; 47 | P.C_Y_0 = 0.0; 48 | P.C_Y_beta = -0.98; 49 | P.C_Y_p = 0.0; 50 | P.C_Y_r = 0.0; 51 | P.C_Y_delta_a = 0.0; 52 | P.C_Y_delta_r = -0.17; 53 | P.C_ell_0 = 0.0; 54 | P.C_ell_beta = -0.12; 55 | P.C_ell_p = -0.26; 56 | P.C_ell_r = 0.14; 57 | P.C_ell_delta_a = 0.08; 58 | P.C_ell_delta_r = 0.105; 59 | P.C_n_0 = 0.0; 60 | P.C_n_beta = 0.25; 61 | P.C_n_p = 0.022; 62 | P.C_n_r = -0.35; 63 | P.C_n_delta_a = 0.06; 64 | P.C_n_delta_r = -0.032; 65 | P.C_prop = 1.0; 66 | P.M = 50; 67 | P.epsilon = 0.1592; 68 | P.alpha0 = 0.4712; 69 | 70 | % wind parameters 71 | P.wind_n = 0; 72 | P.wind_e = 0; 73 | P.wind_d = 0; 74 | P.L_u = 200; 75 | P.L_v = 200; 76 | P.L_w = 50; 77 | P.sigma_u = 1.06; 78 | P.sigma_v = 1.06; 79 | P.sigma_w = .7; 80 | 81 | % r1 - r8 82 | P.r = P.Jx*P.Jz-P.Jxz^2; 83 | P.r1 = P.Jxz*(P.Jx-P.Jy+P.Jz)/P.r; 84 | P.r2 = P.Jz*(P.Jz-P.Jy)+P.Jxz^2; 85 | P.r3 = P.Jz/P.r; 86 | P.r4 = P.Jxz/P.r; 87 | P.r5 = (P.Jz-P.Jx)/P.Jy; 88 | P.r6 = P.Jxz/P.Jy; 89 | P.r7 = ((P.Jx-P.Jy)*P.Jx+P.Jxz^2)/P.r; 90 | P.r8 = P.Jx/P.r; 91 | 92 | % C parameters on p.62 93 | P.C_p_0 = P.r3 * P.C_ell_0 + P.r4 * P.C_n_0; 94 | P.C_p_beta = P.r3 * P.C_ell_beta + P.r4 * P.C_n_beta; 95 | P.C_p_p = P.r3 * P.C_ell_p + P.r4 * P.C_n_p; 96 | P.C_p_r = P.r3 * P.C_ell_r + P.r4 * P.C_n_r; 97 | P.C_p_delta_a = P.r3 * P.C_ell_delta_a + P.r4 * P.C_n_delta_a; 98 | P.C_p_delta_r = P.r3 * P.C_ell_delta_r + P.r4 * P.C_n_delta_r; 99 | P.C_r_0 = P.r4 * P.C_ell_0 + P.r8 * P.C_n_0; 100 | P.C_r_beta = P.r4 * P.C_ell_beta + P.r8 * P.C_n_beta; 101 | P.C_r_p = P.r4 * P.C_ell_p + P.r8 * P.C_n_p; 102 | P.C_r_r = P.r4 * P.C_ell_r + P.r8 * P.C_n_r; 103 | P.C_r_delta_a = P.r4 * P.C_ell_delta_a + P.r8 * P.C_n_delta_a; 104 | P.C_r_delta_r = P.r4 * P.C_ell_delta_r + P.r8 * P.C_n_delta_r; 105 | 106 | 107 | % compute trim conditions using 'mavsim_chap5_trim.slx' 108 | % initial airspeed 109 | P.Va0 = 35; 110 | gamma = 0; %5*pi/180; % desired flight path angle (radians) 111 | R = inf; %150; % desired radius (m) - use (+) for right handed orbit, can't be 0 112 | 113 | % autopilot sample rate 114 | P.Ts = 0.01; 115 | P.tau = 0.5; 116 | 117 | % first cut at initial conditions 118 | 119 | P.pn0 = 0; % initial North position 120 | P.pe0 = 0; % initial East position 121 | P.pd0 = 0; % initial Down position (negative altitude) 122 | 123 | P.u0 = P.Va0; % initial velocity along body x-axis 124 | P.v0 = 0; % initial velocity along body y-axis 125 | P.w0 = 0; % initial velocity along body z-axis 126 | P.phi0 = 0; % initial roll angle 127 | P.theta0 = 0; % initial pitch angle 128 | P.psi0 = 0; % initial yaw angle 129 | P.p0 = 0; % initial body frame roll rate 130 | P.q0 = 0; % initial body frame pitch rate 131 | P.r0 = 0; % initial body frame yaw rate 132 | 133 | 134 | % run trim commands 135 | [x_trim, u_trim]=compute_trim('mavsim_trim',P.Va0,gamma,R); 136 | P.u_trim = u_trim; 137 | P.x_trim = x_trim; 138 | 139 | % set initial conditions to trim conditions 140 | % initial conditions 141 | P.pn0 = Xplot2(1,6*i-5); 142 | P.pe0 = Xplot2(1,6*i-4); 143 | P.pd0 = -Xplot2(1,6*i-3); 144 | 145 | % P.pn0 = 0; % initial North position 146 | % P.pe0 = 0; % initial East position 147 | % P.pd0 = 0; % initial Down position (negative altitude) 148 | 149 | 150 | P.u0 = x_trim(4); % initial velocity along body x-axis 151 | P.v0 = x_trim(5); % initial velocity along body y-axis 152 | P.w0 = x_trim(6); % initial velocity along body z-axis 153 | P.phi0 = x_trim(7); % initial roll angle 154 | P.theta0 = x_trim(8); % initial pitch angle 155 | P.psi0 = x_trim(9); % initial yaw angle 156 | P.p0 = x_trim(10); % initial body frame roll rate 157 | P.q0 = x_trim(11); % initial body frame pitch rate 158 | P.r0 = x_trim(12); % initial body frame yaw rate 159 | 160 | % compute different transfer functions 161 | [T_phi_delta_a,T_chi_phi,T_theta_delta_e,T_h_theta,T_h_Va,T_Va_delta_t,T_Va_theta,T_v_delta_r]... 162 | = compute_tf_model(x_trim,u_trim,P); 163 | 164 | % linearize the equations of motion around trim conditions 165 | [A_lon, B_lon, A_lat, B_lat] = compute_ss_model('mavsim_trim',x_trim,u_trim); 166 | 167 | eig_lon = eig(A_lon); 168 | eig_lat = eig(A_lat); 169 | 170 | compute_gains; 171 | 172 | % longitudinal state-machine parameters 173 | P.altitude_take_off_zone = 50; 174 | P.altitude_hold_zone = 10; 175 | 176 | % sensor parameters(chapter 7) 177 | P.sigma_gyro = 0.13; 178 | P.sigma_accel = 0.0025; 179 | P.beta_abs_pres = 0.125; 180 | P.sigma_abs_pres = 0.01; 181 | P.beta_diff_pres = 0.02; 182 | P.sigma_diff_pres = 0.002; 183 | 184 | P.Ts_gps = 1; 185 | P.k_gps = 1/1000; 186 | P.sigma_gps_n = 0.21; 187 | P.sigma_gps_e = 0.21; 188 | P.sigma_gps_altitude = 0.4; 189 | P.sigma_gps_V_g = 0.05; 190 | 191 | % filter parameters(chapter 8) 192 | P.alpha_lpf_gyro = 0.1; 193 | P.alpha_lpf_static_pres = 0.1; 194 | P.alpha_lpf_diff_pres = 0.1; 195 | 196 | P.bias_gyro_x = 0; % x-gyro bias 197 | P.bias_gyro_y = 0; % y-gyro bias 198 | P.bias_gyro_z = 0; % z-gyro bias 199 | 200 | % autopilot guidance model coefficients(chapter 9) 201 | wn_h = 1.5; 202 | zeta_h = 0.707; 203 | P.b_hdot = 2 * zeta_h * wn_h; 204 | P.b_h = wn_h^2; 205 | 206 | wn_chi = 0.7; 207 | zeta_chi = 0.707; 208 | P.b_chidot = 2 * zeta_chi * wn_chi; 209 | P.b_chi = wn_chi^2; 210 | 211 | P.b_Va = 10; 212 | P.gamma_max = 45*pi/180; 213 | 214 | % chapter 10 parameters 215 | P.k_path = 0.05; 216 | P.chi_inf = 40*pi/180; 217 | P.k_orbit = 0.01; 218 | 219 | % chapter 11 - path manager 220 | % number of waypoints in data structure 221 | P.size_waypoint_array = 100; 222 | P.R_min = P.Va0^2/P.gravity/tan(P.roll_max); 223 | 224 | % create random city map 225 | city_width = 2000; % the city is of size (width)x(width) 226 | building_height = 300; % maximum height of buildings 227 | %building_height = 1; % maximum height of buildings (for camera) 228 | num_blocks = 5; % number of blocks in city 229 | street_width = .8; % percent of block that is street. 230 | P.h0 = 100; 231 | P.pd0 = -P.h0; % initial height of MAV 232 | P.map = createWorld(city_width, building_height, num_blocks, street_width); 233 | -------------------------------------------------------------------------------- /formation-code/uavShow/path_follow1.m: -------------------------------------------------------------------------------- 1 | % path follow 2 | % - follow straight line path or orbit 3 | % 4 | % Modified: 5 | % 3/25/2010 - RB 6 | % 6/5/2010 - RB 7 | % 11/08/2010 - RB 8 | % 14/11/2014 - RWB 9 | % 10 | % input is: 11 | % flag - if flag==1, follow waypoint path 12 | % if flag==2, follow orbit 13 | % 14 | % Va^d - desired airspeed 15 | % r - inertial position of start of waypoint path 16 | % q - unit vector that defines inertial direction of waypoint path 17 | % c - center of orbit 18 | % rho - radius of orbit 19 | % lambda - direction of orbit (+1 for CW, -1 for CCW) 20 | % xhat - estimated MAV states (pn, pe, pd, Va, alpha, beta, phi, theta, chi, p, q, r, Vg, wn, we, psi) 21 | % 22 | % output is: 23 | % Va_c - airspeed command 24 | % h_c - altitude command 25 | % chi_c - heading command 26 | % phi_ff - feed forward roll command 27 | % 28 | function out = path_follow(in,P) 29 | 30 | NN = 0; 31 | flag = in(1+NN); 32 | Va_d = in(2+NN); 33 | r_path = [in(3+NN); in(4+NN); in(5+NN)]; 34 | q_path = [in(6+NN); in(7+NN); in(8+NN)]; 35 | c_orbit = [in(9+NN); in(10+NN); in(11+NN)]; 36 | rho_orbit = in(12+NN); 37 | lam_orbit = in(13+NN); 38 | NN = NN + 13; 39 | pn = in(1+NN); 40 | pe = in(2+NN); 41 | h = in(3+NN); 42 | Va = in(4+NN); 43 | % alpha = in(5+NN); 44 | % beta = in(6+NN); 45 | phi = in(7+NN); 46 | theta = in(8+NN); 47 | chi = in(9+NN); 48 | % p = in(10+NN); 49 | % q = in(11+NN); 50 | r = in(12+NN); 51 | % Vg = in(13+NN); 52 | % wn = in(14+NN); 53 | % we = in(15+NN); 54 | % psi = in(16+NN); 55 | NN = NN + 16; 56 | t = in(1+NN); 57 | 58 | switch flag, 59 | case 1, % follow straight line path specified by r and q 60 | 61 | e_p = [pn; pe; -h] - r_path; 62 | n = cross(q_path, [0; 0; 1]) / norm(cross(q_path, [0; 0; 1])); 63 | s = e_p - dot(e_p, n)*n; 64 | h_c = -r_path(3) - sqrt(s(1)^2 + s(2)^2) * q_path(3) / sqrt(q_path(1)^2+q_path(2)^2); 65 | chi_q = atan2(q_path(2), q_path(1)); 66 | while chi_q-chi < -pi 67 | chi_q = chi_q + 2*pi; 68 | end 69 | while chi_q-chi > pi 70 | chi_q = chi_q - 2*pi; 71 | end 72 | e_py = -sin(chi_q)*(pn-r_path(1)) + cos(chi_q)*(pe-r_path(2)); 73 | 74 | chi_c = chi_q - P.chi_inf*2/pi*atan(P.k_path*e_py); 75 | 76 | phi_ff = 0; 77 | 78 | case 2, % follow orbit specified by c, rho, lam 79 | h_c = -c_orbit(3); 80 | d = sqrt((pn-c_orbit(1))^2 + (pe-c_orbit(2))^2); 81 | barphi = atan2(pe-c_orbit(2), pn-c_orbit(1)); 82 | while barphi-chi < -pi 83 | barphi = barphi + 2*pi; 84 | end 85 | while barphi-chi > pi 86 | barphi = barphi - 2*pi; 87 | end 88 | 89 | chi_c = barphi + lam_orbit*(pi/2 + atan(P.k_orbit*(d-rho_orbit))); 90 | phi_ff = 0; 91 | end 92 | 93 | % command airspeed equal to desired airspeed 94 | Va_c = Va_d; 95 | 96 | % create output 97 | out = [Va_c; h_c; chi_c; phi_ff]; 98 | end 99 | 100 | 101 | -------------------------------------------------------------------------------- /formation-code/uavShow/path_manager1.m: -------------------------------------------------------------------------------- 1 | % path manager 2 | % 3 | % Modified: 4 | % - 3/25/2010 - RWB 5 | % - 4/7/2010 - RWB 6 | % 7 | % input is: 8 | % num_waypoints - number of waypoint configurations 9 | % waypoints - an array of dimension 5 by P.size_waypoint_array. 10 | % - the first num_waypoints rows define waypoint 11 | % configurations 12 | % - format for each waypoint configuration: 13 | % [wn, we, wd, chi_d, Va_d] 14 | % where the (wn, we, wd) is the NED position of the 15 | % waypoint, chi_d is the desired course at the waypoint, 16 | % and Va_d is the desired airspeed along the path. If 17 | % abs(chi_d)<2*pi then Dubins paths will be followed 18 | % between waypoint configurations. If abs(chi_d)>=2*pi 19 | % then straight-line paths (with fillets) are commanded. 20 | % 21 | % output is: 22 | % flag - if flag==1, follow waypoint path 23 | % if flag==2, follow orbit 24 | % Va_d - desired airspeed 25 | % r - inertial position of start of waypoint path 26 | % q - unit vector that defines inertial direction of waypoint path 27 | % c - center of orbit 28 | % rho - radius of orbit 29 | % lambda = direction of orbit (+1 for CW, -1 for CCW) 30 | % 31 | function out = path_manager1(in,P) 32 | 33 | persistent start_of_simulation 34 | 35 | t = in(end); 36 | if t==0 37 | start_of_simulation = 1; 38 | end 39 | 40 | NN = 0; 41 | num_waypoints = in(1+NN); 42 | if num_waypoints==0 % start of simulation 43 | flag = 1; % following straight line path 44 | Va_d = P.Va0; % desired airspeed along waypoint path 45 | NN = NN + 1 + 5*P.size_waypoint_array; 46 | pn = in(1+NN); 47 | pe = in(2+NN); 48 | h = in(3+NN); 49 | chi = in(9+NN); 50 | r = [pn; pe; -h]; 51 | q = [cos(chi); sin(chi); 0]; 52 | c = [0; 0; 0]; 53 | rho = 0; 54 | lambda = 0; 55 | state = in(1+NN:16+NN); 56 | flag_need_new_waypoints = 1; 57 | out = [flag; Va_d; r; q; c; rho; lambda; state; flag_need_new_waypoints]; 58 | else 59 | waypoints = reshape(in(2+NN:5*P.size_waypoint_array+1+NN),5,P.size_waypoint_array); 60 | 61 | if abs(waypoints(4,1))>=2*pi 62 | out = path_manager_line(in,P,start_of_simulation); % follows straight-lines and switches at waypoints 63 | % out = path_manager_fillet(in,P,start_of_simulation); % smooths through waypoints with fillets 64 | start_of_simulation=0; 65 | else 66 | out = path_manager_dubins(in,P,start_of_simulation); % follows Dubins paths between waypoint configurations 67 | start_of_simulation=0; 68 | end 69 | end 70 | 71 | end -------------------------------------------------------------------------------- /formation-code/uavShow/path_manager_fillet.m: -------------------------------------------------------------------------------- 1 | % path_manager_fillet 2 | % - follow lines between waypoints. Smooth transition with fillets 3 | % 4 | % 5 | % input is: 6 | % num_waypoints - number of waypoint configurations 7 | % waypoints - an array of dimension 5 by P.size_waypoint_array. 8 | % - the first num_waypoints rows define waypoint 9 | % configurations 10 | % - format for each waypoint configuration: 11 | % [wn, we, wd, dont_care, Va_d] 12 | % where the (wn, we, wd) is the NED position of the 13 | % waypoint, and Va_d is the desired airspeed along the 14 | % path. 15 | % 16 | % output is: 17 | % flag - if flag==1, follow waypoint path 18 | % if flag==2, follow orbit 19 | % 20 | % Va^d - desired airspeed 21 | % r - inertial position of start of waypoint path 22 | % q - unit vector that defines inertial direction of waypoint path 23 | % c - center of orbit 24 | % rho - radius of orbit 25 | % lambda = direction of orbit (+1 for CW, -1 for CCW) 26 | % 27 | function out = path_manager_fillet(in,P,start_of_simulation) 28 | 29 | NN = 0; 30 | num_waypoints = in(1+NN); 31 | waypoints = reshape(in(2+NN:5*P.size_waypoint_array+1+NN),5,P.size_waypoint_array); 32 | NN = NN + 1 + 5*P.size_waypoint_array; 33 | pn = in(1+NN); 34 | pe = in(2+NN); 35 | h = in(3+NN); 36 | % Va = in(4+NN); 37 | % alpha = in(5+NN); 38 | % beta = in(6+NN); 39 | % phi = in(7+NN); 40 | % theta = in(8+NN); 41 | % chi = in(9+NN); 42 | % p = in(10+NN); 43 | % q = in(11+NN); 44 | % r = in(12+NN); 45 | % Vg = in(13+NN); 46 | % wn = in(14+NN); 47 | % we = in(15+NN); 48 | % psi = in(16+NN); 49 | state = in(1+NN:16+NN); 50 | NN = NN + 16; 51 | t = in(1+NN); 52 | 53 | p = [pn; pe; -h]; 54 | 55 | 56 | persistent waypoints_old % stored copy of old waypoints 57 | persistent ptr_a % waypoint pointer 58 | persistent state_transition % state of transition state machine 59 | persistent flag_need_new_waypoints % flag that request new waypoints from path planner 60 | 61 | 62 | if start_of_simulation || isempty(waypoints_old), 63 | waypoints_old = zeros(5,P.size_waypoint_array); 64 | flag_need_new_waypoints = 0; 65 | 66 | end 67 | 68 | persistent ptr_b 69 | persistent ptr_c 70 | 71 | % if the waypoints have changed, update the waypoint pointer 72 | if min(min(waypoints==waypoints_old))==0, 73 | ptr_a = 1; 74 | ptr_b = 2; 75 | ptr_c = 3; 76 | waypoints_old = waypoints; 77 | state_transition = 1; 78 | flag_need_new_waypoints = 0; 79 | end 80 | 81 | % define current and next two waypoints 82 | r = waypoints(1:3,ptr_a); % previous waypoint position (a waypoint that MAV is flying from) 83 | w = waypoints(1:3,ptr_b); % current waypoint position (a waypoint that MAV is flying to) 84 | w_next = waypoints(1:3,ptr_c); % next waypoint position 85 | q_prev = (w-r)/norm(w-r); 86 | q_next = (w_next-w)/norm(w_next-w); 87 | barrho = acos(-q_prev'*q_next); 88 | 89 | % define transition state machine 90 | switch state_transition, 91 | case 1, % follow straight line from wpp_a to wpp_b 92 | flag = 1; % following straight line path 93 | Va_d = waypoints(5,ptr_a); % desired airspeed along waypoint path 94 | r = r; 95 | q = q_prev; 96 | c = [0; 0; 0]; 97 | rho = 1; 98 | lambda = 1; 99 | z = w - (P.R_min/tan(barrho/2))*q_prev; 100 | if (p-z)'*q_prev >= 0 101 | state_transition = 2; 102 | end 103 | 104 | case 2, % follow orbit from wpp_a-wpp_b to wpp_b-wpp_c 105 | flag = 2; % following orbit 106 | Va_d = waypoints(5,ptr_a); % desired airspeed along waypoint path 107 | r = r; 108 | q = q_prev; 109 | q_next = q_next; 110 | c = w - (P.R_min/sin(barrho/2))*(q_prev-q_next)/norm(q_prev-q_next); 111 | rho = P.R_min; 112 | lambda = sign(q_prev(1)*q_next(2) - q_prev(2)*q_next(1)); 113 | z = w + (P.R_min/tan(barrho/2))*q_next; 114 | if (p-z)'*q_next >= 0 115 | state_transition = 1; 116 | ptr_a = ptr_a + 1; 117 | ptr_b = ptr_b + 1; 118 | ptr_c = ptr_c + 1; 119 | end 120 | if ptr_c == num_waypoints+1 121 | ptr_c = 1; 122 | end 123 | if ptr_b == num_waypoints+1 124 | ptr_b = 1; 125 | end 126 | if ptr_a == num_waypoints+1 127 | ptr_a = 1; 128 | end 129 | end 130 | 131 | out = [flag; Va_d; r; q; c; rho; lambda; state; flag_need_new_waypoints]; 132 | 133 | end -------------------------------------------------------------------------------- /formation-code/uavShow/path_manager_line.m: -------------------------------------------------------------------------------- 1 | % path_manager_line 2 | % - follow lines between waypoints. 3 | % 4 | % input is: 5 | % num_waypoints - number of waypoint configurations 6 | % waypoints - an array of dimension 5 by P.size_waypoint_array. 7 | % - the first num_waypoints rows define waypoint 8 | % configurations 9 | % - format for each waypoint configuration: 10 | % [wn, we, wd, dont_care, Va_d] 11 | % where the (wn, we, wd) is the NED position of the 12 | % waypoint, and Va_d is the desired airspeed along the 13 | % path. 14 | % 15 | % output is: 16 | % flag - if flag==1, follow waypoint path 17 | % if flag==2, follow orbit 18 | % 19 | % Va^d - desired airspeed 20 | % r - inertial position of start of waypoint path 21 | % q - unit vector that defines inertial direction of waypoint path 22 | % c - center of orbit 23 | % rho - radius of orbit 24 | % lambda = direction of orbit (+1 for CW, -1 for CCW) 25 | % 26 | function out = path_manager_line(in,P,start_of_simulation) 27 | 28 | NN = 0; 29 | num_waypoints = in(1+NN); 30 | waypoints = reshape(in(2+NN:5*P.size_waypoint_array+1+NN),5,P.size_waypoint_array); 31 | NN = NN + 1 + 5*P.size_waypoint_array; 32 | pn = in(1+NN); 33 | pe = in(2+NN); 34 | h = in(3+NN); 35 | % Va = in(4+NN); 36 | % alpha = in(5+NN); 37 | % beta = in(6+NN); 38 | % phi = in(7+NN); 39 | % theta = in(8+NN); 40 | % chi = in(9+NN); 41 | % p = in(10+NN); 42 | % q = in(11+NN); 43 | % r = in(12+NN); 44 | % Vg = in(13+NN); 45 | % wn = in(14+NN); 46 | % we = in(15+NN); 47 | % psi = in(16+NN); 48 | state = in(1+NN:16+NN); 49 | NN = NN + 16; 50 | t = in(1+NN); 51 | 52 | 53 | p = [pn; pe; -h]; 54 | 55 | persistent waypoints_old % stored copy of old waypoints 56 | persistent ptr_a % waypoint pointer 57 | persistent flag_need_new_waypoints % flag that request new waypoints from path planner 58 | 59 | 60 | if start_of_simulation || isempty(waypoints_old), 61 | waypoints_old = zeros(5,P.size_waypoint_array); 62 | flag_need_new_waypoints = 0; 63 | 64 | end 65 | 66 | persistent ptr_b 67 | persistent ptr_c 68 | 69 | % if the waypoints have changed, update the waypoint pointer 70 | if min(min(waypoints==waypoints_old))==0, 71 | ptr_a = 1; 72 | ptr_b = 2; 73 | ptr_c = 3; 74 | waypoints_old = waypoints; 75 | flag_need_new_waypoints = 0; 76 | end 77 | 78 | % algorithm 5 in UAV book (p. 189) 79 | r = waypoints(1:3,ptr_a); % previous waypoint position (a waypoint that MAV is flying from) 80 | w = waypoints(1:3,ptr_b); % current waypoint position (a waypoint that MAV is flying to) 81 | w_next = waypoints(1:3,ptr_c); % next waypoint position 82 | q_prev = (w-r)/norm(w-r); 83 | q = (w_next-w)/norm(w_next-w); 84 | n = (q_prev+q)/norm(q_prev+q); 85 | 86 | % construct output for path follower 87 | flag = 1; % following straight line path 88 | Va_d = waypoints(5,ptr_a); % desired airspeed along waypoint path 89 | r = r; 90 | q = q_prev; 91 | c = [0; 0; 0]; 92 | rho = 1; 93 | lambda = 1; 94 | 95 | out = [flag; Va_d; r; q; c; rho; lambda; state; flag_need_new_waypoints]; 96 | 97 | % determine if next waypoint path has been reached, and rotate ptrs if 98 | % necessary 99 | if (p-w)'*n >= 0 100 | ptr_a = ptr_a + 1; 101 | ptr_b = ptr_b + 1; 102 | ptr_c = ptr_c + 1; 103 | end 104 | if ptr_c == num_waypoints+1 105 | ptr_c = 1; 106 | end 107 | if ptr_b == num_waypoints+1 108 | ptr_b = 1; 109 | end 110 | if ptr_a == num_waypoints+1 111 | ptr_a = 1; 112 | end 113 | 114 | end -------------------------------------------------------------------------------- /formation-code/uavShow/path_planner1.m: -------------------------------------------------------------------------------- 1 | % path planner 2 | % 3 | % Modified: 4 | % - 4/06/2010 - RWB 5 | % 6 | % output is a vector containing P.num_waypoints waypoints 7 | % 8 | % input is the map of the environment and desired goal location 9 | function out = path_planner1(in,P,map) 10 | load 'uavW.mat' 11 | i1 = uavW; 12 | clear uavW; 13 | 14 | NN = 0; 15 | pn = in(1+NN); 16 | pe = in(2+NN); 17 | h = in(3+NN); 18 | % Va = in(4+NN); 19 | % alpha = in(5+NN); 20 | % beta = in(6+NN); 21 | % phi = in(7+NN); 22 | % theta = in(8+NN); 23 | chi = in(9+NN); 24 | % p = in(10+NN); 25 | % q = in(11+NN); 26 | % r = in(12+NN); 27 | % Vg = in(13+NN); 28 | % wn = in(14+NN); 29 | % we = in(15+NN); 30 | % psi = in(16+NN); 31 | flag_new_waypoints = in(17+NN); 32 | NN = NN + 17; 33 | t = in(1+NN); 34 | 35 | 36 | persistent num_waypoints 37 | persistent wpp 38 | 39 | 40 | if (t==0) || flag_new_waypoints 41 | % format for each point is [pn, pe, pd, chi, Va^d] where the position 42 | % of the waypoint is (pn, pe, pd), the desired course at the waypoint 43 | % is chi, and the desired airspeed between waypoints is Va 44 | % if chi!=-9999, then Dubins paths will be used between waypoints. 45 | switch 1 46 | case 1 47 | 48 | [num_waypoints , wpp] = getWpp(P,i1); 49 | 50 | case 2 % Dubins 51 | num_waypoints = 4; 52 | wpp = [... 53 | 0, 0, -100, 0, P.Va0;... 54 | 300, 0, -100, 45*pi/180, P.Va0;... 55 | 0, 300, -100, 45*pi/180, P.Va0;... 56 | 300, 300, -100, -135*pi/180, P.Va0;... 57 | ]; 58 | case 3 % path through city using Dubin's paths 59 | % current configuration 60 | wpp_start = [pn, pe, -h, chi, P.Va0]; 61 | % desired end waypoint 62 | if norm([pn; pe; -h]-[map.width; map.width; -h])= downAtNE(map, X, Y), 109 | collision_flag = 1; 110 | end 111 | % check to see if outside of world 112 | if (X>map.width) | (X<0) | (Y>map.width) | (Y<0), 113 | collision_flag = 1; 114 | end 115 | end 116 | end 117 | 118 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 119 | %% downAtNE 120 | %% find the map down coordinate at a specified (n,e) location 121 | function down = downAtNE(map, n, e) 122 | 123 | [d_n,idx_n] = min(abs(n - map.buildings_n)); 124 | [d_e,idx_e] = min(abs(e - map.buildings_e)); 125 | 126 | if (d_n<=map.BuildingWidth) & (d_e<=map.BuildingWidth), 127 | down = -map.heights(idx_e,idx_n); 128 | else 129 | down = 0; 130 | end 131 | end 132 | 133 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 134 | %% findReturn 135 | %% compute the return value at a particular location 136 | function return_value = findReturn(pn,pe,return_map,map); 137 | 138 | [pn_max,pe_max] = size(return_map); 139 | fn = pn_max*pn/map.width; 140 | fn = min(pn_max,round(fn)); 141 | fn = max(1,fn); 142 | fe = pe_max*pe/map.width; 143 | fe = min(pe_max,round(fe)); 144 | fe = max(1,fe); 145 | return_value = return_map(fn,fe); 146 | end 147 | 148 | 149 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 150 | %% findMaxReturnPath 151 | %% find the maximum return path in the tree 152 | function path = findMaxReturnPath(tree) 153 | 154 | % find node with max return 155 | [tmp,idx] = max(tree(:,5)); 156 | 157 | % construct path with maximum return 158 | path = tree(idx,:); 159 | parent_node = tree(idx,6); 160 | %while parent_node>1, 161 | while parent_node>1, 162 | path = [tree(parent_node,:); path]; 163 | parent_node = tree(parent_node,6); 164 | end 165 | end 166 | 167 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 168 | %% updateReturnMap 169 | %% update the return map to indicate where MAV has been 170 | function new_return_map = updateReturnMap(path,return_map,map) 171 | 172 | new_return_map = return_map; 173 | for i=1:size(path,1), 174 | pn = path(i,1); 175 | pe = path(i,2); 176 | [pn_max,pe_max] = size(return_map); 177 | fn = pn_max*pn/map.width; 178 | fn = min(pn_max,round(fn)); 179 | fn = max(1,fn); 180 | fe = pe_max*pe/map.width; 181 | fe = min(pe_max,round(fe)); 182 | fe = max(1,fe); 183 | 184 | new_return_map(fn,fe) = return_map(fn,fe) - 50; 185 | end 186 | end 187 | 188 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 189 | %% plotReturnMap 190 | %% plot the return map 191 | function plotReturnMap(return_map) 192 | 193 | figure(2), clf 194 | mesh(return_map) 195 | end 196 | 197 | 198 | -------------------------------------------------------------------------------- /formation-code/uavShow/planCoverRRTDubins.m: -------------------------------------------------------------------------------- 1 | %% planRRT 2 | %% - basic coverage algorithm 3 | %% 4 | %% Last Modified - 11/15/2010 - R. Beard 5 | 6 | function path=planCoverRRTDubins(wpp_start, R, map) 7 | 8 | % desired down position is down position of start node 9 | pd = wpp_start(3); 10 | 11 | % specify start node from wpp_start 12 | start_node = [wpp_start(1), wpp_start(2), pd, 0, 0, 0]; 13 | % format is [N, E, D, chi, cost, parent] 14 | 15 | 16 | % return map 17 | returnMapSize = 30; % this is a critical parameter! 18 | return_map = 50*ones(returnMapSize,returnMapSize)+ rand(returnMapSize,returnMapSize); 19 | plotReturnMap(return_map), %pause 20 | 21 | % construct search path by doing N search cycles 22 | SEARCH_CYCLES = 100; % number of search cycles 23 | 24 | % look ahead tree parameters 25 | L = 2*R+1; % segment Length 26 | vartheta = pi/4; % search angle 27 | depth = 5; % depth of look ahead tree 28 | 29 | % initialize path and tree 30 | path = start_node; 31 | for i=1:SEARCH_CYCLES, 32 | tree = extendTree(path(end,:),L,vartheta,depth,map,return_map,pd); 33 | next_path = findMaxReturnPath(tree); 34 | path = [path; next_path(1,:)]; 35 | % update the return map 36 | return_map = updateReturnMap(next_path(1,:),return_map,map); 37 | plotReturnMap(return_map), %pause 38 | % set the end of the path as the root of the tree 39 | end 40 | 41 | % remove path segments where there is no turn 42 | path_=path; 43 | path = path(1,:); 44 | for i=2:size(path_,1), 45 | if path_(i,4)~=path_(i-1,4); 46 | path = [path; path_(i,:)]; 47 | end 48 | end 49 | 50 | % % specify that these are straight-line paths. 51 | % for i=1:size(path,1), 52 | % path(i,4)=-9999; 53 | % end 54 | 55 | end 56 | 57 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 58 | %% extendTree 59 | %% extend tree by randomly selecting point and growing tree toward that 60 | %% point 61 | function tree = extendTree(start_node,L,vartheta,depth,map,return_map,pd) 62 | 63 | tree_ = [start_node, 0]; % the last variable marks node as expanded 64 | 65 | % extend tree initially 66 | for d = 1:depth, 67 | newnodes = []; 68 | for j=1:size(tree_,1), 69 | if tree_(j,7)~=1, % process unexpanded nodes 70 | for i=1:3, 71 | if i==1, theta = tree_(j,4)-vartheta; 72 | elseif i==2, theta = tree_(j,4); 73 | elseif i==3, theta = tree_(j,4)+vartheta; 74 | end 75 | newnode_ = [... 76 | tree_(j,1)+L*cos(theta),... 77 | tree_(j,2)+L*sin(theta),... 78 | tree_(j,3),... 79 | theta,... 80 | 0,... 81 | j,... 82 | 0,... 83 | ]; 84 | if collision(tree_(j,:), newnode_, map)==0, 85 | newnode_(5) = tree_(j,5)... 86 | +findReturn(newnode_(1),newnode_(2),return_map,map); 87 | newnodes = [newnodes; newnode_]; 88 | end 89 | end 90 | tree_(j,7)=1; % mark as expanded 91 | end 92 | end 93 | tree_ = [tree_; newnodes]; 94 | end 95 | tree = tree_(:,1:6); 96 | end 97 | 98 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 99 | %% collision 100 | %% check to see if a node is in collsion with obstacles 101 | function collision_flag = collision(start_node, end_node, map) 102 | collision_flag = 0; 103 | sigma = 0:.1:1; % define where collisions will be checked 104 | for i=1:length(sigma), 105 | X = (1-sigma(i))*start_node(1) + sigma(i)*end_node(1); 106 | Y = (1-sigma(i))*start_node(2) + sigma(i)*end_node(2); 107 | Z = (1-sigma(i))*start_node(3) + sigma(i)*end_node(3); 108 | if Z >= downAtNE(map, X, Y), 109 | collision_flag = 1; 110 | end 111 | % check to see if outside of world 112 | if (X>map.width) | (X<0) | (Y>map.width) | (Y<0), 113 | collision_flag = 1; 114 | end 115 | end 116 | end 117 | 118 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 119 | %% downAtNE 120 | %% find the map down coordinate at a specified (n,e) location 121 | function down = downAtNE(map, n, e) 122 | 123 | [d_n,idx_n] = min(abs(n - map.buildings_n)); 124 | [d_e,idx_e] = min(abs(e - map.buildings_e)); 125 | 126 | if (d_n<=map.BuildingWidth) & (d_e<=map.BuildingWidth), 127 | down = -map.heights(idx_e,idx_n); 128 | else 129 | down = 0; 130 | end 131 | end 132 | 133 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 134 | %% findReturn 135 | %% compute the return value at a particular location 136 | function return_value = findReturn(pn,pe,return_map,map); 137 | 138 | [pn_max,pe_max] = size(return_map); 139 | fn = pn_max*pn/map.width; 140 | fn = min(pn_max,round(fn)); 141 | fn = max(1,fn); 142 | fe = pe_max*pe/map.width; 143 | fe = min(pe_max,round(fe)); 144 | fe = max(1,fe); 145 | return_value = return_map(fn,fe); 146 | end 147 | 148 | 149 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 150 | %% findMaxReturnPath 151 | %% find the maximum return path in the tree 152 | function path = findMaxReturnPath(tree) 153 | 154 | % find node with max return 155 | [tmp,idx] = max(tree(:,5)); 156 | 157 | % construct path with maximum return 158 | path = tree(idx,:); 159 | parent_node = tree(idx,6); 160 | %while parent_node>1, 161 | while parent_node>1, 162 | path = [tree(parent_node,:); path]; 163 | parent_node = tree(parent_node,6); 164 | end 165 | end 166 | 167 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 168 | %% updateReturnMap 169 | %% update the return map to indicate where MAV has been 170 | function new_return_map = updateReturnMap(path,return_map,map) 171 | 172 | new_return_map = return_map; 173 | for i=1:size(path,1), 174 | pn = path(i,1); 175 | pe = path(i,2); 176 | [pn_max,pe_max] = size(return_map); 177 | fn = pn_max*pn/map.width; 178 | fn = min(pn_max,round(fn)); 179 | fn = max(1,fn); 180 | fe = pe_max*pe/map.width; 181 | fe = min(pe_max,round(fe)); 182 | fe = max(1,fe); 183 | 184 | new_return_map(fn,fe) = return_map(fn,fe) - 50; 185 | end 186 | end 187 | 188 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 189 | %% plotReturnMap 190 | %% plot the return map 191 | function plotReturnMap(return_map) 192 | 193 | figure(2), clf 194 | mesh(return_map) 195 | end 196 | 197 | 198 | -------------------------------------------------------------------------------- /formation-code/uavShow/plotxyz.m: -------------------------------------------------------------------------------- 1 | load uav1u 2 | a1 = A; 3 | load uav2u 4 | a2 = A; 5 | load uav3u 6 | a3 = A; 7 | 8 | n1 = 3890; 9 | plot3(a1(1:n1,2), a1(1:n1,3),a1(1:n1,4)); hold on; 10 | plot3(a2(1:n1,2), a2(1:n1,3),a2(1:n1,4)); hold on; 11 | plot3(a3(1:n1,2), a3(1:n1,3),a3(1:n1,4)); hold on; 12 | axis equal -------------------------------------------------------------------------------- /formation-code/uavShow/predator.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/predator.jpg -------------------------------------------------------------------------------- /formation-code/uavShow/readme.txt: -------------------------------------------------------------------------------- 1 | 单机数据的生成 2 | 3 | 需要生成 path waypoints x 根据i序号给数据带上标记 4 | 改文件 param_chap1.m i = 2 5 | path_planner1.m i1 = 2 6 | 直接运行New mavsim chap12 7 | 工作空间会得到path waypoints x 8 | 根据相应的序号更改名字并保存 -------------------------------------------------------------------------------- /formation-code/uavShow/sensors.m: -------------------------------------------------------------------------------- 1 | % sensors.m 2 | % Compute the output of rate gyros, accelerometers, and pressure sensors 3 | % 4 | % Revised: 5 | % 3/5/2010 - RB 6 | % 5/14/2010 - RB 7 | 8 | function y = sensors(uu, P) 9 | 10 | % relabel the inputs 11 | % pn = uu(1); 12 | % pe = uu(2); 13 | pd = uu(3); 14 | % u = uu(4); 15 | % v = uu(5); 16 | % w = uu(6); 17 | phi = uu(7); 18 | theta = uu(8); 19 | % psi = uu(9); 20 | p = uu(10); 21 | q = uu(11); 22 | r = uu(12); 23 | F_x = uu(13); 24 | F_y = uu(14); 25 | F_z = uu(15); 26 | % M_l = uu(16); 27 | % M_m = uu(17); 28 | % M_n = uu(18); 29 | Va = uu(19); 30 | % alpha = uu(20); 31 | % beta = uu(21); 32 | % wn = uu(22); 33 | % we = uu(23); 34 | % wd = uu(24); 35 | 36 | % simulate rate gyros (units are rad/sec) 37 | y_gyro_x = p + P.sigma_gyro * randn; 38 | y_gyro_y = q + P.sigma_gyro * randn; 39 | y_gyro_z = r + P.sigma_gyro * randn; 40 | 41 | % simulate accelerometers (units of g) 42 | y_accel_x = F_x / P.mass / P.gravity + sin(theta) + P.sigma_accel * randn; 43 | y_accel_y = F_y / P.mass / P.gravity - cos(theta) * sin(phi) + P.sigma_accel * randn; 44 | y_accel_z = F_z / P.mass / P.gravity - cos(theta) * cos(phi) + P.sigma_accel * randn; 45 | 46 | % simulate pressure sensors 47 | y_static_pres = P.rho * P.gravity * (-pd) + P.beta_abs_pres + P.sigma_abs_pres * randn; 48 | y_diff_pres = P.rho * Va^2 / 2 + P.beta_diff_pres + P.sigma_diff_pres * randn; 49 | 50 | % construct total output 51 | y = [... 52 | y_gyro_x;... 53 | y_gyro_y;... 54 | y_gyro_z;... 55 | y_accel_x;... 56 | y_accel_y;... 57 | y_accel_z;... 58 | y_static_pres;... 59 | y_diff_pres;... 60 | ]; 61 | 62 | end 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /formation-code/uavShow/slprj/sim/varcache/New_mavsim_chap12/checksumOfCache.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/slprj/sim/varcache/New_mavsim_chap12/checksumOfCache.mat -------------------------------------------------------------------------------- /formation-code/uavShow/slprj/sim/varcache/New_mavsim_chap12/tmwinternal/simulink_cache.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | WuRqSDBRbmd5z/EQMMA63w== 5 | 6 | -------------------------------------------------------------------------------- /formation-code/uavShow/slprj/sim/varcache/New_mavsim_chap12/varInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/slprj/sim/varcache/New_mavsim_chap12/varInfo.mat -------------------------------------------------------------------------------- /formation-code/uavShow/slprj/sim/varcache/mavsim_chap12/checksumOfCache.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/slprj/sim/varcache/mavsim_chap12/checksumOfCache.mat -------------------------------------------------------------------------------- /formation-code/uavShow/slprj/sim/varcache/mavsim_chap12/tmwinternal/simulink_cache.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | CANcFgG0TkTtRz7fpypkCQ== 5 | 6 | -------------------------------------------------------------------------------- /formation-code/uavShow/slprj/sim/varcache/mavsim_chap12/varInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/slprj/sim/varcache/mavsim_chap12/varInfo.mat -------------------------------------------------------------------------------- /formation-code/uavShow/slprj/sim/varcache/mavsim_chap12_model/checksumOfCache.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/slprj/sim/varcache/mavsim_chap12_model/checksumOfCache.mat -------------------------------------------------------------------------------- /formation-code/uavShow/slprj/sim/varcache/mavsim_chap12_model/tmwinternal/simulink_cache.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | GnEJzjsQWqwygUunqqgD+A== 5 | 6 | -------------------------------------------------------------------------------- /formation-code/uavShow/slprj/sim/varcache/mavsim_chap12_model/varInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/slprj/sim/varcache/mavsim_chap12_model/varInfo.mat -------------------------------------------------------------------------------- /formation-code/uavShow/slprj/sim/varcache/mavsim_show/checksumOfCache.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/slprj/sim/varcache/mavsim_show/checksumOfCache.mat -------------------------------------------------------------------------------- /formation-code/uavShow/slprj/sim/varcache/mavsim_show/tmwinternal/simulink_cache.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | zl9QvmX7MVs6OT5+Femr9Q== 5 | 6 | -------------------------------------------------------------------------------- /formation-code/uavShow/slprj/sim/varcache/mavsim_show/varInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/slprj/sim/varcache/mavsim_show/varInfo.mat -------------------------------------------------------------------------------- /formation-code/uavShow/slprj/sim/varcache/mavsim_show123/checksumOfCache.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/slprj/sim/varcache/mavsim_show123/checksumOfCache.mat -------------------------------------------------------------------------------- /formation-code/uavShow/slprj/sim/varcache/mavsim_show123/tmwinternal/simulink_cache.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 4oMMJTJBzDXI87IW9fGOLw== 5 | 6 | -------------------------------------------------------------------------------- /formation-code/uavShow/slprj/sim/varcache/mavsim_show123/varInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/slprj/sim/varcache/mavsim_show123/varInfo.mat -------------------------------------------------------------------------------- /formation-code/uavShow/slprj/sim/varcache/mavsim_trim/checksumOfCache.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/slprj/sim/varcache/mavsim_trim/checksumOfCache.mat -------------------------------------------------------------------------------- /formation-code/uavShow/slprj/sim/varcache/mavsim_trim/tmwinternal/simulink_cache.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | +Dq/H/Qmlios84tVlwqmuQ== 5 | 6 | -------------------------------------------------------------------------------- /formation-code/uavShow/slprj/sim/varcache/mavsim_trim/varInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/slprj/sim/varcache/mavsim_trim/varInfo.mat -------------------------------------------------------------------------------- /formation-code/uavShow/totxt.m: -------------------------------------------------------------------------------- 1 | load uav3 2 | abs = states.data; 3 | t = 0; 4 | n = size(abs,1); 5 | A = []; 6 | for i = 1:n 7 | a1 = [t abs(i,1) abs(i,2) abs(i,3) abs(i,4) abs(i,8) abs(i,7) abs(i,16)]; 8 | A = [A;a1]; 9 | 10 | t = t+0.01; 11 | end 12 | 13 | n1 = n; 14 | plot3(A(1:n1,2), A(1:n1,3),A(1:n1,4)); 15 | axis equal -------------------------------------------------------------------------------- /formation-code/uavShow/true_states.m: -------------------------------------------------------------------------------- 1 | function xhat = estimate_states(uu,P) 2 | % 3 | % fake state estimation for mavsim 4 | % - this function will be replaced with a state estimator in a later 5 | % chapter. 6 | % 7 | % Outputs are: 8 | % pnhat - estimated North position, 9 | % pehat - estimated East position, 10 | % hhat - estimated altitude, 11 | % Vahat - estimated airspeed, 12 | % alphahat - estimated angle of attack 13 | % betahat - estimated sideslip angle 14 | % phihat - estimated roll angle, 15 | % thetahat - estimated pitch angel, 16 | % chihat - estimated course, 17 | % phat - estimated roll rate, 18 | % qhat - estimated pitch rate, 19 | % rhat - estimated yaw rate, 20 | % Vghat - estimated ground speed, 21 | % wnhat - estimate of North wind, 22 | % wehat - estimate of East wind 23 | % psihat - estimate of heading angle 24 | % bxhat - estimate of x-gyro bias 25 | % byhat - estimate of y-gyro bias 26 | % bzhat - estimate of z-gyro bias 27 | % 28 | % 29 | % Modification History: 30 | % 2/11/2010 - RWB 31 | % 5/14/2010 - RWB 32 | % 33 | 34 | % process inputs 35 | NN = 0; 36 | pn = uu(1+NN); % inertial North position 37 | pe = uu(2+NN); % inertial East position 38 | h = -uu(3+NN); % altitude 39 | % u = uu(4+NN); % inertial velocity along body x-axis 40 | % v = uu(5+NN); % inertial velocity along body y-axis 41 | % w = uu(6+NN); % inertial velocity along body z-axis 42 | phi = uu(7+NN); % roll angle 43 | theta = uu(8+NN); % pitch angle 44 | psi = uu(9+NN); % yaw angle 45 | p = uu(10+NN); % body frame roll rate 46 | q = uu(11+NN); % body frame pitch rate 47 | r = uu(12+NN); % body frame yaw rate 48 | NN = NN+12; 49 | Va = uu(1+NN); % airspeed 50 | alpha = uu(2+NN); % angle of attack 51 | beta = uu(3+NN); % sideslip angle 52 | wn = uu(4+NN); % wind North 53 | we = uu(5+NN); % wind East 54 | % wd = uu(6+NN); % wind down 55 | NN = NN+6; 56 | % t = uu(1+NN); % time 57 | 58 | % estimate states (using real state data) 59 | pnhat = pn; 60 | pehat = pe; 61 | hhat = h; 62 | Vahat = Va; 63 | alphahat = alpha; 64 | betahat = beta; 65 | phihat = phi; 66 | thetahat = theta; 67 | chihat = atan2(Va*sin(psi)+we, Va*cos(psi)+wn); 68 | phat = p; 69 | qhat = q; 70 | rhat = r; 71 | Vghat = sqrt((Va*cos(psi)+wn)^2 + (Va*sin(psi)+we)^2); 72 | wnhat = wn; 73 | wehat = we; 74 | psihat = psi; 75 | % bxhat = P.bias_gyro_x; 76 | % byhat = P.bias_gyro_y; 77 | % bzhat = P.bias_gyro_z; 78 | bxhat = 0; 79 | byhat = 0; 80 | bzhat = 0; 81 | 82 | xhat = [... 83 | pnhat;... 84 | pehat;... 85 | hhat;... 86 | Vahat;... 87 | alphahat;... 88 | betahat;... 89 | phihat;... 90 | thetahat;... 91 | chihat;... 92 | phat;... 93 | qhat;... 94 | rhat;... 95 | Vghat;... 96 | wnhat;... 97 | wehat;... 98 | psihat;... 99 | bxhat;... 100 | byhat;... 101 | bzhat;... 102 | ]; 103 | 104 | end -------------------------------------------------------------------------------- /formation-code/uavShow/tv.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavShow/tv.jpg -------------------------------------------------------------------------------- /formation-code/uavW.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/uavW.mat -------------------------------------------------------------------------------- /formation-code/x1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/x1.mat -------------------------------------------------------------------------------- /formation-code/x2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/x2.mat -------------------------------------------------------------------------------- /formation-code/x3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/x3.mat -------------------------------------------------------------------------------- /formation-code/x4.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/x4.mat -------------------------------------------------------------------------------- /formation-code/x5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/formation-code/x5.mat -------------------------------------------------------------------------------- /gif/1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/gif/1.gif -------------------------------------------------------------------------------- /gif/2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/gif/2.gif -------------------------------------------------------------------------------- /gif/3.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/gif/3.gif -------------------------------------------------------------------------------- /gif/4.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linqingbh/Consensus-based-DMPC/97f15b1a11c8cdc47b978a2ed893c2e8ebb93045/gif/4.gif --------------------------------------------------------------------------------