├── LICENSE ├── PersianReport.pdf ├── Project ├── 1-PID Control Quadcopter │ ├── CreateDesiredTraj.m │ ├── Main.m │ ├── PID_Controller.m │ ├── Parameters.m │ └── RotI2B.m ├── 2-LQR Control Quadcopter │ ├── Main.m │ ├── Parameters.m │ ├── SF_Controller.m │ └── State_Space.m ├── 3-Feedback Linearization Control Quadcopter │ ├── FL_Controller.m │ ├── Main.m │ └── Parameters.m ├── 4-Sliding Mode Control Quadcopter │ ├── Main.m │ ├── Parameters.m │ ├── Quad_Animation.m │ └── SMC_Controller.m ├── 5-Backstepping Control Quadcopter │ ├── BS_Controller.m │ ├── Main.m │ └── Parameters.m ├── 6-MRAC Control Quadcopter │ ├── Main.m │ ├── Parameters.m │ ├── RK4.m │ ├── ReferenceModel.m │ ├── SF_Controller.m │ └── State_Space.m ├── CreateDesiredTrajectory.m ├── MatFiles │ ├── BSC_Data.mat │ ├── Comparison.m │ ├── FLC_Data.mat │ ├── LQR_Data.mat │ ├── MRAC_Data.mat │ ├── MyPlot.m │ ├── PID_Data.mat │ ├── Plotter.m │ ├── SMC_Data.mat │ └── Variable Trajectory │ │ ├── BSC_Data_Varriable_Trajectory.mat │ │ ├── Comparison.m │ │ ├── FLC_Data_Varriable_Trajectory.mat │ │ ├── Plotter.m │ │ ├── SMC_Data_Varriable_Trajectory.mat │ │ ├── X_response.fig │ │ ├── Y_response.fig │ │ └── Z_response.fig ├── MyPlot.m ├── NonLinDynamic_Quadcopter.m └── Simulation_Params.m ├── README.md └── image ├── Attitude MRAC.png ├── X MRAC.png ├── Y MRAC.png ├── Y comparison.png ├── Z MRAC.png ├── Z comparison.png └── x comparision.png /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 iman sharifi 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /PersianReport.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iman-sharifi-ghb/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/5bbfb45ae5765414332bb6df5766a0138010bd09/PersianReport.pdf -------------------------------------------------------------------------------- /Project/1-PID Control Quadcopter/CreateDesiredTraj.m: -------------------------------------------------------------------------------- 1 | function [DesPosition, DesAttitude] = CreateDesiredTraj(t) 2 | 3 | Xd = 1; 4 | Yd = 2; 5 | Zd = 3; 6 | 7 | dXd = 0; 8 | dYd = 0; 9 | dZd = 0; 10 | 11 | ddXd = 0; 12 | ddYd = 0; 13 | ddZd = 0; 14 | 15 | phid = 0; 16 | Ttad = 0; 17 | Psid = 0;%deg2rad(1); 18 | 19 | dphid = 0; 20 | dTtad = 0; 21 | dPsid = 0; 22 | 23 | ddphid = 0; 24 | ddTtad = 0; 25 | ddPsid = 0; 26 | 27 | DesAttitude = [phid;dphid;ddphid;Ttad;dTtad;ddTtad;Psid;dPsid;ddPsid]; 28 | 29 | DesPosition = [Zd;dZd;ddZd;Xd;dXd;ddXd;Yd;dYd;ddYd]; 30 | end 31 | 32 | -------------------------------------------------------------------------------- /Project/1-PID Control Quadcopter/Main.m: -------------------------------------------------------------------------------- 1 | clc;clear; 2 | close all 3 | 4 | global dt 5 | Path = 'C:\Users\Iman Sharifi\Documents\MATLAB\Nonlinear Control\Project'; 6 | addpath(Path) 7 | Simulation_Params; 8 | Parameters; 9 | 10 | t = 0:dt:Tf; 11 | N = Tf/dt+1; 12 | 13 | x = x0; 14 | X = []; 15 | x_ddot = zeros(6,1); 16 | Uxy = [0;0]; 17 | 18 | for i=1:N 19 | 20 | [U, Uxy] = PID_Controller(x, x_ddot, Uxy); 21 | k1 = NonLinDynamic_Quadcopter(x, U); 22 | k2 = NonLinDynamic_Quadcopter(x+dt/2*k1, U); 23 | k3 = NonLinDynamic_Quadcopter(x+dt/2*k2, U); 24 | k4 = NonLinDynamic_Quadcopter(x+dt*k3, U); 25 | 26 | x = x + dt/6*(k1+2*k2+2*k3+k4); 27 | x_ddot = NonLinDynamic_Quadcopter(x, U); 28 | x_ddot = x_ddot(2:2:end); 29 | X = [X;x']; 30 | end 31 | %% Plot Results 32 | figure 33 | % 3D Position 34 | MyPlot(t,X(:,9),'-b');hold on 35 | MyPlot(t,X(:,11),'-r'); 36 | MyPlot(t,X(:,7),'-g') 37 | grid on;title('Position Response');legend('X','Y','Z') 38 | xlabel('Time'), ylabel('Amp') 39 | 40 | % figure 41 | % % Attitudes 42 | % MyPlot(t,rad2deg(X(:,1)),'-b');hold on 43 | % MyPlot(t,rad2deg(X(:,3)),'-r'); 44 | % grid on;title('Roll & Pitch Angles');legend('\phi','\theta') 45 | % xlabel('Time'), ylabel('Amp') 46 | % % 47 | % figure 48 | % MyPlot(t,rad2deg(X(:,5)),'-g') 49 | % grid on;title('Yaw Angle');legend('\psi') 50 | % xlabel('Time'), ylabel('Amp') 51 | %% Save Data 52 | filename = [Path '\MatFiles\PID_Data.mat']; 53 | Data = [X,t']; 54 | save(filename, 'Data') -------------------------------------------------------------------------------- /Project/1-PID Control Quadcopter/PID_Controller.m: -------------------------------------------------------------------------------- 1 | function [U, Uxy] = PID_Controller(X,X_ddot,Uxy) 2 | global g dt 3 | global Kp_phi Ki_phi Kd_phi 4 | global Kp_tta Ki_tta Kd_tta 5 | global Kp_psi Ki_psi Kd_psi 6 | global Kp_z Ki_z Kd_z 7 | global Kp_y Ki_y Kd_y 8 | global Kp_x Ki_x Kd_x 9 | global int_e_phi int_e_tta int_e_psi 10 | global int_e_z int_e_x int_e_y 11 | 12 | % calculate desired state as function of t 13 | x = X(9) ;x_dot = X(10);x_ddot = X_ddot(5); 14 | y = X(11);y_dot = X(12);y_ddot = X_ddot(6); 15 | z = X(7) ;z_dot = X(8); 16 | phi = X(1);phi_dot = X(2); 17 | tta = X(3);tta_dot = X(4); 18 | psi = X(5);psi_dot = X(6); 19 | 20 | %% Desired States 21 | %% Desired States 22 | [DesPos, DesAtt] = CreateDesiredTraj(0); 23 | 24 | Xd = DesPos(4); 25 | Yd = DesPos(7); 26 | Zd = DesPos(1); 27 | 28 | Xd_dot = DesPos(5); 29 | Yd_dot = DesPos(8); 30 | Zd_dot = DesPos(2); 31 | 32 | Xd_ddot = DesPos(6); 33 | Yd_ddot = DesPos(9); 34 | Zd_ddot = DesPos(3); 35 | 36 | Phid = DesAtt(1); 37 | Ttad = DesAtt(4); 38 | Psid = DesAtt(7); 39 | 40 | Phid_dot = DesAtt(2); 41 | Ttad_dot = DesAtt(5); 42 | Psid_dot = DesAtt(8); 43 | 44 | Phid_ddot = DesAtt(3); 45 | Ttad_ddot = DesAtt(6); 46 | Psid_ddot = DesAtt(9); 47 | 48 | %% Calculate XYZ errors 49 | e_x = Xd - x; 50 | e_dot_x = Xd_dot- x_dot; 51 | 52 | e_y = Yd- y; 53 | e_dot_y = Yd_dot- y_dot; 54 | 55 | e_z = Zd - z; 56 | e_dot_z = Zd_dot - z_dot; 57 | 58 | Ux = Kp_x*e_x + Ki_x*int_e_x + Kd_x*e_dot_x; 59 | Ux_dot = (Ux - Uxy(1))/dt; 60 | 61 | Uy = -(Kp_y*e_y + Ki_y*int_e_y + Kd_y*e_dot_y); 62 | Uy_dot = (Uy - Uxy(2))/dt; 63 | 64 | %% Calculate Angle Errors from Desired Angles 65 | e_phi = Uy - phi; 66 | e_dot_phi = Uy_dot - phi_dot; 67 | 68 | e_tta = Ux - tta; 69 | e_dot_tta = Ux_dot - tta_dot; 70 | 71 | e_psi = Psid - psi; 72 | e_dot_psi = Psid_dot - psi_dot; 73 | 74 | int_e_phi = int_e_phi + e_phi; 75 | int_e_tta = int_e_tta + e_tta; 76 | int_e_psi = int_e_psi + e_psi; 77 | int_e_z = int_e_z + e_z; 78 | int_e_x = int_e_x + e_x; 79 | int_e_y = int_e_y + e_y; 80 | 81 | %% PD Loop to Calculate Torques, Thrusts 82 | T = RotI2B(phi, tta, psi)*[0;0;g] + e_z*Kp_z + e_dot_z*Kd_z + int_e_z*Ki_z; 83 | T = max([T(3) 0]); 84 | 85 | tau_phi = (e_phi*Kp_phi + e_dot_phi*Kd_phi + int_e_phi*Ki_phi); 86 | tau_tta = (e_tta*Kp_tta + e_dot_tta*Kd_tta + int_e_tta*Ki_tta); 87 | tau_psi = (e_psi*Kp_psi + e_dot_psi*Kd_psi + int_e_psi*Ki_psi); 88 | 89 | % disp(['e_phi = ' num2str(e_phi) ', e_tta = ' num2str(e_tta)]) 90 | disp(['e_x = ' num2str(e_x) ', e_y = ' num2str(e_y)]) 91 | U = [T tau_phi tau_tta tau_psi]'; 92 | Uxy = [Ux;Uy]; 93 | end -------------------------------------------------------------------------------- /Project/1-PID Control Quadcopter/Parameters.m: -------------------------------------------------------------------------------- 1 | % Dynamic Params 2 | global m g 3 | global l b d 4 | global Jx Jy Jz 5 | global a1 a2 a3 6 | global b1 b2 b3 7 | 8 | m = 0.65; 9 | g = 9.81; 10 | l = 0.23; 11 | Jx = 7.5e-3; 12 | Jy = 7.5e-3; 13 | Jz = 1.3e-2; 14 | b = 3.13e-5; 15 | d = 7.5e-5; 16 | 17 | a1 = (Jy-Jz)/Jx; 18 | a2 = (Jz-Jx)/Jy; 19 | a3 = (Jx-Jy)/Jz; 20 | 21 | b1 = l/Jx; 22 | b2 = l/Jy; 23 | b3 = 1/Jz; 24 | 25 | % Control Params 26 | global Kp_phi Ki_phi Kd_phi 27 | global Kp_tta Ki_tta Kd_tta 28 | global Kp_psi Ki_psi Kd_psi 29 | global Kp_z Ki_z Kd_z 30 | global Kp_y Ki_y Kd_y 31 | global Kp_x Ki_x Kd_x 32 | 33 | Kp_z = 10*m; 34 | Ki_z = 3.5*10^-2; 35 | Kd_z = 10*m; 36 | 37 | Kp_psi = 15; 38 | Ki_psi = 0.01; 39 | Kd_psi = 15; 40 | 41 | Kp_phi = 1; 42 | Ki_phi = 0.001; 43 | Kd_phi = 0.20; 44 | 45 | Kp_y = 10*pi/180;%50*pi/180; 46 | Ki_y = 0.0004; 47 | Kd_y = 6*pi/180;%20*pi/180; 48 | 49 | Kp_tta = Kp_phi; 50 | Ki_tta = Ki_phi; 51 | Kd_tta = Kd_phi; 52 | 53 | Kp_x = 3*pi/180; 54 | Ki_x = 0.000015; 55 | Kd_x = 5*pi/180;%5*pi/180; 56 | 57 | 58 | % Reset Integral terms 59 | global int_e_x int_e_y int_e_z 60 | global int_e_phi int_e_tta int_e_psi 61 | 62 | int_e_x = 0; 63 | int_e_y = 0; 64 | int_e_phi = 0; 65 | int_e_tta = 0; 66 | int_e_psi = 0; 67 | int_e_z = 0; 68 | -------------------------------------------------------------------------------- /Project/1-PID Control Quadcopter/RotI2B.m: -------------------------------------------------------------------------------- 1 | function [ R ] = RotI2B( phi, theta, psi ) 2 | R = [cos(psi)*cos(theta), cos(psi)*sin(theta)*sin(phi) - sin(psi)*cos(phi), cos(psi)*sin(theta)*sin(phi) + sin(psi)*sin(phi); ... 3 | sin(psi)*sin(theta), sin(psi)*sin(theta)*sin(phi) + cos(psi)*cos(phi), sin(psi)*sin(theta)*sin(phi) - cos(psi)*sin(phi); ... 4 | -sin(theta), cos(theta)*sin(phi), cos(theta)*sin(phi)]; 5 | end -------------------------------------------------------------------------------- /Project/2-LQR Control Quadcopter/Main.m: -------------------------------------------------------------------------------- 1 | clc;clear; 2 | close all 3 | global dt 4 | global A B C D desiredPoles 5 | global Q R N 6 | 7 | Path = 'C:\Users\Iman Sharifi\Documents\MATLAB\Nonlinear Control\Project'; 8 | addpath(Path) 9 | Simulation_Params; 10 | Parameters; 11 | 12 | [A,B,C,D]=State_Space(); 13 | n = length(A); 14 | r = size(B, 2); 15 | 16 | % State Feedback params 17 | start = 3;step = 0.1; 18 | desiredPoles = [-1:-1:-12];%-start:-step:-(start+step*(n-1)); 19 | desiredPoles(5) = -20; 20 | desiredPoles(6) = -30; 21 | % LQR params 22 | Q = 10*eye(n); 23 | Q(7,7) = 1000000; 24 | Q(8,8) = 0; 25 | Q(5,5) = 20; 26 | R = 5*eye(r); 27 | N = 0; 28 | 29 | K = SF_Controller('place'); 30 | 31 | KI = B\(A-B*K); 32 | r = zeros(12,1);r(7:2:11) = 1; 33 | r(5) = deg2rad(1); 34 | 35 | % dt = 0.005; 36 | % Tf = 50; 37 | t = 0:dt:Tf; 38 | N = Tf/dt+1; 39 | % x0 = zeros(12,1); 40 | % x0(5) = deg2rad(1); 41 | % x0(7:2:11) = 1; 42 | x = x0; 43 | X = []; 44 | x_ddot = zeros(6,1); 45 | Uxy = [0;0]; 46 | 47 | for i=1:N 48 | 49 | [DesPos, DesAtt] = CreateDesiredTrajectory(0); 50 | r = [DesAtt([1:2,4:5,7:8]); DesPos([1:2,4:5,7:8])]; 51 | U = -K*x - KI*r; 52 | k1 = NonLinDynamic_Quadcopter(x, U); 53 | k2 = NonLinDynamic_Quadcopter(x+dt/2*k1, U); 54 | k3 = NonLinDynamic_Quadcopter(x+dt/2*k2, U); 55 | k4 = NonLinDynamic_Quadcopter(x+dt*k3, U); 56 | 57 | x = x + dt/6*(k1+2*k2+2*k3+k4); 58 | x_ddot = NonLinDynamic_Quadcopter(x, U); 59 | x_ddot = x_ddot(2:2:end); 60 | X = [X;x']; 61 | end 62 | %% Plot Results 63 | figure 64 | % 3D Position 65 | MyPlot(t,X(:,9),'-b');hold on 66 | MyPlot(t,X(:,11),'-r'); 67 | MyPlot(t,X(:,7),'-g') 68 | grid on;title('Position Response');legend('X','Y','Z') 69 | 70 | figure 71 | % Attitudes 72 | MyPlot(t,rad2deg(X(:,1)),'-b');hold on 73 | MyPlot(t,rad2deg(X(:,3)),'-r'); 74 | grid on;title('Roll & Pitch Angles');legend('\phi','\theta') 75 | 76 | figure 77 | MyPlot(t,rad2deg(X(:,5)),'-g') 78 | grid on;title('Yaw Angle');legend('\psi') 79 | %% Save Data 80 | filename = [Path '\MatFiles\LQR_Data.mat']; 81 | Data = [X,t']; 82 | save(filename, 'Data') -------------------------------------------------------------------------------- /Project/2-LQR Control Quadcopter/Parameters.m: -------------------------------------------------------------------------------- 1 | % Dynamic Params 2 | global m g 3 | global l b d 4 | global Jx Jy Jz 5 | global a1 a2 a3 6 | global b1 b2 b3 7 | 8 | m = 0.65; 9 | g = 9.81; 10 | l = 0.23; 11 | Jx = 7.5e-3; 12 | Jy = 7.5e-3; 13 | Jz = 1.3e-2; 14 | b = 3.13e-5; 15 | d = 7.5e-5; 16 | 17 | a1 = (Jy-Jz)/Jx; 18 | a2 = (Jz-Jx)/Jy; 19 | a3 = (Jx-Jy)/Jz; 20 | 21 | b1 = l/Jx; 22 | b2 = l/Jy; 23 | b3 = 1/Jz; 24 | 25 | % Control Params 26 | global c1 c2 c3 c4 c5 c6 27 | global k1 k2 k3 k4 k5 k6 k7 k8 k9 k10 k11 k12 28 | 29 | c = 1; 30 | k = 1; 31 | 32 | c1 = c;c2 = c; c3 = 2; c4 = c;c5 = c; c6 = c; 33 | 34 | k1 = k;k2 = k;k3 = k;k4 = k;k5 = 1; 35 | k6 = 1;k7 = k;k8 = k;k9 = k;k10 = k; 36 | k11 = k;k12 = k; -------------------------------------------------------------------------------- /Project/2-LQR Control Quadcopter/SF_Controller.m: -------------------------------------------------------------------------------- 1 | function K = SF_Controller(Type) 2 | 3 | global A B desiredPoles 4 | global Q R N 5 | 6 | Type = lower(Type); 7 | 8 | if isempty(Type) 9 | Type = 'place'; 10 | end 11 | 12 | switch Type 13 | case 'place' 14 | K = place(A, B, desiredPoles); 15 | case 'lqr' 16 | [K, P, e] = lqr(A, B, Q, R, N); 17 | end 18 | 19 | end 20 | 21 | -------------------------------------------------------------------------------- /Project/2-LQR Control Quadcopter/State_Space.m: -------------------------------------------------------------------------------- 1 | function [A,B,C,D]=State_Space() 2 | 3 | global m g 4 | global a1 a2 a3 5 | global b1 b2 b3 6 | 7 | syms x1 x2 x3 x4 x5 x6 8 | syms x7 x8 x9 x10 x11 x12 9 | syms U1 U2 U3 U4 10 | 11 | Ux = cos(x1)*sin(x3)*cos(x5)+sin(x1)*sin(x5); 12 | Uy = cos(x1)*sin(x3)*sin(x5)-sin(x1)*cos(x5); 13 | 14 | dx1 = x2; 15 | dx2 = x4*x6*a1+ U2*b1; 16 | dx3 = x4; 17 | dx4 = x2*x6*a2+ U3*b2; 18 | dx5 = x6; 19 | dx6 = x2*x4*a3+ U4*b3; 20 | dx7 = x8; 21 | dx8 = U1/m*(cos(x1)*cos(x3))-g; 22 | dx9 = x10; 23 | dx10 = U1/m*Ux; 24 | dx11 = x12; 25 | dx12 = U1/m*Uy; 26 | 27 | x = [x1;x2;x3;x4;x5;x6;x7;x8;x9;x10;x11;x12]; 28 | dx = [dx1;dx2;dx3;dx4;dx5;dx6;dx7;dx8;dx9;dx10;dx11;dx12]; 29 | u = [U1;U2;U3;U4]; 30 | 31 | A = jacobian(dx,x); 32 | A = simplify(A); 33 | B = jacobian(dx,u); 34 | B = simplify(B); 35 | 36 | n = length(x); 37 | r = length(u); 38 | 39 | State_Input_Vars = [x.', u.']; 40 | xeq = [zeros(6,1);x7;0;x9;0;x11;0]; 41 | ueq = [m*g;0;0;0]; 42 | eq_point = [xeq.', ueq.']; 43 | 44 | A = subs(A, State_Input_Vars, eq_point); 45 | B = subs(B, State_Input_Vars, eq_point); 46 | 47 | A = vpa(A,6); 48 | B = vpa(B,6); 49 | A = double(A); 50 | B = double(B); 51 | C = zeros(n,1); 52 | D = zeros(r,1); 53 | 54 | end 55 | 56 | -------------------------------------------------------------------------------- /Project/3-Feedback Linearization Control Quadcopter/FL_Controller.m: -------------------------------------------------------------------------------- 1 | function [U, Uxy] = FL_Controller(t, x, y, Uxy) 2 | 3 | global X_desired 4 | 5 | %% Actual States 6 | X = x(9) ;X_dot = x(10);X_ddot = y(5); 7 | Y = x(11);Y_dot = x(12);Y_ddot = y(6); 8 | Z = x(7) ;Z_dot = x(8);Z_ddot = y(4); 9 | 10 | Phi = x(1);Phi_dot = x(2);Phi_ddot = y(1); 11 | Tta = x(3);Tta_dot = x(4);Tta_ddot = y(2); 12 | Psi = x(5);Psi_dot = x(6);Psi_ddot = y(3); 13 | 14 | %% Desired States 15 | [DesPos, DesAtt] = CreateDesiredTrajectory(t); 16 | 17 | Xd = DesPos(4); 18 | Yd = DesPos(7); 19 | Zd = DesPos(1); 20 | 21 | Xd_dot = DesPos(5); 22 | Yd_dot = DesPos(8); 23 | Zd_dot = DesPos(2); 24 | 25 | Xd_ddot = DesPos(6); 26 | Yd_ddot = DesPos(9); 27 | Zd_ddot = DesPos(3); 28 | 29 | Phid = DesAtt(1); 30 | Ttad = DesAtt(4); 31 | Psid = DesAtt(7); 32 | 33 | Phid_dot = DesAtt(2); 34 | Ttad_dot = DesAtt(5); 35 | Psid_dot = DesAtt(8); 36 | 37 | Phid_ddot = DesAtt(3); 38 | Ttad_ddot = DesAtt(6); 39 | Psid_ddot = DesAtt(9); 40 | 41 | X_desired = [X_desired;[Phid,Ttad,Psid,Zd,Xd,Yd]]; 42 | 43 | %% Parameter Definition 44 | global m g dt 45 | global a1 a2 a3 46 | global b1 b2 b3 47 | global K1 K2 %K3 K4 K5 K6 K7 K8 K9 K10 K11 K12 48 | global KI 49 | global int_e_Z int_e_X int_e_Y 50 | global int_e_Phi int_e_Tta int_e_Psi 51 | 52 | %% Altitude Control 53 | e4 = Zd - Z; 54 | e4_dot = Zd_dot - Z_dot; 55 | int_e_Z = int_e_Z + e4; 56 | 57 | v1 = [K1 K2]*[e4;e4_dot]+Zd_ddot;%+KI*int_e_Z; 58 | 59 | U1 = m/cos(Phi)/cos(Tta)*(g+v1); 60 | %% x Motion Control 61 | e5 = Xd - X; 62 | e5_dot = Xd_dot - X_dot; 63 | disp(['Error x: ' num2str(e5)]) 64 | int_e_X = int_e_X + e5; 65 | 66 | vx = [K1 K2]*[e5;e5_dot]+Xd_ddot;%+KI*int_e_X; 67 | 68 | Ux = m/U1*vx; 69 | Ux_dot = (Ux - Uxy(1))/dt; 70 | %% y Motion Control 71 | e6 = Yd - Y; 72 | e6_dot = Yd_dot - Y_dot; 73 | int_e_Y = int_e_Y + e6; 74 | 75 | vy = [K1 K2]*[e6;e6_dot]+Yd_ddot;%+KI*int_e_Y; 76 | 77 | Uy = m/U1*vy; 78 | Uy_dot = (Uy - Uxy(2))/dt; 79 | %% Roll Control 80 | e1 = -Uy - Phi; 81 | e1_dot = -Uy_dot - Phi_dot; 82 | int_e_Phi = int_e_Phi + e1; 83 | 84 | v2 = [K1 K2]*[e1;e1_dot];%+KI*int_e_Phi; 85 | U2 = 1/b1*(-a1*Tta_dot*Psi_dot+v2); 86 | %% Pitch Control 87 | e2 = Ux - Tta; 88 | e2_dot = Ux_dot - Tta_dot; 89 | int_e_Tta = int_e_Tta + e2; 90 | 91 | v3 = [K1 K2]*[e2;e2_dot];%+KI*int_e_Tta; 92 | U3 = 1/b2*(-a2*Phi_dot*Psi_dot+v3); 93 | %% Yaw Control 94 | e3 = Psid - Psi; 95 | e3_dot = Psid_dot - Psi_dot; 96 | int_e_Psi = int_e_Psi + e3; 97 | 98 | v4 = [K1 K2]*[e3;e3_dot]+Psid_ddot;%+KI*int_e_Psi; 99 | % disp([num2str(Psid) ' and ' num2str(Psi)]) 100 | 101 | U4 = 1/b3*(-a3*Phi_dot*Tta_dot+v4); 102 | %2- PID: K1*e3+K2*e3_dot+K3*e3_ddot;K1 = 0;K2 = 0;K3 = 1; 103 | %3- K51*s3/(abs(s3)+delta)+;delta = 0.3; 104 | %% Control Inputs 105 | U = [U1;U2;U3;U4]; 106 | Uxy = [Ux;Uy]; 107 | 108 | end 109 | 110 | %% 111 | function out = SignApprox(S, Type, a) 112 | 113 | if nargin<2 114 | Type = 1; 115 | end 116 | 117 | switch Type 118 | case 1 119 | out = sign(S); 120 | case 2 121 | phi = a; 122 | out = Saturation(S/phi, [-1 1]); 123 | case 3 124 | coef = a; 125 | out = tanh(coef*S); 126 | end 127 | 128 | end 129 | function out = Saturation(S, bound) 130 | 131 | if S > max(bound) 132 | out = max(bound); 133 | elseif S < min(bound) 134 | out = min(bound); 135 | else 136 | out = S; 137 | end 138 | 139 | end 140 | %% 141 | 142 | -------------------------------------------------------------------------------- /Project/3-Feedback Linearization Control Quadcopter/Main.m: -------------------------------------------------------------------------------- 1 | clc;clear; 2 | close all 3 | 4 | global dt 5 | global X_desired 6 | X_desired = []; 7 | 8 | Path = 'C:\Users\Iman Sharifi\Documents\MATLAB\Nonlinear Control\Project'; 9 | addpath(Path) 10 | Parameters; 11 | Simulation_Params; 12 | 13 | t = 0:dt:Tf; 14 | N = Tf/dt+1; 15 | % x0 = zeros(12,1); 16 | % % x0(7:2:11) = 1; 17 | x = x0; 18 | X = []; 19 | x_ddot = zeros(6,1); 20 | Uxy = [0;0]; 21 | 22 | for i=1:N 23 | 24 | [U, Uxy] = FL_Controller(t(i), x, x_ddot, Uxy); 25 | k1 = NonLinDynamic_Quadcopter(x, U); 26 | k2 = NonLinDynamic_Quadcopter(x+dt/2*k1, U); 27 | k3 = NonLinDynamic_Quadcopter(x+dt/2*k2, U); 28 | k4 = NonLinDynamic_Quadcopter(x+dt*k3, U); 29 | 30 | x = x + dt/6*(k1+2*k2+2*k3+k4); 31 | x_ddot = NonLinDynamic_Quadcopter(x, U); 32 | x_ddot = x_ddot(2:2:end); 33 | X = [X;x']; 34 | 35 | if isnan(x(1)) 36 | error('Oops! NaN occured. ---') 37 | end 38 | end 39 | %% Plot Results 40 | figure 41 | % 3D Position 42 | MyPlot(t,X(:,9),'-b');hold on 43 | MyPlot(t,X(:,11),'-r'); 44 | MyPlot(t,X(:,7),'-g') 45 | grid on;title('Position Response');legend('X','Y','Z') 46 | 47 | figure 48 | % Attitudes 49 | MyPlot(t,rad2deg(X(:,1)),'-b');hold on 50 | MyPlot(t,rad2deg(X(:,3)),'-r'); 51 | grid on;title('Roll & Pitch Angles');legend('\phi','\theta') 52 | 53 | figure 54 | MyPlot(t,rad2deg(X(:,5)),'-g') 55 | grid on;title('Yaw Angle');legend('\psi') 56 | 57 | %% Save Data 58 | filename = [Path '\MatFiles\FLC_Data_Varriable_Trajectory.mat']; 59 | Data = [X ,X_desired, t']; 60 | save(filename, 'Data') -------------------------------------------------------------------------------- /Project/3-Feedback Linearization Control Quadcopter/Parameters.m: -------------------------------------------------------------------------------- 1 | % Dynamic Params 2 | global m g 3 | global l b d 4 | global Jx Jy Jz 5 | global a1 a2 a3 6 | global b1 b2 b3 7 | 8 | m = 0.65; 9 | g = 9.81; 10 | l = 0.23; 11 | Jx = 7.5e-3; 12 | Jy = 7.5e-3; 13 | Jz = 1.3e-2; 14 | b = 3.13e-5; 15 | d = 7.5e-5; 16 | 17 | a1 = (Jy-Jz)/Jx; 18 | a2 = (Jz-Jx)/Jy; 19 | a3 = (Jx-Jy)/Jz; 20 | 21 | b1 = l/Jx; 22 | b2 = l/Jy; 23 | b3 = 1/Jz; 24 | 25 | % Control Params 26 | global K1 K2 %K3 K4 K5 K6 K7 K8 K9 K10 K11 K12 27 | global KI 28 | 29 | % K = 1; 30 | % K1 = K;K2 = K;K3 = K;K4 = K;K5 = 1; 31 | % K6 = 1;K7 = K;K8 = K;K9 = K;K10 = K; 32 | % K11 = K;K12 = K; 33 | K1 = 2;K2 = 3; 34 | KI = 6; 35 | 36 | global int_e_Z int_e_X int_e_Y 37 | global int_e_Phi int_e_Tta int_e_Psi 38 | 39 | int_e_Z = 0; int_e_X = 0; int_e_Y = 0; 40 | int_e_Phi = 0; int_e_Tta = 0; int_e_Psi = 0; -------------------------------------------------------------------------------- /Project/4-Sliding Mode Control Quadcopter/Main.m: -------------------------------------------------------------------------------- 1 | clc;clear; 2 | close all 3 | 4 | global dt 5 | global S S_dot X_desired 6 | S = [];S_dot = [];X_desired = []; 7 | 8 | Path = 'C:\Users\Iman Sharifi\Documents\MATLAB\Nonlinear Control\Project'; 9 | addpath(Path) 10 | Parameters; 11 | Simulation_Params; 12 | 13 | t = 0:dt:Tf; 14 | N = Tf/dt+1; 15 | x = x0; 16 | X = []; 17 | x_ddot = zeros(6,1); 18 | Uxy = [0;0]; 19 | 20 | for i=1:N 21 | 22 | [U, Uxy] = SMC_Controller(t(i), x, x_ddot, Uxy); 23 | k1 = NonLinDynamic_Quadcopter(x, U); 24 | k2 = NonLinDynamic_Quadcopter(x+dt/2*k1, U); 25 | k3 = NonLinDynamic_Quadcopter(x+dt/2*k2, U); 26 | k4 = NonLinDynamic_Quadcopter(x+dt*k3, U); 27 | 28 | x = x + dt/6*(k1+2*k2+2*k3+k4); 29 | x_ddot = NonLinDynamic_Quadcopter(x, U); 30 | x_ddot = x_ddot(2:2:end); 31 | X = [X;x']; 32 | end 33 | %% Plot Results 34 | figure 35 | % 3D Position 36 | MyPlot(t,X(:,9),'-b');hold on 37 | MyPlot(t,X(:,11),'-r'); 38 | MyPlot(t,X(:,7),'-g') 39 | grid on;title('Position Response');legend('X','Y','Z') 40 | xlabel('Time'), ylabel('Amp') 41 | 42 | figure 43 | % Attitudes 44 | MyPlot(t,rad2deg(X(:,1)),'-b');hold on 45 | MyPlot(t,rad2deg(X(:,3)),'-r');hold on 46 | MyPlot(t,rad2deg(X(:,5)),'-g') 47 | grid on;title('Euler Angles');legend('\phi','\theta','\psi') 48 | xlabel('Time'), ylabel('Amp') 49 | 50 | % figure 51 | % MyPlot(t,rad2deg(X(:,5)),'-g') 52 | % grid on;title('Yaw Angle');legend('\psi') 53 | % xlabel('Time'), ylabel('Amp') 54 | %% Sliding Surfaces 55 | figure 56 | title('Sliding Surafaces'); 57 | % 3D Position 58 | subplot(221); MyPlot(S(:,1),S_dot(:,1)); 59 | grid on; xlabel('S_1'), ylabel('DS_1') 60 | subplot(222); MyPlot(S(:,2),S_dot(:,2)); 61 | grid on; xlabel('S_2'), ylabel('DS_2') 62 | subplot(223); MyPlot(S(:,3),S_dot(:,3)); 63 | grid on; xlabel('S_3'), ylabel('DS_3') 64 | subplot(224); MyPlot(S(:,4),S_dot(:,4)); 65 | grid on; xlabel('S_4'), ylabel('DS_4') 66 | 67 | %% Video Animation 68 | makeVideo = input('Would you like to show Animation???'); 69 | if makeVideo==1 70 | close all 71 | tic; 72 | figure; 73 | Xd = X_desired(:,5); 74 | Yd = X_desired(:,6); 75 | Zd = X_desired(:,4); 76 | Quad_Animation(t,X(:,9),X(:,11),X(:,7),X(:,1),X(:,3),X(:,5),Xd,Yd,Zd) 77 | toc 78 | end 79 | 80 | %% Save Data 81 | % filename = [Path '\MatFiles\SMC_Data_Varriable_trajectory.mat']; 82 | % Data = [X, X_desired, t']; 83 | % save(filename, 'Data') -------------------------------------------------------------------------------- /Project/4-Sliding Mode Control Quadcopter/Parameters.m: -------------------------------------------------------------------------------- 1 | % Dynamic Params 2 | global m g 3 | global l b d 4 | global Jx Jy Jz 5 | global a1 a2 a3 6 | global b1 b2 b3 7 | 8 | m = 0.65; 9 | g = 9.81; 10 | l = 0.23; 11 | Jx = 7.5e-3; 12 | Jy = 7.5e-3; 13 | Jz = 1.3e-2; 14 | b = 3.13e-5; 15 | d = 7.5e-5; 16 | 17 | a1 = (Jy-Jz)/Jx; 18 | a2 = (Jz-Jx)/Jy; 19 | a3 = (Jx-Jy)/Jz; 20 | 21 | b1 = l/Jx; 22 | b2 = l/Jy; 23 | b3 = 1/Jz; 24 | 25 | % Control Params 26 | global c1 c2 c3 c4 c5 c6 27 | global k1 k2 k3 k4 k5 k6 k7 k8 k9 k10 k11 k12 28 | 29 | c = 1; 30 | k = 1; 31 | 32 | c1 = c;c2 = c; c3 = 2; c4 = c;c5 = c; c6 = c; 33 | 34 | k1 = k;k2 = k;k3 = k;k4 = k;k5 = 1; 35 | k6 = 1;k7 = k;k8 = k;k9 = k;k10 = k; 36 | k11 = k;k12 = k; -------------------------------------------------------------------------------- /Project/4-Sliding Mode Control Quadcopter/Quad_Animation.m: -------------------------------------------------------------------------------- 1 | function Quad_Animation(t,q1,q2,q3,q4,q5,q6,q1d,q2d,q3d) 2 | %ANIMATION PART 3 | global l 4 | r = l/3; 5 | a = 1.1*l+r; 6 | t1 = 0:0.01:2*pi; 7 | hold on 8 | Len=length(t); 9 | for i=1:Len 10 | 11 | R=[ cos(q4(i))*cos(q5(i)), cos(q5(i))*sin(q4(i)), -sin(q5(i)) 12 | cos(q4(i))*sin(q5(i))*sin(q6(i)) - cos(q6(i))*sin(q4(i)), cos(q4(i))*cos(q6(i)) + sin(q4(i))*sin(q5(i))*sin(q6(i)), cos(q5(i))*sin(q6(i)) 13 | sin(q4(i))*sin(q6(i)) + cos(q4(i))*cos(q6(i))*sin(q5(i)), cos(q6(i))*sin(q4(i))*sin(q5(i)) - cos(q4(i))*sin(q6(i)), cos(q5(i))*cos(q6(i))]; 14 | 15 | %Quad Motors Location 16 | P1 = [q1(i);q2(i);q3(i)]+R*[l;0;0]; 17 | P2 = [q1(i);q2(i);q3(i)]+R*[-l;0;0]; 18 | P3 = [q1(i);q2(i);q3(i)]+R*[0;l;0]; 19 | P4 = [q1(i);q2(i);q3(i)]+R*[0;-l;0]; 20 | 21 | P11 = [q1(i);q2(i);q3(i)]+R*[l-r;0;0]; 22 | P22 = [q1(i);q2(i);q3(i)]+R*[-(l-r);0;0]; 23 | P33 = [q1(i);q2(i);q3(i)]+R*[0;l-r;0]; 24 | P44 = [q1(i);q2(i);q3(i)]+R*[0;-(l-r);0]; 25 | 26 | V12 = [P11,P22]; V34 = [P33,P44]; 27 | 28 | plot3(q1d,q2d,q3d,'-g','linewidth',2);hold on 29 | 30 | C1 = P1 + r*[cos(t1);sin(t1);zeros(size(t1))]; 31 | plot3(C1(1,:),C1(2,:),P1(3)*ones(1,length(C1)),'-k','linewidth',2);hold on 32 | 33 | C2 = P2 + r*[cos(t1);sin(t1);zeros(size(t1))]; 34 | plot3(C2(1,:),C2(2,:),P2(3)*ones(1,length(C2)),'-k','linewidth',2);hold on 35 | 36 | C3 = P3 + r*[cos(t1);sin(t1);zeros(size(t1))]; 37 | plot3(C3(1,:),C3(2,:),P3(3)*ones(1,length(C3)),'-k','linewidth',2);hold on 38 | 39 | C4 = P4 + r*[cos(t1);sin(t1);zeros(size(t1))]; 40 | plot3(C4(1,:),C4(2,:),P4(3)*ones(1,length(C4)),'-k','linewidth',2);hold on 41 | 42 | plot3(V12(1,:),V12(2,:),V12(3,:),'k','linewidth',3);title('Quadcopter') 43 | xlabel('X');ylabel('Y');zlabel('Z');hold on 44 | 45 | plot3(V34(1,:),V34(2,:),V34(3,:),'k','linewidth',3);hold on 46 | plot3(q1(1:i),q2(1:i),q3(1:i),'b','linewidth',1) 47 | 48 | axis equal 49 | grid on 50 | axis([min(q1)-a max(q1)+a min(q2)-a max(q2)+a min(q3)-a max(q3)+a]) 51 | str=['Time = ',num2str(t(i))]; 52 | text(min(q1),min(q1),str) 53 | 54 | hold off 55 | pause(0.0001) 56 | 57 | end 58 | end 59 | 60 | -------------------------------------------------------------------------------- /Project/4-Sliding Mode Control Quadcopter/SMC_Controller.m: -------------------------------------------------------------------------------- 1 | function [U, Uxy] = SMC_Controller(t, x, y, Uxy) 2 | 3 | global X_desired 4 | 5 | %% Actual States 6 | X = x(9) ;X_dot = x(10);X_ddot = y(5); 7 | Y = x(11);Y_dot = x(12);Y_ddot = y(6); 8 | Z = x(7) ;Z_dot = x(8);Z_ddot = y(4); 9 | 10 | Phi = x(1);Phi_dot = x(2);Phi_ddot = y(1); 11 | Tta = x(3);Tta_dot = x(4);Tta_ddot = y(2); 12 | Psi = x(5);Psi_dot = x(6);Psi_ddot = y(3); 13 | 14 | %% Desired States 15 | [DesPos, DesAtt] = CreateDesiredTrajectory(t); 16 | 17 | Xd = DesPos(4); 18 | Yd = DesPos(7); 19 | Zd = DesPos(1); 20 | 21 | Xd_dot = DesPos(5); 22 | Yd_dot = DesPos(8); 23 | Zd_dot = DesPos(2); 24 | 25 | Xd_ddot = DesPos(6); 26 | Yd_ddot = DesPos(9); 27 | Zd_ddot = DesPos(3); 28 | 29 | Phid = DesAtt(1); 30 | Ttad = DesAtt(4); 31 | Psid = DesAtt(7); 32 | 33 | Phid_dot = DesAtt(2); 34 | Ttad_dot = DesAtt(5); 35 | Psid_dot = DesAtt(8); 36 | 37 | Phid_ddot = DesAtt(3); 38 | Ttad_ddot = DesAtt(6); 39 | Psid_ddot = DesAtt(9); 40 | 41 | X_desired = [X_desired;[Phid,Ttad,Psid,Zd,Xd,Yd]]; 42 | 43 | %% Parameter Definition 44 | global S S_dot 45 | global m g dt 46 | global a1 a2 a3 47 | global b1 b2 b3 48 | global c1 c2 c3 c4 c5 c6 49 | global k1 k2 k3 k4 k5 k6 k7 k8 k9 k10 k11 k12 50 | 51 | %% Altitude Control 52 | e4 = Zd - Z; 53 | e4_dot = Zd_dot - Z_dot; 54 | s4 = e4_dot + c4*e4; 55 | 56 | k7 = 5; k8 = 3; c4 = 5; 57 | 58 | U1 = m/cos(Phi)/cos(Tta)*(k7*SignApprox(s4, 3, 2)+k8*s4+g+Zd_ddot+c4*e4_dot); 59 | %% x Motion Control 60 | e5 = Xd - X; 61 | disp(['Error x: ' num2str(e5)]) 62 | e5_dot = Xd_dot - X_dot; 63 | s5 = e5_dot + c5*e5; 64 | k9 = 5;k10 = 1;c5 = 5; 65 | 66 | Ux = m/U1*(k9*SignApprox(s5, 3, 2)+k10*s5+Xd_ddot+c5*e5_dot); 67 | Ux_dot = (Ux - Uxy(1))/dt; 68 | %% y Motion Control 69 | e6 = Yd - Y; 70 | e6_dot = Yd_dot - Y_dot; 71 | s6 = e6_dot + c6*e6; 72 | 73 | k11 = 5; k12 = 1; c6 = 5; 74 | Uy = m/U1*(k11*SignApprox(s6, 3, 2)+k12*s6+Yd_ddot+c6*e6_dot); 75 | Uy_dot = (Uy - Uxy(2))/dt; 76 | %% Roll Control 77 | e1 = -Uy - Phi; 78 | e1_dot = -Uy_dot - Phi_dot; 79 | s1 = e1_dot + c1*e1; 80 | 81 | k1 = 3; k2 = 0; c1 = 10; 82 | U2 = 1/b1*(k1*SignApprox(s1, 3, 2)+k2*s1-a1*Tta_dot*Psi_dot+Phid_ddot+c1*e1_dot); 83 | %% Pitch Control 84 | e2 = Ux - Tta; 85 | e2_dot = Ux_dot - Tta_dot; 86 | s2 = e2_dot + c2*e2; 87 | 88 | k3 = 3; k4 = 0; c2 = 10; 89 | U3 = 1/b2*(k3*SignApprox(s2, 3, 2)+k4*s2-a2*Phi_dot*Psi_dot+Ttad_ddot+c2*e2_dot); 90 | %% Yaw Control 91 | e3 = Psid - Psi; 92 | e3_dot = Psid_dot - Psi_dot; 93 | s3 = e3_dot + c3*e3; 94 | 95 | % disp([num2str(Psid) ' and ' num2str(Psi)]) 96 | 97 | 98 | k5 = 0.1;k6 = 5; c3 = 5; 99 | 100 | U4 = 1/b3*(k5*SignApprox(s2, 3, 2)+k6*s3-a3*Phi_dot*Tta_dot+Psid_ddot+c3*e3_dot); 101 | %2- PID: K1*e3+K2*e3_dot+K3*e3_ddot;K1 = 0;K2 = 0;K3 = 1; 102 | %3- k51*s3/(abs(s3)+delta)+;delta = 0.3; 103 | %% Control Inputs 104 | U = [U1;U2;U3;U4]; 105 | Uxy = [Ux;Uy]; 106 | 107 | %% Save Slide Surfaces 108 | e3_ddot = Psid_ddot - Psi_ddot; 109 | s3_dot = e3_ddot + c3*e3_dot; 110 | e4_ddot = Zd_ddot - Z_ddot; 111 | s4_dot = e4_ddot + c4*e4_dot; 112 | e5_ddot = Xd_ddot - X_ddot; 113 | s5_dot = e5_ddot + c5*e5_dot; 114 | e6_ddot = Yd_ddot - Y_ddot; 115 | s6_dot = e6_ddot + c6*e6_dot; 116 | S = [S;[s3,s4,s5,s6]]; 117 | S_dot = [S_dot;[s3_dot,s4_dot,s5_dot,s6_dot]]; 118 | 119 | end 120 | 121 | %% 122 | function out = SignApprox(S, Type, a) 123 | 124 | if nargin<2 125 | Type = 1; 126 | end 127 | 128 | switch Type 129 | case 1 130 | out = sign(S); 131 | case 2 132 | phi = a; 133 | out = Saturation(S/phi, [-1 1]); 134 | case 3 135 | coef = a; 136 | out = tanh(coef*S); 137 | end 138 | 139 | end 140 | function out = Saturation(S, bound) 141 | 142 | if S > max(bound) 143 | out = max(bound); 144 | elseif S < min(bound) 145 | out = min(bound); 146 | else 147 | out = S; 148 | end 149 | 150 | end 151 | %% 152 | 153 | -------------------------------------------------------------------------------- /Project/5-Backstepping Control Quadcopter/BS_Controller.m: -------------------------------------------------------------------------------- 1 | function [U, Uxy] = BS_Controller(t, x, Uxy, Uxy2) 2 | 3 | global X_desired 4 | 5 | %% Actual States 6 | X = x(9) ;X_dot = x(10); 7 | Y = x(11);Y_dot = x(12); 8 | Z = x(7) ;Z_dot = x(8); 9 | 10 | Phi = x(1);Phi_dot = x(2); 11 | Tta = x(3);Tta_dot = x(4); 12 | Psi = x(5);Psi_dot = x(6); 13 | 14 | %% Desired States 15 | [DesPos, DesAtt] = CreateDesiredTrajectory(t); 16 | 17 | Xd = DesPos(4); 18 | Yd = DesPos(7); 19 | Zd = DesPos(1); 20 | 21 | Xd_dot = DesPos(5); 22 | Yd_dot = DesPos(8); 23 | Zd_dot = DesPos(2); 24 | 25 | Xd_ddot = DesPos(6); 26 | Yd_ddot = DesPos(9); 27 | Zd_ddot = DesPos(3); 28 | 29 | Phid = DesAtt(1); 30 | Ttad = DesAtt(4); 31 | Psid = DesAtt(7); 32 | 33 | Phid_dot = DesAtt(2); 34 | Ttad_dot = DesAtt(5); 35 | Psid_dot = DesAtt(8); 36 | 37 | Phid_ddot = DesAtt(3); 38 | Ttad_ddot = DesAtt(6); 39 | Psid_ddot = DesAtt(9); 40 | 41 | X_desired = [X_desired;[Phid,Ttad,Psid,Zd,Xd,Yd]]; 42 | 43 | %% Parameter Definition 44 | global m g dt 45 | global a1 a2 a3 46 | global b1 b2 b3 47 | global c1 c2 c3 c4 c5 c6 48 | global c7 c8 c9 c10 c11 c12 49 | 50 | %% Altitude Control 51 | e7 = Zd - Z; 52 | e8 = Z_dot - Zd_dot - c7*e7; 53 | 54 | U1 = m/cos(Phi)/cos(Tta)*(e7+g+Zd_ddot-c8*e8+c7*(Zd_dot-Z_dot)); 55 | %% x Motion Control 56 | e9 = Xd - X; 57 | e10 = X_dot - Xd_dot - c9*e9; 58 | 59 | Ux = m/U1*(e9+Xd_ddot-c10*e10+c9*(Xd_dot-X_dot)); 60 | Ux_dot = (Ux - Uxy(1))/dt; 61 | Ux_ddot = (Ux - Uxy2(1))/2/dt; 62 | %% y Motion Control 63 | e11 = Yd - Y; 64 | e12 = Y_dot - Yd_dot - c11*e11; 65 | 66 | Uy = m/U1*(e11+Yd_ddot-c12*e12+c11*(Yd_dot-Y_dot)); 67 | Uy_dot = (Uy - Uxy(2))/dt; 68 | Uy_ddot = (Uy - Uxy2(2))/2/dt; 69 | %% Roll Control 70 | Phid = -Uy; 71 | Phid_dot = -Uy_dot; 72 | Phid_ddot = -Uy_ddot; 73 | 74 | e1 = Phid - Phi; 75 | e2 = Phi_dot - Phid_dot - c1*e1; 76 | 77 | c1 = 2; c2 = 2; 78 | 79 | U2 = 1/b1*(e1-a1*Tta_dot*Psi_dot + Phid_ddot-c2*e2+c1*(Phid_dot-Phi_dot)); 80 | %% Pitch Control 81 | Ttad = Ux; 82 | Ttad_dot = Ux_dot; 83 | Ttad_ddot = Ux_ddot; 84 | 85 | e3 = Ttad - Tta; 86 | e4 = Tta_dot - Ttad_dot - c3*e3; 87 | 88 | c3 = 2; c4 = 2; 89 | 90 | U3 = 1/b2*(e3-a2*Phi_dot*Psi_dot+Ttad_ddot-c4*e4+c3*(Ttad_dot-Tta_dot)); 91 | %% Yaw Control 92 | e5 = Psid - Psi; 93 | e6 = Psi_dot - Psid_dot - c5*e5; 94 | 95 | U4 = 1/b3*(e5-a3*Phi_dot*Tta_dot+Psid_ddot-c6*e6+c5*(Psid_dot-Psi_dot)); 96 | %% Control Inputs 97 | U = [U1;U2;U3;U4]; 98 | Uxy = [Ux;Uy]; 99 | 100 | end 101 | -------------------------------------------------------------------------------- /Project/5-Backstepping Control Quadcopter/Main.m: -------------------------------------------------------------------------------- 1 | clc;clear; 2 | close all 3 | 4 | global dt 5 | global X_desired 6 | X_desired = []; 7 | 8 | Path = 'C:\Users\Iman Sharifi\Documents\MATLAB\Nonlinear Control\Project'; 9 | addpath(Path) 10 | Parameters; 11 | Simulation_Params; 12 | 13 | t = 0:dt:Tf; 14 | N = Tf/dt+1; 15 | x = x0; 16 | X = []; 17 | x_ddot = zeros(6,1); 18 | Uxy = zeros(2, N); 19 | Uxy2 = [0;0]; 20 | 21 | for i=1:N 22 | 23 | [U, Uxy(:,i+1)] = BS_Controller(t(i), x, Uxy(:,i), Uxy2); 24 | Uxy2 = Uxy(:,i); 25 | 26 | k1 = NonLinDynamic_Quadcopter(x, U); 27 | k2 = NonLinDynamic_Quadcopter(x+dt/2*k1, U); 28 | k3 = NonLinDynamic_Quadcopter(x+dt/2*k2, U); 29 | k4 = NonLinDynamic_Quadcopter(x+dt*k3, U); 30 | 31 | x = x + dt/6*(k1+2*k2+2*k3+k4); 32 | x_ddot = NonLinDynamic_Quadcopter(x, U); 33 | x_ddot = x_ddot(2:2:end); 34 | X = [X;x']; 35 | end 36 | %% Plot Results 37 | figure 38 | % 3D Position 39 | MyPlot(t,X(:,9),'-b');hold on 40 | MyPlot(t,X(:,11),'-r'); 41 | MyPlot(t,X(:,7),'-g') 42 | grid on;title('Position Response');legend('X','Y','Z') 43 | 44 | figure 45 | % Attitudes 46 | MyPlot(t,rad2deg(X(:,1)),'-b');hold on 47 | MyPlot(t,rad2deg(X(:,3)),'-r'); 48 | grid on;title('Roll & Pitch Angles');legend('\phi','\theta') 49 | 50 | figure 51 | MyPlot(t,rad2deg(X(:,5)),'-g') 52 | grid on;title('Yaw Angle');legend('\psi') 53 | 54 | %% Save Data 55 | filename = [Path '\MatFiles\BSC_Data_Varriable_Trajectory.mat']; 56 | Data = [X, X_desired, t']; 57 | save(filename, 'Data') -------------------------------------------------------------------------------- /Project/5-Backstepping Control Quadcopter/Parameters.m: -------------------------------------------------------------------------------- 1 | % Dynamic Params 2 | global m g 3 | global l b d 4 | global Jx Jy Jz 5 | global a1 a2 a3 6 | global b1 b2 b3 7 | 8 | m = 0.65; 9 | g = 9.81; 10 | l = 0.23; 11 | Jx = 7.5e-3; 12 | Jy = 7.5e-3; 13 | Jz = 1.3e-2; 14 | b = 3.13e-5; 15 | d = 7.5e-5; 16 | 17 | a1 = (Jy-Jz)/Jx; 18 | a2 = (Jz-Jx)/Jy; 19 | a3 = (Jx-Jy)/Jz; 20 | 21 | b1 = l/Jx; 22 | b2 = l/Jy; 23 | b3 = 1/Jz; 24 | 25 | % Control Params 26 | global c1 c2 c3 c4 c5 c6 27 | global c7 c8 c9 c10 c11 c12 28 | 29 | c = 1; 30 | c1 = c;c2 = c; c3 = 0.2; c4 = c;c5 = c; c6 = c; 31 | c7 = c;c8 = c;c9 = c;c10 = c; c11 = c;c12 = c; -------------------------------------------------------------------------------- /Project/6-MRAC Control Quadcopter/Main.m: -------------------------------------------------------------------------------- 1 | clc;clear; 2 | close all 3 | global dt 4 | global A B C D desiredPoles 5 | global Q R N 6 | global Am Bm 7 | 8 | Path = 'C:\Users\Iman Sharifi\Documents\MATLAB\Nonlinear Control\Project'; 9 | addpath(Path) 10 | Simulation_Params; 11 | Parameters; 12 | 13 | % Linearization 14 | [A,B,C,D]=State_Space(); 15 | n = length(A); 16 | ud = size(B, 2); 17 | 18 | % State Feedback params 19 | desiredPoles = -linspace(1,n,n); 20 | % LQR params 21 | Q = 10*eye(n); 22 | Q(7,7) = 1000000; 23 | Q(8,8) = 0; 24 | Q(5,5) = 20; 25 | R = 5*eye(ud); 26 | N = 0; 27 | 28 | Type = 'lqr'; % or 'place' 29 | K = SF_Controller(Type); 30 | 31 | KI = B\(A-B*K); 32 | ud = zeros(12,1);ud(7:2:11) = 1; 33 | 34 | % Initialization Main Loop 35 | dt = 0.005; 36 | Tf = 60; 37 | t = 0:dt:Tf; 38 | N = Tf/dt+1; 39 | x0 = zeros(12,1); 40 | % x0(1:2:5) = deg2rad(1); 41 | % x0(7:2:11) = 5; 42 | x = x0; 43 | xd = x0; 44 | X = []; 45 | Xd = []; 46 | x_ddot = zeros(6,1); 47 | Uxy = [0;0]; 48 | 49 | %% Reference Model params 50 | Am = A - B*K; 51 | Bm = B; 52 | Q = 600*eye(12); 53 | P = lyap(Am',Q); 54 | 55 | % Learning Rates 56 | gamma_x = 0.01*eye(12); 57 | gamma_r = 0.0005*eye(4); 58 | 59 | % Adaption gains initialization 60 | Kx = -K; 61 | Kr = eye(4); 62 | 63 | %% Desired Trajectories 64 | x_ref =[repelem([1 2 1 0 1 2], floor(N/6)) 2]; 65 | y_ref =[repelem([1 2 1 0 1 2], floor(N/6)) 2]; 66 | z_ref =[repelem([1 2 1 0 1 2], floor(N/6)) 2]+1; 67 | 68 | ref = repmat(zeros(12,1), [1, N]); 69 | ref(9,:) = x_ref; 70 | ref(11,:)= y_ref; 71 | ref(7,:) = z_ref; 72 | 73 | %% Main Loop 74 | for i=1:N 75 | 76 | r = -KI*ref(:,i); 77 | u = Kx*x + Kr'*r; 78 | 79 | x = RK4(@NonLinDynamic_Quadcopter, x, u); 80 | 81 | ud = - KI*ref(:,i); 82 | xd = RK4(@ReferenceModel, xd, ud); 83 | 84 | e = x - xd; 85 | disp(['Error X is: ' num2str(e(9))]) 86 | 87 | Kx = Kx - (gamma_x * x * e' * P * B)'*dt ; 88 | Kr = Kr - (gamma_r * r * e' * P * B)'*dt ; 89 | 90 | X = [X; x']; 91 | Xd= [Xd; xd']; 92 | 93 | end 94 | %% Plot Results 95 | % 3D Position 96 | figure 97 | MyPlot(t,X(:,9 ),'-b');hold on 98 | MyPlot(t,Xd(:,9),'-r') 99 | MyPlot(t,ref(9,:),'-g') 100 | grid on;title('X');legend('X','X desired','X reference') 101 | xlabel('Time'), ylabel('Amp') 102 | figure 103 | MyPlot(t,X(:,11),'-b');hold on 104 | MyPlot(t,Xd(:,11),'-r') 105 | MyPlot(t,ref(11,:),'-g') 106 | grid on;title('Y');legend('Y','Y desired','Y reference') 107 | xlabel('Time'), ylabel('Amp') 108 | figure 109 | MyPlot(t,X(:,7),'-b');hold on 110 | MyPlot(t,Xd(:,7),'-r') 111 | MyPlot(t,ref(7,:),'-g') 112 | grid on;title('Z');legend('Z','Z desired','Z reference') 113 | xlabel('Time'), ylabel('Amp') 114 | 115 | figure 116 | % Attitudes 117 | MyPlot(t,rad2deg(X(:,1)),'-b');hold on 118 | MyPlot(t,rad2deg(X(:,3)),'-r');hold on 119 | MyPlot(t,rad2deg(X(:,5)),'-g') 120 | grid on;title('Euler Angles');legend('\phi','\theta','\psi') 121 | xlabel('Time'), ylabel('Amp') 122 | 123 | % figure 124 | % MyPlot(t,rad2deg(X(:,5)),'-g') 125 | % grid on;title('Yaw Angle');legend('\psi') 126 | %% Save Data 127 | filename = [Path '\MatFiles\MRAC_Data.mat']; 128 | Data = [X,Xd,ref',t']; 129 | save(filename, 'Data') -------------------------------------------------------------------------------- /Project/6-MRAC Control Quadcopter/Parameters.m: -------------------------------------------------------------------------------- 1 | % Dynamic Params 2 | global m g 3 | global l b d 4 | global Jx Jy Jz 5 | global a1 a2 a3 6 | global b1 b2 b3 7 | 8 | m = 0.65; 9 | g = 9.81; 10 | l = 0.23; 11 | Jx = 7.5e-3; 12 | Jy = 7.5e-3; 13 | Jz = 1.3e-2; 14 | b = 3.13e-5; 15 | d = 7.5e-5; 16 | 17 | a1 = (Jy-Jz)/Jx; 18 | a2 = (Jz-Jx)/Jy; 19 | a3 = (Jx-Jy)/Jz; 20 | 21 | b1 = l/Jx; 22 | b2 = l/Jy; 23 | b3 = 1/Jz; 24 | 25 | % Control Params 26 | global c1 c2 c3 c4 c5 c6 27 | global k1 k2 k3 k4 k5 k6 k7 k8 k9 k10 k11 k12 28 | 29 | c = 1; 30 | k = 1; 31 | 32 | c1 = c;c2 = c; c3 = 2; c4 = c;c5 = c; c6 = c; 33 | 34 | k1 = k;k2 = k;k3 = k;k4 = k;k5 = 1; 35 | k6 = 1;k7 = k;k8 = k;k9 = k;k10 = k; 36 | k11 = k;k12 = k; -------------------------------------------------------------------------------- /Project/6-MRAC Control Quadcopter/RK4.m: -------------------------------------------------------------------------------- 1 | function x = RK4(func, x, u) 2 | 3 | global dt 4 | k1 = func(x, u); 5 | k2 = func(x+dt/2*k1, u); 6 | k3 = func(x+dt/2*k2, u); 7 | k4 = func(x+dt*k3, u); 8 | x = x + dt/6*(k1+2*k2+2*k3+k4); 9 | 10 | end 11 | 12 | -------------------------------------------------------------------------------- /Project/6-MRAC Control Quadcopter/ReferenceModel.m: -------------------------------------------------------------------------------- 1 | function x_dot = ReferenceModel(x, u) 2 | 3 | global Am Bm 4 | x_dot = Am*x + Bm*u; 5 | 6 | end 7 | 8 | -------------------------------------------------------------------------------- /Project/6-MRAC Control Quadcopter/SF_Controller.m: -------------------------------------------------------------------------------- 1 | function K = SF_Controller(Type) 2 | 3 | global A B desiredPoles 4 | global Q R N 5 | 6 | Type = lower(Type); 7 | 8 | if isempty(Type) 9 | Type = 'place'; 10 | end 11 | 12 | switch Type 13 | case 'place' 14 | K = place(A, B, desiredPoles); 15 | case 'lqr' 16 | [K, P, e] = lqr(A, B, Q, R, N); 17 | end 18 | 19 | end 20 | 21 | -------------------------------------------------------------------------------- /Project/6-MRAC Control Quadcopter/State_Space.m: -------------------------------------------------------------------------------- 1 | function [A,B,C,D]=State_Space() 2 | 3 | global m g 4 | global a1 a2 a3 5 | global b1 b2 b3 6 | 7 | syms x1 x2 x3 x4 x5 x6 8 | syms x7 x8 x9 x10 x11 x12 9 | syms U1 U2 U3 U4 10 | 11 | Ux = cos(x1)*sin(x3)*cos(x5)+sin(x1)*sin(x5); 12 | Uy = cos(x1)*sin(x3)*sin(x5)-sin(x1)*cos(x5); 13 | 14 | dx1 = x2; 15 | dx2 = x4*x6*a1+ U2*b1; 16 | dx3 = x4; 17 | dx4 = x2*x6*a2+ U3*b2; 18 | dx5 = x6; 19 | dx6 = x2*x4*a3+ U4*b3; 20 | dx7 = x8; 21 | dx8 = U1/m*(cos(x1)*cos(x3))-g; 22 | dx9 = x10; 23 | dx10 = U1/m*Ux; 24 | dx11 = x12; 25 | dx12 = U1/m*Uy; 26 | 27 | x = [x1;x2;x3;x4;x5;x6;x7;x8;x9;x10;x11;x12]; 28 | dx = [dx1;dx2;dx3;dx4;dx5;dx6;dx7;dx8;dx9;dx10;dx11;dx12]; 29 | u = [U1;U2;U3;U4]; 30 | 31 | A = jacobian(dx,x); 32 | A = simplify(A); 33 | B = jacobian(dx,u); 34 | B = simplify(B); 35 | 36 | n = length(x); 37 | r = length(u); 38 | 39 | State_Input_Vars = [x.', u.']; 40 | xeq = [zeros(6,1);x7;0;x9;0;x11;0]; 41 | ueq = [m*g;0;0;0]; 42 | eq_point = [xeq.', ueq.']; 43 | 44 | A = subs(A, State_Input_Vars, eq_point); 45 | B = subs(B, State_Input_Vars, eq_point); 46 | 47 | A = vpa(A,6); 48 | B = vpa(B,6); 49 | A = double(A); 50 | B = double(B); 51 | C = zeros(n,1); 52 | D = zeros(r,1); 53 | 54 | end 55 | 56 | -------------------------------------------------------------------------------- /Project/CreateDesiredTrajectory.m: -------------------------------------------------------------------------------- 1 | function [DesPosition, DesAttitude] = CreateDesiredTrajectory(t) 2 | r = 2; w = 2*pi/40;a = deg2rad(5); 3 | Xd = r*cos(w*t);%1; 4 | Yd = r*sin(w*t);%2; 5 | Zd = t/10; 6 | 7 | dXd = -r*w*sin(w*t); 8 | dYd = r*w*cos(w*t); 9 | dZd = 1/10; 10 | 11 | ddXd = -r*w^2*cos(w*t); 12 | ddYd = -r*w^2*sin(w*t); 13 | ddZd = 0; 14 | 15 | phid = 0; 16 | Ttad = 0; 17 | Psid = a*sin(w*t); 18 | 19 | dphid = 0; 20 | dTtad = 0; 21 | dPsid = a*w*cos(w*t); 22 | 23 | ddphid = 0; 24 | ddTtad = 0; 25 | ddPsid = -a*w^2*sin(w*t); 26 | 27 | DesAttitude = [phid;dphid;ddphid;Ttad;dTtad;ddTtad;Psid;dPsid;ddPsid]; 28 | 29 | DesPosition = [Zd;dZd;ddZd;Xd;dXd;ddXd;Yd;dYd;ddYd]; 30 | end 31 | 32 | -------------------------------------------------------------------------------- /Project/MatFiles/BSC_Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iman-sharifi-ghb/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/5bbfb45ae5765414332bb6df5766a0138010bd09/Project/MatFiles/BSC_Data.mat -------------------------------------------------------------------------------- /Project/MatFiles/Comparison.m: -------------------------------------------------------------------------------- 1 | clc; 2 | clear; close all 3 | 4 | load SMC_Data.mat 5 | X4 = Data; 6 | load FLC_Data.mat 7 | X3 = Data; 8 | load BSC_Data.mat 9 | X5 = Data; 10 | load PID_Data.mat 11 | X1 = Data; 12 | load LQR_Data.mat 13 | X2 = Data; 14 | 15 | t = X1(:,end); 16 | 17 | %% Plot Results 18 | figure 19 | MyPlot(t,X1(:,9 ),'-b');hold on 20 | MyPlot(t,X2(:,9 ),'-r');hold on 21 | MyPlot(t,X3(:,9 ),'-g');hold on 22 | MyPlot(t,X4(:,9 ),'-y');hold on 23 | MyPlot(t,X5(:,9 ),'-p'); 24 | grid on;title('X');legend('PID','LQR','FLC','SMC','BSC') 25 | xlabel('Time'), ylabel('Amp') 26 | figure 27 | MyPlot(t,X1(:,11 ),'-b');hold on 28 | MyPlot(t,X2(:,11 ),'-r');hold on 29 | MyPlot(t,X3(:,11 ),'-g');hold on 30 | MyPlot(t,X4(:,11 ),'-y');hold on 31 | MyPlot(t,X5(:,11 ),'-p'); 32 | grid on;title('Y');legend('PID','LQR','FLC','SMC','BSC') 33 | xlabel('Time'), ylabel('Amp') 34 | figure 35 | MyPlot(t,X1(:,7 ),'-b');hold on 36 | MyPlot(t,X2(:,7 ),'-r');hold on 37 | MyPlot(t,X3(:,7 ),'-g');hold on 38 | MyPlot(t,X4(:,7 ),'-y');hold on 39 | MyPlot(t,X5(:,7 ),'-p'); 40 | grid on;title('Z');legend('PID','LQR','FLC','SMC','BSC') 41 | xlabel('Time'), ylabel('Amp') 42 | 43 | % figure 44 | % % Attitudes 45 | % MyPlot(t,rad2deg(X(:,1)),'-b');hold on 46 | % MyPlot(t,rad2deg(X(:,3)),'-r');hold on 47 | % MyPlot(t,rad2deg(X(:,5)),'-g') 48 | % grid on;title('Euler Angles');legend('\phi','\theta','\psi') 49 | % xlabel('Time'), ylabel('Amp') -------------------------------------------------------------------------------- /Project/MatFiles/FLC_Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iman-sharifi-ghb/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/5bbfb45ae5765414332bb6df5766a0138010bd09/Project/MatFiles/FLC_Data.mat -------------------------------------------------------------------------------- /Project/MatFiles/LQR_Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iman-sharifi-ghb/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/5bbfb45ae5765414332bb6df5766a0138010bd09/Project/MatFiles/LQR_Data.mat -------------------------------------------------------------------------------- /Project/MatFiles/MRAC_Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iman-sharifi-ghb/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/5bbfb45ae5765414332bb6df5766a0138010bd09/Project/MatFiles/MRAC_Data.mat -------------------------------------------------------------------------------- /Project/MatFiles/MyPlot.m: -------------------------------------------------------------------------------- 1 | function [] = MyPlot(x1,x2, Title, labelX, labelY) 2 | 3 | linewidth = 3; 4 | fontsize = 14; 5 | 6 | if nargin == 2 7 | Title = ''; 8 | labelX = ''; 9 | labelY = ''; 10 | elseif nargin == 3 11 | labelX = ''; 12 | labelY = ''; 13 | elseif nargin ==4 14 | labelY = ''; 15 | end 16 | 17 | plot(x1,x2,'linewidth',linewidth) 18 | grid on 19 | set(gca, 'fontsize', fontsize) 20 | title(Title); 21 | xlabel(labelX) 22 | ylabel(labelY) 23 | 24 | end 25 | 26 | -------------------------------------------------------------------------------- /Project/MatFiles/PID_Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iman-sharifi-ghb/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/5bbfb45ae5765414332bb6df5766a0138010bd09/Project/MatFiles/PID_Data.mat -------------------------------------------------------------------------------- /Project/MatFiles/Plotter.m: -------------------------------------------------------------------------------- 1 | clc; 2 | clear; close all 3 | 4 | % load SMC_Data.mat 5 | % load FLC_Data.mat 6 | % load BSC_Data.mat 7 | % load PID_Data.mat 8 | load LQR_Data.mat 9 | 10 | X = Data; 11 | t = X(:,end); 12 | 13 | %% Plot Results 14 | figure 15 | % 3D Position 16 | MyPlot(t,X(:,9),'-b');hold on 17 | MyPlot(t,X(:,11),'-r'); 18 | MyPlot(t,X(:,7),'-g') 19 | grid on;title('Position Response');legend('X','Y','Z') 20 | xlabel('Time'), ylabel('Amp') 21 | 22 | figure 23 | % Attitudes 24 | MyPlot(t,rad2deg(X(:,1)),'-b');hold on 25 | MyPlot(t,rad2deg(X(:,3)),'-r');hold on 26 | MyPlot(t,rad2deg(X(:,5)),'-g') 27 | grid on;title('Euler Angles');legend('\phi','\theta','\psi') 28 | xlabel('Time'), ylabel('Amp') 29 | 30 | % figure 31 | % MyPlot(t,rad2deg(X(:,5)),'-g') 32 | % grid on;title('Yaw Angle');legend('\psi') 33 | % xlabel('Time'), ylabel('Amp') -------------------------------------------------------------------------------- /Project/MatFiles/SMC_Data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iman-sharifi-ghb/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/5bbfb45ae5765414332bb6df5766a0138010bd09/Project/MatFiles/SMC_Data.mat -------------------------------------------------------------------------------- /Project/MatFiles/Variable Trajectory/BSC_Data_Varriable_Trajectory.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iman-sharifi-ghb/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/5bbfb45ae5765414332bb6df5766a0138010bd09/Project/MatFiles/Variable Trajectory/BSC_Data_Varriable_Trajectory.mat -------------------------------------------------------------------------------- /Project/MatFiles/Variable Trajectory/Comparison.m: -------------------------------------------------------------------------------- 1 | clc; 2 | clear; close all 3 | 4 | load SMC_Data_Varriable_Trajectory.mat 5 | X4 = Data; 6 | load FLC_Data_Varriable_Trajectory.mat 7 | X3 = Data; 8 | load BSC_Data_Varriable_Trajectory.mat 9 | X5 = Data; 10 | % load PID_Data.mat 11 | % X1 = Data; 12 | % load LQR_Data.mat 13 | % X2 = Data; 14 | 15 | t = X3(:,end); 16 | 17 | %% Plot Results 18 | figure 19 | plot(t,X3(:,9 ),'-b','LineWidth', 3);hold on 20 | plot(t,X4(:,9 ),'-r','LineWidth', 3);hold on 21 | plot(t,X5(:,9 ),'-y','LineWidth', 3); 22 | plot(t,X3(:,17 ),'--g','LineWidth', 3); 23 | 24 | grid on;title('X');legend('FLC','SMC','BSC','Reference') 25 | xlabel('Time'), ylabel('Amp') 26 | set(gca, 'FontSize', 14) 27 | figure 28 | plot(t,X3(:,11 ),'-b','LineWidth', 3);hold on 29 | plot(t,X4(:,11 ),'-r','LineWidth', 3);hold on 30 | plot(t,X5(:,11 ),'-y','LineWidth', 3); 31 | plot(t,X3(:,18 ),'--g','LineWidth', 3); 32 | grid on;title('Y');legend('FLC','SMC','BSC','Reference') 33 | xlabel('Time'), ylabel('Amp') 34 | set(gca, 'FontSize', 14) 35 | figure 36 | plot(t,X3(:,7 ),'-b','LineWidth', 3);hold on 37 | plot(t,X4(:,7 ),'-r','LineWidth', 3);hold on 38 | plot(t,X5(:,7 ),'-y','LineWidth', 3); 39 | plot(t,X3(:,16 ),'--g','LineWidth', 3); 40 | grid on;title('Z');legend('FLC','SMC','BSC','Reference') 41 | xlabel('Time'), ylabel('Amp') 42 | set(gca, 'FontSize', 14) 43 | 44 | % figure 45 | % % Attitudes 46 | % plot(t,rad2deg(X(:,1)),'-b','LineWidth', 3);hold on 47 | % plot(t,rad2deg(X(:,3)),'-r','LineWidth', 3);hold on 48 | % plot(t,rad2deg(X(:,5)),'-g','LineWidth', 3) 49 | % grid on;title('Euler Angles');legend('\phi','\theta','\psi') 50 | % xlabel('Time'), ylabel('Amp') -------------------------------------------------------------------------------- /Project/MatFiles/Variable Trajectory/FLC_Data_Varriable_Trajectory.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iman-sharifi-ghb/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/5bbfb45ae5765414332bb6df5766a0138010bd09/Project/MatFiles/Variable Trajectory/FLC_Data_Varriable_Trajectory.mat -------------------------------------------------------------------------------- /Project/MatFiles/Variable Trajectory/Plotter.m: -------------------------------------------------------------------------------- 1 | clc; 2 | clear; close all 3 | 4 | load SMC_Data_Varriable_Trajectory.mat 5 | % load FLC_Data_Varriable_Trajectory.mat 6 | % load BSC_Data_Varriable_Trajectory.mat 7 | % load PID_Data_Varriable_Trajectory.mat 8 | % load LQR_Data_Varriable_Trajectory.mat 9 | 10 | X = Data; 11 | t = X(:,end); 12 | 13 | %% Plot Results 14 | figure 15 | % 3D Position 16 | plot(t,X(:,9),'-b','LineWidth', 3);hold on 17 | plot(t,X(:,17),'--g','LineWidth', 3) 18 | grid on;title('X');legend('X','X desired') 19 | xlabel('Time'), ylabel('Amp') 20 | set(gca, 'FontSize', 14) 21 | 22 | figure 23 | plot(t,X(:,9),'-b','LineWidth', 3);hold on 24 | plot(t,X(:,17),'--g','LineWidth', 3) 25 | grid on;title('X');legend('X','X desired') 26 | xlabel('Time'), ylabel('Amp') 27 | set(gca, 'FontSize', 14) 28 | 29 | figure 30 | plot(t,X(:,7),'-b','LineWidth', 3);hold on 31 | plot(t,X(:,16),'--g','LineWidth', 3) 32 | grid on;title('Z');legend('Z','Z desired') 33 | xlabel('Time'), ylabel('Amp') 34 | set(gca, 'FontSize', 14) 35 | 36 | figure 37 | % Attitudes 38 | MyPlot(t,rad2deg(X(:,1)),'-b');hold on 39 | MyPlot(t,rad2deg(X(:,3)),'-r');hold on 40 | MyPlot(t,rad2deg(X(:,5)),'-g') 41 | grid on;title('Euler Angles');legend('\phi','\theta','\psi') 42 | xlabel('Time'), ylabel('Amp') 43 | 44 | % figure 45 | % MyPlot(t,rad2deg(X(:,5)),'-g') 46 | % grid on;title('Yaw Angle');legend('\psi') 47 | % xlabel('Time'), ylabel('Amp') -------------------------------------------------------------------------------- /Project/MatFiles/Variable Trajectory/SMC_Data_Varriable_Trajectory.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iman-sharifi-ghb/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/5bbfb45ae5765414332bb6df5766a0138010bd09/Project/MatFiles/Variable Trajectory/SMC_Data_Varriable_Trajectory.mat -------------------------------------------------------------------------------- /Project/MatFiles/Variable Trajectory/X_response.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iman-sharifi-ghb/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/5bbfb45ae5765414332bb6df5766a0138010bd09/Project/MatFiles/Variable Trajectory/X_response.fig -------------------------------------------------------------------------------- /Project/MatFiles/Variable Trajectory/Y_response.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iman-sharifi-ghb/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/5bbfb45ae5765414332bb6df5766a0138010bd09/Project/MatFiles/Variable Trajectory/Y_response.fig -------------------------------------------------------------------------------- /Project/MatFiles/Variable Trajectory/Z_response.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iman-sharifi-ghb/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/5bbfb45ae5765414332bb6df5766a0138010bd09/Project/MatFiles/Variable Trajectory/Z_response.fig -------------------------------------------------------------------------------- /Project/MyPlot.m: -------------------------------------------------------------------------------- 1 | function [] = MyPlot(x1,x2, Title, labelX, labelY) 2 | 3 | linewidth = 3; 4 | fontsize = 14; 5 | 6 | if nargin == 2 7 | Title = ''; 8 | labelX = ''; 9 | labelY = ''; 10 | elseif nargin == 3 11 | labelX = ''; 12 | labelY = ''; 13 | elseif nargin ==4 14 | labelY = ''; 15 | end 16 | 17 | plot(x1,x2,'linewidth',linewidth) 18 | grid on 19 | set(gca, 'fontsize', fontsize) 20 | title(Title); 21 | xlabel(labelX) 22 | ylabel(labelY) 23 | 24 | end 25 | 26 | -------------------------------------------------------------------------------- /Project/NonLinDynamic_Quadcopter.m: -------------------------------------------------------------------------------- 1 | function [X_dot] = NonLinDynamic_Quadcopter(x,U) 2 | 3 | global m g 4 | global a1 a2 a3 5 | global b1 b2 b3 6 | 7 | X_dot = zeros(12,1); 8 | 9 | Ux = cos(x(1))*sin(x(3))*cos(x(5))+sin(x(1))*sin(x(5)); 10 | Uy = cos(x(1))*sin(x(3))*sin(x(5))-sin(x(1))*cos(x(5)); 11 | 12 | X_dot(1) = x(2); 13 | X_dot(2) = x(4)*x(6)*a1+ U(2)*b1; 14 | X_dot(3) = x(4); 15 | X_dot(4) = x(2)*x(6)*a2+ U(3)*b2; 16 | X_dot(5) = x(6); 17 | X_dot(6) = x(2)*x(4)*a3+ U(4)*b3; 18 | X_dot(7) = x(8); 19 | X_dot(8) = U(1)/m*(cos(x(1))*cos(x(3)))-g; 20 | X_dot(9) = x(10); 21 | X_dot(10)= U(1)/m*Ux; 22 | X_dot(11)= x(12); 23 | X_dot(12)= U(1)/m*Uy; 24 | 25 | end 26 | 27 | -------------------------------------------------------------------------------- /Project/Simulation_Params.m: -------------------------------------------------------------------------------- 1 | dt = 0.01; 2 | Tf = 80; 3 | x0 = zeros(12,1); -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Quadrotor-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms 2 | In this project, we aim to control a 6-Dof, 4-motor robot using Linear, Nonlinear, and Adaptive algorithms. Then, we compare their performance in different scenarios and discuss the results. 3 | 4 | Creator: _Iman Sharifi_ 5 | 6 | Email: iman.sharifi.edu@gmail.com 7 | 8 | 1- [PID Control](https://github.com/98210184/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/tree/main/Project/1-PID%20Control%20Quadcopter) 9 | 10 | 2- [Linear Quadratic Regulator (LQR)](https://github.com/98210184/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/tree/main/Project/2-LQR%20Control%20Quadcopter) 11 | 12 | 3- [Feedback Linearization Control (FLC)](https://github.com/98210184/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/tree/main/Project/3-Feedback%20Linearization%20Control%20Quadcopter) 13 | 14 | 4- [Sliding Mode Control (SMC)](https://github.com/98210184/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/tree/main/Project/4-Sliding%20Mode%20Control%20Quadcopter) 15 | 16 | 5- [Back-Stepping Control (BSC)](https://github.com/98210184/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/tree/main/Project/5-Backstepping%20Control%20Quadcopter) 17 | 18 | 6- [Model Reference Adaptive Control (MRAC)](https://github.com/98210184/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/tree/main/Project/6-MRAC%20Control%20Quadcopter) 19 | 20 | #### Results 21 | you can see the step response of different methods in position control: 22 | 23 | Comparison of X: 24 | 25 | 26 | 27 | Comparison of Y: 28 | 29 | 30 | 31 | Comparison of Z: 32 | 33 | 34 | 35 | #### Model Refrence Adaptive Control (MRAC) method: 36 | 37 | X response: 38 | 39 | 40 | 41 | Y response: 42 | 43 | 44 | 45 | Z response: 46 | 47 | 48 | 49 | Attitude Control: 50 | 51 | 52 | 53 | You can access the Persian report in this [link](https://github.com/98210184/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/blob/main/PersianReport.pdf). 54 | 55 | -------------------------------------------------------------------------------- /image/Attitude MRAC.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iman-sharifi-ghb/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/5bbfb45ae5765414332bb6df5766a0138010bd09/image/Attitude MRAC.png -------------------------------------------------------------------------------- /image/X MRAC.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iman-sharifi-ghb/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/5bbfb45ae5765414332bb6df5766a0138010bd09/image/X MRAC.png -------------------------------------------------------------------------------- /image/Y MRAC.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iman-sharifi-ghb/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/5bbfb45ae5765414332bb6df5766a0138010bd09/image/Y MRAC.png -------------------------------------------------------------------------------- /image/Y comparison.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iman-sharifi-ghb/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/5bbfb45ae5765414332bb6df5766a0138010bd09/image/Y comparison.png -------------------------------------------------------------------------------- /image/Z MRAC.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iman-sharifi-ghb/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/5bbfb45ae5765414332bb6df5766a0138010bd09/image/Z MRAC.png -------------------------------------------------------------------------------- /image/Z comparison.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iman-sharifi-ghb/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/5bbfb45ae5765414332bb6df5766a0138010bd09/image/Z comparison.png -------------------------------------------------------------------------------- /image/x comparision.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iman-sharifi-ghb/Quadcopter-Trajectory-Tracking-using-Adaptive-Nonlinear-Algorithms/5bbfb45ae5765414332bb6df5766a0138010bd09/image/x comparision.png --------------------------------------------------------------------------------