├── AnimationTrajectory.m ├── Main_NMPC_quadrotor_PS.m ├── Main_NMPC_quadrotor_TT.m ├── PlotResponse.m ├── PlotSystemResponse.m ├── PlotTrackingPerformance.m ├── QuadrotorEoM.m ├── README.md ├── ReferenceTrajectory.m ├── RemovePlotWhiteArea.m ├── YiHsuan_Final_Presentation.pdf ├── casadiVariablesDef.m └── shift.m /AnimationTrajectory.m: -------------------------------------------------------------------------------- 1 | % Define body frame 2 | qlw = 1.5; len = 1; 3 | fig = figure(); ax = gca; 4 | xb = quiver3(0,0,0,len,0,0,'r-','LineWidth',qlw); hold on; 5 | yb = quiver3(0,0,0,0,len,0,'g-','LineWidth',qlw); 6 | zb = quiver3(0,0,0,0,0,len,'b-','LineWidth',qlw); 7 | xlim([-10 10]); ylim([-10 10]); zlim([-5 15]); 8 | % xlim([-5 22]); ylim([-5 5]); zlim([0 8]); 9 | xlabel('$x$ (m)'); ylabel('$y$ (m)'); zlabel('$z$ (m)'); 10 | set(ax.XLabel,'Interpreter','latex'); 11 | set(ax.YLabel,'Interpreter','latex'); 12 | set(ax.ZLabel,'Interpreter','latex'); 13 | traj = plot3(x,y,z,'b:','LineWidth',1.2); 14 | tt(1) = plot3(x,y,z,'b:','LineWidth',1.2); 15 | tt(2) = plot3(xd,yd,zd,'r--','LineWidth',1.2); 16 | 17 | %% Function handle of rotation matrices 18 | DCM_Z = @(z) [ cos(z) sin(z) 0; 19 | -sin(z) cos(z) 0; 20 | 0 0 1]; 21 | 22 | DCM_Y = @(y) [ cos(y) 0 -sin(y); 23 | 0 1 0; 24 | sin(y) 0 cos(y)]; 25 | 26 | DCM_X = @(x) [ 1 0 0; 27 | 0 cos(x) sin(x); 28 | 0 -sin(x) cos(x)]; 29 | %% Draw Animation 30 | if recordVideo == 1 31 | writerObj = VideoWriter(videoName,'MPEG-4'); 32 | writerObj.FrameRate = 30; 33 | writerObj.Quality = 100; 34 | open(writerObj); 35 | % text information 36 | H_axes = title(sprintf('Time = %.2f(s), Roll = %.2f(deg), Pitch = %.2f(deg), Yaw = %.2f(deg)',... 37 | t, rad2deg(phi(i)), rad2deg(theta(i)), rad2deg(psi(i))),'Interpreter','latex'); 38 | end 39 | 40 | divider = 8; 41 | RT = zeros(4,4); 42 | for i = 1:divider:length(t) 43 | T = [x(i) y(i) z(i)]'; 44 | % Compute rotation matrix Cbv 45 | C1v = DCM_Z(psi(i)); 46 | C21 = DCM_Y(theta(i)); 47 | Cb2 = DCM_X(phi(i)); 48 | Cbv = Cb2*C21*C1v; 49 | DCM = Cbv'; 50 | RT = [ DCM T ]; 51 | RT = [ RT; [ 0 0 0 1 ] ]; % homogeneous rotation matrix 52 | xr = RT*[1;0;0;0]; xr(4,:) = []; 53 | yr = RT*[0;1;0;0]; yr(4,:) = []; 54 | zr = RT*[0;0;1;0]; zr(4,:) = []; 55 | 56 | if recordVideo == 1 57 | set(H_axes,'String', sprintf('t = %.2f(s), Roll = %.2f(deg), Pitch = %.2f(deg), Yaw = %.2f(deg)',... 58 | t(i), rad2deg(wrapToPi(phi(i))), rad2deg(wrapToPi(theta(i))), rad2deg(wrapToPi(psi(i))))); 59 | set(xb,'XData',x(i),'YData',y(i),'ZData',z(i),'UData',xr(1),'VData',xr(2),'WData',xr(3)); 60 | set(yb,'XData',x(i),'YData',y(i),'ZData',z(i),'UData',yr(1),'VData',yr(2),'WData',yr(3)); 61 | set(zb,'XData',x(i),'YData',y(i),'ZData',z(i),'UData',zr(1),'VData',zr(2),'WData',zr(3)); 62 | set(traj,'XData',x(1:i),'YData',y(1:i),'ZData',z(1:i)); 63 | drawnow; 64 | % Get Frame 65 | writeVideo(writerObj,getframe(fig)); 66 | else 67 | quiver3(x(i),y(i),z(i),xr(1),xr(2),xr(3),'r-','LineWidth',qlw); hold on; 68 | quiver3(x(i),y(i),z(i),yr(1),yr(2),yr(3),'g-','LineWidth',qlw); 69 | quiver3(x(i),y(i),z(i),zr(1),zr(2),zr(3),'b-','LineWidth',qlw); 70 | end 71 | end 72 | % pstar = plot3(18,0,5,'Marker','p','MarkerSize',16,... 73 | % 'MarkerEdgeColor','k',... 74 | % 'MarkerFaceColor','y'); 75 | % legend(pstar,'target point','Interpreter','latex'); 76 | legend(tt,'Actual position','Desired position','Interpreter','latex'); 77 | 78 | if recordVideo == 1 79 | close(writerObj); 80 | end -------------------------------------------------------------------------------- /Main_NMPC_quadrotor_PS.m: -------------------------------------------------------------------------------- 1 | % Quadrotor P2P flight control simulation 2 | % point stabilization + Multiple shooting + Runge Kutta 3 | % Edited by Yi-Hsuan Chen, 03/22/2021 4 | 5 | clc; clear; close all; 6 | 7 | % CasADi v3.5.1 8 | casadipath = 'C:\Users\yihsu\Desktop\KAUST\2021 Spring courses\ECE 372 - Dynamic Programming and Optimal Control\Final Project\casadi-windows-matlabR2016a-v3.5.5'; 9 | addpath(casadipath); 10 | import casadi.* 11 | 12 | %% ======================= User Define Parameters ========================= 13 | %------------------------- Flight path Setting ---------------------------- 14 | uConstraint = 1; % 1: constrained / 0: unconstrained 15 | uUpper = 15; % upper limit of propeller thrust 16 | uLower = 0; % lower limit of propeller thrust 17 | recordVideo = 0; % 1: on / 0: off 18 | videoName = 'NMPC_Quadrotor_PS'; 19 | %% ============= Define symbolic state and control variables ============== 20 | casadiVariablesDef; 21 | 22 | state = [x y z phi theta psi dx dy dz dphi dtheta dpsi]'; 23 | control = [u1;u2;u3;u4]; 24 | 25 | n_state = length(state); 26 | n_control = length(control); 27 | 28 | %% ================= Define equation of motion (rhs) ====================== 29 | QuadrotorEoM; 30 | 31 | % system dynamics Xdot = f(x,u) 32 | rhs = SX.sym('rhs',12,1); 33 | rhs(1:3) = [dx; dy; dz]; 34 | rhs(4:6) = [dphi; dtheta; dpsi]; 35 | rhs(7:9) = [0;0;-grav] + R*[0;0;thrust]/m; % translational dynamics 36 | rhs(10:12) = inv(J)*(tau_b-C*[dphi;dtheta;dpsi]); % rotational dynamics 37 | 38 | % nonlinear mapping function f(x,u) 39 | f = Function('f',{state,control},{rhs}); 40 | 41 | %% ================ Define MPC and reformulate into NLP =================== 42 | dt = 0.2; % sampling time 43 | N = 10; % prediction horizon 44 | U = SX.sym('U',n_control,N); % Decision variables (controls) 45 | P = SX.sym('P',n_state*2); % parameters (initial state + reference state) 46 | X = SX.sym('X',n_state,(N+1)); % state variables (current state + predicted states) 47 | 48 | % initialize the objective function and constraint vector 49 | obj = 0; 50 | g = []; 51 | 52 | % Define weighting matrices Q and R 53 | Q = zeros(n_state,n_state); 54 | Q(1:3,1:3) = 50*eye(3); Q(4:5,4:5) = eye(2); Q(6,6) = 20; 55 | R = eye(n_control,n_control); 56 | 57 | 58 | % initial state and condition constraint 59 | st = X(:,1); 60 | g = [g; st-P(1:n_state)]; 61 | 62 | % compute objective symbolically 63 | for k = 1:N 64 | st = X(:,k); con = U(:,k); 65 | xref = P(n_state+1:end); 66 | obj = obj+(st-xref)'*Q*(st-xref)+con'*R*con; 67 | % calculate x(k+1) using numerical integration (RK4) 68 | st_next = X(:,k+1); 69 | k1 = f(st,con); 70 | k2 = f(st + dt/2*k1, con); 71 | k3 = f(st + dt/2*k2, con); 72 | k4 = f(st + dt*k3, con); 73 | st_next_rk4 = st + dt/6*(k1+2*k2+2*k3+k4); % predicted states 74 | g = [g;st_next-st_next_rk4]; 75 | end 76 | 77 | % reshape the decision variables into one column vector 78 | OPT_variables = [reshape(X,n_state*(N+1),1); reshape(U,n_control*N,1)]; 79 | 80 | % Define NLP problem 81 | nlp_prob = struct('f',obj,'x',OPT_variables,'g',g,'p',P); 82 | 83 | % NLP solver settings 84 | opts = struct; 85 | opts.ipopt.max_iter = 2000; 86 | opts.ipopt.print_level = 0; % 0,3 87 | opts.print_time = 0; 88 | opts.ipopt.acceptable_tol = 1e-8; 89 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 90 | 91 | solver = nlpsol('solver','ipopt',nlp_prob,opts); 92 | 93 | args = struct; 94 | % Equality constraints (dynamics equation) 95 | args.lbg(1:n_state*(N+1)) = 0; 96 | args.ubg(1:n_state*(N+1)) = 0; 97 | 98 | % state constraints (unconstrained) 99 | args.lbx(1:n_state*(N+1)) = -inf; 100 | args.ubx(1:n_state*(N+1)) = inf; 101 | 102 | % control constraints 103 | switch uConstraint 104 | case 0 % unconstrained 105 | umax = inf*ones(4,1); 106 | umin = -inf*ones(4,1); 107 | case 1 % constrained 108 | umax = [uUpper;uUpper;uUpper;uUpper]; 109 | umin = [uLower;uLower;uLower;uLower]; 110 | end 111 | 112 | args.lbx(n_state*(N+1)+1:n_control:n_state*(N+1)+n_control*N) = umin(1); 113 | args.ubx(n_state*(N+1)+1:n_control:n_state*(N+1)+n_control*N) = umax(1); 114 | args.lbx(n_state*(N+1)+2:n_control:n_state*(N+1)+n_control*N) = umin(2); 115 | args.ubx(n_state*(N+1)+2:n_control:n_state*(N+1)+n_control*N) = umax(2); 116 | args.lbx(n_state*(N+1)+3:n_control:n_state*(N+1)+n_control*N) = umin(3); 117 | args.ubx(n_state*(N+1)+3:n_control:n_state*(N+1)+n_control*N) = umax(3); 118 | args.lbx(n_state*(N+1)+4:n_control:n_state*(N+1)+n_control*N) = umin(4); 119 | args.ubx(n_state*(N+1)+4:n_control:n_state*(N+1)+n_control*N) = umax(4); 120 | 121 | %% ================== The simulation loop start here ====================== 122 | t0 = 0; tf = 20; 123 | x0 = [0;0;0;0;0;0;0;0;0;0;0;0]; 124 | xd = 3; yd = 0; zd = 5; 125 | xs = [xd;yd;zd;0;0;0;0;0;0;0;0;0]; 126 | 127 | xx(:,1) = x0; % store the state history 128 | t(1) = t0; % time series 129 | 130 | u0 = zeros(N,n_control); % four control inputs 131 | X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables 132 | 133 | sim_time = tf+dt; % maximum simulation time 134 | tol = 1e-2; % error tolerance 135 | 136 | mpciter = 0; 137 | xx1 = []; 138 | u_cl = []; 139 | 140 | % main simulation loop - it works as long as the error is greater than tol 141 | % and the number of MPC steps is less than its maximum value 142 | while ( norm((x0-xs),2) > tol && mpciter < sim_time / dt) 143 | args.p = [x0;xs]; % set the values of the parameters vector 144 | % initialize the optimization variables 145 | args.x0 = [reshape(X0',n_state*(N+1),1); reshape(u0',n_control*N,1)]; 146 | sol = solver('x0',args.x0,'lbx',args.lbx,'ubx',args.ubx,... 147 | 'lbg',args.lbg,'ubg',args.ubg,'p',args.p); 148 | % get the control inputs from the solution 149 | u = reshape(full(sol.x(n_state*(N+1)+1:end))',n_control,N)'; 150 | % get state trajectory 151 | xx1(:,1:n_state,mpciter+1) = reshape(full(sol.x(1:n_state*(N+1)))',n_state,N+1)'; 152 | % apply only the first element of control sequence 153 | u_cl = [u_cl; u(1,:)]; 154 | t(mpciter+1) = t0; 155 | % Update the control and shift the solution 156 | [t0,x0,u0] = shift(dt,t0,x0,u,f); 157 | xx(:,mpciter+2) = x0; 158 | % get solution trajectory 159 | X0 = reshape(full(sol.x(1:n_state*(N+1)))',n_state,N+1)'; 160 | % shift trajectory to initializa the next step 161 | X0 = [X0(2:end,:); X0(end,:)]; 162 | mpciter = mpciter + 1; 163 | end 164 | ss_error = norm((x0-xs),2); 165 | 166 | %%======================= Draw Point Stabilization response ========================== 167 | PlotSystemResponse; 168 | %%========================== Animation ==================================== 169 | AnimationTrajectory; -------------------------------------------------------------------------------- /Main_NMPC_quadrotor_TT.m: -------------------------------------------------------------------------------- 1 | % Quadrotor trajectory tracking flight control simulation 2 | % trajectory tracking + Multiple shooting + Runge Kutta 3 | % Edited by Yi-Hsuan Chen 03/22/2020 4 | 5 | clc; clear; close all; 6 | casadipath = 'C:\Users\yihsu\Desktop\KAUST\2021 Spring courses\ECE 372 - Dynamic Programming and Optimal Control\Final Project\casadi-windows-matlabR2016a-v3.5.5'; 7 | addpath(casadipath); 8 | import casadi.* 9 | 10 | %% ======================= User Define Parameters ========================= 11 | %------------------------- Flight path Setting ---------------------------- 12 | flightpath = 'H'; % e: eight / c: circular / h: helix / H: complex helix / p: P2P 13 | %------------------------- MPC Setting ---------------------------- 14 | dt = 0.2; % sampling time 15 | N = 30; % prediction horizon 16 | uConstraint = 1; % 1: constrained / 0: unconstrained 17 | uUpper = 15; % upper limit of propeller thrust 18 | uLower = 0; % lower limit of propeller thrust 19 | %------------------------- Data saving ---------------------------- 20 | recordVideo = 0; % 1: on / 0: off 21 | saveFig = 0; % 1: on / 0: off 22 | videoName = 'NMPC_Quadrotor_e'; 23 | %% ============= Define symbolic state and control variables ============== 24 | casadiVariablesDef; 25 | state = [x y z phi theta psi dx dy dz dphi dtheta dpsi]'; 26 | control = [u1;u2;u3;u4]; % u: propeller thrust fi = k*wi^2 27 | 28 | n_state = length(state); 29 | n_control = length(control); 30 | n_opt = n_state + n_control; %... new variable 31 | 32 | %% ================= Define equation of motion (rhs) ====================== 33 | QuadrotorEoM; 34 | 35 | % system dynamics Xdot = f(x,u) 36 | rhs = SX.sym('rhs',12,1); 37 | rhs(1:3) = [dx; dy; dz]; 38 | rhs(4:6) = [dphi; dtheta; dpsi]; 39 | rhs(7:9) = [0;0;-grav] + R*[0;0;thrust]/m; % translational dynamics 40 | rhs(10:12) = inv(J)*(tau_b-C*[dphi;dtheta;dpsi]); % rotational dynamics 41 | 42 | % nonlinear mapping function f(x,u) 43 | f = Function('f',{state,control},{rhs}); 44 | 45 | %% ================ Define MPC and reformulate into NLP =================== 46 | U = SX.sym('U',n_control,N); % Decision variables (controls) 47 | P = SX.sym('P',n_state+n_opt*N); % new... parameters (initial state + reference trajectory + uref) 48 | X = SX.sym('X',n_state,(N+1)); % state variables (current state + predicted states) 49 | 50 | % initialize the objective function and constraint vector 51 | obj = 0; g = []; 52 | 53 | % Define weighting matrices Q and R 54 | Q = zeros(n_state,n_state); 55 | Q(1:3,1:3) = 50*eye(3); Q(4:5,4:5) = eye(2); Q(6,6) = 20; 56 | R = eye(n_control,n_control); 57 | 58 | % initial state and condition constraint 59 | st = X(:,1); 60 | g = [g; st-P(1:n_state)]; 61 | 62 | % compute objective symbolically 63 | for k = 1:N 64 | st = X(:,k); con = U(:,k); 65 | xref = P(n_opt*k-n_control+1 : n_opt*k-n_control+n_state); % new... reference trajectoryt 66 | uref = P(n_opt*k-n_control+n_state+1 : n_opt*k+n_state); % new... reference control 67 | obj = obj+(st-xref)'*Q*(st-xref) + (con-uref)'*R*(con-uref) ; % new... calculate obj 68 | % calculate x(k+1) using numerical integration (RK4) 69 | st_next = X(:,k+1); 70 | k1 = f(st,con); 71 | k2 = f(st + dt/2*k1, con); 72 | k3 = f(st + dt/2*k2, con); 73 | k4 = f(st + dt*k3, con); 74 | st_next_rk4 = st + dt/6*(k1+2*k2+2*k3+k4); % predicted states 75 | g = [g;st_next-st_next_rk4]; 76 | end 77 | 78 | % reshape the decision variables into one column vector 79 | OPT_variables = [reshape(X,n_state*(N+1),1); reshape(U,n_control*N,1)]; 80 | 81 | % Define NLP problem 82 | nlp_prob = struct('f',obj,'x',OPT_variables,'g',g,'p',P); 83 | 84 | % NLP solver settings 85 | opts = struct; 86 | opts.ipopt.max_iter = 2000; 87 | opts.ipopt.print_level = 0; % 0,3 88 | opts.print_time = 0; 89 | opts.ipopt.acceptable_tol = 1e-8; 90 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 91 | 92 | solver = nlpsol('solver','ipopt',nlp_prob,opts); 93 | 94 | args = struct; 95 | % Equality constraints (dynamics equation) 96 | args.lbg(1:n_state*(N+1)) = 0; 97 | args.ubg(1:n_state*(N+1)) = 0; 98 | 99 | % state constraints (unconstrained) 100 | args.lbx(1:n_state*(N+1)) = -inf; 101 | args.ubx(1:n_state*(N+1)) = inf; 102 | 103 | % control constraints 104 | switch uConstraint 105 | case 0 % unconstrained 106 | umax = inf*ones(4,1); 107 | umin = -inf*ones(4,1); 108 | case 1 % constrained 109 | umax = [uUpper;uUpper;uUpper;uUpper]; 110 | umin = [uLower;uLower;uLower;uLower]; 111 | end 112 | 113 | args.lbx(n_state*(N+1)+1:n_control:n_state*(N+1)+n_control*N) = umin(1); 114 | args.ubx(n_state*(N+1)+1:n_control:n_state*(N+1)+n_control*N) = umax(1); 115 | args.lbx(n_state*(N+1)+2:n_control:n_state*(N+1)+n_control*N) = umin(2); 116 | args.ubx(n_state*(N+1)+2:n_control:n_state*(N+1)+n_control*N) = umax(2); 117 | args.lbx(n_state*(N+1)+3:n_control:n_state*(N+1)+n_control*N) = umin(3); 118 | args.ubx(n_state*(N+1)+3:n_control:n_state*(N+1)+n_control*N) = umax(3); 119 | args.lbx(n_state*(N+1)+4:n_control:n_state*(N+1)+n_control*N) = umin(4); 120 | args.ubx(n_state*(N+1)+4:n_control:n_state*(N+1)+n_control*N) = umax(4); 121 | 122 | %% ================== The simulation loop start here ====================== 123 | t0 = 0; tf = 60; 124 | x0 = [0;0;0;0;0;0;0;0;0;0;0;0]; 125 | 126 | xx(:,1) = x0; % store the state history 127 | t(1) = t0; % time series 128 | 129 | u0 = zeros(N,n_control); % four control inputs 130 | X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables 131 | 132 | sim_time = tf; % maximum simulation time 133 | tol = 1e-2; % error tolerance 134 | 135 | mpciter = 0; 136 | xx1 = []; u_cl = []; 137 | 138 | % main simulation loop - it works as long as the error is greater than tol 139 | % and the number of MPC steps is less than its maximum value 140 | while ( mpciter < sim_time / dt) % new... condition for ending the loop 141 | current_time = mpciter*dt; % new... get the current time 142 | %====================================================================== 143 | args.p(1:n_state) = x0; % new... initial condition of the quadrotor 144 | for k = 1:N 145 | t_predict = current_time + (k-1)*dt; 146 | stateref = ReferenceTrajectory(t_predict,flightpath); 147 | unominal = m*grav/4*ones(4,1); 148 | args.p(n_opt*k-n_control+1 : n_opt*k-n_control+n_state) = stateref; 149 | args.p(n_opt*k-n_control+n_state+1 : n_opt*k+n_state) = unominal; 150 | end 151 | %====================================================================== 152 | % initialize the optimization variables 153 | args.x0 = [reshape(X0',n_state*(N+1),1); reshape(u0',n_control*N,1)]; 154 | sol = solver('x0',args.x0,'lbx',args.lbx,'ubx',args.ubx,... 155 | 'lbg',args.lbg,'ubg',args.ubg,'p',args.p); 156 | % get the control inputs from the solution 157 | u = reshape(full(sol.x(n_state*(N+1)+1:end))',n_control,N)'; 158 | % get state trajectory 159 | xx1(:,1:n_state,mpciter+1) = reshape(full(sol.x(1:n_state*(N+1)))',n_state,N+1)'; 160 | % apply only the first element of control sequence 161 | u_cl = [u_cl; u(1,:)]; 162 | t(mpciter+1) = t0; 163 | % Update the control and shift the solution 164 | [t0,x0,u0] = shift(dt,t0,x0,u,f); 165 | xx(:,mpciter+2) = x0; 166 | % get solution trajectory 167 | X0 = reshape(full(sol.x(1:n_state*(N+1)))',n_state,N+1)'; 168 | % shift trajectory to initializa the next step 169 | X0 = [X0(2:end,:); X0(end,:)]; 170 | mpciter = mpciter + 1; 171 | end 172 | 173 | %%======================= Draw tracking response ========================== 174 | PlotTrackingPerformance; 175 | %%========================== Animation ==================================== 176 | AnimationTrajectory; -------------------------------------------------------------------------------- /PlotResponse.m: -------------------------------------------------------------------------------- 1 | %%... control forces u = [de T] 2 | fig(1) = figure(); 3 | subplot(211); ax(1) = gca; 4 | p(1) = stairs(t,u_cl(:,1),'b'); hold on; 5 | p(2:3) = plot(t,umax(1)*ones(length(t),1),'k--',t,umin(1)*ones(length(t),1),'k--'); 6 | ylabel('$f_1$ (N)'); 7 | subplot(212); ax(2) = gca; 8 | p(4) = stairs(t,u_cl(:,2),'b'); hold on; 9 | p(5:6) = plot(t,umax(2)*ones(length(t),1),'k--',t,umin(2)*ones(length(t),1),'k--'); 10 | ylabel('$f_2$ (N)'); 11 | 12 | %%... regulation performance 13 | time = 0:dt:sim_time; xx = xx'; 14 | u = xx(:,1); w = xx(:,2); q = xx(:,3); 15 | theta = xx(:,4); h = xx(:,5); 16 | 17 | fig(2) = figure(); 18 | subplot(231); ax(3) = gca; 19 | p(7) = plot(time,u,'b'); 20 | xlabel('time (sec)'); ylabel('$u$ (m/s)'); 21 | subplot(232); ax(4) = gca; 22 | p(8) = plot(time,w,'b'); 23 | xlabel('time (sec)'); ylabel('$w$ (m/s)'); 24 | subplot(233); ax(5) = gca; 25 | p(9) = plot(time,q,'b'); 26 | xlabel('time (sec)'); ylabel('$q$ (rad/s)') 27 | subplot(234); ax(6) = gca; 28 | p(10) = plot(time,theta,'b'); 29 | xlabel('time (sec)'); ylabel('$\theta$ (rad)') 30 | subplot(235); ax(7) = gca; 31 | p(11) = plot(time,h,'b'); 32 | xlabel('time (sec)'); ylabel('$h$ (m)') 33 | 34 | buffer = 3; 35 | set(p,'Linewidth',1); 36 | set(p(1:3:10),'Linewidth',1.5) 37 | set(p(13:end),'Linewidth',1.5) 38 | 39 | set(ax(1:2),'XLim',[0 t(end)],'YLim',[umin(1)-buffer umax(1)+buffer]); 40 | set(ax,'XGrid','on','YGrid','on'); 41 | 42 | for i = 1:length(ax) 43 | set(ax(i).XLabel,'Interpreter','latex'); 44 | set(ax(i).YLabel,'Interpreter','latex'); 45 | % RemovePlotWhiteArea(ax(i)); 46 | end 47 | 48 | % for i = 1:length(fig) 49 | % print(fig(i),['Figure\' fig(i).Name],'-depsc'); 50 | % end -------------------------------------------------------------------------------- /PlotSystemResponse.m: -------------------------------------------------------------------------------- 1 | %%... control forces u = [f1 f2 f3 f4] 2 | fig(1) = figure(); 3 | subplot(411); ax(1) = gca; 4 | p(1) = stairs(t,u_cl(:,1),'b'); hold on; 5 | p(2:3) = plot(t,umax(1)*ones(length(t),1),'k--',t,umin(1)*ones(length(t),1),'k--'); 6 | ylabel('$f_1$ (N)'); 7 | subplot(412); ax(2) = gca; 8 | p(4) = stairs(t,u_cl(:,2),'b'); hold on; 9 | p(5:6) = plot(t,umax(2)*ones(length(t),1),'k--',t,umin(2)*ones(length(t),1),'k--'); 10 | ylabel('$f_2$ (N)'); 11 | subplot(413); ax(3) = gca; 12 | p(7) = stairs(t,u_cl(:,3),'b'); hold on; 13 | p(8:9) = plot(t,umax(3)*ones(length(t),1),'k--',t,umin(3)*ones(length(t),1),'k--'); 14 | ylabel('$f_3$ (N)') 15 | subplot(414); ax(4) = gca; 16 | p(10) = stairs(t,u_cl(:,4),'b'); hold on; 17 | p(11:12) = plot(t,umax(4)*ones(length(t),1),'k--',t,umin(4)*ones(length(t),1),'k--'); 18 | xlabel('time (sec)'); ylabel('$f_4$ (N)') 19 | 20 | %%... tracking performance 21 | time = 0:dt:sim_time; xx = xx'; 22 | x = xx(:,1); y = xx(:,2); z = xx(:,3); 23 | phi = xx(:,4); theta = xx(:,5); psi = xx(:,6); 24 | 25 | fig(2) = figure(); 26 | subplot(231); ax(5) = gca; 27 | p(13:14) = plot(time,x,'b',time,xd*ones(length(time),1),'r--'); 28 | xlabel('time (sec)'); ylabel('$x$ (m)'); 29 | subplot(232); ax(6) = gca; 30 | p(15:16) = plot(time,y,'b',time,yd*ones(length(time),1),'r--'); 31 | xlabel('time (sec)'); ylabel('$y$ (m)'); 32 | subplot(233); ax(7) = gca; 33 | p(17:18) = plot(time,z,'b',time,zd*ones(length(time),1),'r--'); 34 | xlabel('time (sec)'); ylabel('$z$ (m)') 35 | subplot(234); ax(8) = gca; 36 | p(19) = plot(time,phi,'b'); 37 | xlabel('time (sec)'); ylabel('$\phi$ (rad)') 38 | subplot(235); ax(9) = gca; 39 | p(20) = plot(time,theta,'b'); 40 | xlabel('time (sec)'); ylabel('$\theta$ (rad)') 41 | subplot(236); ax(10) = gca; 42 | p(21) = plot(time,psi,'b'); 43 | xlabel('time (sec)'); ylabel('$\psi$ (rad)'); 44 | 45 | buffer = 3; 46 | set(p,'Linewidth',1); 47 | set(p(1:3:10),'Linewidth',1.5) 48 | set(p(13:end),'Linewidth',1.5) 49 | 50 | set(ax(1:4),'XLim',[0 t(end)],'YLim',[umin(1)-buffer umax(1)+buffer]); 51 | set(ax,'XGrid','on','YGrid','on'); 52 | 53 | for i = 1:length(ax) 54 | set(ax(i).XLabel,'Interpreter','latex'); 55 | set(ax(i).YLabel,'Interpreter','latex'); 56 | % RemovePlotWhiteArea(ax(i)); 57 | end 58 | 59 | % for i = 1:length(fig) 60 | % print(fig(i),['Figure\' fig(i).Name],'-depsc'); 61 | % end -------------------------------------------------------------------------------- /PlotTrackingPerformance.m: -------------------------------------------------------------------------------- 1 | %%... control forces u = [f1 f2 f3 f4] 2 | fig(1) = figure('Name','p2p_u'); 3 | subplot(411); ax(1) = gca; 4 | p(1) = stairs(t,u_cl(:,1),'b'); hold on; 5 | p(2:3) = plot(t,umax(1)*ones(length(t),1),'k--',t,umin(1)*ones(length(t),1),'k--'); 6 | ylabel('$f_1$ (N)'); 7 | subplot(412); ax(2) = gca; 8 | p(4) = stairs(t,u_cl(:,2),'b'); hold on; 9 | p(5:6) = plot(t,umax(2)*ones(length(t),1),'k--',t,umin(2)*ones(length(t),1),'k--'); 10 | ylabel('$f_2$ (N)'); 11 | subplot(413); ax(3) = gca; 12 | p(7) = stairs(t,u_cl(:,3),'b'); hold on; 13 | p(8:9) = plot(t,umax(3)*ones(length(t),1),'k--',t,umin(3)*ones(length(t),1),'k--'); 14 | ylabel('$f_3$ (N)') 15 | subplot(414); ax(4) = gca; 16 | p(10) = stairs(t,u_cl(:,4),'b'); hold on; 17 | p(11:12) = plot(t,umax(4)*ones(length(t),1),'k--',t,umin(4)*ones(length(t),1),'k--'); 18 | xlabel('time (sec)'); ylabel('$f_4$ (N)') 19 | 20 | 21 | %%... tracking performance 22 | time = 0:dt:sim_time; xx = xx'; 23 | x = xx(:,1); y = xx(:,2); z = xx(:,3); 24 | phi = xx(:,4); theta = xx(:,5); psi = xx(:,6); 25 | 26 | % getting desired trajectory 27 | traj = ReferenceTrajectory(time,flightpath); 28 | xd = traj(1,:)'; yd = traj(2,:)'; zd = traj(3,:)'; 29 | phid = traj(4,:)'; thetad = traj(5,:)'; psid = traj(6,:)'; 30 | 31 | 32 | fig(2) = figure('Name','p2p_x'); ax(5) = gca; 33 | p(13:14) = plot(time,x,'b',time,xd,'r--'); 34 | xlabel('time (sec)'); ylabel('$x$ (m)'); 35 | 36 | fig(3) = figure('Name','p2p_y'); ax(6) = gca; 37 | p(15:16) = plot(time,y,'b',time,yd,'r--'); 38 | xlabel('time (sec)'); ylabel('$y$ (m)'); 39 | 40 | fig(4) = figure('Name','p2p_z'); ax(7) = gca; 41 | p(17:18) = plot(time,z,'b',time,zd,'r--'); 42 | xlabel('time (sec)'); ylabel('$z$ (m)'); 43 | 44 | fig(5) = figure('Name','p2p_phi'); ax(8) = gca; 45 | p(19) = plot(time,phi,'b'); 46 | xlabel('time (sec)'); ylabel('$\phi$ (rad)'); 47 | 48 | fig(6) = figure('Name','p2p_theta'); ax(9) = gca; 49 | p(20) = plot(time,theta,'b'); 50 | xlabel('time (sec)'); ylabel('$\theta$ (rad)'); 51 | 52 | fig(7) = figure('Name','p2p_psi'); ax(10) = gca; 53 | p(21) = plot(time,psi,'b'); 54 | xlabel('time (sec)'); ylabel('$\psi$ (rad)'); 55 | 56 | 57 | % fig(2) = figure('Name','p2p_x'); 58 | % subplot(231); ax(5) = gca; 59 | % p(13:14) = plot(time,x,'b',time,xd,'r--'); 60 | % xlabel('time (sec)'); ylabel('$x$ (m)'); 61 | % subplot(232); ax(6) = gca; 62 | % p(15:16) = plot(time,y,'b',time,yd,'r--'); 63 | % xlabel('time (sec)'); ylabel('$y$ (m)'); 64 | % subplot(233); ax(7) = gca; 65 | % p(17:18) = plot(time,z,'b',time,zd,'r--'); 66 | % xlabel('time (sec)'); ylabel('$z$ (m)'); 67 | % subplot(234); ax(8) = gca; 68 | % p(19) = plot(time,phi,'b'); 69 | % xlabel('time (sec)'); ylabel('$\phi$ (rad)') 70 | % subplot(235); ax(9) = gca; 71 | % p(20) = plot(time,theta,'b'); 72 | % xlabel('time (sec)'); ylabel('$\theta$ (rad)') 73 | % subplot(236); ax(10) = gca; 74 | % p(21) = plot(time,psi,'b'); 75 | % xlabel('time (sec)'); ylabel('$\psi$ (rad)'); 76 | 77 | buffer = 3; 78 | set(p,'Linewidth',1); 79 | set(p(1:3:10),'Linewidth',1.5) 80 | set(p(13:end),'Linewidth',1.5) 81 | 82 | set(ax(1:4),'XLim',[0 t(end)],'YLim',[umin(1)-buffer umax(1)+buffer]); 83 | set(ax,'XGrid','on','YGrid','on'); 84 | 85 | for i = 1:length(ax) 86 | set(ax(i).XLabel,'Interpreter','latex'); 87 | set(ax(i).YLabel,'Interpreter','latex'); 88 | RemovePlotWhiteArea(ax(i)); 89 | end 90 | 91 | if saveFig == 1 92 | for i = 1:length(fig) 93 | print(fig(i),['Figure\' fig(i).Name],'-depsc'); 94 | end 95 | end -------------------------------------------------------------------------------- /QuadrotorEoM.m: -------------------------------------------------------------------------------- 1 | %% ================ EoM of a Quadrotor (Euler angle) ====================== 2 | % system parameters 3 | Ix = 1.2; Iy = 1.2; Iz = 2; 4 | k = 1; l = 0.25; m = 2; b = 0.2; grav = 9.81; 5 | 6 | % rotation matrix (inertia to body) 7 | Rz = [cos(psi) -sin(psi) 0; 8 | sin(psi) cos(psi) 0; 9 | 0 0 1]; 10 | Ry = [cos(theta) 0 sin(theta); 11 | 0 1 0 ; 12 | -sin(theta) 0 cos(theta)]; 13 | Rx = [ 1 0 0; 14 | 0 cos(phi) -sin(phi); 15 | 0 sin(phi) cos(phi)]; 16 | R = Rz*Ry*Rx; 17 | 18 | % Euler kinematics equations 19 | W = [1 0 -sin(theta); 20 | 0 cos(phi) sin(phi)*cos(theta); 21 | 0 -sin(phi) cos(phi)*cos(theta)]; 22 | 23 | % Jacobian matrix J 24 | I = [Ix 0 0; 0 Iy 0; 0 0 Iz]; 25 | J = W'*I*W; 26 | 27 | %%... Definition of sin-cos variables 28 | sP = sin(phi); cP = cos(phi); 29 | sT = sin(theta); cT = cos(theta); 30 | sS = sin(psi); cS = cos(psi); 31 | 32 | % Coriolis term C containing the gyroscopic and centripetal term 33 | C11 = 0; 34 | C12 = (Iy-Iz)*(dtheta*cP*sP + dpsi*sP^2*cT) + (Iz-Iy)*dpsi*cP^2*cT - Ix*dpsi*cT; 35 | C13 = (Iz-Iy)*dpsi*cP*sP*cT^2; 36 | 37 | C21 = (Iz-Iy)*(dtheta*cP*sP + dpsi*sP*cT) + (Iy-Iz)*dpsi*cP^2*cT + Ix*dpsi*cT; 38 | C22 = (Iz-Iy)*dphi*cP*sP; 39 | C23 = -Ix*dpsi*sT*cT + Iy*dpsi*sP^2*sT*cT + Iz*dpsi*cP^2*sT*cT; 40 | 41 | C31 = (Iy-Iz)*dpsi*cT^2*sP*cP - Ix*dtheta*cT; 42 | C32 = (Iz-Iy)*(dtheta*cP*sP*sT + dphi*sP^2*cT) + (Iy-Iz)*dphi*cP^2*cT + ... 43 | Ix*dpsi*sT*cT - Iy*dpsi*sP^2*sT*cT - Iz*dpsi*cP^2*sT*cT; 44 | C33 = (Iy-Iz)*dphi*cP*sP*cT^2 - Iy*dtheta*sP^2*cT*sT - Iz*dtheta*cP^2*cT*sT + Ix*dtheta*cT*sT; 45 | 46 | C = [C11 C12 C13; 47 | C21 C22 C23; 48 | C31 C32 C33]; 49 | 50 | % Define total thrust and torques 51 | thrust = k*(u1+u2+u3+u4); 52 | tau_b = [l*k*(-u2+u4); l*k*(-u1+u3); b*(-u1+u2-u3+u4)]; -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | This project is the course project of EE 372 Dynamic Programming and Optimal Control at King Abdullah University of Science and Technology (KAUST). 2 | The aim of this project is to develope an NMPC-based controller to achieve trajectory tracking of a quadrotor subject to constrained inputs. 3 | 4 | The optimal control problem is reformulated into a nonlinear programming problem, then solved by 5 | open-source optimization tool CasADi. 6 | -------------------------------------------------------------------------------- /ReferenceTrajectory.m: -------------------------------------------------------------------------------- 1 | function [ xdesired ] = ReferenceTrajectory(t,path) 2 | % This function generates reference signal for nonlinear MPC controller 3 | % used in the quadrotor path following example. 4 | 5 | % Copyright 2019 The MathWorks, Inc. 6 | 7 | switch path 8 | case 'e' %... eight 9 | x = 8*sin(t/3); 10 | y = -8*sin(t/3).*cos(t/3); 11 | z = 8*cos(t/3); 12 | case 'c' %... circular 13 | x = 8*sin(t/3); 14 | y = 8*cos(t/3); 15 | z = 10*ones(1,length(t)); 16 | case 'h' %... Helix 17 | x = 8*sin(0.3*t); 18 | y = 8*cos(0.3*t); 19 | z = 0.15*t; 20 | case 'H' %... Complex Helix 21 | x = 15*(sin(0.2*t)-(sin(0.2*t)).^3); 22 | y = 15*(cos(0.2*t)-(cos(0.2*t)).^3); 23 | z = 0.15*t; 24 | case 'p' %... P2P 25 | x = 18*ones(1,length(t)); 26 | y = 0*ones(1,length(t)); 27 | z = 5*ones(1,length(t)); 28 | end 29 | 30 | phi = zeros(1,length(t)); 31 | theta = zeros(1,length(t)); 32 | psi = zeros(1,length(t)); 33 | xdot = zeros(1,length(t)); 34 | ydot = zeros(1,length(t)); 35 | zdot = zeros(1,length(t)); 36 | phidot = zeros(1,length(t)); 37 | thetadot = zeros(1,length(t)); 38 | psidot = zeros(1,length(t)); 39 | 40 | xdesired = [x;y;z;phi;theta;psi;xdot;ydot;zdot;phidot;thetadot;psidot]; 41 | end 42 | 43 | -------------------------------------------------------------------------------- /RemovePlotWhiteArea.m: -------------------------------------------------------------------------------- 1 | function [] = RemovePlotWhiteArea(gca) 2 | % TightInset 3 | inset_vectior = get(gca, 'TightInset'); 4 | inset_x = inset_vectior(1); 5 | inset_y = inset_vectior(2); 6 | inset_w = inset_vectior(3); 7 | inset_h = inset_vectior(4); 8 | % OuterPosition 9 | outer_vector = get(gca, 'OuterPosition'); 10 | pos_new_x = outer_vector(1) + inset_x; 11 | pos_new_y = outer_vector(2) + inset_y; 12 | pos_new_w = outer_vector(3) - inset_w - inset_x; 13 | pos_new_h = outer_vector(4) - inset_h - inset_y; 14 | % set Position 15 | set(gca, 'Position', [pos_new_x, pos_new_y, pos_new_w, pos_new_h]); -------------------------------------------------------------------------------- /YiHsuan_Final_Presentation.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yi-hsuan-chen/Nonlinear-Model-Predictive-Control/55621e3ea88f66f3dab7becd21257c5abbdcf1ae/YiHsuan_Final_Presentation.pdf -------------------------------------------------------------------------------- /casadiVariablesDef.m: -------------------------------------------------------------------------------- 1 | %% ================= CaSAdi Symbolic Variables Definition ================= 2 | %%. CasADi v3.5.1 3 | import casadi.* 4 | % state = [x y z phi theta psi xdot ydot zdot phidot thetadot psidot]'; 5 | x = SX.sym('x'); 6 | y = SX.sym('y'); 7 | z = SX.sym('z'); 8 | phi = SX.sym('phi'); 9 | theta = SX.sym('theta'); 10 | psi = SX.sym('psi'); 11 | dx = SX.sym('dx'); 12 | dy = SX.sym('dy'); 13 | dz = SX.sym('dz'); 14 | dphi = SX.sym('phi'); 15 | dtheta = SX.sym('theta'); 16 | dpsi = SX.sym('psi'); 17 | 18 | % control input: propeller thrust fi = k*wi^2 19 | u1 = SX.sym('u1'); 20 | u2 = SX.sym('u2'); 21 | u3 = SX.sym('u3'); 22 | u4 = SX.sym('u4'); -------------------------------------------------------------------------------- /shift.m: -------------------------------------------------------------------------------- 1 | function [t0, x0, u0] = shift(T, t0, x0, u,f) 2 | st = x0; 3 | con = u(1,:)'; 4 | 5 | %%... Euler 6 | % f_value = f(st,con); 7 | % st = st+ (T*f_value); 8 | 9 | %%... RK4 10 | k1 = f(st, con); % new 11 | k2 = f(st + T/2*k1, con); % new 12 | k3 = f(st + T/2*k2, con); % new 13 | k4 = f(st + T*k3, con); % new 14 | st = st +T/6*(k1 +2*k2 +2*k3 +k4); 15 | x0 = full(st); 16 | 17 | t0 = t0 + T; 18 | u0 = [u(2:size(u,1),:); u(size(u,1),:)]; 19 | end --------------------------------------------------------------------------------