├── gibiansky ├── Quadcopter Dynamics, Simulation, and Control.pdf ├── comparison_data.mat ├── controller.m ├── rotation.m ├── simulate.asv ├── simulate.m ├── theta.mat ├── tune.m └── visualize.m ├── log.txt ├── omari ├── Airmodel.m ├── All_Variables.m ├── Forces.m ├── Go1.m ├── IMU_meas.m ├── Kalman_X.m ├── Kalman_X2.m ├── Kalman_Y.m ├── Kalman_Y2.m ├── Kalman_Z.m ├── Kalman_Z2.m ├── Kalman_function1.m ├── Kalman_phi.m ├── Kalman_phi2.m ├── Kalman_psi.m ├── Kalman_psi2.m ├── Kalman_theta.m ├── Kalman_theta2.m ├── Kalmanfilter_prob1_sfun.mexw64 ├── Map.m ├── Motors_Speed.m ├── Motors_Speed_2.m ├── PID_X.m ├── PID_X_NC.m ├── PID_Y.m ├── PID_Y_NC.m ├── PID_Z.m ├── PID_Z_NC.m ├── PID_heading.m ├── PID_heading_NC.m ├── PID_pitch.m ├── PID_pitch_NC.m ├── PID_roll.m ├── PID_roll_NC.m ├── Quadrotor.mat ├── XY_meas.m ├── Y_Backstepping.m ├── Z_meas.m ├── copter_model.m ├── copter_model_3D.m ├── example.m ├── phi_meas.m ├── plot_X.m ├── plot_XY.m ├── plot_Y.m ├── plot_Z.m ├── plot_forces.m ├── plot_motors.m ├── plot_phi.m ├── plot_psi.m ├── plot_quad.m ├── plot_quad_3D.m ├── plot_theta.m ├── quadmodel.m ├── rotateXYZ.m ├── rotateXYZ2.m └── script2run.m ├── v1 (first rebuild of gibianksy ├── PD.asv ├── PD.m ├── acceleration.m ├── angular_acceleration.m ├── arbitrary_inputs.m ├── grapher.m ├── newcontroller.m ├── omega2thetadot.m ├── rotation.m ├── simulate.asv ├── simulate.m ├── thetadot2omega.m ├── thrust.m ├── torques.m ├── visualiser notes.psd ├── visualize.m └── visualizer.m ├── v2 (from scratch -- not working) ├── PD.m ├── acceleration.m ├── angular_acceleration.m ├── grapher.m ├── multiplot.m ├── omega2thetadot.m ├── rotation.m ├── simulate.m └── thetadot2omega.m ├── v3 (all graphs working) ├── PD.m ├── acceleration.m ├── angular_acceleration.m ├── grapher.m ├── omega2thetadot.m ├── rotation.m ├── simulate.m ├── thetadot2omega.m ├── visualiser notes.psd └── visualize.m ├── v4 (same as v3) ├── PD.m ├── acceleration.m ├── angular_acceleration.asv ├── angular_acceleration.m ├── grapher.asv ├── grapher.m ├── omega2thetadot.m ├── rotation.m ├── simulate.m ├── thetadot2omega.m ├── visualiser notes.psd └── visualize.m ├── v5 (linear velocity controllers) ├── PD.m ├── acceleration.m ├── angular_acceleration.m ├── grapher.m ├── omega2thetadot.m ├── rotation.m ├── simulate.m ├── thetadot2omega.m ├── visualiser notes.psd └── visualize.m ├── v6 (path plot and PID) ├── PD.m ├── PID.m ├── acceleration.m ├── angular_acceleration.m ├── grapher.m ├── omega2thetadot.m ├── rotation.m ├── simulate.m ├── thetadot2omega.m ├── visualiser notes.psd └── visualize.m ├── v7 (simulation time trimmed) ├── PD.m ├── PID.m ├── acceleration.m ├── angular_acceleration.m ├── grapher.m ├── omega2thetadot.m ├── rotation.m ├── simulate.m └── thetadot2omega.m ├── v8 (results) ├── PD.m ├── PID.m ├── acceleration.m ├── angular_acceleration.m ├── grapher.m ├── grossresult.m ├── mean.asv ├── meanresult.m ├── nographsimulate.m ├── omega2thetadot.m ├── rotation.m ├── simulate.m ├── simulate2.m ├── thetadot2omega.m ├── totalgrossresult.m └── totalmeanresult.m └── v9 (mean times) ├── Final Dissertation.pdf ├── PD.m ├── PID.m ├── acceleration.m ├── angular_acceleration.m ├── grapher.m ├── grossresult.m ├── mean.asv ├── meanresult.m ├── meantimes.m ├── nographsimulate.m ├── omega2thetadot.m ├── rotation.m ├── simulate.m ├── simulate2.m ├── thetadot2omega.m ├── totalgrossresult.m └── totalmeanresult.m /gibiansky/Quadcopter Dynamics, Simulation, and Control.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/samuelgilonis/Quadcopter-PID-controller/5aae0d5a14cb32cb6a54b4c1b8df19d9bffbc83f/gibiansky/Quadcopter Dynamics, Simulation, and Control.pdf -------------------------------------------------------------------------------- /gibiansky/comparison_data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/samuelgilonis/Quadcopter-PID-controller/5aae0d5a14cb32cb6a54b4c1b8df19d9bffbc83f/gibiansky/comparison_data.mat -------------------------------------------------------------------------------- /gibiansky/controller.m: -------------------------------------------------------------------------------- 1 | % Create a controller based on it's name, using a look-up table. 2 | function c = controller(name, Kd, Kp, Ki) 3 | % Use manually tuned parameters, unless arguments provide the parameters. 4 | if nargin == 1 5 | Kd = 4; 6 | Kp = 3; 7 | Ki = 5.5; 8 | elseif nargin == 2 || nargin > 4 9 | error('Incorrect number of parameters.'); 10 | end 11 | 12 | if strcmpi(name, 'pd') 13 | c = @(state, thetadot) pd_controller(state, thetadot, Kd, Kp); 14 | elseif strcmpi(name, 'pid') 15 | c = @(state, thetadot) pid_controller(state, thetadot, Kd, Kp, Ki); 16 | else 17 | error(sprintf('Unknown controller type "%s"', name)); 18 | end 19 | end 20 | 21 | % Implement a PD controller. See simulate(controller). 22 | function [input, state] = pd_controller(state, thetadot, Kd, Kp) 23 | % Initialize integral to zero when it doesn't exist. 24 | if ~isfield(state, 'integral') 25 | state.integral = zeros(3, 1); 26 | end 27 | 28 | % Compute total thrust. 29 | total = state.m * state.g / state.k / ... 30 | (cos(state.integral(1)) * cos(state.integral(2))); 31 | 32 | % Compute PD error and inputs. 33 | err = Kd * thetadot + Kp * state.integral; 34 | input = err2inputs(state, err, total); 35 | 36 | % Update controller state. 37 | state.integral = state.integral + state.dt .* thetadot; 38 | end 39 | 40 | % Implement a PID controller. See simulate(controller). 41 | function [input, state] = pid_controller(state, thetadot, Kd, Kp, Ki) 42 | % Initialize integrals to zero when it doesn't exist. 43 | if ~isfield(state, 'integral') 44 | state.integral = zeros(3, 1); 45 | state.integral2 = zeros(3, 1); 46 | end 47 | 48 | % Prevent wind-up 49 | if max(abs(state.integral2)) > 0.01 50 | state.integral2(:) = 0; 51 | end 52 | 53 | % Compute total thrust. 54 | total = state.m * state.g / state.k / ... 55 | (cos(state.integral(1)) * cos(state.integral(2))); 56 | 57 | % Compute error and inputs. 58 | err = Kd * thetadot + Kp * state.integral - Ki * state.integral2; 59 | input = err2inputs(state, err, total); 60 | 61 | % Update controller state. 62 | state.integral = state.integral + state.dt .* thetadot; 63 | state.integral2 = state.integral2 + state.dt .* state.integral; 64 | end 65 | 66 | % Given desired torques, desired total thrust, and physical parameters, 67 | % solve for required system inputs. 68 | function inputs = err2inputs(state, err, total) 69 | e1 = err(1); 70 | e2 = err(2); 71 | e3 = err(3); 72 | Ix = state.I(1, 1); 73 | Iy = state.I(2, 2); 74 | Iz = state.I(3, 3); 75 | k = state.k; 76 | L = state.L; 77 | b = state.b; 78 | 79 | inputs = zeros(4, 1); 80 | inputs(1) = total/4 -(2 * b * e1 * Ix + e3 * Iz * k * L)/(4 * b * k * L); 81 | inputs(2) = total/4 + e3 * Iz/(4 * b) - (e2 * Iy)/(2 * k * L); 82 | inputs(3) = total/4 -(-2 * b * e1 * Ix + e3 * Iz * k * L)/(4 * b * k * L); 83 | inputs(4) = total/4 + e3 * Iz/(4 * b) + (e2 * Iy)/(2 * k * L); 84 | end 85 | -------------------------------------------------------------------------------- /gibiansky/rotation.m: -------------------------------------------------------------------------------- 1 | % Compute rotation matrix for a set of angles. 2 | function R = rotation(angles) 3 | phi = angles(3); 4 | theta = angles(2); 5 | psi = angles(1); 6 | 7 | R = zeros(3); 8 | R(:, 1) = [ 9 | cos(phi) * cos(theta) 10 | cos(theta) * sin(phi) 11 | - sin(theta) 12 | ]; 13 | R(:, 2) = [ 14 | cos(phi) * sin(theta) * sin(psi) - cos(psi) * sin(phi) 15 | cos(phi) * cos(psi) + sin(phi) * sin(theta) * sin(psi) 16 | cos(theta) * sin(psi) 17 | ]; 18 | R(:, 3) = [ 19 | sin(phi) * sin(psi) + cos(phi) * cos(psi) * sin(theta) 20 | cos(psi) * sin(phi) * sin(theta) - cos(phi) * sin(psi) 21 | cos(theta) * cos(psi) 22 | ]; 23 | end 24 | -------------------------------------------------------------------------------- /gibiansky/theta.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/samuelgilonis/Quadcopter-PID-controller/5aae0d5a14cb32cb6a54b4c1b8df19d9bffbc83f/gibiansky/theta.mat -------------------------------------------------------------------------------- /omari/Airmodel.m: -------------------------------------------------------------------------------- 1 | global A 2 | -------------------------------------------------------------------------------- /omari/Forces.m: -------------------------------------------------------------------------------- 1 | function Forces 2 | global A 3 | % this function will calculates the forces and torques on the quadrotor 4 | % give the speed of each propeler. 5 | 6 | % Inputs 7 | % O1 = Front (CCW) 8 | % O2 = Right (CW) 9 | % O3 = Rear (CCW) 10 | % O4 = Left (CW) 11 | 12 | % Outputs 13 | % U1 = Throttle 14 | % U2 = Roll movement 15 | % U3 = Pitch movement 16 | % U4 = Yaw movement 17 | 18 | % Constants 19 | % l = Where l [m] is the distance between the center of the quadrotor and 20 | % the center of a propeller. 21 | % b = 22 | 23 | % A.U11_plot(A.counter) = A.U1; 24 | A.U1 = A.b*(sign(A.O1)*A.O1^2 + sign(A.O2)*A.O2^2 + sign(A.O3)*A.O3^2 + sign(A.O4)*A.O4^2); 25 | A.U1_plot(A.counter) = A.U1; 26 | 27 | % A.U21_plot(A.counter) = A.U1; 28 | A.U2 = A.b*A.l*(sign(A.O4)*A.O4^2 - sign(A.O2)*A.O2^2); 29 | A.U2_plot(A.counter) = A.U2; 30 | 31 | % A.U31_plot(A.counter) = A.U1; 32 | A.U3 = A.b*A.l*(sign(A.O3)*A.O3^2 - sign(A.O1)*A.O1^2); 33 | A.U3_plot(A.counter) = A.U3; 34 | 35 | % A.U41_plot(A.counter) = A.U1; 36 | A.U4 = A.d*(sign(A.O2)*A.O2^2 + sign(A.O4)*A.O4^2 - sign(A.O1)*A.O1^2 - sign(A.O3)*A.O3^2); 37 | A.U4_plot(A.counter) = A.U4; 38 | 39 | A.O = (-A.O1 + A.O2 - A.O3 + A.O4); 40 | A.O_plot(A.counter) = A.O; 41 | 42 | 43 | end -------------------------------------------------------------------------------- /omari/Go1.m: -------------------------------------------------------------------------------- 1 | global A -------------------------------------------------------------------------------- /omari/IMU_meas.m: -------------------------------------------------------------------------------- 1 | function IMU_meas 2 | global A 3 | 4 | A.phi_meas = A.phi+A.phi_error(A.counter); %erros is +- 2 mrad 5 | A.theta_meas = A.theta+A.theta_error(A.counter); %erros is +- 2 mrad 6 | A.psi_meas = A.psi+A.psi_error(A.counter); %erros is +- 2 mrad 7 | 8 | end -------------------------------------------------------------------------------- /omari/Kalman_X.m: -------------------------------------------------------------------------------- 1 | function Kalman_X 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = (A.U1/A.m*A.theta_kalman - (A.X_dot*cos(A.psi_kalman)+A.Y_dot*sin(A.psi_kalman))*1.2)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.X_kalman = xp+K*(A.X_meas-H*xp); 15 | 16 | A.X_kalman_plot(A.counter) = A.X_kalman; 17 | % Update covariance 18 | Pp=(I-K*H)*Pp; 19 | xp = A.X_kalman; 20 | % End or function -------------------------------------------------------------------------------- /omari/Kalman_X2.m: -------------------------------------------------------------------------------- 1 | function Kalman_X2 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = ((sin(A.psi_kalman)*sin(A.phi_kalman) + cos(A.psi_kalman)*sin(A.theta_kalman)*cos(A.phi_kalman))*A.U1/A.m - A.X_dot*1.2)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.X_kalman = xp+K*(A.X_meas-H*xp); 15 | 16 | A.X_kalman_plot(A.counter) = A.X_kalman; 17 | % Update covariance 18 | Pp=(I-K*H)*Pp; 19 | xp = A.X_kalman; 20 | % End or function -------------------------------------------------------------------------------- /omari/Kalman_Y.m: -------------------------------------------------------------------------------- 1 | function Kalman_Y 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = (-A.U1/A.m*A.phi_kalman - (A.Y_dot*cos(A.psi_kalman)-A.X_dot*sin(A.psi_kalman))*1.2)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.Y_kalman = xp+K*(A.Y_meas-H*xp); 15 | A.Y_kalman 16 | A.Y_kalman_plot(A.counter) = A.Y_kalman; 17 | % Update covariance 18 | Pp=(I-K*H)*Pp; 19 | xp = A.Y_kalman; 20 | % End or function -------------------------------------------------------------------------------- /omari/Kalman_Y2.m: -------------------------------------------------------------------------------- 1 | function Kalman_Y2 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = ((-cos(A.psi_kalman)*sin(A.phi_kalman) + sin(A.psi_kalman)*sin(A.theta_kalman)*cos(A.phi_kalman))*A.U1/A.m - A.Y_dot*1.2)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.Y_kalman = xp+K*(A.Y_meas-H*xp); 15 | 16 | A.Y_kalman_plot(A.counter) = A.Y_kalman; 17 | % Update covariance 18 | Pp=(I-K*H)*Pp; 19 | xp = A.Y_kalman; 20 | % End or function -------------------------------------------------------------------------------- /omari/Kalman_Z.m: -------------------------------------------------------------------------------- 1 | function Kalman_Z 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = (-A.g + A.U1/A.m)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.Z_kalman = xp+K*(A.Z_meas-H*xp); 15 | A.Z_kalman_plot(A.counter) = A.Z_kalman; 16 | % Update covariance 17 | Pp=(I-K*H)*Pp; 18 | xp = A.Z_kalman; 19 | % End or function -------------------------------------------------------------------------------- /omari/Kalman_Z2.m: -------------------------------------------------------------------------------- 1 | function Kalman_Z2 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = (-A.g + (cos(A.theta_kalman)*cos(A.phi_kalman))*A.U1/A.m)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.Z_kalman = xp+K*(A.Z_meas-H*xp); 15 | A.Z_kalman_plot(A.counter) = A.Z_kalman; 16 | % Update covariance 17 | Pp=(I-K*H)*Pp; 18 | xp = A.Z_kalman; 19 | % End or function -------------------------------------------------------------------------------- /omari/Kalman_function1.m: -------------------------------------------------------------------------------- 1 | function xhat = Kalman_function1(z,xpp,RR,QQ) 2 | persistent Pp Q R xp A H I 3 | if isempty(Pp) 4 | Pp=1; Q=QQ; R=RR; 5 | xp=xpp; A=0.5; H=1; I=eye(1); 6 | end 7 | % Compute priori 8 | xp=A*xp; 9 | Pp=A*Pp*A'+Q; 10 | % Measurement update 11 | K = Pp*H'*inv(H*Pp*H'+R); 12 | xhat = xp+K*(z-H*xp); 13 | % Update covariance 14 | Pp=(I-K*H)*Pp; 15 | xp = xhat; 16 | % End or function -------------------------------------------------------------------------------- /omari/Kalman_phi.m: -------------------------------------------------------------------------------- 1 | function Kalman_phi 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=A.p*A.Ts + xp; 10 | % Pp=A1*Pp*A1'+Q 11 | % Pp = 6.5331; 12 | % Measurement update 13 | % K = Pp*H'*inv(H*Pp*H'+R) 14 | K = 0.0613; 15 | A.phi_kalman = xp+K*(A.phi_meas-xp); 16 | A.phi_kalman_plot(A.counter) = A.phi_kalman; 17 | % Update covariance 18 | % Pp=(I-K*H)*Pp 19 | xp = A.phi_kalman; 20 | % End or function -------------------------------------------------------------------------------- /omari/Kalman_phi2.m: -------------------------------------------------------------------------------- 1 | function Kalman_phi2 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=(A.p + sin(A.phi_kalman)*tan(A.theta_kalman)*A.q + cos(A.phi_kalman)*tan(A.theta_kalman)*A.r)*A.Ts + xp; 10 | % Pp=A1*Pp*A1'+Q 11 | % Pp = 6.5331; 12 | % Measurement update 13 | % K = Pp*H'*inv(H*Pp*H'+R) 14 | K = 0.0613; 15 | A.phi_kalman = xp+K*(A.phi_meas-xp); 16 | A.phi_kalman_plot(A.counter) = A.phi_kalman; 17 | % Update covariance 18 | % Pp=(I-K*H)*Pp 19 | xp = A.phi_kalman; 20 | % End or function -------------------------------------------------------------------------------- /omari/Kalman_psi.m: -------------------------------------------------------------------------------- 1 | function Kalman_psi 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=A.r*A.Ts + xp; 10 | Pp=A1*Pp*A1'+Q; 11 | % Measurement update 12 | K = Pp*H'*inv(H*Pp*H'+R); 13 | A.psi_kalman = xp+K*(A.psi_meas-H*xp); 14 | A.psi_kalman_plot(A.counter) = A.psi_kalman; 15 | % Update covariance 16 | Pp=(I-K*H)*Pp; 17 | xp = A.psi_kalman; 18 | % End or function -------------------------------------------------------------------------------- /omari/Kalman_psi2.m: -------------------------------------------------------------------------------- 1 | function Kalman_psi2 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=(sin(A.phi_kalman)/cos(A.theta_kalman)*A.q + cos(A.phi_kalman)/cos(A.theta_kalman)*A.r)*A.Ts + xp; 10 | Pp=A1*Pp*A1'+Q; 11 | % Measurement update 12 | K = Pp*H'*inv(H*Pp*H'+R); 13 | A.psi_kalman = xp+K*(A.psi_meas-H*xp); 14 | A.psi_kalman_plot(A.counter) = A.psi_kalman; 15 | % Update covariance 16 | Pp=(I-K*H)*Pp; 17 | xp = A.psi_kalman; 18 | % End or function -------------------------------------------------------------------------------- /omari/Kalman_theta.m: -------------------------------------------------------------------------------- 1 | function Kalman_theta 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=A.q*A.Ts + xp; 10 | Pp=A1*Pp*A1'+Q; 11 | % Measurement update 12 | K = Pp*H'*inv(H*Pp*H'+R); 13 | A.theta_kalman = xp+K*(A.theta_meas-H*xp); 14 | A.theta_kalman_plot(A.counter) = A.theta_kalman; 15 | % Update covariance 16 | Pp=(I-K*H)*Pp; 17 | xp = A.theta_kalman; 18 | % End or function -------------------------------------------------------------------------------- /omari/Kalman_theta2.m: -------------------------------------------------------------------------------- 1 | function Kalman_theta2 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=(cos(A.phi_kalman)*A.q - sin(A.phi_kalman)*A.r)*A.Ts + xp; 10 | Pp=A1*Pp*A1'+Q; 11 | % Measurement update 12 | K = Pp*H'*inv(H*Pp*H'+R); 13 | A.theta_kalman = xp+K*(A.theta_meas-H*xp); 14 | A.theta_kalman_plot(A.counter) = A.theta_kalman; 15 | % Update covariance 16 | Pp=(I-K*H)*Pp; 17 | xp = A.theta_kalman; 18 | % End or function -------------------------------------------------------------------------------- /omari/Kalmanfilter_prob1_sfun.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/samuelgilonis/Quadcopter-PID-controller/5aae0d5a14cb32cb6a54b4c1b8df19d9bffbc83f/omari/Kalmanfilter_prob1_sfun.mexw64 -------------------------------------------------------------------------------- /omari/Map.m: -------------------------------------------------------------------------------- 1 | global A 2 | % imshow('AUS.jpg') 3 | A.aus = imread('AUS.jpg'); 4 | axis equal 5 | % [x,y,z] = sphere(50); 6 | A.t1=-250:1:249; 7 | A.c1=-250:1:249; 8 | A.x1=[]; 9 | A.y1=[]; 10 | A.z1=zeros(500,500); 11 | A.b=0; 12 | while A.b<500; 13 | A.x1=[A.x1;A.c1]; 14 | A.y1=[A.y1;A.t1]; 15 | A.b = A.b+1; 16 | end 17 | A.y1=A.y1'; 18 | A.props.AmbientStrength = 0.1; 19 | A.props.DiffuseStrength = 1; 20 | A.props.SpecularColorReflectance = .5; 21 | A.props.SpecularExponent = 20; 22 | A.props.SpecularStrength = 1; 23 | A.props.FaceColor= 'texture'; 24 | A.props.EdgeColor = 'none'; 25 | A.props.FaceLighting = 'phong'; 26 | A.props.Cdata = A.aus; 27 | surface(A.x1,A.y1,A.z1,A.props); -------------------------------------------------------------------------------- /omari/Motors_Speed.m: -------------------------------------------------------------------------------- 1 | function Motors_Speed 2 | global A 3 | 4 | BB1 = A.U1/(4*A.b) - A.U3/(2*A.b*A.l) - A.U4/(4*A.d); 5 | BB2 = A.U1/(4*A.b) - A.U2/(2*A.b*A.l) + A.U4/(4*A.d); 6 | BB3 = A.U1/(4*A.b) + A.U3/(2*A.b*A.l) - A.U4/(4*A.d); 7 | BB4 = A.U1/(4*A.b) + A.U2/(2*A.b*A.l) + A.U4/(4*A.d); 8 | 9 | if abs(BB1)>A.Motors_limit 10 | BB1 = sign(BB1)*A.Motors_limit; 11 | end 12 | 13 | if abs(BB2)>A.Motors_limit 14 | BB2 = sign(BB2)*A.Motors_limit; 15 | end 16 | 17 | if abs(BB3)>A.Motors_limit 18 | BB3 = sign(BB3)*A.Motors_limit; 19 | end 20 | 21 | if abs(BB4)>A.Motors_limit 22 | BB4 = sign(BB4)*A.Motors_limit; 23 | end 24 | 25 | A.O1 = sign(BB1)*sqrt(abs(BB1)); % Front M 26 | A.O2 = sign(BB2)*sqrt(abs(BB2)); % Right M 27 | A.O3 = sign(BB3)*sqrt(abs(BB3)); % Rear M 28 | A.O4 = sign(BB4)*sqrt(abs(BB4)); % Left M 29 | 30 | A.O1_plot(A.counter) = A.O1; 31 | A.O2_plot(A.counter) = A.O2; 32 | A.O3_plot(A.counter) = A.O3; 33 | A.O4_plot(A.counter) = A.O4; 34 | 35 | end -------------------------------------------------------------------------------- /omari/Motors_Speed_2.m: -------------------------------------------------------------------------------- 1 | function Motors_Speed_2 2 | global A 3 | 4 | BB1 = A.U1/(4*A.b) - A.U3/(2*A.b*A.l) - A.U4/(4*A.d); 5 | BB2 = A.U1/(4*A.b) - A.U2/(2*A.b*A.l) + A.U4/(4*A.d); 6 | BB3 = A.U1/(4*A.b) + A.U3/(2*A.b*A.l) - A.U4/(4*A.d); 7 | BB4 = A.U1/(4*A.b) + A.U2/(2*A.b*A.l) + A.U4/(4*A.d); 8 | 9 | if BB1>A.Motors_limit 10 | BB1 = sign(BB1)*A.Motors_limit; 11 | end 12 | 13 | if BB2>A.Motors_limit 14 | BB2 = sign(BB2)*A.Motors_limit; 15 | end 16 | 17 | if BB3>A.Motors_limit 18 | BB3 = sign(BB3)*A.Motors_limit; 19 | end 20 | 21 | if BB4>A.Motors_limit 22 | BB4 = sign(BB4)*A.Motors_limit; 23 | end 24 | 25 | if BB1 pi/4) % limiter 12 | A.theta_des = sign(A.theta_des)*pi/4; 13 | end 14 | -------------------------------------------------------------------------------- /omari/PID_X_NC.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | A.X_des = A.X_des_EF*cos(A.psi_meas)+A.Y_des_EF*sin(A.psi_meas); % calculating the desired X in BF 4 | 5 | A.X_BF = A.X_meas*cos(A.psi_meas)+A.Y_meas*sin(A.psi_meas); % calculating X in BF 6 | A.X_dot_BF = A.X_dot*cos(A.psi_meas)+A.Y_dot*sin(A.psi_meas); % calculating X_dot in BF 7 | 8 | % PD controller for X_position 9 | A.theta_des = A.X_KP*(A.X_des - A.X_BF) + A.X_KD*A.X_dot; 10 | 11 | if(abs(A.theta_des) > pi/18) % limiter 12 | A.theta_des = sign(A.theta_des)*pi/18; 13 | end 14 | -------------------------------------------------------------------------------- /omari/PID_Y.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | A.Y_des = A.Y_des_EF*cos(A.psi_kalman)-A.X_des_EF*sin(A.psi_kalman); % calculating the desired X in BF 4 | A.Y_BF = A.Y_kalman*cos(A.psi_kalman)-A.X_kalman*sin(A.psi_kalman); % calculating X in BF 5 | A.Y_dot_BF = A.Y_dot*cos(A.psi_kalman)-A.X_dot*sin(A.psi_kalman); % calculating X_dot in BF 6 | 7 | % PD controller for X_position 8 | A.phi_des = -1*(A.Y_KP*(A.Y_des - A.Y_BF) + A.Y_KD*A.Y_dot); 9 | 10 | if(abs(A.phi_des) > pi/4) % limiter 11 | A.phi_des = sign(A.phi_des)*pi/4; 12 | end 13 | -------------------------------------------------------------------------------- /omari/PID_Y_NC.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | A.Y_des = A.Y_des_EF*cos(A.psi_meas)-A.X_des_EF*sin(A.psi_meas); % calculating the desired X in BF 4 | A.Y_BF = A.Y_meas*cos(A.psi_meas)-A.X_meas*sin(A.psi_meas); % calculating X in BF 5 | A.Y_dot_BF = A.Y_dot*cos(A.psi_meas)-A.X_dot*sin(A.psi_meas); % calculating X_dot in BF 6 | 7 | % PD controller for X_position 8 | A.phi_des = -1*(A.Y_KP*(A.Y_des - A.Y_BF) + A.Y_KD*A.Y_dot); 9 | 10 | if(abs(A.phi_des) > pi/18) % limiter 11 | A.phi_des = sign(A.phi_des)*pi/18; 12 | end 13 | -------------------------------------------------------------------------------- /omari/PID_Z.m: -------------------------------------------------------------------------------- 1 | function PID_Z 2 | global A 3 | persistent ui; 4 | persistent errork1; 5 | persistent vhatk1; 6 | % initialize persistent variables at beginning of simulation 7 | if A.init==0 8 | ui = 0; 9 | errork1 = 0; 10 | vhatk1 = 0; 11 | end 12 | 13 | error = A.Z_des-A.Z_kalman; 14 | % proportional term1 15 | up = A.Z_KP * error; 16 | 17 | % integral term 18 | ui = ui + A.Z_KI * A.Ts/2 * (error + errork1); 19 | 20 | % dirty derivative of pe to get vhat 21 | vhat = (2*A.tau-A.Ts)/(2*A.tau+A.Ts)*vhatk1 + (2/(2*A.tau+A.Ts))*(error - errork1); 22 | ud = A.Z_KD*vhat; 23 | 24 | F = up + ui + ud; 25 | 26 | A.U1 = A.m*(A.g + F)/cos(A.phi_kalman)/cos(A.theta_kalman); 27 | A.U1_plot(A.counter) = A.U1; 28 | 29 | % update stored variables 30 | errork1 = error; 31 | vhatk1 = vhat; 32 | 33 | end -------------------------------------------------------------------------------- /omari/PID_Z_NC.m: -------------------------------------------------------------------------------- 1 | function PID_Z_NC 2 | global A 3 | persistent ui; 4 | persistent errork1; 5 | persistent vhatk1; 6 | % initialize persistent variables at beginning of simulation 7 | if A.init==0 8 | ui = 0; 9 | errork1 = 0; 10 | vhatk1 = 0; 11 | end 12 | 13 | error = A.Z_des-A.Z_meas; 14 | % proportional term1 15 | up = A.Z_KP * error; 16 | 17 | % integral term 18 | ui = ui + A.Z_KI * A.Ts/2 * (error + errork1); 19 | 20 | % dirty derivative of pe to get vhat 21 | vhat = (2*A.tau-A.Ts)/(2*A.tau+A.Ts)*vhatk1 + (2/(2*A.tau+A.Ts))*(error - errork1); 22 | ud = A.Z_KD*vhat; 23 | 24 | F = up + ui + ud; 25 | 26 | A.U1 = A.m*(A.g + F)/cos(A.phi_meas)/cos(A.theta_meas); 27 | A.U1_plot(A.counter) = A.U1; 28 | 29 | % update stored variables 30 | errork1 = error; 31 | vhatk1 = vhat; 32 | 33 | end -------------------------------------------------------------------------------- /omari/PID_heading.m: -------------------------------------------------------------------------------- 1 | function PID_heading 2 | global A 3 | persistent uik1; 4 | persistent errork1; 5 | % initialize persistent variables at beginning of simulation 6 | if A.init==0, 7 | uik1 = 0; 8 | errork1 = 0; 9 | end 10 | 11 | error = A.psi_des - A.psi_kalman ; 12 | 13 | % proportional term 14 | up = A.psi_KP * error; 15 | 16 | % integral term 17 | ui = uik1 + A.psi_KI * A.Ts/2 * (error + errork1); 18 | 19 | % derivative term 20 | ud = A.psi_KD*A.r; 21 | 22 | 23 | % implement PID control 24 | A.U4 = up + ui + ud; 25 | 26 | % update stored variables 27 | uik1 = ui; 28 | errork1 = error; -------------------------------------------------------------------------------- /omari/PID_heading_NC.m: -------------------------------------------------------------------------------- 1 | function PID_heading_NC 2 | global A 3 | persistent uik1; 4 | persistent errork1; 5 | % initialize persistent variables at beginning of simulation 6 | if A.init==0, 7 | uik1 = 0; 8 | errork1 = 0; 9 | end 10 | 11 | error = A.psi_des - A.psi_meas ; 12 | 13 | % proportional term 14 | up = A.psi_KP * error; 15 | 16 | % integral term 17 | ui = uik1 + A.psi_KI * A.Ts/2 * (error + errork1); 18 | 19 | % derivative term 20 | ud = A.psi_KD*A.r; 21 | 22 | 23 | % implement PID control 24 | A.U4 = up + ui + ud; 25 | 26 | % update stored variables 27 | uik1 = ui; 28 | errork1 = error; -------------------------------------------------------------------------------- /omari/PID_pitch.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % pitch_attitude_hold 3 | % - regulate pitch attitude 4 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 5 | function PID_pitch 6 | global A 7 | persistent uik1; 8 | persistent errork1; 9 | % initialize persistent variables at beginning of simulation 10 | if A.init==0, 11 | uik1 = 0; 12 | errork1 = 0; 13 | end 14 | 15 | % error equation 16 | error = A.theta_des - A.theta_kalman; 17 | 18 | % proportional term 19 | up = A.theta_KP * error; 20 | 21 | % integral term 22 | ui = uik1 + A.theta_KI * A.Ts/2 * (error + errork1); 23 | 24 | % derivative term 25 | ud = A.theta_KD*A.q; 26 | 27 | 28 | % implement PID control 29 | A.U3 = up + ui + ud; 30 | 31 | % update stored variables 32 | uik1 = ui; 33 | errork1 = error; 34 | 35 | end -------------------------------------------------------------------------------- /omari/PID_pitch_NC.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % pitch_attitude_hold 3 | % - regulate pitch attitude 4 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 5 | function PID_pitch_NC 6 | global A 7 | persistent uik1; 8 | persistent errork1; 9 | % initialize persistent variables at beginning of simulation 10 | if A.init==0, 11 | uik1 = 0; 12 | errork1 = 0; 13 | end 14 | 15 | % error equation 16 | error = A.theta_des - A.theta_meas; 17 | 18 | % proportional term 19 | up = A.theta_KP * error; 20 | 21 | % integral term 22 | ui = uik1 + A.theta_KI * A.Ts/2 * (error + errork1); 23 | 24 | % derivative term 25 | ud = A.theta_KD*A.q; 26 | 27 | 28 | % implement PID control 29 | A.U3 = up + ui + ud; 30 | 31 | % update stored variables 32 | uik1 = ui; 33 | errork1 = error; 34 | 35 | end -------------------------------------------------------------------------------- /omari/PID_roll.m: -------------------------------------------------------------------------------- 1 | function PID_roll 2 | global A 3 | persistent uik1; 4 | persistent errork1; 5 | % initialize persistent variables at beginning of simulation 6 | if A.init==0, 7 | uik1 = 0; 8 | errork1 = 0; 9 | end 10 | 11 | % error equation 12 | error = A.phi_des - A.phi_kalman; 13 | 14 | % proportional term 15 | up = A.phi_KP * error; 16 | 17 | % integral term 18 | ui = uik1 + A.phi_KI * A.Ts/2 * (error + errork1); 19 | 20 | % derivative term 21 | ud = A.phi_KD*A.p; 22 | 23 | % implement PID control 24 | A.U2 = up + ui + ud; 25 | 26 | % update stored variables 27 | uik1 = ui; 28 | errork1 = error; -------------------------------------------------------------------------------- /omari/PID_roll_NC.m: -------------------------------------------------------------------------------- 1 | function PID_roll_NC 2 | global A 3 | persistent uik1; 4 | persistent errork1; 5 | % initialize persistent variables at beginning of simulation 6 | if A.init==0, 7 | uik1 = 0; 8 | errork1 = 0; 9 | end 10 | 11 | % error equation 12 | error = A.phi_des - A.phi_meas; 13 | 14 | % proportional term 15 | up = A.phi_KP * error; 16 | 17 | % integral term 18 | ui = uik1 + A.phi_KI * A.Ts/2 * (error + errork1); 19 | 20 | % derivative term 21 | ud = A.phi_KD*A.p; 22 | 23 | % implement PID control 24 | A.U2 = up + ui + ud; 25 | 26 | % update stored variables 27 | uik1 = ui; 28 | errork1 = error; -------------------------------------------------------------------------------- /omari/Quadrotor.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/samuelgilonis/Quadcopter-PID-controller/5aae0d5a14cb32cb6a54b4c1b8df19d9bffbc83f/omari/Quadrotor.mat -------------------------------------------------------------------------------- /omari/XY_meas.m: -------------------------------------------------------------------------------- 1 | function XY_meas 2 | global A 3 | 4 | A.X_meas = A.X+A.X_error(A.counter); %erros is +- 1 cm 5 | A.Y_meas = A.Y+A.Y_error(A.counter); %erros is +- 1 cm 6 | end -------------------------------------------------------------------------------- /omari/Y_Backstepping.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | if(A.U1~=0) 4 | A.U2 = .1*(-5*(A.Y - A.Y_des) -10*A.Y_dot - 9*A.U1*A.S_psi*A.C_phi ... 5 | -4*A.U1*A.psi_dot*A.C_psi*A.C_phi + A.U1*A.psi_dot^2*A.S_psi*A.C_phi ... 6 | +2*A.U1*A.psi_dot*A.S_psi*A.S_phi + A.U1*A.psi_dot*A.phi_dot*A.C_psi*A.S_phi ... 7 | -A.U1*A.phi_dot*A.psi_dot*A.C_psi*A.S_phi - A.U1*A.phi_dot^2*A.S_psi*A.C_phi)/(A.U1*A.C_psi*A.C_phi); 8 | else 9 | A.U2 = 0; 10 | end -------------------------------------------------------------------------------- /omari/Z_meas.m: -------------------------------------------------------------------------------- 1 | function Z_meas 2 | global A 3 | 4 | A.Z_meas = A.Z+A.Z_error(A.counter); %erros is +- 2 cm 5 | end -------------------------------------------------------------------------------- /omari/copter_model.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | x1=-.27; 4 | x2=.27; 5 | y1=-.01; 6 | y2=.01; 7 | z1=-.01; 8 | z2=.01; 9 | 10 | acon=[x1 y1 z1; 11 | x2 y1 z1; 12 | x2 y2 z1; 13 | x1 y2 z1; 14 | x1 y1 z2; 15 | x2 y1 z2; 16 | x2 y2 z2; 17 | x1 y2 z2]; 18 | 19 | bcon=[1 2 6 5; 20 | 2 3 7 6; 21 | 3 4 8 7; 22 | 4 1 5 8; 23 | 1 2 3 4; 24 | 5 6 7 8]; 25 | 26 | 27 | body1=patch('faces',bcon,... 28 | 'vertices',acon,... 29 | 'facecolor',[.8 .8 .8],... 30 | 'edgecolor',[0 0 0],'facecolor',[.4 .5 1]); 31 | 32 | y1=-.27; 33 | y2=.27; 34 | x1=-.01; 35 | x2=.01; 36 | z1=-.01; 37 | z2=.01; 38 | 39 | acon=[x1 y1 z1; 40 | x2 y1 z1; 41 | x2 y2 z1; 42 | x1 y2 z1; 43 | x1 y1 z2; 44 | x2 y1 z2; 45 | x2 y2 z2; 46 | x1 y2 z2]; 47 | 48 | body2=patch('faces',bcon,... 49 | 'vertices',acon,... 50 | 'facecolor',[.8 .8 .8],... 51 | 'edgecolor',[0 0 0],'facecolor',[.4 .5 1]); 52 | 53 | t=0:pi/10:2*pi; 54 | X=.13*cos(t); 55 | Y=.13*sin(t); 56 | Z=zeros(size(t))+.015; 57 | C=zeros(size(t)); 58 | A.C = C; 59 | 60 | RotorF = patch(X+.27,Y,Z,C,'facealpha',.3,'facecolor','k'); 61 | RotorB = patch(X-.27,Y,Z,C,'facealpha',.3,'facecolor','k'); 62 | RotorL = patch(X,Y+.27,Z,C,'facealpha',.3,'facecolor','k'); 63 | RotorR = patch(X,Y-.27,Z,C,'facealpha',.3,'facecolor','k'); 64 | 65 | A.Body1X = get(body1,'xdata'); 66 | A.Body1Y = get(body1,'ydata'); 67 | A.Body1Z = get(body1,'zdata'); 68 | 69 | A.Body2X = get(body2,'xdata'); 70 | A.Body2Y = get(body2,'ydata'); 71 | A.Body2Z = get(body2,'zdata'); 72 | 73 | A.RotorFX = get(RotorF,'xdata'); 74 | A.RotorFY = get(RotorF,'ydata'); 75 | A.RotorFZ = get(RotorF,'zdata'); 76 | 77 | A.RotorBX = get(RotorB,'xdata'); 78 | A.RotorBY = get(RotorB,'ydata'); 79 | A.RotorBZ = get(RotorB,'zdata'); 80 | 81 | A.RotorRX = get(RotorR,'xdata'); 82 | A.RotorRY = get(RotorR,'ydata'); 83 | A.RotorRZ = get(RotorR,'zdata'); 84 | 85 | A.RotorLX = get(RotorL,'xdata'); 86 | A.RotorLY = get(RotorL,'ydata'); 87 | A.RotorLZ = get(RotorL,'zdata'); 88 | 89 | % xlabel('x') 90 | % grid on 91 | % axis([-1 1 -1 1 -1 1]); 92 | axis equal 93 | 94 | -------------------------------------------------------------------------------- /omari/copter_model_3D.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | load Quadrotor 4 | 5 | A.Body1 = patch('xdata',A.Body1X,'ydata',A.Body1Y,'zdata',A.Body1Z,'facealpha',.3,'facecolor','b'); 6 | A.Body2 = patch('xdata',A.Body2X,'ydata',A.Body2Y,'zdata',A.Body2Z,'facealpha',.3,'facecolor','b'); 7 | A.RotorF = patch('xdata',A.RotorFX,'ydata',A.RotorFY,'zdata',A.RotorFZ,'facealpha',.3,'facecolor','k'); 8 | A.RotorB = patch('xdata',A.RotorBX,'ydata',A.RotorBY,'zdata',A.RotorBZ,'facealpha',.3,'facecolor','k'); 9 | A.RotorR = patch('xdata',A.RotorRX,'ydata',A.RotorRY,'zdata',A.RotorRZ,'facealpha',.3,'facecolor','k'); 10 | A.RotorL = patch('xdata',A.RotorLX,'ydata',A.RotorLY,'zdata',A.RotorLZ,'facealpha',.3,'facecolor','k'); 11 | 12 | -------------------------------------------------------------------------------- /omari/example.m: -------------------------------------------------------------------------------- 1 | load wind 2 | wind_speed = sqrt(u.^2 + v.^2 + w.^2); 3 | 4 | % hpatch = patch(isosurface(x,y,z,wind_speed,35)); 5 | % isonormals(x,y,z,wind_speed,hpatch) 6 | % set(hpatch,'FaceColor','red','EdgeColor','none'); 7 | % 8 | % [f vt] = reducepatch(isosurface(x,y,z,wind_speed,45),0.05); 9 | % daspect([1,1,1]); 10 | % hcone = coneplot(x,y,z,u,v,w,vt(:,1),vt(:,2),vt(:,3),2); 11 | % set(hcone,'FaceColor','blue','EdgeColor','none'); 12 | 13 | camproj perspective 14 | camva(25) 15 | 16 | hlight = camlight('headlight'); 17 | % set(hpatch,'AmbientStrength',.1,... 18 | % 'SpecularStrength',1,... 19 | % 'DiffuseStrength',1); 20 | % set(hcone,'SpecularStrength',1); 21 | % set(gcf,'Color','k') 22 | 23 | lighting gouraud 24 | set(gcf,'Renderer','OpenGL') 25 | 26 | hsline = streamline(x,y,z,u,v,w,80,30,11); 27 | xd = get(hsline,'XData'); 28 | yd = get(hsline,'YData'); 29 | zd = get(hsline,'ZData'); 30 | delete(hsline) 31 | tic 32 | for i=1:length(xd)-50 33 | campos([xd(i),yd(i),zd(i)]) 34 | camtarget([xd(i+5)+min(xd)/100,yd(i),zd(i)]) 35 | camlight(hlight,'headlight') 36 | drawnow 37 | toc 38 | end -------------------------------------------------------------------------------- /omari/phi_meas.m: -------------------------------------------------------------------------------- 1 | function IMU_meas 2 | global A 3 | 4 | A.phi_meas = A.phi+A.phi_error(A.counter) %erros is +- 2 cm 5 | end -------------------------------------------------------------------------------- /omari/plot_X.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | % cla 5 | hold on 6 | plot(A.t_plot(1:A.counter),A.X_plot(1:A.counter)+A.X_error(1:A.counter),'y') 7 | plot(A.t_plot(1:A.counter),A.X_plot(1:A.counter),'r','linewidth',1) 8 | plot(A.t_plot(1:A.counter),A.X_ref_plot(1:A.counter),'b') 9 | plot(A.t_plot(1:A.counter),A.X_dis_plot(1:A.counter),'g') 10 | plot(A.t_plot(1:A.counter),A.X_kalman_plot(1:A.counter),'color',[1 .4 .8]) 11 | legend('measured response','actual response','set value','disturbances','kalman filter') 12 | 13 | xlabel('time (s)') 14 | ylabel('altitude (m)') 15 | title('altitude vs. time') -------------------------------------------------------------------------------- /omari/plot_XY.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | % cla 5 | hold on 6 | plot3(A.X_plot(1:A.counter-1),A.Y_plot(1:A.counter-1),A.Z_plot(1:A.counter-1)) 7 | view(30,30) 8 | grid on 9 | axis equal 10 | axis([min(A.X_plot)-.2 max(A.X_plot)+.2 min(A.Y_plot)-.2 max(A.Y_plot)+.2 min(A.Z_plot)-.2 max(A.Z_plot)+.2]) 11 | % plot(A.t_plot(1:A.counter),A.Y_plot(1:A.counter),'r','linewidth',1) 12 | % plot(A.t_plot(1:A.counter),A.Y_ref_plot(1:A.counter),'b') 13 | % plot(A.t_plot(1:A.counter),A.Y_dis_plot(1:A.counter),'g') 14 | legend('Actual path') 15 | 16 | xlabel('X axis (m)') 17 | ylabel('Y axis (m)') 18 | zlabel('Z axis (m)') 19 | 20 | title('path traveled') -------------------------------------------------------------------------------- /omari/plot_Y.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | % cla 5 | hold on 6 | plot(A.t_plot(1:A.counter),A.Y_plot(1:A.counter)+A.Y_error(1:A.counter),'y') 7 | plot(A.t_plot(1:A.counter),A.Y_plot(1:A.counter),'r','linewidth',1) 8 | plot(A.t_plot(1:A.counter),A.Y_ref_plot(1:A.counter),'b') 9 | plot(A.t_plot(1:A.counter),A.Y_dis_plot(1:A.counter),'g') 10 | plot(A.t_plot(1:A.counter),A.Y_kalman_plot(1:A.counter),'color',[1 .4 .8]) 11 | legend('measured response','actual response','set value','disturbances','kalman filter') 12 | 13 | xlabel('time (s)') 14 | ylabel('altitude (m)') 15 | title('altitude vs. time') -------------------------------------------------------------------------------- /omari/plot_Z.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | % cla 5 | hold on 6 | plot(A.t_plot(1:A.counter),A.Z_plot(1:A.counter)+A.Z_error(1:A.counter),'y') 7 | plot(A.t_plot(1:A.counter),A.Z_plot(1:A.counter),'r','linewidth',1) 8 | plot(A.t_plot(1:A.counter),A.Z_ref_plot(1:A.counter),'b') 9 | plot(A.t_plot(1:A.counter),A.Z_dis_plot(1:A.counter),'g') 10 | plot(A.t_plot(1:A.counter),A.Z_kalman_plot(1:A.counter),'color',[1 .4 .8]) 11 | legend('measured response','actual response','set value','disturbances','kalman filter') 12 | 13 | xlabel('time (s)') 14 | ylabel('altitude (m)') 15 | title('altitude vs. time') -------------------------------------------------------------------------------- /omari/plot_forces.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | cla 4 | hold on 5 | plot(A.t_plot(1:A.counter-1),A.U1_plot(1:A.counter-1),'r') 6 | plot(A.t_plot(1:A.counter-1),A.U2_plot(1:A.counter-1),'g') 7 | plot(A.t_plot(1:A.counter-1),A.U3_plot(1:A.counter-1),'b') 8 | plot(A.t_plot(1:A.counter-1),A.U4_plot(1:A.counter-1),'k') 9 | % plot(A.t_plot(1:A.counter),A.Z_ref_plot(1:A.counter),'b') 10 | % plot(A.t_plot(1:A.counter),A.Z_dis_plot(1:A.counter),'g') 11 | legend('Throttle','Roll','Pitch','Yaw') 12 | xlabel('time (s)') 13 | ylabel('value') 14 | title('Forces and Torques vs. time') -------------------------------------------------------------------------------- /omari/plot_motors.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | cla 4 | hold on 5 | plot(A.t_plot(1:A.counter),A.O1_plot(1:A.counter),'r') 6 | plot(A.t_plot(1:A.counter),A.O2_plot(1:A.counter),'g') 7 | plot(A.t_plot(1:A.counter),A.O3_plot(1:A.counter),'b') 8 | plot(A.t_plot(1:A.counter),A.O4_plot(1:A.counter),'k') 9 | % plot(A.t_plot(1:A.counter),A.Z_ref_plot(1:A.counter),'b') 10 | % plot(A.t_plot(1:A.counter),A.Z_dis_plot(1:A.counter),'g') 11 | legend('front','right','rear','left') 12 | xlabel('time (s)') 13 | ylabel('value') 14 | title('Forces and Torques vs. time') -------------------------------------------------------------------------------- /omari/plot_phi.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | hold on 5 | plot(A.t_plot(1:A.counter),A.phi_plot(1:A.counter)+A.phi_error(1:A.counter),'y') 6 | plot(A.t_plot(1:A.counter),A.phi_plot(1:A.counter),'r') 7 | plot(A.t_plot(1:A.counter),A.phi_ref_plot(1:A.counter),'b') 8 | plot(A.t_plot(1:A.counter),A.phi_dis_plot(1:A.counter),'g') 9 | plot(A.t_plot(1:A.counter),A.phi_kalman_plot(1:A.counter),'color',[1 .4 .8]) 10 | legend('measured response','actual response','set value','disturbances','kalman filter') 11 | xlabel('time (s)') 12 | ylabel('phi (rad)') 13 | title('phi vs. time') -------------------------------------------------------------------------------- /omari/plot_psi.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | hold on 5 | plot(A.t_plot(1:A.counter),A.psi_plot(1:A.counter)+A.psi_error(1:A.counter),'y') 6 | plot(A.t_plot(1:A.counter),A.psi_plot(1:A.counter),'r') 7 | plot(A.t_plot(1:A.counter),A.psi_ref_plot(1:A.counter),'b') 8 | plot(A.t_plot(1:A.counter),A.psi_dis_plot(1:A.counter),'g') 9 | plot(A.t_plot(1:A.counter),A.psi_kalman_plot(1:A.counter),'color',[1 .4 .8]) 10 | legend('measured response','actual response','set value','disturbances','kalman filter') 11 | xlabel('time (s)') 12 | ylabel('psi (rad)') 13 | title('psi vs. time') -------------------------------------------------------------------------------- /omari/plot_quad.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.X2,A.Y2,A.Z2,A.phi,A.theta,A.psi); 4 | set(A.L2,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 5 | 6 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.FPX,A.FPY,A.FPZ,A.phi,A.theta,A.psi); 7 | set(A.L3,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 8 | 9 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.RPX,A.RPY,A.RPZ,A.phi,A.theta,A.psi); 10 | set(A.L4,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 11 | 12 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.rPX,A.rPY,A.rPZ,A.phi,A.theta,A.psi); 13 | set(A.L5,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 14 | 15 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.LPX,A.LPY,A.LPZ,A.phi,A.theta,A.psi); 16 | set(A.L6,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 17 | 18 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.X1,A.Y1,A.Z1,A.phi,A.theta,A.psi); 19 | set(A.L1,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 20 | -------------------------------------------------------------------------------- /omari/plot_quad_3D.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.Body1X,A.Body1Y,A.Body1Z,A.phi,A.theta,A.psi); 4 | set(A.Body1,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z) 5 | 6 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.Body2X,A.Body2Y,A.Body2Z,A.phi,A.theta,A.psi); 7 | set(A.Body2,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z) 8 | 9 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.RotorFX,A.RotorFY,A.RotorFZ,A.phi,A.theta,A.psi); 10 | set(A.RotorF,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z) 11 | 12 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.RotorBX,A.RotorBY,A.RotorBZ,A.phi,A.theta,A.psi); 13 | set(A.RotorB,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z) 14 | 15 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.RotorLX,A.RotorLY,A.RotorLZ,A.phi,A.theta,A.psi); 16 | set(A.RotorL,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z) 17 | 18 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.RotorRX,A.RotorRY,A.RotorRZ,A.phi,A.theta,A.psi); 19 | set(A.RotorR,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z) -------------------------------------------------------------------------------- /omari/plot_theta.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | hold on 5 | plot(A.t_plot(1:A.counter),A.theta_plot(1:A.counter)+A.theta_error(1:A.counter),'y') 6 | plot(A.t_plot(1:A.counter),A.theta_plot(1:A.counter),'r') 7 | plot(A.t_plot(1:A.counter),A.theta_ref_plot(1:A.counter),'b') 8 | plot(A.t_plot(1:A.counter),A.theta_dis_plot(1:A.counter),'g') 9 | plot(A.t_plot(1:A.counter),A.theta_kalman_plot(1:A.counter),'color',[1 .4 .8]) 10 | legend('measured response','actual response','set value','disturbances','kalman filter') 11 | xlabel('time (s)') 12 | ylabel('theta (rad)') 13 | title('theta vs. time') -------------------------------------------------------------------------------- /omari/quadmodel.m: -------------------------------------------------------------------------------- 1 | function quadmodel 2 | global A 3 | % this function simulate the dynamics of the quadrotor, it will take the 4 | % Euler angels and forces as inputs and give the linear accelerations WRT 5 | % E-Frame and rotational accelerations WRT B-Frame. 6 | 7 | A.X_ddot = (sin(A.psi)*sin(A.phi) + cos(A.psi)*sin(A.theta)*cos(A.phi))*A.U1/A.m; 8 | A.Y_ddot = (-cos(A.psi)*sin(A.phi) + sin(A.psi)*sin(A.theta)*cos(A.phi))*A.U1/A.m; 9 | A.Z_ddot = -A.g + (cos(A.theta)*cos(A.phi))*A.U1/A.m; 10 | 11 | A.p_dot = ((A.Iyy - A.Izz)*A.q*A.r -A.Jtp*A.q*A.O + A.U2)/A.Ixx; 12 | A.q_dot = ((A.Izz - A.Ixx)*A.p*A.r +A.Jtp*A.p*A.O + A.U3)/A.Iyy; 13 | A.r_dot = ((A.Ixx - A.Iyy)*A.p*A.q + A.U4)/A.Izz; 14 | 15 | A.phi_dot = A.p + sin(A.phi)*tan(A.theta)*A.q + cos(A.phi)*tan(A.theta)*A.r; 16 | A.theta_dot = cos(A.phi)*A.q - sin(A.phi)*A.r; 17 | A.psi_dot = sin(A.phi)/cos(A.theta)*A.q + cos(A.phi)/cos(A.theta)*A.r; 18 | 19 | % Air friction model 20 | A.X_ddot = A.X_ddot - A.X_dot*1.2; 21 | A.Y_ddot = A.Y_ddot - A.Y_dot*1.2; 22 | 23 | % Disturbance model 24 | A.Z_ddot = A.Z_ddot + A.Z_dis/A.m; 25 | 26 | A.psi_dot = A.psi_dot + A.psi_dis/A.Izz*A.Ts; 27 | 28 | A.theta_dot = A.theta_dot + A.theta_dis/A.Iyy*A.Ts; 29 | 30 | A.phi_dot = A.phi_dot + A.phi_dis/A.Ixx*A.Ts; 31 | 32 | 33 | % Calculating the Z velocity & position 34 | A.Z_dot = A.Z_ddot*A.Ts + A.Z_dot; 35 | A.Z = A.Z_dot*A.Ts + A.Z; 36 | 37 | % Calculating the X velocity & position 38 | A.X_dot = A.X_ddot*A.Ts + A.X_dot; 39 | A.X = A.X_dot*A.Ts + A.X; 40 | 41 | % Calculating the Y velocity & position 42 | A.Y_dot = A.Y_ddot*A.Ts + A.Y_dot; 43 | A.Y = A.Y_dot*A.Ts + A.Y; 44 | 45 | % Calculating p,q,r 46 | A.p = A.p_dot*A.Ts+A.p; 47 | A.q = A.q_dot*A.Ts+A.q; 48 | A.r = A.r_dot*A.Ts+A.r; 49 | 50 | % Calculating phi,theta,psi 51 | A.phi = A.phi_dot*A.Ts+A.phi; 52 | A.theta = A.theta_dot*A.Ts+A.theta; 53 | A.psi = A.psi_dot*A.Ts+A.psi; 54 | 55 | % Plotting variables 56 | A.Z_plot(A.counter) = A.Z; 57 | A.Z_ref_plot(A.counter) = A.Z_des; 58 | A.Z_dis_plot(A.counter) = A.Z_dis; 59 | 60 | A.X_plot(A.counter) = A.X; 61 | A.X_ref_plot(A.counter) = A.X_des; 62 | A.X_dis_plot(A.counter) = A.X_dis; 63 | 64 | A.Y_plot(A.counter) = A.Y; 65 | A.Y_ref_plot(A.counter) = A.Y_des; 66 | A.Y_dis_plot(A.counter) = A.Y_dis; 67 | 68 | A.phi_plot(A.counter) = A.phi; 69 | A.phi_ref_plot(A.counter) = A.phi_des; 70 | A.phi_dis_plot(A.counter) = A.phi_dis; 71 | 72 | A.theta_plot(A.counter) = A.theta; 73 | A.theta_ref_plot(A.counter) = A.theta_des; 74 | A.theta_dis_plot(A.counter) = A.theta_dis; 75 | 76 | A.psi_plot(A.counter) = A.psi; 77 | A.psi_ref_plot(A.counter) = A.psi_des; 78 | A.psi_dis_plot(A.counter) = A.psi_dis; 79 | 80 | A.counter = A.counter + 1; 81 | 82 | % Disturbance removal 83 | if(A.Z_dis ~= 0) 84 | A.Z_dis = 0; 85 | end 86 | 87 | if(A.psi_dis ~= 0) 88 | A.psi_dis = 0; 89 | end 90 | 91 | if(A.theta_dis ~= 0) 92 | A.theta_dis = 0; 93 | end 94 | 95 | if(A.phi_dis ~= 0) 96 | A.phi_dis = 0; 97 | end 98 | 99 | end -------------------------------------------------------------------------------- /omari/rotateXYZ.m: -------------------------------------------------------------------------------- 1 | function [X,Y,Z]=rotateXYZ(X,Y,Z,phi,theta,psi) 2 | % define rotation matrix 3 | R_roll = [... 4 | 1, 0, 0;... 5 | 0, cos(phi), -sin(phi);... 6 | 0, sin(phi), cos(phi)]; 7 | R_pitch = [... 8 | cos(theta), 0, sin(theta);... 9 | 0, 1, 0;... 10 | -sin(theta), 0, cos(theta)]; 11 | R_yaw = [... 12 | cos(psi), -sin(psi), 0;... 13 | sin(psi), cos(psi), 0;... 14 | 0, 0, 1]; 15 | R = R_roll'*R_pitch'*R_yaw'; 16 | 17 | % rotate vertices 18 | for i=1:length(X) 19 | pts = [X(i), Y(i), Z(i)]*R; 20 | 21 | X(i) = pts(:,1); 22 | Y(i) = pts(:,2); 23 | Z(i) = pts(:,3); 24 | end 25 | end -------------------------------------------------------------------------------- /omari/rotateXYZ2.m: -------------------------------------------------------------------------------- 1 | function [X,Y,Z]=rotateXYZ2(X,Y,Z,phi,theta,psi) 2 | % define rotation matrix 3 | R_roll = [... 4 | 1, 0, 0;... 5 | 0, cos(phi), -sin(phi);... 6 | 0, sin(phi), cos(phi)]; 7 | R_pitch = [... 8 | cos(theta), 0, sin(theta);... 9 | 0, 1, 0;... 10 | -sin(theta), 0, cos(theta)]; 11 | R_yaw = [... 12 | cos(psi), -sin(psi), 0;... 13 | sin(psi), cos(psi), 0;... 14 | 0, 0, 1]; 15 | R = R_roll'*R_pitch'*R_yaw'; 16 | 17 | % rotate vertices 18 | B=size(X); 19 | 20 | for i=1:B(2)*B(1) 21 | pts = [X(i), Y(i), Z(i)]*R; 22 | 23 | X(i) = pts(:,1); 24 | Y(i) = pts(:,2); 25 | Z(i) = pts(:,3); 26 | end 27 | end -------------------------------------------------------------------------------- /v1 (first rebuild of gibianksy/PD.asv: -------------------------------------------------------------------------------- 1 | function c = PD(Kd, Kp) 2 | % Use manually tuned parameters, unless arguments provide the parameters. 3 | if nargin == 0 4 | Kd = 4; 5 | Kp = 3; 6 | 7 | elseif nargin == 1 || nargin > 2 8 | error('Incorrect number of parameters.'); 9 | end 10 | 11 | 12 | c = @(state, thetadot) pd_controller(state, thetadot, Kd, Kp); 13 | 14 | end 15 | 16 | 17 | 18 | function [inputs,state] = pd_controller(state, thetadot, Kd, Kp) 19 | %the "state" argument is the structured array from which the controller 20 | %must access the necessary information. This is the controller_params 21 | %struct in the simulator code. 22 | 23 | %Create the theta (orientation) vector in the structured array if it does 24 | %not yet exist. This will be updated each time this code is run in the 25 | %simulator for-loop. 26 | if ~isfield(state,'orientation') 27 | state.orientation = zeros (3,1); 28 | end 29 | 30 | %Now create the inputs that will be sent to the quadcopter. 31 | %There are four inputs (an angular velocity^2 for each prop) 32 | inputs = zeros(4,1); 33 | 34 | %Now extract from the structured array the necessary information to compute 35 | %the inputs. 36 | 37 | %Set desired heading 38 | heading=deg2rad(90); 39 | 40 | previouserror=0; 41 | dt=state.dt 42 | yawerror=heading-state.orientation(3); 43 | yawerrordot=yawerror-previouserror; 44 | 45 | errors=zeros(3,1); 46 | errors(1) = Kd*thetadot(1) + Kp*state.orientation(1); 47 | errors(2) = Kd*thetadot(2) + Kp*state.orientation(2); 48 | errors(3) = Kd*yawerrordot + Kp*yawerror; 49 | 50 | e_phi=errors(1); 51 | e_theta=errors(2); 52 | e_psi=errors(3); 53 | 54 | Ixx=state.I(1,1); 55 | Iyy=state.I(2,2); 56 | Izz=state.I(3,3); 57 | 58 | b=state.b; 59 | k=state.k; 60 | L=state.L; 61 | m=state.m; 62 | g=state.g; 63 | 64 | phi=state.orientation(1); 65 | theta=state.orientation(2); 66 | psi=state.orientation(3); 67 | 68 | %Now compute total thrust (to simplify input equations) 69 | thrust = m*g/(k*cos(phi)*cos(theta)); 70 | 71 | %Now compute the inputs 72 | inputs(1)= thrust/4 - (2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 73 | inputs(2)= thrust/4 - (2*b*e_theta*Iyy - k*L*e_psi*Izz)/(4*b*L*k); 74 | inputs(3)= thrust/4 - (-2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 75 | inputs(4)= thrust/4 + (2*b*e_theta*Iyy + k*L*e_psi*Izz)/(4*b*L*k); 76 | 77 | %Now advance the controller state (this essentially means updating the 78 | %orientation vector: 79 | 80 | state.orientation = state.orientation + state.dt*thetadot; 81 | 82 | previouserror=yawerror; 83 | end 84 | 85 | 86 | 87 | 88 | 89 | 90 | -------------------------------------------------------------------------------- /v1 (first rebuild of gibianksy/PD.m: -------------------------------------------------------------------------------- 1 | function c = PD(Kd, Kp) 2 | % Use manually tuned parameters, unless arguments provide the parameters. 3 | if nargin == 0 4 | Kd = 4; 5 | Kp = 3; 6 | 7 | elseif nargin == 1 || nargin > 2 8 | error('Incorrect number of parameters.'); 9 | end 10 | 11 | 12 | c = @(state, thetadot) pd_controller(state, thetadot, Kd, Kp); 13 | 14 | end 15 | 16 | 17 | 18 | function [inputs,state] = pd_controller(state, thetadot, Kd, Kp) 19 | %the "state" argument is the structured array from which the controller 20 | %must access the necessary information. This is the controller_params 21 | %struct in the simulator code. 22 | 23 | %Create the theta (orientation) vector in the structured array if it does 24 | %not yet exist. This will be updated each time this code is run in the 25 | %simulator for-loop. 26 | if ~isfield(state,'orientation') 27 | state.orientation = zeros (3,1); 28 | end 29 | 30 | %Now create the inputs that will be sent to the quadcopter. 31 | %There are four inputs (an angular velocity^2 for each prop) 32 | inputs = zeros(4,1); 33 | 34 | %Now extract from the structured array the necessary information to compute 35 | %the inputs. 36 | errors = Kd*thetadot + Kp*state.orientation; 37 | 38 | e_phi=errors(1); 39 | e_theta=errors(2); 40 | e_psi=errors(3); 41 | 42 | Ixx=state.I(1,1); 43 | Iyy=state.I(2,2); 44 | Izz=state.I(3,3); 45 | 46 | b=state.b; 47 | k=state.k; 48 | L=state.L; 49 | m=state.m; 50 | g=state.g; 51 | 52 | phi=state.orientation(1); 53 | theta=state.orientation(2); 54 | psi=state.orientation(3); 55 | 56 | %Now compute total thrust (to simplify input equations) 57 | thrust = m*g/(k*cos(phi)*cos(theta)); 58 | 59 | %Now compute the inputs 60 | inputs(1)= thrust/4 - (2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 61 | inputs(2)= thrust/4 - (2*b*e_theta*Iyy - k*L*e_psi*Izz)/(4*b*L*k); 62 | inputs(3)= thrust/4 - (-2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 63 | inputs(4)= thrust/4 + (2*b*e_theta*Iyy + k*L*e_psi*Izz)/(4*b*L*k); 64 | 65 | %Now advance the controller state (this essentially means updating the 66 | %orientation vector: 67 | 68 | state.orientation = state.orientation + state.dt*thetadot; 69 | end 70 | 71 | 72 | 73 | 74 | 75 | 76 | -------------------------------------------------------------------------------- /v1 (first rebuild of gibianksy/acceleration.m: -------------------------------------------------------------------------------- 1 | function a = acceleration(inputs,angles,velocities,m,g,k,kd) 2 | 3 | %The acceleration of the quadcopter is a function of gravity, the thrust 4 | %and drag forces. From our model we have (a is the acceleration vector, v 5 | %is the velocity vector): 6 | 7 | %m*a=[0 0 -mg].' + T_I +F_D 8 | % =[0 0 -mg].' + R*T_B + F_D 9 | % therefore a = [0 0 -g].' + 1/m*(R*[0 0 k*sum(angular accelerations^2)] + 10 | % kd*v) 11 | 12 | %The angular velocities are the inputs, thrust (T is in F_B) is a function 13 | %of angular velocity and k. 14 | 15 | gravity = [0 0 -g].'; 16 | R = rotation(angles); 17 | T = R*thrust(inputs,k); 18 | Fd = -kd*velocities; 19 | 20 | a = [0 0 -g].' +1/m*T +1/m*Fd; 21 | 22 | 23 | end 24 | 25 | -------------------------------------------------------------------------------- /v1 (first rebuild of gibianksy/angular_acceleration.m: -------------------------------------------------------------------------------- 1 | function omegadot = angular_acceleration(inputs,omega,I,L,b,k) 2 | tau=torques(inputs,L,b,k); 3 | 4 | %Now use the Euler equation for rigid body motion to obtain angular 5 | %acceleration: 6 | 7 | omegadot=inv(I)*(tau-cross(omega,I*omega)); 8 | 9 | 10 | end 11 | 12 | -------------------------------------------------------------------------------- /v1 (first rebuild of gibianksy/arbitrary_inputs.m: -------------------------------------------------------------------------------- 1 | function in = arbitrary_inputs() 2 | in=zeros(4,1); 3 | in(1)=700; 4 | in(2)=750; 5 | in(3)=700; 6 | in(4)=750; 7 | in=in.^2; 8 | %These are arbitrary test inputs that are not based on any controller, in 9 | %order to test the simulator. If there was any disturbance then the 10 | %quadcopter would catastrophically fail. 11 | 12 | %The values shown after in(i) are the angular velocities of each propeller 13 | %which are then squared to obtain the gamma value for simpler notation. 14 | 15 | 16 | end 17 | 18 | -------------------------------------------------------------------------------- /v1 (first rebuild of gibianksy/grapher.m: -------------------------------------------------------------------------------- 1 | %This is a graphical simulation of the quadcopter's motion 2 | 3 | %CREATE THE GRAPH 4 | 5 | function v = grapher(data) 6 | figure; 7 | %Keeps the plots on the same graph 8 | plots = [subplot(2,1,1), subplot(2,1,2)]; 9 | %Creates 3 plots for the 3D visualisation and for angular velocity and 10 | %displacement against time. 11 | 12 | % Use the bottom two parts for angular velocity and displacement. 13 | for t = 1:2:length(data.t); 14 | subplot(plots(1)); 15 | multiplot(data, data.angvel, t); 16 | xlabel('Time (s)'); 17 | ylabel('Angular Velocity (rad/s)'); 18 | title('Angular Velocity'); 19 | 20 | subplot(plots(2)); 21 | multiplot(data, data.theta, t); 22 | xlabel('Time (s)'); 23 | ylabel('Angular Displacement (rad)'); 24 | title('Angular Displacement'); 25 | end 26 | 27 | end 28 | 29 | function multiplot(data, values, ind) 30 | % Select the parts of the data to plot. 31 | times = data.t(:, 1:ind); 32 | values = values(:, 1:ind); 33 | 34 | % Plot in RGB, with different markers for different components. 35 | plot(times, values(1, :), 'r-', times, values(2, :), 'g', times, values(3, :), 'b-.'); 36 | 37 | % Set axes to remain constant throughout plotting. 38 | xmin = min(data.t); 39 | xmax = max(data.t); 40 | ymin = 1.1 * min(min(values)); 41 | ymax = 1.1 * max(max(values)); 42 | axis([xmin xmax ymin ymax]); 43 | end -------------------------------------------------------------------------------- /v1 (first rebuild of gibianksy/newcontroller.m: -------------------------------------------------------------------------------- 1 | function c = newcontroller(Kd, Kp) 2 | % Use manually tuned parameters, unless arguments provide the parameters. 3 | if nargin == 0 4 | Kd = 4; 5 | Kp = 3; 6 | 7 | elseif nargin == 1 || nargin > 2 8 | error('Incorrect number of parameters.'); 9 | end 10 | 11 | 12 | c = @(state, thetadot) pd_controller(state, thetadot, Kd, Kp); 13 | 14 | end 15 | 16 | 17 | 18 | function [inputs,state] = pd_controller(state, thetadot, Kd, Kp) 19 | %the "state" argument is the structured array from which the controller 20 | %must access the necessary information. This is the controller_params 21 | %struct in the simulator code. 22 | 23 | %Create the theta (orientation) vector in the structured array if it does 24 | %not yet exist. This will be updated each time this code is run in the 25 | %simulator for-loop. 26 | if ~isfield(state,'orientation') 27 | state.orientation = zeros (3,1); 28 | end 29 | 30 | %Now create the inputs that will be sent to the quadcopter. 31 | %There are four inputs (an angular velocity^2 for each prop) 32 | inputs = zeros(4,1); 33 | 34 | %Now extract from the structured array the necessary information to compute 35 | %the inputs. 36 | errors = Kd*thetadot + Kp*state.orientation; 37 | 38 | e_phi=errors(1); 39 | e_theta=errors(2); 40 | e_psi=errors(3); 41 | 42 | Ixx=state.I(1,1); 43 | Iyy=state.I(2,2); 44 | Izz=state.I(3,3); 45 | 46 | b=state.b; 47 | k=state.k; 48 | L=state.L; 49 | m=state.m; 50 | g=state.g; 51 | 52 | phi=state.orientation(1); 53 | theta=state.orientation(2); 54 | psi=state.orientation(3); 55 | 56 | %Now compute total thrust (to simplify input equations) 57 | thrust = m*g/(k*cos(phi)*cos(theta)); 58 | 59 | %Now compute the inputs 60 | inputs(1)= thrust/4 - (2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 61 | inputs(2)= thrust/4 - (2*b*e_theta*Iyy - k*L*e_psi*Izz)/(4*b*L*k); 62 | inputs(3)= thrust/4 - (-2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 63 | inputs(4)= thrust/4 + (2*b*e_theta*Iyy + k*L*e_psi*Izz)/(4*b*L*k); 64 | 65 | %Now advance the controller state (this essentially means updating the 66 | %orientation vector: 67 | 68 | state.orientation = state.orientation + state.dt*thetadot; 69 | end 70 | 71 | 72 | 73 | 74 | 75 | 76 | -------------------------------------------------------------------------------- /v1 (first rebuild of gibianksy/omega2thetadot.m: -------------------------------------------------------------------------------- 1 | function thetadot = omega2thetadot(omega,angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | A=zeros(3); 7 | A(1,:)=[ 8 | 1 9 | 0 10 | -sin(theta) 11 | ]; 12 | 13 | A(2,:) =[ 14 | 0 15 | cos(phi) 16 | cos(theta)*sin(phi) 17 | ]; 18 | 19 | A(3,:) =[ 20 | 0 21 | -sin(phi) 22 | cos(phi)*cos(theta) 23 | ]; 24 | 25 | W=inv(A); 26 | thetadot=W*omega; 27 | 28 | %This function converts from angular velocity to the time derivatives 29 | %of roll, pitch and yaw. 30 | end 31 | -------------------------------------------------------------------------------- /v1 (first rebuild of gibianksy/rotation.m: -------------------------------------------------------------------------------- 1 | function R = rotation(angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | R=zeros(3); 7 | R(1,:) = [ 8 | cos(theta)*cos(psi) 9 | cos(psi)*sin(phi)*sin(theta)-cos(phi)*sin(psi) 10 | sin(phi)*sin(psi)+cos(phi)*cos(psi)*sin(theta) 11 | ]; 12 | 13 | R(2,:) = [ 14 | cos(theta)*sin(psi) 15 | cos(phi)*cos(psi) + sin(phi)*sin(theta)*sin(psi) 16 | cos(phi)*sin(theta)*sin(psi)-cos(psi)*sin(phi) 17 | ]; 18 | 19 | R(3,:) = [ 20 | -sin(theta) 21 | cos(theta)*sin(phi) 22 | cos(phi)*cos(theta) 23 | ]; 24 | 25 | %This rotation matrix maps between the body-fixed frame and the inertial 26 | %frame. 27 | 28 | end 29 | 30 | -------------------------------------------------------------------------------- /v1 (first rebuild of gibianksy/thetadot2omega.m: -------------------------------------------------------------------------------- 1 | function omega = thetadot2omega(thetadot,angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | A=zeros(3); 7 | A(1,:)=[ 8 | 1 9 | 0 10 | -sin(theta) 11 | ]; 12 | A(2,:) =[ 13 | 0 14 | cos(phi) 15 | cos(theta)*sin(phi) 16 | ]; 17 | A(3,:) =[ 18 | 0 19 | -sin(phi) 20 | cos(phi)*cos(theta) 21 | ]; 22 | 23 | omega=A*thetadot; 24 | 25 | 26 | %This function converts time derivatives of roll, pitch and yaw into 27 | %angular velocity. A is the necessary rotation matrix for this operation. 28 | %We can define the reverse function omega2thetadot using the inverse of 29 | %this matrix if desired. 30 | 31 | end 32 | 33 | -------------------------------------------------------------------------------- /v1 (first rebuild of gibianksy/thrust.m: -------------------------------------------------------------------------------- 1 | function T=thrust(inputs,k) 2 | 3 | T=[0 0 k*sum(inputs)].'; 4 | %This function computes the thrust in F_B from the inputs (angular 5 | %velocity^2 of each propeller) 6 | 7 | 8 | end 9 | 10 | -------------------------------------------------------------------------------- /v1 (first rebuild of gibianksy/torques.m: -------------------------------------------------------------------------------- 1 | function tau = torques(inputs,L,b,k); 2 | %Note that inputs here denote the gamma values (angular velocity^2) NOT 3 | %angular velocity itself. 4 | tau = [ 5 | L*k*(inputs(1)-inputs(3)) 6 | L*k*(inputs(2)-inputs(4)) 7 | b*(inputs(1)-inputs(2)+inputs(3)-inputs(4)) 8 | ]; 9 | 10 | %This function computes the torques on the quadcopter in F_B based upon the 11 | %angular velocities of each propeller. 12 | 13 | 14 | end 15 | 16 | -------------------------------------------------------------------------------- /v1 (first rebuild of gibianksy/visualiser notes.psd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/samuelgilonis/Quadcopter-PID-controller/5aae0d5a14cb32cb6a54b4c1b8df19d9bffbc83f/v1 (first rebuild of gibianksy/visualiser notes.psd -------------------------------------------------------------------------------- /v1 (first rebuild of gibianksy/visualizer.m: -------------------------------------------------------------------------------- 1 | %This is a graphical simulation of the quadcopter's motion 2 | 3 | %CREATE THE GRAPH 4 | 5 | function v = visualizer(data) 6 | figure; 7 | %Keeps the plots on the same graph 8 | plots = [subplot(2,1,1), subplot(2,1,2)]; 9 | %Creates 3 plots for the 3D visualisation and for angular velocity and 10 | %displacement against time. 11 | 12 | % Use the bottom two parts for angular velocity and displacement. 13 | for t = 1:2:length(data.t); 14 | subplot(plots(1)); 15 | multiplot(data, data.angvel, t); 16 | xlabel('Time (s)'); 17 | ylabel('Angular Velocity (rad/s)'); 18 | title('Angular Velocity'); 19 | 20 | subplot(plots(2)); 21 | multiplot(data, data.theta, t); 22 | xlabel('Time (s)'); 23 | ylabel('Angular Displacement (rad)'); 24 | title('Angular Displacement'); 25 | end 26 | 27 | end 28 | 29 | function multiplot(data, values, ind) 30 | % Select the parts of the data to plot. 31 | times = data.t(:, 1:ind); 32 | values = values(:, 1:ind); 33 | 34 | % Plot in RGB, with different markers for different components. 35 | plot(times, values(1, :), 'r-', times, values(2, :), 'g', times, values(3, :), 'b-.'); 36 | 37 | % Set axes to remain constant throughout plotting. 38 | xmin = min(data.t); 39 | xmax = max(data.t); 40 | ymin = 1.1 * min(min(values)); 41 | ymax = 1.1 * max(max(values)); 42 | axis([xmin xmax ymin ymax]); 43 | end -------------------------------------------------------------------------------- /v2 (from scratch -- not working)/PD.m: -------------------------------------------------------------------------------- 1 | function [inputs,previous_error2] = PD(m,g,K,B,I,l,start_time,end_time... 2 | ,dt,orientation,desired_orientation,eulerdot) 3 | %eulerdot represents the time derivatives of the euler angles. 4 | 5 | inputs=zeros(4,1); 6 | previous_error2=zeros(3,1); %At the end of the code the function will set 7 | %the error as the new previous error. In 8 | %themain simulator code previous_error2 9 | %will be set as the previous_error argument 10 | %for this function. 11 | 12 | Ixx=I(1,1); 13 | Iyy=I(2,2); 14 | Izz=I(3,3); 15 | 16 | phi=orientation(1); 17 | theta=orientation(2); 18 | psi=orientation(3); 19 | 20 | Kp_phi=4; %the tuning parameters 21 | Kd_phi=3; 22 | Kp_theta=4; 23 | Kd_theta=3; 24 | Kp_psi=4; 25 | Kd_psi=3; 26 | 27 | phi_des=desired_orientation(1); 28 | theta_des=desired_orientation(2); 29 | psi_des=desired_orientation(3); 30 | 31 | error_phi=phi_des-phi; 32 | error_theta=theta_des-theta; 33 | error_psi=psi_des-psi; 34 | 35 | 36 | % errordot_phi=(error_phi-previous_error_phi)/dt; 37 | % errordot_theta=(error_theta-previous_error_theta)/dt; 38 | % errordot_psi=(error_psi-previous_error_psi)/dt; 39 | 40 | errordot_phi=eulerdot(1); 41 | errordot_theta=eulerdot(2); 42 | errordot_psi=eulerdot(3); 43 | 44 | thrust=m*g/(K*cos(phi)*cos(theta)); 45 | 46 | e_phi=Kp_phi*error_phi+Kd_phi*errordot_phi; 47 | e_theta=Kp_theta*error_theta+Kd_theta*errordot_theta; 48 | e_psi=Kp_psi*error_psi+Kd_psi*errordot_psi; 49 | 50 | inputs(1)=thrust/4 + 1/(4*l*B*K)*(-2*B*e_phi*Iyy-l*K*e_psi*Izz); 51 | 52 | inputs(2)=thrust/4 + 1/(4*l*B*K)*(-2*B*e_theta*Ixx-l*K*e_psi*Izz); 53 | 54 | inputs(3)=thrust/4 + 1/(4*l*B*K)*(2*B*e_phi*Iyy+ l*K*e_psi*Izz); 55 | 56 | inputs(4)=thrust/4 + 1/(4*l*B*K)*(2*B*e_theta*Ixx-l*K*e_psi*Izz); 57 | 58 | 59 | orientation = orientation +dt*eulerdot; 60 | end 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /v2 (from scratch -- not working)/acceleration.m: -------------------------------------------------------------------------------- 1 | function a= acceleration(orientation,m,K,inputs,velocities,Cx,Cy,Cz,g); 2 | 3 | gravity = [0 0 -g].'; 4 | thrust_FB = [0; 0; K*sum(inputs)]; %thrust in the body-fixed frame 5 | R = rotation(orientation); %rotation matrix to map vectors from F_B to F_I 6 | thrust_FI = R*thrust_FB; %thrust in the inertial frame 7 | drag = [-Cx*velocities(1);-Cy*velocities(2);-Cz*velocities(3)]; 8 | 9 | a = gravity + 1/m*thrust_FI + 1/m*drag; 10 | 11 | %This function computes the acceleration of the quadcopter based upon 12 | %its orientation, mass, thrust proportionality constant, inputs (the 13 | %squares of each propeller's angular velocity, the linear velocity, the 14 | %drag constants and the acceleration due to gravity. 15 | 16 | end -------------------------------------------------------------------------------- /v2 (from scratch -- not working)/angular_acceleration.m: -------------------------------------------------------------------------------- 1 | function a = angular_acceleration(I,l,K,B,inputs,angular_velocities) 2 | 3 | i1=inputs(1); 4 | i2=inputs(2); 5 | i3=inputs(3); 6 | i4=inputs(4); 7 | 8 | p=angular_velocities(1); 9 | q=angular_velocities(2); 10 | r=angular_velocities(3); 11 | 12 | 13 | Ixx=I(1,1); 14 | Iyy=I(2,2); 15 | Izz=I(3,3); 16 | 17 | actuator_action=[ 18 | l*K*(i2-i4); 19 | l*K*(i1-i3); 20 | 0; 21 | ]; 22 | 23 | dragtorque=[ 24 | 0; 25 | 0; 26 | B*(i1-i2+i3-i4); 27 | ]; 28 | 29 | % gyrotorque=[ 30 | % (Iyy-Izz)*q*r; 31 | % (Izz-Ixx)*p*r; 32 | % (Ixx-Iyy)*p*q; 33 | % ]; 34 | 35 | a = inv(I)*((actuator_action + dragtorque)-cross(angular_velocities,I*angular_velocities)); %+ gyrotorque 36 | 37 | %This function computes the angular acceleration of the quadcopter 38 | %based upon the inertia matrix, the length of the quadcopter arms, the 39 | %thrust proportionality constant, the inputs (squares of the angular 40 | %velocities of the propellers) and the angular velocity of the 41 | %quadcopter. 42 | 43 | 44 | end 45 | -------------------------------------------------------------------------------- /v2 (from scratch -- not working)/grapher.m: -------------------------------------------------------------------------------- 1 | %This is a graphical simulation of the quadcopter's motion 2 | 3 | %CREATE THE GRAPH 4 | 5 | function v = grapher(results) 6 | figure; 7 | %Keeps the plots on the same graph 8 | plots = [subplot(2,1,1), subplot(2,1,2)]; 9 | %Creates 3 plots for the 3D visualisation and for angular velocity and 10 | %displacement against time. 11 | 12 | % Use the bottom two parts for angular velocity and displacement. 13 | for t = 1:2:length(results.t); 14 | subplot(plots(1)); 15 | multiplot(results, results.angvel, t); 16 | xlabel('Time (s)'); 17 | ylabel('Angular Velocity (rad/s)'); 18 | title('Angular Velocity'); 19 | legend 20 | 21 | subplot(plots(2)); 22 | multiplot(results, results.theta, t); 23 | xlabel('Time (s)'); 24 | ylabel('Angular Displacement (rad)'); 25 | title('Angular Displacement'); 26 | legend 27 | end 28 | 29 | end 30 | 31 | function multiplot(results, values, ind) 32 | % Select the parts of the results to plot. 33 | times = results.t(:, 1:ind); 34 | values = values(:, 1:ind); 35 | 36 | % Plot in RGB, with different markers for different components. 37 | plot(times, values(1, :), 'r-', times, values(2, :), 'g', times, values(3, :), 'b-.'); 38 | 39 | % Set axes to remain constant throughout plotting. 40 | xmin = min(results.t); 41 | xmax = max(results.t); 42 | ymin = 1.1 * min(min(values)); 43 | ymax = 1.1 * max(max(values)); 44 | axis([xmin xmax ymin ymax]); 45 | end -------------------------------------------------------------------------------- /v2 (from scratch -- not working)/multiplot.m: -------------------------------------------------------------------------------- 1 | function multiplot(results, values, ind) 2 | % Select the parts of the results to plot. 3 | times = results.t(:, 1:ind); 4 | values = values(:, 1:ind); 5 | 6 | % Plot in RGB, with different markers for different components. 7 | plot(times, values(1, :), 'r-', times, values(2, :), 'g', times, values(3, :), 'b-.'); 8 | 9 | % Set axes to remain constant throughout plotting. 10 | xmin = min(results.t); 11 | xmax = max(results.t); 12 | ymin = 1.1 * min(min(values)); 13 | ymax = 1.1 * max(max(values)); 14 | axis([xmin xmax ymin ymax]); 15 | end -------------------------------------------------------------------------------- /v2 (from scratch -- not working)/omega2thetadot.m: -------------------------------------------------------------------------------- 1 | function thetadot = omega2thetadot(omega,angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | A=zeros(3); 7 | A=[1 0 -sin(theta);0 cos(phi) cos(theta)*sin(phi);0 -sin(phi) cos(phi)*sin(phi)]; 8 | 9 | W=inv(A); 10 | thetadot=W*omega; 11 | 12 | %This function converts from angular velocity to the time derivatives 13 | %of roll, pitch and yaw. 14 | end 15 | -------------------------------------------------------------------------------- /v2 (from scratch -- not working)/rotation.m: -------------------------------------------------------------------------------- 1 | function R = rotation(angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | R=zeros(3); 7 | R(1,:) = [ 8 | cos(theta)*cos(psi) 9 | cos(psi)*sin(phi)*sin(theta)-cos(phi)*sin(psi) 10 | sin(phi)*sin(psi)+cos(phi)*cos(psi)*sin(theta) 11 | ]; 12 | 13 | R(2,:) = [ 14 | cos(theta)*sin(psi) 15 | cos(phi)*cos(psi) + sin(phi)*sin(theta)*sin(psi) 16 | cos(phi)*sin(theta)*sin(psi)-cos(psi)*sin(phi) 17 | ]; 18 | 19 | R(3,:) = [ 20 | -sin(theta) 21 | cos(theta)*sin(phi) 22 | cos(phi)*cos(theta) 23 | ]; 24 | 25 | %This rotation matrix maps between the body-fixed frame and the inertial 26 | %frame. 27 | 28 | end 29 | 30 | -------------------------------------------------------------------------------- /v2 (from scratch -- not working)/simulate.m: -------------------------------------------------------------------------------- 1 | function simulate() 2 | 3 | m=0.5; %mass of quadcopter 4 | g=9.81; %gravitational acceleration 5 | l=0.25; %length of quadcopter arms 6 | K=3e-6; %thrust constant of proportionality 7 | B=1e-7; %drag constant of proportionality 8 | I=diag([5e-3, 5e-3, 10e-3]); %symmetrical inertial matrix 9 | 10 | Cx=0.25; %frictional coefficients 11 | Cy=0.25; 12 | Cz=0.25; 13 | 14 | start_time=0; %simulation times 15 | end_time=5; 16 | dt=0.005; 17 | 18 | times=start_time:dt:end_time; 19 | N=numel(times); 20 | 21 | position=[0;0;10]; 22 | orientation=zeros(3,1); 23 | velocities=zeros(3,1); 24 | 25 | phi_des=0; %defining the desired orientation of the quadcopter 26 | theta_des=0; 27 | psi_des=0; 28 | desired_orientation=[phi_des;theta_des;psi_des]; 29 | 30 | 31 | eulerdot=deg2rad(randi([-90,90],3,1)); %eulerdot represents the time 32 | %derivatives of the Euler angles. 33 | %this line sets an initial 34 | %disturbance for the controller to 35 | %stabilise. 36 | 37 | %Now create arrays for the outputs 38 | position_out=zeros(3,N); 39 | velocity_out=zeros(3,N); 40 | orientation_out=zeros(3,N); 41 | eulerdot_out=zeros(3,N); 42 | inputs_out=zeros(4,N); 43 | PE_out=zeros(3,N); 44 | 45 | ind=0; %creates a counter for the for-loop to help store ouputs 46 | 47 | for t = times 48 | ind=ind+1; 49 | 50 | %Caculate acceleration and angular acceleration 51 | 52 | [inputs,previous_error2] = PD(m,g,K,B,I,l,start_time,end_time,... 53 | dt,orientation,desired_orientation,eulerdot); 54 | 55 | previous_error=previous_error2; 56 | 57 | acc=acceleration(orientation,m,K,inputs,velocities,Cx,Cy,Cz,g); 58 | 59 | angular_velocities=thetadot2omega(eulerdot,orientation); 60 | 61 | ang_acc=angular_acceleration(I,l,K,B,inputs,angular_velocities); 62 | 63 | %Advance system state 64 | 65 | angular_velocities=angular_velocities+ang_acc*dt; 66 | eulerdot=omega2thetadot(angular_velocities,orientation); 67 | orientation=orientation+eulerdot*dt; 68 | velocities=velocities+acc*dt; 69 | position=position+velocities*dt; 70 | 71 | %Store simulation state for ouput 72 | position_out(:,ind)=position; 73 | velocity_out(:,ind)=velocities; 74 | orientation_out(:,ind)=orientation; 75 | eulerdot_out(:,ind)=eulerdot; 76 | inputs_out(:,ind)=inputs; 77 | PE_out(:,ind)=previous_error; 78 | 79 | end 80 | 81 | %Store outputs in a structured array 82 | result = struct('position',position_out,'orientation',orientation_out... 83 | ,'velocity',velocity_out,'angvel',eulerdot_out,'t',times, 'dt',dt,'input',inputs_out) 84 | 85 | figure; 86 | %Keeps the plots on the same graph 87 | plots = [subplot(2,1,1), subplot(2,1,2)]; 88 | %Creates 3 plots for the 3D visualisation and for angular velocity and 89 | %displacement against time. 90 | 91 | %Plot results 92 | for t = 1:2:length(result.t); 93 | subplot(plots(1)); 94 | multiplot(result, result.angvel, t); 95 | xlabel('Time (s)'); 96 | ylabel('Angular Velocity (rad/s)'); 97 | title('Angular Velocity'); 98 | 99 | subplot(plots(2)); 100 | multiplot(result, result.orientation, t); 101 | xlabel('Time (s)'); 102 | ylabel('Angular Displacement (rad)'); 103 | title('Angular Displacement'); 104 | end 105 | 106 | end 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | -------------------------------------------------------------------------------- /v2 (from scratch -- not working)/thetadot2omega.m: -------------------------------------------------------------------------------- 1 | function omega = thetadot2omega(thetadot,angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | A=zeros(3); 7 | A(1,:)=[ 8 | 1 9 | 0 10 | -sin(theta) 11 | ]; 12 | A(2,:) =[ 13 | 0 14 | cos(phi) 15 | cos(theta)*sin(phi) 16 | ]; 17 | A(3,:) =[ 18 | 0 19 | -sin(phi) 20 | cos(phi)*cos(theta) 21 | ]; 22 | 23 | omega=A*thetadot; 24 | 25 | 26 | %This function converts time derivatives of roll, pitch and yaw into 27 | %angular velocity. A is the necessary rotation matrix for this operation. 28 | %We can define the reverse function omega2thetadot using the inverse of 29 | %this matrix if desired. 30 | 31 | end 32 | 33 | -------------------------------------------------------------------------------- /v3 (all graphs working)/PD.m: -------------------------------------------------------------------------------- 1 | function c = PD(Kp_phi, Kd_phi, Kp_theta, Kd_theta, Kp_psi, Kd_psi) 2 | % Use manually tuned parameters, unless arguments provide the parameters. 3 | if nargin == 0 4 | Kp_phi = 5; 5 | Kd_phi = 4; 6 | Kp_theta = 5; 7 | Kd_theta = 4; 8 | Kp_psi = 5; 9 | Kd_psi = 4; 10 | 11 | elseif nargin == 1 || nargin > 2 12 | error('Incorrect number of parameters.'); 13 | end 14 | 15 | 16 | c = @(state, thetadot) pd_controller(state, thetadot, Kp_phi, Kd_phi, Kp_theta, Kd_theta, Kp_psi, Kd_psi); 17 | 18 | end 19 | 20 | 21 | 22 | function [inputs,state] = pd_controller(state, thetadot, Kp_phi, Kd_phi, Kp_theta, Kd_theta, Kp_psi, Kd_psi) 23 | %the "state" argument is the structured array from which the controller 24 | %must access the necessary information. This is the controller_params 25 | %struct in the simulator code. 26 | 27 | %Create the theta (orientation) vector in the structured array if it does 28 | %not yet exist. This will be updated each time this code is run in the 29 | %simulator for-loop. 30 | if ~isfield(state,'orientation') 31 | state.orientation = zeros (3,1); 32 | end 33 | 34 | %Now create the inputs that will be sent to the quadcopter. 35 | %There are four inputs (an angular velocity^2 for each prop) 36 | inputs = zeros(4,1); 37 | 38 | %Now extract from the structured array the necessary information to compute 39 | %the inputs. 40 | 41 | vels=state.xdot; 42 | 43 | phi_des=0.0; 44 | theta_des=0.0; 45 | psi_des=0.0; 46 | 47 | phi=state.orientation(1); 48 | theta=state.orientation(2); 49 | psi=state.orientation(3); 50 | 51 | phidot=thetadot(1); 52 | thetadot2=thetadot(2); 53 | psidot=thetadot(3); 54 | 55 | error_phi=phi_des-phi; 56 | error_theta=theta_des-theta; 57 | error_psi=psi_des-psi; 58 | 59 | e = zeros(3,1); 60 | e(1)=Kp_phi*-error_phi+Kd_phi*phidot; 61 | e(2)=Kp_theta*-error_theta+Kd_theta*thetadot2; 62 | e(3)=Kp_psi*-error_psi+Kd_psi*psidot; 63 | 64 | e_phi=e(1); 65 | e_theta=e(2); 66 | e_psi=e(3); 67 | 68 | Ixx=state.I(1,1); 69 | Iyy=state.I(2,2); 70 | Izz=state.I(3,3); 71 | 72 | b=state.b; 73 | k=state.k; 74 | L=state.L; 75 | m=state.m; 76 | g=state.g; 77 | 78 | 79 | 80 | %Now compute total thrust (to simplify input equations) 81 | thrust = m*g/(k*cos(phi)*cos(theta)); 82 | 83 | %Now compute the inputs 84 | inputs(1)= thrust/4 - (2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 85 | inputs(2)= thrust/4 - (2*b*e_theta*Iyy - k*L*e_psi*Izz)/(4*b*L*k); 86 | inputs(3)= thrust/4 - (-2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 87 | inputs(4)= thrust/4 + (2*b*e_theta*Iyy + k*L*e_psi*Izz)/(4*b*L*k); 88 | 89 | %Now advance the controller state (this essentially means updating the 90 | %orientation vector: 91 | 92 | state.orientation = state.orientation + state.dt*thetadot; 93 | end 94 | 95 | 96 | 97 | 98 | 99 | 100 | -------------------------------------------------------------------------------- /v3 (all graphs working)/acceleration.m: -------------------------------------------------------------------------------- 1 | function a = acceleration(inputs,angles,velocities,m,g,k,kd) 2 | 3 | %The acceleration of the quadcopter is a function of gravity, the thrust 4 | %and drag forces. From our model we have (a is the acceleration vector, v 5 | %is the velocity vector): 6 | 7 | %m*a=[0 0 -mg].' + T_I +F_D 8 | % =[0 0 -mg].' + R*T_B + F_D 9 | % therefore a = [0 0 -g].' + 1/m*(R*[0 0 k*sum(angular accelerations^2)] + 10 | % kd*v) 11 | 12 | %The angular velocities are the inputs, thrust (T is in F_B) is a function 13 | %of angular velocity and k. 14 | 15 | gravity = [0 0 -g].'; 16 | R = rotation(angles); 17 | thrust_FB=[0;0;k*sum(inputs)]; 18 | thrust_FI=R*thrust_FB; 19 | 20 | Fd = -kd*velocities; 21 | 22 | a = gravity +1/m*thrust_FI +1/m*Fd; 23 | 24 | 25 | end 26 | 27 | -------------------------------------------------------------------------------- /v3 (all graphs working)/angular_acceleration.m: -------------------------------------------------------------------------------- 1 | function omegadot = angular_acceleration(inputs,omega,I,L,b,k) 2 | 3 | i1=inputs(1); 4 | i2=inputs(2); 5 | i3=inputs(3); 6 | i4=inputs(4); 7 | 8 | p=omega(1); 9 | q=omega(2); 10 | r=omega(3); 11 | 12 | Ixx=I(1,1); 13 | Iyy=I(2,2); 14 | Izz=I(3,3); 15 | 16 | actuator_action = [ 17 | L*k*(i1-i3) 18 | L*k*(i2-i4) 19 | b*(i1-i2+i3-i4) 20 | ]; 21 | 22 | dragtorque=[ 23 | 0 24 | 0 25 | b*(i1-i2+i3-i4) 26 | ]; 27 | 28 | gyrotorque=[ 29 | (Iyy-Izz)*q*r 30 | (Izz-Ixx)*p*r 31 | (Ixx-Iyy)*p*q 32 | ]; 33 | 34 | tau = actuator_action+dragtorque+gyrotorque; 35 | 36 | %Now use the Euler equation for rigid body motion to obtain angular 37 | %acceleration: 38 | 39 | omegadot=inv(I)*(actuator_action-cross(omega,I*omega)); 40 | 41 | 42 | end 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /v3 (all graphs working)/grapher.m: -------------------------------------------------------------------------------- 1 | 2 | function g = grapher(result) 3 | times=result.t; 4 | 5 | xs=result.x(1,:); 6 | ys=result.x(2,:); 7 | zs=result.x(3,:); 8 | 9 | xvels=result.vel(1,:); 10 | yvels=result.vel(2,:); 11 | zvels=result.vel(3,:); 12 | 13 | phis=result.theta(1,:); 14 | thetas=result.theta(2,:); 15 | psis=result.theta(3,:); 16 | 17 | phisdot=result.angvel(1,:); 18 | thetasdot=result.angvel(2,:); 19 | psisdot=result.angvel(3,:); 20 | 21 | 22 | figure; 23 | plots=[subplot(2,1,1), subplot(2,1,2)]; 24 | 25 | subplot(plots(1)); 26 | plot(times,phisdot, 'r', 'Linewidth',1.2) 27 | title('Angular Velocities') 28 | xlabel('time (s)') 29 | ylabel('radians/s') 30 | hold on 31 | plot(times,thetasdot, '--b', 'Linewidth',1.2) 32 | hold on 33 | plot(times,psisdot, ':g', 'Linewidth',1.2) 34 | legend('Roll','Pitch','Yaw') 35 | 36 | subplot(plots(2)); 37 | plot(times,phis, 'r', 'Linewidth',1.2) 38 | title('Angular Displacements') 39 | xlabel('time (s)') 40 | ylabel('radians') 41 | hold on 42 | plot(times,thetas, '--b', 'Linewidth',1.2) 43 | hold on 44 | plot(times,psis, ':g', 'Linewidth',1.2) 45 | 46 | figure 47 | plots=[subplot(2,1,1), subplot(2,1,2)]; 48 | 49 | subplot(plots(1)); 50 | plot(times,xvels, 'r', 'Linewidth',1.2) 51 | title('Velocities') 52 | xlabel('time (s)') 53 | ylabel('m/s') 54 | hold on 55 | plot(times,yvels, '--b', 'Linewidth',1.2) 56 | hold on 57 | plot(times,zvels, ':g', 'Linewidth',1.2) 58 | legend('x','y','z') 59 | 60 | subplot(plots(2)); 61 | plot(times,xs, 'r', 'Linewidth',1.2) 62 | title('Displacements') 63 | xlabel('time (s)') 64 | ylabel('metres') 65 | hold on 66 | plot(times,ys, '--b', 'Linewidth',1.2) 67 | hold on 68 | plot(times,zs, ':g', 'Linewidth',1.2) 69 | 70 | 71 | 72 | end -------------------------------------------------------------------------------- /v3 (all graphs working)/omega2thetadot.m: -------------------------------------------------------------------------------- 1 | function thetadot = omega2thetadot(omega,angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | A=zeros(3); 7 | A(1,:)=[ 8 | 1 9 | 0 10 | -sin(theta) 11 | ]; 12 | 13 | A(2,:) =[ 14 | 0 15 | cos(phi) 16 | cos(theta)*sin(phi) 17 | ]; 18 | 19 | A(3,:) =[ 20 | 0 21 | -sin(phi) 22 | cos(phi)*cos(theta) 23 | ]; 24 | 25 | W=inv(A); 26 | thetadot=W*omega; 27 | 28 | %This function converts from angular velocity to the time derivatives 29 | %of roll, pitch and yaw. 30 | end 31 | -------------------------------------------------------------------------------- /v3 (all graphs working)/rotation.m: -------------------------------------------------------------------------------- 1 | function R = rotation(angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | R=zeros(3); 7 | R(1,:) = [ 8 | cos(theta)*cos(psi) 9 | cos(psi)*sin(phi)*sin(theta)-cos(phi)*sin(psi) 10 | sin(phi)*sin(psi)+cos(phi)*cos(psi)*sin(theta) 11 | ]; 12 | 13 | R(2,:) = [ 14 | cos(theta)*sin(psi) 15 | cos(phi)*cos(psi) + sin(phi)*sin(theta)*sin(psi) 16 | cos(phi)*sin(theta)*sin(psi)-cos(psi)*sin(phi) 17 | ]; 18 | 19 | R(3,:) = [ 20 | -sin(theta) 21 | cos(theta)*sin(phi) 22 | cos(phi)*cos(theta) 23 | ]; 24 | 25 | %This rotation matrix maps between the body-fixed frame and the inertial 26 | %frame. 27 | 28 | end 29 | 30 | -------------------------------------------------------------------------------- /v3 (all graphs working)/thetadot2omega.m: -------------------------------------------------------------------------------- 1 | function omega = thetadot2omega(thetadot,angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | A=zeros(3); 7 | A(1,:)=[ 8 | 1 9 | 0 10 | -sin(theta) 11 | ]; 12 | A(2,:) =[ 13 | 0 14 | cos(phi) 15 | cos(theta)*sin(phi) 16 | ]; 17 | A(3,:) =[ 18 | 0 19 | -sin(phi) 20 | cos(phi)*cos(theta) 21 | ]; 22 | 23 | omega=A*thetadot; 24 | 25 | 26 | %This function converts time derivatives of roll, pitch and yaw into 27 | %angular velocity. A is the necessary rotation matrix for this operation. 28 | %We can define the reverse function omega2thetadot using the inverse of 29 | %this matrix if desired. 30 | 31 | end 32 | 33 | -------------------------------------------------------------------------------- /v3 (all graphs working)/visualiser notes.psd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/samuelgilonis/Quadcopter-PID-controller/5aae0d5a14cb32cb6a54b4c1b8df19d9bffbc83f/v3 (all graphs working)/visualiser notes.psd -------------------------------------------------------------------------------- /v4 (same as v3)/PD.m: -------------------------------------------------------------------------------- 1 | function c = PD(Kp_phi, Kd_phi, Kp_theta, Kd_theta, Kp_psi, Kd_psi) 2 | % Use manually tuned parameters, unless arguments provide the parameters. 3 | if nargin == 0 4 | Kp_phi = 5; 5 | Kd_phi = 4; 6 | Kp_theta = 5; 7 | Kd_theta = 4; 8 | Kp_psi = 5; 9 | Kd_psi = 4; 10 | 11 | elseif nargin == 1 || nargin > 2 12 | error('Incorrect number of parameters.'); 13 | end 14 | 15 | 16 | c = @(state, thetadot) pd_controller(state, thetadot, Kp_phi, Kd_phi, Kp_theta, Kd_theta, Kp_psi, Kd_psi); 17 | 18 | end 19 | 20 | 21 | 22 | function [inputs,state] = pd_controller(state, thetadot, Kp_phi, Kd_phi, Kp_theta, Kd_theta, Kp_psi, Kd_psi) 23 | %the "state" argument is the structured array from which the controller 24 | %must access the necessary information. This is the controller_params 25 | %struct in the simulator code. 26 | 27 | %Create the theta (orientation) vector in the structured array if it does 28 | %not yet exist. This will be updated each time this code is run in the 29 | %simulator for-loop. 30 | if ~isfield(state,'orientation') 31 | state.orientation = zeros (3,1); 32 | end 33 | 34 | %Now create the inputs that will be sent to the quadcopter. 35 | %There are four inputs (an angular velocity^2 for each prop) 36 | inputs = zeros(4,1); 37 | 38 | %Now extract from the structured array the necessary information to compute 39 | %the inputs. 40 | 41 | phi_des=0.0; 42 | theta_des=0.0; 43 | psi_des=0.0; 44 | 45 | phi=state.orientation(1); 46 | theta=state.orientation(2); 47 | psi=state.orientation(3); 48 | 49 | phidot=thetadot(1); 50 | thetadot2=thetadot(2); 51 | psidot=thetadot(3); 52 | 53 | error_phi=phi_des-phi; 54 | error_theta=theta_des-theta; 55 | error_psi=psi_des-psi; 56 | 57 | e = zeros(3,1); 58 | e(1)=Kp_phi*-error_phi+Kd_phi*phidot; 59 | e(2)=Kp_theta*-error_theta+Kd_theta*thetadot2; 60 | e(3)=Kp_psi*-error_psi+Kd_psi*psidot; 61 | 62 | e_phi=e(1); 63 | e_theta=e(2); 64 | e_psi=e(3); 65 | 66 | Ixx=state.I(1,1); 67 | Iyy=state.I(2,2); 68 | Izz=state.I(3,3); 69 | 70 | b=state.b; 71 | k=state.k; 72 | L=state.L; 73 | m=state.m; 74 | g=state.g; 75 | 76 | 77 | 78 | %Now compute total thrust (to simplify input equations) 79 | thrust = m*g/(k*cos(phi)*cos(theta)); 80 | 81 | %Now compute the inputs 82 | inputs(1)= thrust/4 - (2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 83 | inputs(2)= thrust/4 - (2*b*e_theta*Iyy - k*L*e_psi*Izz)/(4*b*L*k); 84 | inputs(3)= thrust/4 - (-2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 85 | inputs(4)= thrust/4 + (2*b*e_theta*Iyy + k*L*e_psi*Izz)/(4*b*L*k); 86 | 87 | %Now advance the controller state (this essentially means updating the 88 | %orientation vector: 89 | 90 | state.orientation = state.orientation + state.dt*thetadot; 91 | end 92 | 93 | 94 | 95 | 96 | 97 | 98 | -------------------------------------------------------------------------------- /v4 (same as v3)/acceleration.m: -------------------------------------------------------------------------------- 1 | function a = acceleration(inputs,angles,velocities,m,g,k,kd) 2 | 3 | %The acceleration of the quadcopter is a function of gravity, the thrust 4 | %and drag forces. From our model we have (a is the acceleration vector, v 5 | %is the velocity vector): 6 | 7 | %m*a=[0 0 -mg].' + T_I +F_D 8 | % =[0 0 -mg].' + R*T_B + F_D 9 | % therefore a = [0 0 -g].' + 1/m*(R*[0 0 k*sum(angular accelerations^2)] + 10 | % kd*v) 11 | 12 | %The angular velocities are the inputs, thrust (T is in F_B) is a function 13 | %of angular velocity and k. 14 | 15 | gravity = [0 0 -g].'; 16 | R = rotation(angles); 17 | thrust_FB=[0;0;k*sum(inputs)]; 18 | thrust_FI=R*thrust_FB; 19 | 20 | Fd = -kd*velocities; 21 | 22 | a = gravity +1/m*thrust_FI +1/m*Fd; 23 | 24 | 25 | end 26 | 27 | -------------------------------------------------------------------------------- /v4 (same as v3)/angular_acceleration.asv: -------------------------------------------------------------------------------- 1 | function omegadot = angular_acceleration(inputs,omega,I,L,b,k) 2 | 3 | i1=inputs(1); 4 | i2=inputs(2); 5 | i3=inputs(3); 6 | i4=inputs(4); 7 | 8 | actuator_action = [ 9 | L*k*(i1-13) 10 | L*k*(i2-i4) 11 | b*(i1-i2+i3-i4) 12 | ]; 13 | 14 | dragtorque=[ 15 | 0; 16 | 0; 17 | b*(i1-i2+i3-i4); 18 | ]; 19 | 20 | tau = [ 21 | L*k*(i1-i3) 22 | L*k*(i2-i4) 23 | b*(i1-i2+i3-i4) 24 | ]; 25 | 26 | %Now use the Euler equation for rigid body motion to obtain angular 27 | %acceleration: 28 | 29 | omegadot=inv(I)*(actuator_action-cross(omega,I*omega)); 30 | 31 | 32 | end 33 | 34 | % i1=inputs(1); 35 | % i2=inputs(2); 36 | % i3=inputs(3); 37 | % i4=inputs(4); 38 | % 39 | % actuator_action=[ 40 | % L*k*(i2-14); 41 | % L*k*(i1-i3); 42 | % 0; 43 | % ]; 44 | % 45 | % dragtorque=[ 46 | % 0; 47 | % 0; 48 | % b*(i1-i2+i3-i4); 49 | % ]; 50 | % 51 | % tau=actuator_action+dragtorque; 52 | % 53 | % %Now use the Euler equation for rigid body motion to obtain angular 54 | % %acceleration: 55 | % 56 | % omegadot=inv(I)*(tau-cross(omega,I*omega)); 57 | 58 | 59 | -------------------------------------------------------------------------------- /v4 (same as v3)/angular_acceleration.m: -------------------------------------------------------------------------------- 1 | function omegadot = angular_acceleration(inputs,omega,I,L,b,k) 2 | 3 | i1=inputs(1); 4 | i2=inputs(2); 5 | i3=inputs(3); 6 | i4=inputs(4); 7 | 8 | p=omega(1); 9 | q=omega(2); 10 | r=omega(3); 11 | 12 | Ixx=I(1,1); 13 | Iyy=I(2,2); 14 | Izz=I(3,3); 15 | 16 | actuator_action = [ 17 | L*k*(i1-i3) 18 | L*k*(i2-i4) 19 | b*(i1-i2+i3-i4) 20 | ]; 21 | 22 | dragtorque=[ 23 | 0 24 | 0 25 | b*(i1-i2+i3-i4) 26 | ]; 27 | 28 | gyrotorque=[ 29 | (Iyy-Izz)*q*r 30 | (Izz-Ixx)*p*r 31 | (Ixx-Iyy)*p*q 32 | ]; 33 | 34 | tau = actuator_action+dragtorque+gyrotorque; 35 | 36 | %Now use the Euler equation for rigid body motion to obtain angular 37 | %acceleration: 38 | 39 | omegadot=inv(I)*(actuator_action-cross(omega,I*omega)); 40 | 41 | 42 | end 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /v4 (same as v3)/grapher.asv: -------------------------------------------------------------------------------- 1 | %This is a graphical simulation of the quadcopter's motion 2 | 3 | %CREATE THE GRAPH 4 | 5 | function v = grapher(result) 6 | times=result.t; 7 | 8 | xs=result.x(1,:); 9 | ys=result.x(2,:); 10 | zs=result.x(3,:); 11 | 12 | xvels=result.vel(1,:); 13 | yvels=result.vel(2,:); 14 | zvels=result.vel(3,:); 15 | 16 | phis=result.theta(1,:); 17 | thetas=result.theta(2,:); 18 | psis=result.theta(3,:); 19 | 20 | phisdot=result.angvel(1,:); 21 | thetasdot=result.angvel(2,:); 22 | psisdot=result.angvel(3,:); 23 | 24 | figure; 25 | plots=[subplot(2,1,1), subplot(2,1,2)]; 26 | 27 | subplot(plots(1)); 28 | plot(times,phisdot, 'r', 'Linewidth',1.5) 29 | title('Angular Velocities') 30 | xlabel('time (s)') 31 | ylabel('radians/s') 32 | hold on 33 | plot(times,thetasdot, 'b','--', 'Linewidth',1.5) 34 | hold on 35 | plot(times,psisdot, 'g',':', 'Linewidth',1.5) 36 | legend('Roll','Pitch','Yaw') 37 | 38 | subplot(plots(2)); 39 | plot(times,phis, 'r', 'Linewidth',1.5) 40 | title('Angular Displacements') 41 | xlabel('time (s)') 42 | ylabel('radians') 43 | hold on 44 | plot(times,thetas, 'b','--', 'Linewidth',1.5) 45 | hold on 46 | plot(times,psis, 'g', 'Linewidth',1.5) 47 | 48 | figure 49 | plots=[subplot(2,1,1), subplot(2,1,2)]; 50 | 51 | subplot(plots(1)); 52 | plot(times,xvels, 'r', 'Linewidth',1.5) 53 | title('Velocities') 54 | xlabel('time (s)') 55 | ylabel('m/s') 56 | hold on 57 | plot(times,yvels, 'b','--', 'Linewidth',1.5) 58 | hold on 59 | plot(times,zvels, 'g', 'Linewidth',1.5) 60 | legend('x','y','z') 61 | 62 | subplot(plots(2)); 63 | plot(times,xs, 'r', 'Linewidth',1.5) 64 | title('Displacements') 65 | xlabel('time (s)') 66 | ylabel('metres') 67 | hold on 68 | plot(times,ys, 'b','--', 'Linewidth',1.5) 69 | hold on 70 | plot(times,zs, 'g', 'Linewidth',1.5) 71 | 72 | end -------------------------------------------------------------------------------- /v4 (same as v3)/grapher.m: -------------------------------------------------------------------------------- 1 | 2 | function g = grapher(result) 3 | times=result.t; 4 | 5 | xs=result.x(1,:); 6 | ys=result.x(2,:); 7 | zs=result.x(3,:); 8 | 9 | xvels=result.vel(1,:); 10 | yvels=result.vel(2,:); 11 | zvels=result.vel(3,:); 12 | 13 | phis=result.theta(1,:); 14 | thetas=result.theta(2,:); 15 | psis=result.theta(3,:); 16 | 17 | phisdot=result.angvel(1,:); 18 | thetasdot=result.angvel(2,:); 19 | psisdot=result.angvel(3,:); 20 | 21 | figure; 22 | plots=[subplot(2,1,1), subplot(2,1,2)]; 23 | 24 | subplot(plots(1)); 25 | plot(times,phisdot, 'r', 'Linewidth',1.2) 26 | title('Angular Velocities') 27 | xlabel('time (s)') 28 | ylabel('radians/s') 29 | hold on 30 | plot(times,thetasdot, '--b', 'Linewidth',1.2) 31 | hold on 32 | plot(times,psisdot, ':g', 'Linewidth',1.2) 33 | legend('Roll','Pitch','Yaw') 34 | 35 | subplot(plots(2)); 36 | plot(times,phis, 'r', 'Linewidth',1.2) 37 | title('Angular Displacements') 38 | xlabel('time (s)') 39 | ylabel('radians') 40 | hold on 41 | plot(times,thetas, '--b', 'Linewidth',1.2) 42 | hold on 43 | plot(times,psis, ':g', 'Linewidth',1.2) 44 | 45 | figure 46 | plots=[subplot(2,1,1), subplot(2,1,2)]; 47 | 48 | subplot(plots(1)); 49 | plot(times,xvels, 'r', 'Linewidth',1.2) 50 | title('Velocities') 51 | xlabel('time (s)') 52 | ylabel('m/s') 53 | hold on 54 | plot(times,yvels, '--b', 'Linewidth',1.2) 55 | hold on 56 | plot(times,zvels, ':g', 'Linewidth',1.2) 57 | legend('x','y','z') 58 | 59 | subplot(plots(2)); 60 | plot(times,xs, 'r', 'Linewidth',1.2) 61 | title('Displacements') 62 | xlabel('time (s)') 63 | ylabel('metres') 64 | hold on 65 | plot(times,ys, '--b', 'Linewidth',1.2) 66 | hold on 67 | plot(times,zs, ':g', 'Linewidth',1.2) 68 | 69 | end -------------------------------------------------------------------------------- /v4 (same as v3)/omega2thetadot.m: -------------------------------------------------------------------------------- 1 | function thetadot = omega2thetadot(omega,angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | A=zeros(3); 7 | A(1,:)=[ 8 | 1 9 | 0 10 | -sin(theta) 11 | ]; 12 | 13 | A(2,:) =[ 14 | 0 15 | cos(phi) 16 | cos(theta)*sin(phi) 17 | ]; 18 | 19 | A(3,:) =[ 20 | 0 21 | -sin(phi) 22 | cos(phi)*cos(theta) 23 | ]; 24 | 25 | W=inv(A); 26 | thetadot=W*omega; 27 | 28 | %This function converts from angular velocity to the time derivatives 29 | %of roll, pitch and yaw. 30 | end 31 | -------------------------------------------------------------------------------- /v4 (same as v3)/rotation.m: -------------------------------------------------------------------------------- 1 | function R = rotation(angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | R=zeros(3); 7 | R(1,:) = [ 8 | cos(theta)*cos(psi) 9 | cos(psi)*sin(phi)*sin(theta)-cos(phi)*sin(psi) 10 | sin(phi)*sin(psi)+cos(phi)*cos(psi)*sin(theta) 11 | ]; 12 | 13 | R(2,:) = [ 14 | cos(theta)*sin(psi) 15 | cos(phi)*cos(psi) + sin(phi)*sin(theta)*sin(psi) 16 | cos(phi)*sin(theta)*sin(psi)-cos(psi)*sin(phi) 17 | ]; 18 | 19 | R(3,:) = [ 20 | -sin(theta) 21 | cos(theta)*sin(phi) 22 | cos(phi)*cos(theta) 23 | ]; 24 | 25 | %This rotation matrix maps between the body-fixed frame and the inertial 26 | %frame. 27 | 28 | end 29 | 30 | -------------------------------------------------------------------------------- /v4 (same as v3)/thetadot2omega.m: -------------------------------------------------------------------------------- 1 | function omega = thetadot2omega(thetadot,angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | A=zeros(3); 7 | A(1,:)=[ 8 | 1 9 | 0 10 | -sin(theta) 11 | ]; 12 | A(2,:) =[ 13 | 0 14 | cos(phi) 15 | cos(theta)*sin(phi) 16 | ]; 17 | A(3,:) =[ 18 | 0 19 | -sin(phi) 20 | cos(phi)*cos(theta) 21 | ]; 22 | 23 | omega=A*thetadot; 24 | 25 | 26 | %This function converts time derivatives of roll, pitch and yaw into 27 | %angular velocity. A is the necessary rotation matrix for this operation. 28 | %We can define the reverse function omega2thetadot using the inverse of 29 | %this matrix if desired. 30 | 31 | end 32 | 33 | -------------------------------------------------------------------------------- /v4 (same as v3)/visualiser notes.psd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/samuelgilonis/Quadcopter-PID-controller/5aae0d5a14cb32cb6a54b4c1b8df19d9bffbc83f/v4 (same as v3)/visualiser notes.psd -------------------------------------------------------------------------------- /v5 (linear velocity controllers)/PD.m: -------------------------------------------------------------------------------- 1 | function c = PD(Kp_phi, Kd_phi, Kp_theta, Kd_theta, Kp_psi, Kd_psi, Kp_x, Kd_x, Kp_y, Kd_y, Kp_z, Kd_z) 2 | % Use manually tuned parameters, unless arguments provide the parameters. 3 | if nargin == 0 4 | 5 | Kp_phi = 5; 6 | Kd_phi = 4; 7 | 8 | Kp_theta = 5; 9 | Kd_theta = 4; 10 | 11 | Kp_psi = 5; 12 | Kd_psi = 4; 13 | 14 | Kp_x = 0.3*pi/180; 15 | Kd_x = 0.5*pi/180; 16 | 17 | Kp_y = 0.3*pi/180; 18 | Kd_y = 0.5*pi/180; 19 | 20 | Kp_z = 1000000; 21 | Kd_z = -1000000; 22 | 23 | end 24 | 25 | 26 | c = @(state, thetadot) pd_controller(state, thetadot, Kp_phi, Kd_phi, Kp_theta, Kd_theta, Kp_psi, Kd_psi, Kp_x, Kd_x, Kp_y, Kd_y, Kp_z, Kd_z); 27 | 28 | end 29 | 30 | 31 | 32 | function [inputs,state] = pd_controller(state, thetadot, Kp_phi, Kd_phi, Kp_theta, Kd_theta, Kp_psi, Kd_psi, Kp_x, Kd_x, Kp_y, Kd_y, Kp_z, Kd_z) 33 | %the "state" argument is the structured array from which the controller 34 | %must access the necessary information. This is the controller_params 35 | %struct in the simulator code. 36 | 37 | %Create the theta (orientation) vector in the structured array if it does 38 | %not yet exist. This will be updated each time this code is run in the 39 | %simulator for-loop. 40 | if ~isfield(state,'orientation') 41 | state.orientation = zeros (3,1); 42 | end 43 | 44 | if ~isfield(state,'errors') 45 | state.errors = zeros (3,1); 46 | end 47 | 48 | %Now create the inputs that will be sent to the quadcopter. 49 | %There are four inputs (an angular velocity^2 for each prop) 50 | inputs = zeros(4,1); 51 | 52 | %Now extract from the structured array the necessary information to compute 53 | %the inputs. 54 | 55 | x_des=100.0; 56 | y_des=100.0; 57 | z_des=100.0; 58 | 59 | x=state.x(1); 60 | y=state.x(2); 61 | z=state.x(3); 62 | 63 | error_x=x_des-x; 64 | error_y=y-y_des; 65 | error_z=z_des-z; 66 | errors=[error_x;error_y;error_z]; 67 | 68 | vels=state.xdot; 69 | xdot=vels(1); 70 | ydot=vels(2); 71 | zdot=vels(3); 72 | 73 | errordot_x=xdot; 74 | errordot_y=-ydot; 75 | errordot_z=zdot; 76 | 77 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 78 | %The roll and pitch angles give y and x motion respectively. Therefore 79 | %PD controllers governing the desired roll and pitch angles can allow us to control x and y 80 | %positions. 81 | 82 | phi_des= Kp_y*error_y + Kd_y*errordot_y; 83 | if phi_des>pi/4 84 | phi_des=pi/4; 85 | end 86 | if phi_des<-pi/4 87 | phi_des=-pi/4; 88 | end 89 | 90 | theta_des= Kp_x*error_x + Kd_x*errordot_x; 91 | if theta_des>pi/4 92 | theta_des=pi/4; 93 | end 94 | if theta_des<-pi/4 95 | theta_des=-pi/4; 96 | end 97 | 98 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 99 | 100 | % phi_des= 0.0; 101 | % theta_des=0.0; 102 | psi_des=0.0; 103 | 104 | phi=state.orientation(1); 105 | theta=state.orientation(2); 106 | psi=state.orientation(3); 107 | 108 | phidot=thetadot(1); 109 | thetadot2=thetadot(2); 110 | psidot=thetadot(3); 111 | 112 | error_phi=phi_des-phi; 113 | error_theta=theta_des-theta; 114 | error_psi=psi_des-psi; 115 | 116 | e = zeros(3,1); 117 | e(1)=Kp_phi*-error_phi+Kd_phi*phidot; 118 | e(2)=Kp_theta*-error_theta+Kd_theta*thetadot2; 119 | e(3)=Kp_psi*-error_psi+Kd_psi*psidot; 120 | 121 | e_phi=e(1); 122 | e_theta=e(2); 123 | e_psi=e(3); 124 | 125 | Ixx=state.I(1,1); 126 | Iyy=state.I(2,2); 127 | Izz=state.I(3,3); 128 | 129 | b=state.b; 130 | k=state.k; 131 | L=state.L; 132 | m=state.m; 133 | g=state.g; 134 | 135 | 136 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 137 | %Now compute total thrust (to simplify input equations) 138 | 139 | %To have attitude control (z-axis motion) we are going to need a 140 | %controller to govern the thrust. If thrust = m*g/(k*cos(phi)*cos(theta)); 141 | %then the quadcopter will always be kept at a level altitude but we 142 | %want to be able to have the altitude as an input. 143 | 144 | thrust=Kp_z*error_z+Kd_z*errordot_z; 145 | if thrust<0 146 | thrust=0; 147 | end 148 | 149 | % thrust = m*g/(k*cos(phi)*cos(theta)); 150 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 151 | 152 | %Now compute the inputs 153 | inputs(1)= thrust/4 - (2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 154 | inputs(2)= thrust/4 - (2*b*e_theta*Iyy - k*L*e_psi*Izz)/(4*b*L*k); 155 | inputs(3)= thrust/4 - (-2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 156 | inputs(4)= thrust/4 + (2*b*e_theta*Iyy + k*L*e_psi*Izz)/(4*b*L*k); 157 | 158 | %Now advance the controller state (this essentially means updating the 159 | %orientation vector: 160 | 161 | state.orientation = state.orientation + state.dt*thetadot; 162 | state.phi_des=phi_des; 163 | state.theta_des=theta_des; 164 | state.errors=errors; 165 | state.thrust=thrust; 166 | end 167 | 168 | 169 | 170 | 171 | 172 | 173 | -------------------------------------------------------------------------------- /v5 (linear velocity controllers)/acceleration.m: -------------------------------------------------------------------------------- 1 | function a = acceleration(inputs,angles,velocities,m,g,k,kd) 2 | 3 | %The acceleration of the quadcopter is a function of gravity, the thrust 4 | %and drag forces. From our model we have (a is the acceleration vector, v 5 | %is the velocity vector): 6 | 7 | %m*a=[0 0 -mg].' + T_I +F_D 8 | % =[0 0 -mg].' + R*T_B + F_D 9 | % therefore a = [0 0 -g].' + 1/m*(R*[0 0 k*sum(angular accelerations^2)] + 10 | % kd*v) 11 | 12 | %The angular velocities are the inputs, thrust (T is in F_B) is a function 13 | %of angular velocity and k. 14 | 15 | gravity = [0 0 -g].'; 16 | R = rotation(angles); 17 | thrust_FB=[0;0;k*sum(inputs)]; 18 | thrust_FI=R*thrust_FB; 19 | 20 | Fd = -kd*velocities; 21 | 22 | a = gravity +1/m*thrust_FI +1/m*Fd; 23 | 24 | 25 | end 26 | 27 | -------------------------------------------------------------------------------- /v5 (linear velocity controllers)/angular_acceleration.m: -------------------------------------------------------------------------------- 1 | function omegadot = angular_acceleration(inputs,omega,I,L,b,k) 2 | 3 | i1=inputs(1); 4 | i2=inputs(2); 5 | i3=inputs(3); 6 | i4=inputs(4); 7 | 8 | p=omega(1); 9 | q=omega(2); 10 | r=omega(3); 11 | 12 | Ixx=I(1,1); 13 | Iyy=I(2,2); 14 | Izz=I(3,3); 15 | 16 | actuator_action = [ 17 | L*k*(i1-i3) 18 | L*k*(i2-i4) 19 | b*(i1-i2+i3-i4) 20 | ]; 21 | 22 | dragtorque=[ 23 | 0 24 | 0 25 | b*(i1-i2+i3-i4) 26 | ]; 27 | 28 | gyrotorque=[ 29 | (Iyy-Izz)*q*r 30 | (Izz-Ixx)*p*r 31 | (Ixx-Iyy)*p*q 32 | ]; 33 | 34 | tau = actuator_action+dragtorque+gyrotorque; 35 | 36 | %Now use the Euler equation for rigid body motion to obtain angular 37 | %acceleration: 38 | 39 | omegadot=inv(I)*(actuator_action-cross(omega,I*omega)); 40 | 41 | 42 | end 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /v5 (linear velocity controllers)/grapher.m: -------------------------------------------------------------------------------- 1 | 2 | function g = grapher(result) 3 | times=result.t; 4 | 5 | 6 | xvels=result.vel(1,:); 7 | yvels=result.vel(2,:); 8 | zvels=result.vel(3,:); 9 | 10 | phis=result.theta(1,:); 11 | thetas=result.theta(2,:); 12 | psis=result.theta(3,:); 13 | 14 | phisdot=result.angvel(1,:); 15 | thetasdot=result.angvel(2,:); 16 | psisdot=result.angvel(3,:); 17 | 18 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 19 | phides=result.phides; 20 | thetades=result.thetades; 21 | 22 | xs=result.x(1,:); 23 | ys=result.x(2,:); 24 | zs=result.x(3,:); 25 | 26 | xerrors=result.errors(1,:); 27 | yerrors=result.errors(2,:); 28 | zerrors=result.errors(3,:); 29 | 30 | thrust=result.thrust; 31 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 32 | 33 | figure; 34 | plots=[subplot(2,1,1), subplot(2,1,2)]; 35 | 36 | subplot(plots(1)); 37 | plot(times,phisdot, 'r', 'Linewidth',1.2) 38 | title('Angular Velocities') 39 | xlabel('time (s)') 40 | ylabel('radians/s') 41 | hold on 42 | plot(times,thetasdot, '--b', 'Linewidth',1.2) 43 | hold on 44 | plot(times,psisdot, ':g', 'Linewidth',1.2) 45 | legend('Roll','Pitch','Yaw') 46 | 47 | subplot(plots(2)); 48 | plot(times,phis, 'r', 'Linewidth',1.2) 49 | title('Angular Displacements') 50 | xlabel('time (s)') 51 | ylabel('radians') 52 | hold on 53 | plot(times,thetas, '--b', 'Linewidth',1.2) 54 | hold on 55 | plot(times,psis, ':g', 'Linewidth',1.2) 56 | 57 | figure 58 | plots=[subplot(2,1,1), subplot(2,1,2)]; 59 | 60 | subplot(plots(1)); 61 | plot(times,xvels, '--b', 'Linewidth',1.2) 62 | title('Velocities') 63 | xlabel('time (s)') 64 | ylabel('m/s') 65 | hold on 66 | plot(times,yvels, 'r', 'Linewidth',1.2) 67 | hold on 68 | plot(times,zvels, ':g', 'Linewidth',1.2) 69 | legend('x','y','z') 70 | 71 | subplot(plots(2)); 72 | plot(times,xs, '--b', 'Linewidth',1.2) 73 | title('Displacements') 74 | xlabel('time (s)') 75 | ylabel('metres') 76 | hold on 77 | plot(times,ys, 'r', 'Linewidth',1.2) 78 | hold on 79 | plot(times,zs, ':g', 'Linewidth',1.2) 80 | 81 | 82 | 83 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 84 | % figure; 85 | % plots=[subplot(3,1,1), subplot(3,1,2), subplot(3,1,3)]; 86 | % 87 | % subplot(plots(1)); 88 | % plot(times,phides, 'r', 'Linewidth',1.2) 89 | % hold on 90 | % plot(times,thetades,'--b','Linewidth',1.2) 91 | % title('Desired Roll and Pitch Angles Against Time') 92 | % xlabel('time (s)') 93 | % ylabel('radians') 94 | % legend('Roll','Pitch') 95 | % 96 | % subplot(plots(2)); 97 | % plot(ys,phides, 'r', 'Linewidth',1.2) 98 | % title('Desired Roll Angle Against y-coordinate') 99 | % xlabel('y (m)') 100 | % ylabel('radians') 101 | % legend('Roll') 102 | % 103 | % subplot(plots(3)); 104 | % plot(xs,thetades, '--b', 'Linewidth',1.2) 105 | % title('Desired Pitch Angle Against x-coordinate') 106 | % xlabel('x (m)') 107 | % ylabel('radians') 108 | % legend('Pitch') 109 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 110 | % figure; 111 | % plots=[subplot(2,1,1), subplot(2,1,2)]; 112 | % subplot(plots(1)); 113 | % plot(times,thrust,'k','LineWidth',1.2) 114 | % title('Thrust against time') 115 | % xlabel('time (s)') 116 | % ylabel('') 117 | % 118 | % subplot(plots(2)); 119 | % plot(zs,thrust,'r','LineWidth',1.2) 120 | % title('Thrust against z-coordinate') 121 | % xlabel('z-coordinate (m)') 122 | % ylabel('thrust (N)') 123 | 124 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 125 | 126 | 127 | end -------------------------------------------------------------------------------- /v5 (linear velocity controllers)/omega2thetadot.m: -------------------------------------------------------------------------------- 1 | function thetadot = omega2thetadot(omega,angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | A=zeros(3); 7 | A(1,:)=[ 8 | 1 9 | 0 10 | -sin(theta) 11 | ]; 12 | 13 | A(2,:) =[ 14 | 0 15 | cos(phi) 16 | cos(theta)*sin(phi) 17 | ]; 18 | 19 | A(3,:) =[ 20 | 0 21 | -sin(phi) 22 | cos(phi)*cos(theta) 23 | ]; 24 | 25 | W=inv(A); 26 | thetadot=W*omega; 27 | 28 | %This function converts from angular velocity to the time derivatives 29 | %of roll, pitch and yaw. 30 | end 31 | -------------------------------------------------------------------------------- /v5 (linear velocity controllers)/rotation.m: -------------------------------------------------------------------------------- 1 | function R = rotation(angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | R=zeros(3); 7 | R(1,:) = [cos(theta)*cos(psi); cos(psi)*sin(phi)*sin(theta)-cos(phi)*sin(psi);sin(phi)*sin(psi)+cos(phi)*cos(psi)*sin(theta)]; 8 | 9 | R(2,:) = [cos(theta)*sin(psi);cos(phi)*cos(psi) + sin(phi)*sin(theta)*sin(psi);cos(phi)*sin(theta)*sin(psi)-cos(psi)*sin(phi)]; 10 | 11 | R(3,:) = [-sin(theta);cos(theta)*sin(phi);cos(phi)*cos(theta)]; 12 | 13 | %This rotation matrix maps between the body-fixed frame and the inertial 14 | %frame. 15 | 16 | end 17 | 18 | -------------------------------------------------------------------------------- /v5 (linear velocity controllers)/thetadot2omega.m: -------------------------------------------------------------------------------- 1 | function omega = thetadot2omega(thetadot,angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | A=zeros(3); 7 | A(1,:)=[ 8 | 1 9 | 0 10 | -sin(theta) 11 | ]; 12 | A(2,:) =[ 13 | 0 14 | cos(phi) 15 | cos(theta)*sin(phi) 16 | ]; 17 | A(3,:) =[ 18 | 0 19 | -sin(phi) 20 | cos(phi)*cos(theta) 21 | ]; 22 | 23 | omega=A*thetadot; 24 | 25 | 26 | %This function converts time derivatives of roll, pitch and yaw into 27 | %angular velocity. A is the necessary rotation matrix for this operation. 28 | %We can define the reverse function omega2thetadot using the inverse of 29 | %this matrix if desired. 30 | 31 | end 32 | 33 | -------------------------------------------------------------------------------- /v5 (linear velocity controllers)/visualiser notes.psd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/samuelgilonis/Quadcopter-PID-controller/5aae0d5a14cb32cb6a54b4c1b8df19d9bffbc83f/v5 (linear velocity controllers)/visualiser notes.psd -------------------------------------------------------------------------------- /v6 (path plot and PID)/PD.m: -------------------------------------------------------------------------------- 1 | function c = PD() 2 | c = @(state, thetadot) pd_controller(state, thetadot); 3 | end 4 | 5 | function [inputs,state] = pd_controller(state, thetadot) 6 | %the "state" argument is the structured array from which the controller 7 | %must access the necessary information. This is the controller_params 8 | %struct in the simulator code. 9 | 10 | %Gain Parameters 11 | Kp_phi = 5; 12 | Kd_phi = 4; 13 | 14 | Kp_theta = 5; 15 | Kd_theta = 4; 16 | 17 | Kp_psi = 5; 18 | Kd_psi = 4; 19 | 20 | Kp_x = 0.3*pi/180; 21 | Kd_x = -0.5*pi/180; 22 | 23 | Kp_y = Kp_x; 24 | Kd_y = Kd_x; 25 | 26 | Kp_z = 1.2e6; 27 | Kd_z = -2.4e6; 28 | 29 | %Create the theta (orientation) vector in the structured array if it does 30 | %not yet exist. This will be updated each time this code is run in the 31 | %simulator for-loop. 32 | if ~isfield(state,'orientation') 33 | state.orientation = zeros (3,1); 34 | end 35 | 36 | if ~isfield(state,'errors') 37 | state.errors = zeros (6,1); 38 | end 39 | 40 | 41 | %Now create the inputs that will be sent to the quadcopter. 42 | %There are four inputs (an angular velocity^2 for each prop) 43 | inputs = zeros(4,1); 44 | 45 | %Now extract from the structured array the necessary information to compute 46 | %the inputs. 47 | 48 | x_des=state.xdes; 49 | y_des=state.ydes; 50 | z_des=state.zdes; 51 | 52 | x=state.x(1); 53 | y=state.x(2); 54 | z=state.x(3); 55 | 56 | error_x=x_des-x; 57 | error_y=y-y_des; 58 | error_z=z_des-z; 59 | errors=[error_x;error_y;error_z]; 60 | 61 | vels=state.xdot; 62 | xdot=vels(1); 63 | ydot=vels(2); 64 | zdot=vels(3); 65 | 66 | errordot_x=xdot; 67 | errordot_y=-ydot; 68 | errordot_z=zdot; 69 | 70 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 71 | %The roll and pitch angles give y and x motion respectively. Therefore 72 | %PD controllers governing the desired roll and pitch angles can allow us to control x and y 73 | %positions. 74 | 75 | phi_des= Kp_y*error_y + Kd_y*errordot_y; 76 | if phi_des>pi/4 77 | phi_des=pi/4; 78 | end 79 | if phi_des<-pi/4 80 | phi_des=-pi/4; 81 | end 82 | 83 | theta_des= Kp_x*error_x + Kd_x*errordot_x; 84 | if theta_des>pi/4 85 | theta_des=pi/4; 86 | end 87 | if theta_des<-pi/4 88 | theta_des=-pi/4; 89 | end 90 | 91 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 92 | 93 | % phi_des= 0.0; 94 | % theta_des=0.0; 95 | psi_des=0.0; 96 | 97 | phi=state.orientation(1); 98 | theta=state.orientation(2); 99 | psi=state.orientation(3); 100 | 101 | phidot=thetadot(1); 102 | thetadot2=thetadot(2); 103 | psidot=thetadot(3); 104 | 105 | error_phi=phi_des-phi; 106 | error_theta=theta_des-theta; 107 | error_psi=psi_des-psi; 108 | 109 | e = zeros(3,1); 110 | e(1)=Kp_phi*-error_phi+Kd_phi*phidot; 111 | e(2)=Kp_theta*-error_theta+Kd_theta*thetadot2; 112 | e(3)=Kp_psi*-error_psi+Kd_psi*psidot; 113 | 114 | e_phi=e(1); 115 | e_theta=e(2); 116 | e_psi=e(3); 117 | 118 | Ixx=state.I(1,1); 119 | Iyy=state.I(2,2); 120 | Izz=state.I(3,3); 121 | 122 | b=state.b; 123 | k=state.k; 124 | L=state.L; 125 | m=state.m; 126 | g=state.g; 127 | 128 | 129 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 130 | %Now compute total thrust (to simplify input equations) 131 | 132 | %To have attitude control (z-axis motion) we are going to need a 133 | %controller to govern the thrust. If thrust = m*g/(k*cos(phi)*cos(theta)); 134 | %then the quadcopter will always be kept at a level altitude but we 135 | %want to be able to have the altitude as an input. 136 | 137 | thrust=Kp_z*error_z+Kd_z*errordot_z; 138 | if thrust<0 139 | thrust=0; 140 | end 141 | 142 | % thrust = m*g/(k*cos(phi)*cos(theta)); 143 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 144 | 145 | %Now compute the propeller angular velocities for a given orientation. 146 | inputs(1)= thrust/4 - (2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 147 | inputs(2)= thrust/4 - (2*b*e_theta*Iyy - k*L*e_psi*Izz)/(4*b*L*k); 148 | inputs(3)= thrust/4 - (-2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 149 | inputs(4)= thrust/4 + (2*b*e_theta*Iyy + k*L*e_psi*Izz)/(4*b*L*k); 150 | 151 | %Now advance the controller state (this essentially means updating the 152 | %orientation vector: 153 | 154 | state.orientation = state.orientation + state.dt*thetadot; 155 | state.phi_des=phi_des; 156 | state.theta_des=theta_des; 157 | state.errors=errors; 158 | state.thrust=thrust; 159 | end 160 | 161 | 162 | 163 | 164 | 165 | 166 | -------------------------------------------------------------------------------- /v6 (path plot and PID)/PID.m: -------------------------------------------------------------------------------- 1 | function c = PID() 2 | c = @(state, thetadot) pid_controller(state, thetadot); 3 | end 4 | 5 | function [inputs,state] = pid_controller(state, thetadot) 6 | %the "state" argument is the structured array from which the controller 7 | %must access the necessary information. This is the controller_params 8 | %struct in the simulator code. 9 | 10 | %Gain Parameters 11 | Kp_phi = 5; 12 | Kd_phi = 4; 13 | 14 | Kp_theta = Kp_phi; 15 | Kd_theta = Kd_phi; 16 | 17 | Kp_psi = 5; 18 | Kd_psi = 4; 19 | Ki_psi = 0.06; 20 | 21 | Kp_x = 0.3*pi/180; 22 | Kd_x = 0.5*pi/180; 23 | Ki_x = 1.5e-4*pi/180; 24 | 25 | Kp_y = Kp_x; 26 | Kd_y = Kd_x; 27 | Ki_y = Ki_x; 28 | 29 | Kp_z = 1.2e6; 30 | Kd_z = -2.4e6; 31 | Ki_z = 2.0e5; 32 | 33 | %Create the theta (orientation) vector in the structured array if it does 34 | %not yet exist. This will be updated each time this code is run in the 35 | %simulator for-loop. 36 | if ~isfield(state,'orientation') 37 | state.orientation = zeros (3,1); 38 | end 39 | 40 | if ~isfield(state,'errors') 41 | state.errors = zeros (3,1); 42 | end 43 | 44 | if ~isfield(state,'integral') 45 | state.integral = zeros (6,1); 46 | end 47 | 48 | %Now create the inputs that will be sent to the quadcopter. 49 | %There are four inputs (an angular velocity^2 for each prop) 50 | inputs = zeros(4,1); 51 | 52 | %Now extract from the structured array the necessary information to compute 53 | %the inputs. 54 | 55 | x_des=state.xdes; 56 | y_des=state.ydes; 57 | z_des=state.zdes; 58 | 59 | x=state.x(1); 60 | y=state.x(2); 61 | z=state.x(3); 62 | 63 | error_x=x_des-x; 64 | error_y=y-y_des; 65 | error_z=z_des-z; 66 | errors=[error_x;error_y;error_z]; 67 | 68 | vels=state.xdot; 69 | xdot=vels(1); 70 | ydot=vels(2); 71 | zdot=vels(3); 72 | 73 | errordot_x=xdot; 74 | errordot_y=-ydot; 75 | errordot_z=zdot; 76 | 77 | int_x=state.integral(1); 78 | int_y=-state.integral(2); 79 | int_z=state.integral(3); 80 | int_phi=state.integral(4); 81 | int_theta=state.integral(5); 82 | int_psi=state.integral(6); 83 | 84 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 85 | %The roll and pitch angles give y and x motion respectively. Therefore 86 | %PD controllers governing the desired roll and pitch angles can allow us to control x and y 87 | %positions. 88 | 89 | phi_des= Kp_y*error_y + Kd_y*errordot_y + Ki_y*int_y; 90 | if phi_des>pi/4 91 | phi_des=pi/4; 92 | end 93 | if phi_des<-pi/4 94 | phi_des=-pi/4; 95 | end 96 | 97 | theta_des= Kp_x*error_x + Kd_x*errordot_x + Ki_x*int_x; 98 | if theta_des>pi/4 99 | theta_des=pi/4; 100 | end 101 | if theta_des<-pi/4 102 | theta_des=-pi/4; 103 | end 104 | 105 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 106 | 107 | % phi_des= 0.0; 108 | % theta_des=0.0; 109 | psi_des=0.0; 110 | 111 | phi=state.orientation(1); 112 | theta=state.orientation(2); 113 | psi=state.orientation(3); 114 | 115 | phidot=thetadot(1); 116 | thetadot2=thetadot(2); 117 | psidot=thetadot(3); 118 | 119 | error_phi=phi_des-phi; 120 | error_theta=theta_des-theta; 121 | error_psi=psi_des-psi; 122 | 123 | e = zeros(3,1); 124 | e(1)=Kp_phi*-error_phi+Kd_phi*phidot; 125 | e(2)=Kp_theta*-error_theta+Kd_theta*thetadot2; 126 | e(3)=Kp_psi*-error_psi+Kd_psi*psidot+Ki_psi*int_psi; 127 | 128 | e_phi=e(1); 129 | e_theta=e(2); 130 | e_psi=e(3); 131 | 132 | Ixx=state.I(1,1); 133 | Iyy=state.I(2,2); 134 | Izz=state.I(3,3); 135 | 136 | b=state.b; 137 | k=state.k; 138 | L=state.L; 139 | m=state.m; 140 | g=state.g; 141 | dt=state.dt; 142 | 143 | allerrors=[error_x;error_y;error_z;error_phi;error_theta;error_psi]; 144 | 145 | 146 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 147 | %Now compute total thrust (to simplify input equations) 148 | 149 | %To have attitude control (z-axis motion) we are going to need a 150 | %controller to govern the thrust. If thrust = m*g/(k*cos(phi)*cos(theta)); 151 | %then the quadcopter will always be kept at a level altitude but we 152 | %want to be able to have the altitude as an input. 153 | 154 | thrust=Kp_z*error_z + Kd_z*errordot_z + Ki_z*int_z; 155 | if thrust<0 156 | thrust=0; 157 | end 158 | 159 | % thrust = m*g/(k*cos(phi)*cos(theta)); 160 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 161 | 162 | %Now compute the propeller angular velocities for a given orientation. 163 | inputs(1)= thrust/4 - (2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 164 | inputs(2)= thrust/4 - (2*b*e_theta*Iyy - k*L*e_psi*Izz)/(4*b*L*k); 165 | inputs(3)= thrust/4 - (-2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 166 | inputs(4)= thrust/4 + (2*b*e_theta*Iyy + k*L*e_psi*Izz)/(4*b*L*k); 167 | 168 | %Now advance the controller state (this essentially means updating the 169 | %orientation vector: 170 | 171 | state.orientation = state.orientation + state.dt*thetadot; 172 | state.integral = state.integral + dt*allerrors; 173 | state.phi_des=phi_des; 174 | state.theta_des=theta_des; 175 | state.errors=errors; 176 | state.thrust=thrust; 177 | 178 | end 179 | 180 | -------------------------------------------------------------------------------- /v6 (path plot and PID)/acceleration.m: -------------------------------------------------------------------------------- 1 | function a = acceleration(inputs,angles,velocities,m,g,k,kd) 2 | 3 | %The acceleration of the quadcopter is a function of gravity, the thrust 4 | %and drag forces. From our model we have (a is the acceleration vector, v 5 | %is the velocity vector): 6 | 7 | %m*a=[0 0 -mg].' + T_I +F_D 8 | % =[0 0 -mg].' + R*T_B + F_D 9 | % therefore a = [0 0 -g].' + 1/m*(R*[0 0 k*sum(angular accelerations^2)] + 10 | % kd*v) 11 | 12 | %The angular velocities are the inputs, thrust (T is in F_B) is a function 13 | %of angular velocity and k. 14 | 15 | gravity = [0 0 -g].'; 16 | R = rotation(angles); 17 | thrust_FB=[0;0;k*sum(inputs)]; 18 | thrust_FI=R*thrust_FB; 19 | 20 | Fd = -kd*velocities; 21 | 22 | a = gravity +1/m*thrust_FI +1/m*Fd; 23 | 24 | 25 | end 26 | 27 | -------------------------------------------------------------------------------- /v6 (path plot and PID)/angular_acceleration.m: -------------------------------------------------------------------------------- 1 | function omegadot = angular_acceleration(inputs,omega,I,L,b,k) 2 | 3 | i1=inputs(1); 4 | i2=inputs(2); 5 | i3=inputs(3); 6 | i4=inputs(4); 7 | 8 | p=omega(1); 9 | q=omega(2); 10 | r=omega(3); 11 | 12 | Ixx=I(1,1); 13 | Iyy=I(2,2); 14 | Izz=I(3,3); 15 | 16 | actuator_action = [ 17 | L*k*(i1-i3) 18 | L*k*(i2-i4) 19 | b*(i1-i2+i3-i4) 20 | ]; 21 | 22 | dragtorque=[ 23 | 0 24 | 0 25 | b*(i1-i2+i3-i4) 26 | ]; 27 | 28 | gyrotorque=[ 29 | (Iyy-Izz)*q*r 30 | (Izz-Ixx)*p*r 31 | (Ixx-Iyy)*p*q 32 | ]; 33 | 34 | tau = actuator_action+dragtorque+gyrotorque; 35 | 36 | %Now use the Euler equation for rigid body motion to obtain angular 37 | %acceleration: 38 | 39 | omegadot=inv(I)*(actuator_action-cross(omega,I*omega)); 40 | 41 | 42 | end 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /v6 (path plot and PID)/omega2thetadot.m: -------------------------------------------------------------------------------- 1 | function thetadot = omega2thetadot(omega,angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | A=zeros(3); 7 | A(1,:)=[ 8 | 1 9 | 0 10 | -sin(theta) 11 | ]; 12 | 13 | A(2,:) =[ 14 | 0 15 | cos(phi) 16 | cos(theta)*sin(phi) 17 | ]; 18 | 19 | A(3,:) =[ 20 | 0 21 | -sin(phi) 22 | cos(phi)*cos(theta) 23 | ]; 24 | 25 | W=inv(A); 26 | thetadot=W*omega; 27 | 28 | %This function converts from angular velocity to the time derivatives 29 | %of roll, pitch and yaw. 30 | end 31 | -------------------------------------------------------------------------------- /v6 (path plot and PID)/rotation.m: -------------------------------------------------------------------------------- 1 | function R = rotation(angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | R=zeros(3); 7 | R(1,:) = [cos(theta)*cos(psi); cos(psi)*sin(phi)*sin(theta)-cos(phi)*sin(psi);sin(phi)*sin(psi)+cos(phi)*cos(psi)*sin(theta)]; 8 | 9 | R(2,:) = [cos(theta)*sin(psi);cos(phi)*cos(psi) + sin(phi)*sin(theta)*sin(psi);cos(phi)*sin(theta)*sin(psi)-cos(psi)*sin(phi)]; 10 | 11 | R(3,:) = [-sin(theta);cos(theta)*sin(phi);cos(phi)*cos(theta)]; 12 | 13 | %This rotation matrix maps between the body-fixed frame and the inertial 14 | %frame. 15 | 16 | end 17 | 18 | -------------------------------------------------------------------------------- /v6 (path plot and PID)/simulate.m: -------------------------------------------------------------------------------- 1 | function result = simulate(controller, x_desired,y_desired,z_desired) 2 | %SET DEFAULT DESIRED COORDINATES 3 | if nargin<2 4 | x_desired=100; 5 | y_desired=100; 6 | z_desired=100; 7 | end 8 | 9 | %SIMULATION TIMES 10 | 11 | start_time = 0; 12 | end_time = 180; 13 | dt = 0.01; 14 | times = start_time:dt:end_time; 15 | N = numel(times); 16 | 17 | %DEFINE PHYSICAL CONSTANTS (FOR A HYPOTHETICAL QUADCOPTER) 18 | 19 | g = 9.81; 20 | m = 0.5; 21 | L = 0.25; 22 | k = 3e-6; 23 | b = 1e-7; 24 | I = diag([5e-3, 5e-3, 10e-3]); 25 | kd = 0.25; 26 | 27 | %INITIAL SIMULATION STATE 28 | 29 | %the x vector triple denotes quadcopter position 30 | x=[0 0 10].'; 31 | 32 | %now create empty triples for xdot (time derivate of x) and theta 33 | %(orientation of the quadcopter using the Euler angles). 34 | 35 | xdot=zeros(3,1); 36 | theta=zeros(3,1); 37 | 38 | %thetadot denotes the angular velocities which can be set to an initial 39 | %disturbance. 40 | 41 | if nargin==0 42 | thetadot=zeros(3,1); 43 | else 44 | thetadot=deg2rad(randi([-90,90],3,1)); 45 | %thetadot=zeros(3,1); 46 | %thetadot=[0.6;0.6;0.6]; 47 | end 48 | 49 | %CREATE ARRAYS AND STRUCTURED ARRAY FOR THE OUTPUTS 50 | 51 | xout=zeros(3,N); 52 | xdotout=zeros(3,N); 53 | thetaout=zeros(3,N); 54 | thetadotout=zeros(3,N); 55 | inputout=zeros(4,N); 56 | 57 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 58 | %These arrays are to store data that is not critical for controller 59 | %simulation but is needed for secondary graphs (used in controller 60 | %design). 61 | 62 | phi_desout=zeros(1,N); 63 | theta_desout=zeros(1,N); 64 | errors=zeros(3,N); 65 | thrust=zeros(1,N); 66 | integralerrors=zeros(6,N); 67 | settlingtime=0; 68 | 69 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 70 | 71 | %PARAMETERS FOR THE CONTROLLER 72 | controller_params = struct('dt',dt,'I',I,'k',k,'L',L,'b',b,'m',m,'g',g,... 73 | 'xdes',x_desired,'ydes',y_desired,'zdes',z_desired); 74 | 75 | %STEP THROUGH SIMULATION, UPDATING STATE OF QUADCOPTER 76 | 77 | count=0; 78 | 79 | for t = times 80 | count=count+1; 81 | 82 | controller_params.xdot=xdot; 83 | controller_params.x=x; 84 | 85 | %Here we need to take an input from the controller, or if no 86 | %controller is specified then we can use some default input. The 87 | %inputs are values of gamma (angular velocity^2) for each propeller 88 | 89 | if nargin==0 90 | in=zeros(4,1); 91 | in(1)=700; 92 | in(2)=750; 93 | in(3)=700; 94 | in(4)=750; 95 | in=in.^2; 96 | else 97 | [in,controller_params] = controller(controller_params,thetadot); 98 | end 99 | 100 | %COMPUTE FORCES, TORQUES AND ACCELERATIONS 101 | 102 | %Define angular velocity, omega 103 | omega = thetadot2omega(thetadot,theta); 104 | %Define acceleration, a 105 | a=acceleration(in, theta, xdot, m, g, k, kd); 106 | %Define anglular acceleration, omegadot 107 | omegadot=angular_acceleration(in,omega,I,L,b,k); 108 | 109 | %STORE SETTLING TIME 110 | if settlingtime==0 111 | if x(1)>0.98*x_desired && x(1)<1.02*x_desired... 112 | && x(2)>0.98*y_desired && x(2)<1.02*y_desired... 113 | && x(3)>0.98*z_desired && x(3)<1.02*z_desired 114 | 115 | settlingtime=times(count); 116 | end 117 | end 118 | 119 | %ADVANCE SYSTEM STATE 120 | 121 | omega = omega + omegadot*dt; 122 | thetadot = omega2thetadot(omega,theta); 123 | theta = theta + thetadot*dt; 124 | xdot = xdot + a*dt; 125 | x = x + xdot*dt; 126 | 127 | %STORE SIMULATION STATE FOR OUTPUT 128 | xout(:,count)=x; 129 | xdotout(:,count)=xdot; 130 | thetaout(:,count)=theta; 131 | thetadotout(:,count)=thetadot; 132 | inputout(:,count)=in; 133 | phi_desout(:,count)=controller_params.phi_des; 134 | theta_desout(:,count)=controller_params.theta_des; 135 | errors(:,count)=controller_params.errors; 136 | thrust(:,count)=controller_params.thrust; 137 | %integralerrors(:,count)=controller_params.integral; 138 | 139 | end 140 | 141 | 142 | %PUT OUTPUT INFORMATION INTO A STRUCTURED ARRAY: 143 | result = struct('x',xout,'theta',thetaout,'vel',xdotout,'angvel'... 144 | ,thetadotout,'t',times, 'dt',dt,'input',inputout,'phides'... 145 | ,phi_desout,'thetades',theta_desout,'errors',errors,'thrust',thrust... 146 | ,'integralerrors',integralerrors,'settlingtime',settlingtime); 147 | 148 | %PLOT RESULTS 149 | grapher(result) 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | end -------------------------------------------------------------------------------- /v6 (path plot and PID)/thetadot2omega.m: -------------------------------------------------------------------------------- 1 | function omega = thetadot2omega(thetadot,angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | A=zeros(3); 7 | A(1,:)=[ 8 | 1 9 | 0 10 | -sin(theta) 11 | ]; 12 | A(2,:) =[ 13 | 0 14 | cos(phi) 15 | cos(theta)*sin(phi) 16 | ]; 17 | A(3,:) =[ 18 | 0 19 | -sin(phi) 20 | cos(phi)*cos(theta) 21 | ]; 22 | 23 | omega=A*thetadot; 24 | 25 | 26 | %This function converts time derivatives of roll, pitch and yaw into 27 | %angular velocity. A is the necessary rotation matrix for this operation. 28 | %We can define the reverse function omega2thetadot using the inverse of 29 | %this matrix if desired. 30 | 31 | end 32 | 33 | -------------------------------------------------------------------------------- /v6 (path plot and PID)/visualiser notes.psd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/samuelgilonis/Quadcopter-PID-controller/5aae0d5a14cb32cb6a54b4c1b8df19d9bffbc83f/v6 (path plot and PID)/visualiser notes.psd -------------------------------------------------------------------------------- /v7 (simulation time trimmed)/PD.m: -------------------------------------------------------------------------------- 1 | function c = PD() 2 | c = @(state, thetadot) pd_controller(state, thetadot); 3 | end 4 | 5 | function [inputs,state] = pd_controller(state, thetadot) 6 | %the "state" argument is the structured array from which the controller 7 | %must access the necessary information. This is the controller_params 8 | %struct in the simulator code. 9 | 10 | %Gain Parameters 11 | Kp_phi = 5; 12 | Kd_phi = 4; 13 | 14 | Kp_theta = 5; 15 | Kd_theta = 4; 16 | 17 | Kp_psi = 5; 18 | Kd_psi = 4; 19 | 20 | Kp_x = 0.3*pi/180; 21 | Kd_x = -0.5*pi/180; 22 | 23 | Kp_y = Kp_x; 24 | Kd_y = Kd_x; 25 | 26 | Kp_z = 1.2e6; 27 | Kd_z = -2.4e6; 28 | 29 | %Create the theta (orientation) vector in the structured array if it does 30 | %not yet exist. This will be updated each time this code is run in the 31 | %simulator for-loop. 32 | if ~isfield(state,'orientation') 33 | state.orientation = zeros (3,1); 34 | end 35 | 36 | if ~isfield(state,'errors') 37 | state.errors = zeros (6,1); 38 | end 39 | 40 | 41 | %Now create the inputs that will be sent to the quadcopter. 42 | %There are four inputs (an angular velocity^2 for each prop) 43 | inputs = zeros(4,1); 44 | 45 | %Now extract from the structured array the necessary information to compute 46 | %the inputs. 47 | 48 | x_des=state.xdes; 49 | y_des=state.ydes; 50 | z_des=state.zdes; 51 | 52 | x=state.x(1); 53 | y=state.x(2); 54 | z=state.x(3); 55 | 56 | error_x=x_des-x; 57 | error_y=y-y_des; 58 | error_z=z_des-z; 59 | errors=[error_x;error_y;error_z]; 60 | 61 | vels=state.xdot; 62 | xdot=vels(1); 63 | ydot=vels(2); 64 | zdot=vels(3); 65 | 66 | errordot_x=xdot; 67 | errordot_y=-ydot; 68 | errordot_z=zdot; 69 | 70 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 71 | %The roll and pitch angles give y and x motion respectively. Therefore 72 | %PD controllers governing the desired roll and pitch angles can allow us to control x and y 73 | %positions. 74 | 75 | phi_des= Kp_y*error_y + Kd_y*errordot_y; 76 | if phi_des>pi/4 77 | phi_des=pi/4; 78 | end 79 | if phi_des<-pi/4 80 | phi_des=-pi/4; 81 | end 82 | 83 | theta_des= Kp_x*error_x + Kd_x*errordot_x; 84 | if theta_des>pi/4 85 | theta_des=pi/4; 86 | end 87 | if theta_des<-pi/4 88 | theta_des=-pi/4; 89 | end 90 | 91 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 92 | 93 | % phi_des= 0.0; 94 | % theta_des=0.0; 95 | psi_des=0.0; 96 | 97 | phi=state.orientation(1); 98 | theta=state.orientation(2); 99 | psi=state.orientation(3); 100 | 101 | phidot=thetadot(1); 102 | thetadot2=thetadot(2); 103 | psidot=thetadot(3); 104 | 105 | error_phi=phi_des-phi; 106 | error_theta=theta_des-theta; 107 | error_psi=psi_des-psi; 108 | 109 | e = zeros(3,1); 110 | e(1)=Kp_phi*-error_phi+Kd_phi*phidot; 111 | e(2)=Kp_theta*-error_theta+Kd_theta*thetadot2; 112 | e(3)=Kp_psi*-error_psi+Kd_psi*psidot; 113 | 114 | e_phi=e(1); 115 | e_theta=e(2); 116 | e_psi=e(3); 117 | 118 | Ixx=state.I(1,1); 119 | Iyy=state.I(2,2); 120 | Izz=state.I(3,3); 121 | 122 | b=state.b; 123 | k=state.k; 124 | L=state.L; 125 | m=state.m; 126 | g=state.g; 127 | 128 | 129 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 130 | %Now compute total thrust (to simplify input equations) 131 | 132 | %To have attitude control (z-axis motion) we are going to need a 133 | %controller to govern the thrust. If thrust = m*g/(k*cos(phi)*cos(theta)); 134 | %then the quadcopter will always be kept at a level altitude but we 135 | %want to be able to have the altitude as an input. 136 | 137 | thrust=Kp_z*error_z+Kd_z*errordot_z; 138 | if thrust<0 139 | thrust=0; 140 | end 141 | 142 | % thrust = m*g/(k*cos(phi)*cos(theta)); 143 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 144 | 145 | %Now compute the propeller angular velocities for a given orientation. 146 | inputs(1)= thrust/4 - (2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 147 | inputs(2)= thrust/4 - (2*b*e_theta*Iyy - k*L*e_psi*Izz)/(4*b*L*k); 148 | inputs(3)= thrust/4 - (-2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 149 | inputs(4)= thrust/4 + (2*b*e_theta*Iyy + k*L*e_psi*Izz)/(4*b*L*k); 150 | 151 | %Now advance the controller state (this essentially means updating the 152 | %orientation vector: 153 | 154 | state.orientation = state.orientation + state.dt*thetadot; 155 | state.phi_des=phi_des; 156 | state.theta_des=theta_des; 157 | state.errors=errors; 158 | state.thrust=thrust; 159 | end 160 | 161 | 162 | 163 | 164 | 165 | 166 | -------------------------------------------------------------------------------- /v7 (simulation time trimmed)/PID.m: -------------------------------------------------------------------------------- 1 | function c = PID() 2 | c = @(state, thetadot) pid_controller(state, thetadot); 3 | end 4 | 5 | function [inputs,state] = pid_controller(state, thetadot) 6 | %the "state" argument is the structured array from which the controller 7 | %must access the necessary information. This is the controller_params 8 | %struct in the simulator code. 9 | 10 | %Gain Parameters 11 | Kp_phi = 5; 12 | Kd_phi = 4; 13 | 14 | Kp_theta = Kp_phi; 15 | Kd_theta = Kd_phi; 16 | 17 | Kp_psi = 5; 18 | Kd_psi = 4; 19 | Ki_psi = 0.06; 20 | 21 | Kp_x = 0.3*pi/180; 22 | Kd_x = 0.5*pi/180; 23 | Ki_x = 1.5e-4*pi/180; 24 | 25 | Kp_y = Kp_x; 26 | Kd_y = Kd_x; 27 | Ki_y = Ki_x; 28 | 29 | Kp_z = 1.2e6; 30 | Kd_z = -2.4e6; 31 | Ki_z = 2.0e5; 32 | 33 | %Create the theta (orientation) vector in the structured array if it does 34 | %not yet exist. This will be updated each time this code is run in the 35 | %simulator for-loop. 36 | if ~isfield(state,'orientation') 37 | state.orientation = zeros (3,1); 38 | end 39 | 40 | if ~isfield(state,'errors') 41 | state.errors = zeros (3,1); 42 | end 43 | 44 | if ~isfield(state,'integral') 45 | state.integral = zeros (6,1); 46 | end 47 | 48 | %Now create the inputs that will be sent to the quadcopter. 49 | %There are four inputs (an angular velocity^2 for each prop) 50 | inputs = zeros(4,1); 51 | 52 | %Now extract from the structured array the necessary information to compute 53 | %the inputs. 54 | 55 | x_des=state.xdes; 56 | y_des=state.ydes; 57 | z_des=state.zdes; 58 | 59 | x=state.x(1); 60 | y=state.x(2); 61 | z=state.x(3); 62 | 63 | error_x=x_des-x; 64 | error_y=y-y_des; 65 | error_z=z_des-z; 66 | errors=[error_x;error_y;error_z]; 67 | 68 | vels=state.xdot; 69 | xdot=vels(1); 70 | ydot=vels(2); 71 | zdot=vels(3); 72 | 73 | errordot_x=xdot; 74 | errordot_y=-ydot; 75 | errordot_z=zdot; 76 | 77 | int_x=state.integral(1); 78 | int_y=-state.integral(2); 79 | int_z=state.integral(3); 80 | int_phi=state.integral(4); 81 | int_theta=state.integral(5); 82 | int_psi=state.integral(6); 83 | 84 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 85 | %The roll and pitch angles give y and x motion respectively. Therefore 86 | %PD controllers governing the desired roll and pitch angles can allow us to control x and y 87 | %positions. 88 | 89 | phi_des= Kp_y*error_y + Kd_y*errordot_y + Ki_y*int_y; 90 | if phi_des>pi/4 91 | phi_des=pi/4; 92 | end 93 | if phi_des<-pi/4 94 | phi_des=-pi/4; 95 | end 96 | 97 | theta_des= Kp_x*error_x + Kd_x*errordot_x + Ki_x*int_x; 98 | if theta_des>pi/4 99 | theta_des=pi/4; 100 | end 101 | if theta_des<-pi/4 102 | theta_des=-pi/4; 103 | end 104 | 105 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 106 | 107 | % phi_des= 0.0; 108 | % theta_des=0.0; 109 | psi_des=0.0; 110 | 111 | phi=state.orientation(1); 112 | theta=state.orientation(2); 113 | psi=state.orientation(3); 114 | 115 | phidot=thetadot(1); 116 | thetadot2=thetadot(2); 117 | psidot=thetadot(3); 118 | 119 | error_phi=phi_des-phi; 120 | error_theta=theta_des-theta; 121 | error_psi=psi_des-psi; 122 | 123 | e = zeros(3,1); 124 | e(1)=Kp_phi*-error_phi+Kd_phi*phidot; 125 | e(2)=Kp_theta*-error_theta+Kd_theta*thetadot2; 126 | e(3)=Kp_psi*-error_psi+Kd_psi*psidot+Ki_psi*int_psi; 127 | 128 | e_phi=e(1); 129 | e_theta=e(2); 130 | e_psi=e(3); 131 | 132 | Ixx=state.I(1,1); 133 | Iyy=state.I(2,2); 134 | Izz=state.I(3,3); 135 | 136 | b=state.b; 137 | k=state.k; 138 | L=state.L; 139 | m=state.m; 140 | g=state.g; 141 | dt=state.dt; 142 | 143 | allerrors=[error_x;error_y;error_z;error_phi;error_theta;error_psi]; 144 | 145 | 146 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 147 | %Now compute total thrust (to simplify input equations) 148 | 149 | %To have attitude control (z-axis motion) we are going to need a 150 | %controller to govern the thrust. If thrust = m*g/(k*cos(phi)*cos(theta)); 151 | %then the quadcopter will always be kept at a level altitude but we 152 | %want to be able to have the altitude as an input. 153 | 154 | thrust=Kp_z*error_z + Kd_z*errordot_z + Ki_z*int_z; 155 | if thrust<0 156 | thrust=0; 157 | end 158 | 159 | % thrust = m*g/(k*cos(phi)*cos(theta)); 160 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 161 | 162 | %Now compute the propeller angular velocities for a given orientation. 163 | inputs(1)= thrust/4 - (2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 164 | inputs(2)= thrust/4 - (2*b*e_theta*Iyy - k*L*e_psi*Izz)/(4*b*L*k); 165 | inputs(3)= thrust/4 - (-2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 166 | inputs(4)= thrust/4 + (2*b*e_theta*Iyy + k*L*e_psi*Izz)/(4*b*L*k); 167 | 168 | %Now advance the controller state (this essentially means updating the 169 | %orientation vector: 170 | 171 | state.orientation = state.orientation + state.dt*thetadot; 172 | state.integral = state.integral + dt*allerrors; 173 | state.phi_des=phi_des; 174 | state.theta_des=theta_des; 175 | state.errors=errors; 176 | state.thrust=thrust; 177 | 178 | end 179 | 180 | -------------------------------------------------------------------------------- /v7 (simulation time trimmed)/acceleration.m: -------------------------------------------------------------------------------- 1 | function a = acceleration(inputs,angles,velocities,m,g,k,kd) 2 | 3 | %The acceleration of the quadcopter is a function of gravity, the thrust 4 | %and drag forces. From our model we have (a is the acceleration vector, v 5 | %is the velocity vector): 6 | 7 | %m*a=[0 0 -mg].' + T_I +F_D 8 | % =[0 0 -mg].' + R*T_B + F_D 9 | % therefore a = [0 0 -g].' + 1/m*(R*[0 0 k*sum(angular accelerations^2)] + 10 | % kd*v) 11 | 12 | %The angular velocities are the inputs, thrust (T is in F_B) is a function 13 | %of angular velocity and k. 14 | 15 | gravity = [0 0 -g].'; 16 | R = rotation(angles); 17 | thrust_FB=[0;0;k*sum(inputs)]; 18 | thrust_FI=R*thrust_FB; 19 | 20 | Fd = -kd*velocities; 21 | 22 | a = gravity +1/m*thrust_FI +1/m*Fd; 23 | 24 | 25 | end 26 | 27 | -------------------------------------------------------------------------------- /v7 (simulation time trimmed)/angular_acceleration.m: -------------------------------------------------------------------------------- 1 | function omegadot = angular_acceleration(inputs,omega,I,L,b,k) 2 | 3 | i1=inputs(1); 4 | i2=inputs(2); 5 | i3=inputs(3); 6 | i4=inputs(4); 7 | 8 | p=omega(1); 9 | q=omega(2); 10 | r=omega(3); 11 | 12 | Ixx=I(1,1); 13 | Iyy=I(2,2); 14 | Izz=I(3,3); 15 | 16 | actuator_action = [ 17 | L*k*(i1-i3) 18 | L*k*(i2-i4) 19 | b*(i1-i2+i3-i4) 20 | ]; 21 | 22 | dragtorque=[ 23 | 0 24 | 0 25 | b*(i1-i2+i3-i4) 26 | ]; 27 | 28 | gyrotorque=[ 29 | (Iyy-Izz)*q*r 30 | (Izz-Ixx)*p*r 31 | (Ixx-Iyy)*p*q 32 | ]; 33 | 34 | tau = actuator_action+dragtorque+gyrotorque; 35 | 36 | %Now use the Euler equation for rigid body motion to obtain angular 37 | %acceleration: 38 | 39 | omegadot=inv(I)*(actuator_action-cross(omega,I*omega)); 40 | 41 | 42 | end 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /v7 (simulation time trimmed)/omega2thetadot.m: -------------------------------------------------------------------------------- 1 | function thetadot = omega2thetadot(omega,angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | A=zeros(3); 7 | A(1,:)=[ 8 | 1 9 | 0 10 | -sin(theta) 11 | ]; 12 | 13 | A(2,:) =[ 14 | 0 15 | cos(phi) 16 | cos(theta)*sin(phi) 17 | ]; 18 | 19 | A(3,:) =[ 20 | 0 21 | -sin(phi) 22 | cos(phi)*cos(theta) 23 | ]; 24 | 25 | W=inv(A); 26 | thetadot=W*omega; 27 | 28 | %This function converts from angular velocity to the time derivatives 29 | %of roll, pitch and yaw. 30 | end 31 | -------------------------------------------------------------------------------- /v7 (simulation time trimmed)/rotation.m: -------------------------------------------------------------------------------- 1 | function R = rotation(angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | R=zeros(3); 7 | R(1,:) = [cos(theta)*cos(psi); cos(psi)*sin(phi)*sin(theta)-cos(phi)*sin(psi);sin(phi)*sin(psi)+cos(phi)*cos(psi)*sin(theta)]; 8 | 9 | R(2,:) = [cos(theta)*sin(psi);cos(phi)*cos(psi) + sin(phi)*sin(theta)*sin(psi);cos(phi)*sin(theta)*sin(psi)-cos(psi)*sin(phi)]; 10 | 11 | R(3,:) = [-sin(theta);cos(theta)*sin(phi);cos(phi)*cos(theta)]; 12 | 13 | %This rotation matrix maps between the body-fixed frame and the inertial 14 | %frame. 15 | 16 | end 17 | 18 | -------------------------------------------------------------------------------- /v7 (simulation time trimmed)/simulate.m: -------------------------------------------------------------------------------- 1 | function result = simulate(controller, x_desired,y_desired,z_desired) 2 | %SET DEFAULT DESIRED COORDINATES 3 | if nargin<2 4 | x_desired=100; 5 | y_desired=100; 6 | z_desired=100; 7 | end 8 | 9 | %SIMULATION TIMES 10 | 11 | start_time = 0; 12 | end_time = 60; 13 | dt = 0.01; 14 | times = start_time:dt:end_time; 15 | N = numel(times); 16 | 17 | %DEFINE PHYSICAL CONSTANTS (FOR A HYPOTHETICAL QUADCOPTER) 18 | 19 | g = 9.81; 20 | m = 0.5; 21 | L = 0.25; 22 | k = 3e-6; 23 | b = 1e-7; 24 | I = diag([5e-3, 5e-3, 10e-3]); 25 | kd = 0.25; 26 | 27 | %INITIAL SIMULATION STATE 28 | 29 | %the x vector triple denotes quadcopter position 30 | x=[0 0 10].'; 31 | 32 | %now create empty triples for xdot (time derivate of x) and theta 33 | %(orientation of the quadcopter using the Euler angles). 34 | 35 | xdot=zeros(3,1); 36 | theta=zeros(3,1); 37 | 38 | %thetadot denotes the angular velocities which can be set to an initial 39 | %disturbance. 40 | 41 | if nargin==0 42 | thetadot=zeros(3,1); 43 | else 44 | %thetadot=deg2rad(randi([-90,90],3,1)); 45 | thetadot=zeros(3,1); 46 | %thetadot=[0.6;0.6;0.6]; 47 | end 48 | 49 | %CREATE ARRAYS AND STRUCTURED ARRAY FOR THE OUTPUTS 50 | 51 | xout=zeros(3,N); 52 | xdotout=zeros(3,N); 53 | thetaout=zeros(3,N); 54 | thetadotout=zeros(3,N); 55 | inputout=zeros(4,N); 56 | 57 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 58 | %These arrays are to store data that is not critical for controller 59 | %simulation but is needed for secondary graphs (used in controller 60 | %design). 61 | 62 | phi_desout=zeros(1,N); 63 | theta_desout=zeros(1,N); 64 | errors=zeros(3,N); 65 | thrust=zeros(1,N); 66 | integralerrors=zeros(6,N); 67 | risetime=0; 68 | 69 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 70 | 71 | %PARAMETERS FOR THE CONTROLLER 72 | controller_params = struct('dt',dt,'I',I,'k',k,'L',L,'b',b,'m',m,'g',g,... 73 | 'xdes',x_desired,'ydes',y_desired,'zdes',z_desired); 74 | 75 | %STEP THROUGH SIMULATION, UPDATING STATE OF QUADCOPTER 76 | 77 | count=0; 78 | graphendtime=0; 79 | graphendcount=0; 80 | 81 | for t = times 82 | count=count+1; 83 | 84 | controller_params.xdot=xdot; 85 | controller_params.x=x; 86 | 87 | %Here we need to take an input from the controller, or if no 88 | %controller is specified then we can use some default input. The 89 | %inputs are values of gamma (angular velocity^2) for each propeller 90 | 91 | if nargin==0 92 | in=zeros(4,1); 93 | in(1)=700; 94 | in(2)=750; 95 | in(3)=700; 96 | in(4)=750; 97 | in=in.^2; 98 | else 99 | [in,controller_params] = controller(controller_params,thetadot); 100 | end 101 | 102 | %COMPUTE FORCES, TORQUES AND ACCELERATIONS 103 | 104 | %Define angular velocity, omega 105 | omega = thetadot2omega(thetadot,theta); 106 | %Define acceleration, a 107 | a=acceleration(in, theta, xdot, m, g, k, kd); 108 | %Define anglular acceleration, omegadot 109 | omegadot=angular_acceleration(in,omega,I,L,b,k); 110 | 111 | %STORE RISE TIME 112 | if risetime==0 113 | if x(1)>0.98*x_desired && x(1)<1.02*x_desired... 114 | && x(2)>0.98*y_desired && x(2)<1.02*y_desired... 115 | && x(3)>0.98*z_desired && x(3)<1.02*z_desired 116 | risetime=times(count); 117 | end 118 | end 119 | 120 | %STORE GRAPH END TIME 121 | if risetime ~= 0 122 | graphendtime=risetime+10; 123 | if graphendcount==0 124 | if times(count)>graphendtime 125 | graphendcount=count; 126 | end 127 | end 128 | end 129 | 130 | %ADVANCE SYSTEM STATE 131 | 132 | omega = omega + omegadot*dt; 133 | thetadot = omega2thetadot(omega,theta); 134 | theta = theta + thetadot*dt; 135 | xdot = xdot + a*dt; 136 | x = x + xdot*dt; 137 | 138 | %STORE SIMULATION STATE FOR OUTPUT 139 | xout(:,count)=x; 140 | xdotout(:,count)=xdot; 141 | thetaout(:,count)=theta; 142 | thetadotout(:,count)=thetadot; 143 | inputout(:,count)=in; 144 | phi_desout(:,count)=controller_params.phi_des; 145 | theta_desout(:,count)=controller_params.theta_des; 146 | errors(:,count)=controller_params.errors; 147 | thrust(:,count)=controller_params.thrust; 148 | %integralerrors(:,count)=controller_params.integral; 149 | 150 | end 151 | 152 | 153 | %PUT OUTPUT INFORMATION INTO A STRUCTURED ARRAY: 154 | result = struct('x',xout,'theta',thetaout,'vel',xdotout,'angvel'... 155 | ,thetadotout,'t',times, 'dt',dt,'input',inputout,'phides'... 156 | ,phi_desout,'thetades',theta_desout,'errors',errors,'thrust',thrust... 157 | ,'integralerrors',integralerrors,'risetime',risetime... 158 | ,'gecount',graphendcount'); 159 | 160 | %PLOT RESULTS 161 | grapher(result) 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | end -------------------------------------------------------------------------------- /v7 (simulation time trimmed)/thetadot2omega.m: -------------------------------------------------------------------------------- 1 | function omega = thetadot2omega(thetadot,angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | A=zeros(3); 7 | A(1,:)=[ 8 | 1 9 | 0 10 | -sin(theta) 11 | ]; 12 | A(2,:) =[ 13 | 0 14 | cos(phi) 15 | cos(theta)*sin(phi) 16 | ]; 17 | A(3,:) =[ 18 | 0 19 | -sin(phi) 20 | cos(phi)*cos(theta) 21 | ]; 22 | 23 | omega=A*thetadot; 24 | 25 | 26 | %This function converts time derivatives of roll, pitch and yaw into 27 | %angular velocity. A is the necessary rotation matrix for this operation. 28 | %We can define the reverse function omega2thetadot using the inverse of 29 | %this matrix if desired. 30 | 31 | end 32 | 33 | -------------------------------------------------------------------------------- /v8 (results)/PD.m: -------------------------------------------------------------------------------- 1 | function c = PD() 2 | c = @(state, thetadot) pd_controller(state, thetadot); 3 | end 4 | 5 | function [inputs,state] = pd_controller(state, thetadot) 6 | %the "state" argument is the structured array from which the controller 7 | %must access the necessary information. This is the controller_params 8 | %struct in the simulator code. 9 | 10 | %Gain Parameters 11 | Kp_phi = 5; 12 | Kd_phi = 4; 13 | 14 | Kp_theta = Kp_phi; 15 | Kd_theta = Kd_phi; 16 | 17 | Kp_psi = 5; 18 | Kd_psi = 4; 19 | 20 | Kp_x = 0.3*pi/180; 21 | Kd_x = 0.5*pi/180; 22 | 23 | Kp_y = Kp_x; 24 | Kd_y = Kd_x; 25 | 26 | Kp_z = 1.2e6; 27 | Kd_z = -2.4e6; 28 | 29 | %Create the theta (orientation) vector in the structured array if it does 30 | %not yet exist. This will be updated each time this code is run in the 31 | %simulator for-loop. 32 | if ~isfield(state,'orientation') 33 | state.orientation = zeros (3,1); 34 | end 35 | 36 | if ~isfield(state,'errors') 37 | state.errors = zeros (6,1); 38 | end 39 | 40 | 41 | %Now create the inputs that will be sent to the quadcopter. 42 | %There are four inputs (an angular velocity^2 for each prop) 43 | inputs = zeros(4,1); 44 | 45 | %Now extract from the structured array the necessary information to compute 46 | %the inputs. 47 | 48 | x_des=state.xdes; 49 | y_des=state.ydes; 50 | z_des=state.zdes; 51 | 52 | x=state.x(1); 53 | y=state.x(2); 54 | z=state.x(3); 55 | 56 | error_x=x_des-x; 57 | error_y=y-y_des; 58 | error_z=z_des-z; 59 | errors=[error_x;error_y;error_z]; 60 | 61 | vels=state.xdot; 62 | xdot=vels(1); 63 | ydot=vels(2); 64 | zdot=vels(3); 65 | 66 | errordot_x=xdot; 67 | errordot_y=-ydot; 68 | errordot_z=zdot; 69 | 70 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 71 | %The roll and pitch angles give y and x motion respectively. Therefore 72 | %PD controllers governing the desired roll and pitch angles can allow us to control x and y 73 | %positions. 74 | 75 | phi_des= Kp_y*error_y + Kd_y*errordot_y; 76 | if phi_des>pi/4 77 | phi_des=pi/4; 78 | end 79 | if phi_des<-pi/4 80 | phi_des=-pi/4; 81 | end 82 | 83 | theta_des= Kp_x*error_x + Kd_x*errordot_x; 84 | if theta_des>pi/4 85 | theta_des=pi/4; 86 | end 87 | if theta_des<-pi/4 88 | theta_des=-pi/4; 89 | end 90 | 91 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 92 | 93 | % phi_des= 0.0; 94 | % theta_des=0.0; 95 | psi_des=0.0; 96 | 97 | phi=state.orientation(1); 98 | theta=state.orientation(2); 99 | psi=state.orientation(3); 100 | 101 | phidot=thetadot(1); 102 | thetadot2=thetadot(2); 103 | psidot=thetadot(3); 104 | 105 | error_phi=phi_des-phi; 106 | error_theta=theta_des-theta; 107 | error_psi=psi_des-psi; 108 | 109 | e = zeros(3,1); 110 | e(1)=Kp_phi*-error_phi+Kd_phi*phidot; 111 | e(2)=Kp_theta*-error_theta+Kd_theta*thetadot2; 112 | e(3)=Kp_psi*-error_psi+Kd_psi*psidot; 113 | 114 | e_phi=e(1); 115 | e_theta=e(2); 116 | e_psi=e(3); 117 | 118 | Ixx=state.I(1,1); 119 | Iyy=state.I(2,2); 120 | Izz=state.I(3,3); 121 | 122 | b=state.b; 123 | k=state.k; 124 | L=state.L; 125 | m=state.m; 126 | g=state.g; 127 | 128 | 129 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 130 | %Now compute total thrust (to simplify input equations) 131 | 132 | %To have attitude control (z-axis motion) we are going to need a 133 | %controller to govern the thrust. If thrust = m*g/(k*cos(phi)*cos(theta)); 134 | %then the quadcopter will always be kept at a level altitude but we 135 | %want to be able to have the altitude as an input. 136 | 137 | thrust=Kp_z*error_z+Kd_z*errordot_z; 138 | if thrust<0 139 | thrust=0; 140 | end 141 | 142 | % thrust = m*g/(k*cos(phi)*cos(theta)); 143 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 144 | 145 | %Now compute the propeller angular velocities for a given orientation. 146 | inputs(1)= thrust/4 - (2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 147 | inputs(2)= thrust/4 - (2*b*e_theta*Iyy - k*L*e_psi*Izz)/(4*b*L*k); 148 | inputs(3)= thrust/4 - (-2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 149 | inputs(4)= thrust/4 + (2*b*e_theta*Iyy + k*L*e_psi*Izz)/(4*b*L*k); 150 | 151 | %Now advance the controller state (this essentially means updating the 152 | %orientation vector: 153 | 154 | state.orientation = state.orientation + state.dt*thetadot; 155 | state.phi_des=phi_des; 156 | state.theta_des=theta_des; 157 | state.errors=errors; 158 | state.thrust=thrust; 159 | end 160 | 161 | 162 | 163 | 164 | 165 | 166 | -------------------------------------------------------------------------------- /v8 (results)/PID.m: -------------------------------------------------------------------------------- 1 | function c = PID() 2 | c = @(state, thetadot) pid_controller(state, thetadot); 3 | end 4 | 5 | function [inputs,state] = pid_controller(state, thetadot) 6 | %the "state" argument is the structured array from which the controller 7 | %must access the necessary information. This is the controller_params 8 | %struct in the simulator code. 9 | 10 | %Gain Parameters 11 | Kp_phi = 5; 12 | Kd_phi = 4; 13 | 14 | Kp_theta = Kp_phi; 15 | Kd_theta = Kd_phi; 16 | 17 | Kp_psi = 5; 18 | Kd_psi = 4; 19 | Ki_psi = 0.06; 20 | 21 | Kp_x = 0.3*pi/180; 22 | Kd_x = 0.5*pi/180; 23 | Ki_x = 1.5e-4*pi/180; 24 | 25 | Kp_y = Kp_x; 26 | Kd_y = Kd_x; 27 | Ki_y = Ki_x; 28 | 29 | Kp_z = 1.2e6; 30 | Kd_z = -2.4e6; 31 | Ki_z = 2.0e5; 32 | 33 | %Create the theta (orientation) vector in the structured array if it does 34 | %not yet exist. This will be updated each time this code is run in the 35 | %simulator for-loop. 36 | if ~isfield(state,'orientation') 37 | state.orientation = zeros (3,1); 38 | end 39 | 40 | if ~isfield(state,'errors') 41 | state.errors = zeros (3,1); 42 | end 43 | 44 | if ~isfield(state,'integral') 45 | state.integral = zeros (6,1); 46 | end 47 | 48 | %Now create the inputs that will be sent to the quadcopter. 49 | %There are four inputs (an angular velocity^2 for each prop) 50 | inputs = zeros(4,1); 51 | 52 | %Now extract from the structured array the necessary information to compute 53 | %the inputs. 54 | 55 | x_des=state.xdes; 56 | y_des=state.ydes; 57 | z_des=state.zdes; 58 | 59 | x=state.x(1); 60 | y=state.x(2); 61 | z=state.x(3); 62 | 63 | error_x=x_des-x; 64 | error_y=y-y_des; 65 | error_z=z_des-z; 66 | errors=[error_x;error_y;error_z]; 67 | 68 | vels=state.xdot; 69 | xdot=vels(1); 70 | ydot=vels(2); 71 | zdot=vels(3); 72 | 73 | errordot_x=xdot; 74 | errordot_y=-ydot; 75 | errordot_z=zdot; 76 | 77 | int_x=state.integral(1); 78 | int_y=-state.integral(2); 79 | int_z=state.integral(3); 80 | int_phi=state.integral(4); 81 | int_theta=state.integral(5); 82 | int_psi=state.integral(6); 83 | 84 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 85 | %The roll and pitch angles give y and x motion respectively. Therefore 86 | %PD controllers governing the desired roll and pitch angles can allow us to control x and y 87 | %positions. 88 | 89 | phi_des= Kp_y*error_y + Kd_y*errordot_y + Ki_y*int_y; 90 | if phi_des>pi/4 91 | phi_des=pi/4; 92 | end 93 | if phi_des<-pi/4 94 | phi_des=-pi/4; 95 | end 96 | 97 | theta_des= Kp_x*error_x + Kd_x*errordot_x + Ki_x*int_x; 98 | if theta_des>pi/4 99 | theta_des=pi/4; 100 | end 101 | if theta_des<-pi/4 102 | theta_des=-pi/4; 103 | end 104 | 105 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 106 | 107 | % phi_des= 0.0; 108 | % theta_des=0.0; 109 | psi_des=0.0; 110 | 111 | phi=state.orientation(1); 112 | theta=state.orientation(2); 113 | psi=state.orientation(3); 114 | 115 | phidot=thetadot(1); 116 | thetadot2=thetadot(2); 117 | psidot=thetadot(3); 118 | 119 | error_phi=phi_des-phi; 120 | error_theta=theta_des-theta; 121 | error_psi=psi_des-psi; 122 | 123 | e = zeros(3,1); 124 | e(1)=Kp_phi*-error_phi+Kd_phi*phidot; 125 | e(2)=Kp_theta*-error_theta+Kd_theta*thetadot2; 126 | e(3)=Kp_psi*-error_psi+Kd_psi*psidot+Ki_psi*int_psi; 127 | 128 | e_phi=e(1); 129 | e_theta=e(2); 130 | e_psi=e(3); 131 | 132 | Ixx=state.I(1,1); 133 | Iyy=state.I(2,2); 134 | Izz=state.I(3,3); 135 | 136 | b=state.b; 137 | k=state.k; 138 | L=state.L; 139 | m=state.m; 140 | g=state.g; 141 | dt=state.dt; 142 | 143 | allerrors=[error_x;error_y;error_z;error_phi;error_theta;error_psi]; 144 | 145 | 146 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 147 | %Now compute total thrust (to simplify input equations) 148 | 149 | %To have attitude control (z-axis motion) we are going to need a 150 | %controller to govern the thrust. If thrust = m*g/(k*cos(phi)*cos(theta)); 151 | %then the quadcopter will always be kept at a level altitude but we 152 | %want to be able to have the altitude as an input. 153 | 154 | thrust=Kp_z*error_z + Kd_z*errordot_z + Ki_z*int_z; 155 | if thrust<0 156 | thrust=0; 157 | end 158 | 159 | % thrust = m*g/(k*cos(phi)*cos(theta)); 160 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 161 | 162 | %Now compute the propeller angular velocities for a given orientation. 163 | inputs(1)= thrust/4 - (2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 164 | inputs(2)= thrust/4 - (2*b*e_theta*Iyy - k*L*e_psi*Izz)/(4*b*L*k); 165 | inputs(3)= thrust/4 - (-2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 166 | inputs(4)= thrust/4 + (2*b*e_theta*Iyy + k*L*e_psi*Izz)/(4*b*L*k); 167 | 168 | %Now advance the controller state (this essentially means updating the 169 | %orientation vector: 170 | 171 | state.orientation = state.orientation + state.dt*thetadot; 172 | state.integral = state.integral + dt*allerrors; 173 | state.phi_des=phi_des; 174 | state.theta_des=theta_des; 175 | state.errors=errors; 176 | state.thrust=thrust; 177 | 178 | end 179 | 180 | -------------------------------------------------------------------------------- /v8 (results)/acceleration.m: -------------------------------------------------------------------------------- 1 | function a = acceleration(inputs,angles,velocities,m,g,k,kd) 2 | 3 | %The acceleration of the quadcopter is a function of gravity, the thrust 4 | %and drag forces. From our model we have (a is the acceleration vector, v 5 | %is the velocity vector): 6 | 7 | %m*a=[0 0 -mg].' + T_I +F_D 8 | % =[0 0 -mg].' + R*T_B + F_D 9 | % therefore a = [0 0 -g].' + 1/m*(R*[0 0 k*sum(angular accelerations^2)] + 10 | % kd*v) 11 | 12 | %The angular velocities are the inputs, thrust (T is in F_B) is a function 13 | %of angular velocity and k. 14 | 15 | gravity = [0 0 -g].'; 16 | R = rotation(angles); 17 | thrust_FB=[0;0;k*sum(inputs)]; 18 | thrust_FI=R*thrust_FB; 19 | 20 | Fd = -kd*velocities; 21 | 22 | a = gravity +1/m*thrust_FI +1/m*Fd; 23 | 24 | 25 | end 26 | 27 | -------------------------------------------------------------------------------- /v8 (results)/angular_acceleration.m: -------------------------------------------------------------------------------- 1 | function omegadot = angular_acceleration(inputs,omega,I,L,b,k) 2 | 3 | i1=inputs(1); 4 | i2=inputs(2); 5 | i3=inputs(3); 6 | i4=inputs(4); 7 | 8 | p=omega(1); 9 | q=omega(2); 10 | r=omega(3); 11 | 12 | Ixx=I(1,1); 13 | Iyy=I(2,2); 14 | Izz=I(3,3); 15 | 16 | actuator_action = [ 17 | L*k*(i1-i3) 18 | L*k*(i2-i4) 19 | b*(i1-i2+i3-i4) 20 | ]; 21 | 22 | dragtorque=[ 23 | 0 24 | 0 25 | b*(i1-i2+i3-i4) 26 | ]; 27 | 28 | gyrotorque=[ 29 | (Iyy-Izz)*q*r 30 | (Izz-Ixx)*p*r 31 | (Ixx-Iyy)*p*q 32 | ]; 33 | 34 | tau = actuator_action+dragtorque+gyrotorque; 35 | 36 | %Now use the Euler equation for rigid body motion to obtain angular 37 | %acceleration: 38 | 39 | omegadot=inv(I)*(actuator_action-cross(omega,I*omega)); 40 | 41 | 42 | end 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /v8 (results)/grossresult.m: -------------------------------------------------------------------------------- 1 | %This script is to run the simulation multiple times in order to gather 2 | %mean results. 3 | 4 | function meanrisetime = grossresult(controller,numsim) 5 | %numsim is the number of times the simulation is to be run 6 | 7 | %define arrays: 8 | x=[]; 9 | y=[]; 10 | z=[]; 11 | 12 | xdot=[]; 13 | ydot=[]; 14 | zdot=[]; 15 | 16 | phi=[]; 17 | theta=[]; 18 | psi=[]; 19 | 20 | phidot=[]; 21 | thetadot=[]; 22 | psidot=[]; 23 | 24 | risetime=[]; 25 | 26 | %extract results from simulations: 27 | for n=1:numsim; 28 | if strcmpi(controller,'PID')==1; 29 | result=nographsimulate(PID); 30 | end 31 | if strcmpi(controller,'PD')==1; 32 | result=nographsimulate(PD); 33 | end 34 | 35 | x(n,:)= result.x(1,:); 36 | y(n,:)= result.x(2,:); 37 | z(n,:)= result.x(3,:); 38 | 39 | xdot(n,:)=result.vel(1,:); 40 | ydot(n,:)=result.vel(2,:); 41 | zdot(n,:)=result.vel(3,:); 42 | 43 | phi(n,:)=result.theta(1,:); 44 | theta(n,:)=result.theta(2,:); 45 | psi(n,:)=result.theta(3,:); 46 | 47 | phidot(n,:)=result.angvel(1,:); 48 | thetadot(n,:)=result.angvel(2,:); 49 | psidot(n,:)=result.angvel(3,:); 50 | 51 | risetime(n)=result.risetime; 52 | 53 | end 54 | 55 | %take mean results: 56 | meanx=mean(x); 57 | meany=mean(y); 58 | meanz=mean(z); 59 | 60 | risetime(risetime==0)=[]; 61 | meanrisetime=mean(risetime); 62 | 63 | times=result.t; 64 | 65 | %PLOT ALL RESULTS: 66 | figure 67 | plots=[subplot(2,1,1), subplot(2,1,2)]; 68 | 69 | %VELOCITIES 70 | subplot(plots(1)); 71 | xdotplot=plot(times,xdot, 'b', 'Linewidth',1.2); 72 | titlestr1=strcat({'Velocities when simulated '},num2str(numsim),{' times'}); 73 | title(titlestr1) 74 | xlabel('time (s)') 75 | ylabel('m/s') 76 | hold on 77 | ydotplot=plot(times,ydot, 'r', 'Linewidth',1.2); 78 | hold on 79 | zdotplot=plot(times,zdot, 'g', 'Linewidth',1.2); 80 | xgroup=hggroup; 81 | ygroup=hggroup; 82 | zgroup=hggroup; 83 | set(xdotplot,'Parent',xgroup) 84 | set(ydotplot,'Parent',ygroup) 85 | set(zdotplot,'Parent',zgroup) 86 | set(get(get(xgroup,'Annotation'),'LegendInformation'),... 87 | 'IconDisplayStyle','on'); 88 | set(get(get(ygroup,'Annotation'),'LegendInformation'),... 89 | 'IconDisplayStyle','on'); 90 | set(get(get(zgroup,'Annotation'),'LegendInformation'),... 91 | 'IconDisplayStyle','on'); 92 | legend('x','y','z') 93 | 94 | %DISPLACEMENTS 95 | subplot(plots(2)); 96 | plot(times,x, 'b', 'Linewidth',1.2) 97 | titlestr2=strcat({'Displacements when simulated '},num2str(numsim),{' times'}); 98 | title(titlestr2) 99 | xlabel('time (s)') 100 | ylabel('metres') 101 | hold on 102 | plot(times,y, 'r', 'Linewidth',1.2) 103 | hold on 104 | plot(times,z, 'g', 'Linewidth',1.2) 105 | 106 | figure 107 | plots=[subplot(2,1,1), subplot(2,1,2)]; 108 | 109 | %ANGULAR VELOCITIES 110 | subplot(plots(1)); 111 | phidotplot=plot(times,phidot, 'r', 'Linewidth',1.2); 112 | titlestr3=strcat({'Angular velocities when simulated '},num2str(numsim),{' times'}); 113 | title(titlestr3) 114 | xlabel('time (s)') 115 | ylabel('radians/s') 116 | hold on 117 | thetadotplot=plot(times,thetadot, 'b', 'Linewidth',1.2); 118 | hold on 119 | psidotplot=plot(times,psidot, 'g', 'Linewidth',1.2); 120 | phigroup=hggroup; 121 | thetagroup=hggroup; 122 | psigroup=hggroup; 123 | set(phidotplot,'Parent',phigroup) 124 | set(thetadotplot,'Parent',thetagroup) 125 | set(psidotplot,'Parent',psigroup) 126 | set(get(get(phigroup,'Annotation'),'LegendInformation'),... 127 | 'IconDisplayStyle','on'); 128 | set(get(get(thetagroup,'Annotation'),'LegendInformation'),... 129 | 'IconDisplayStyle','on'); 130 | set(get(get(psigroup,'Annotation'),'LegendInformation'),... 131 | 'IconDisplayStyle','on'); 132 | legend('Roll','Pitch','Yaw') 133 | 134 | %ANGULAR DISPLACEMENTS 135 | subplot(plots(2)); 136 | plot(times,phi, 'r', 'Linewidth',1.2) 137 | titlestr4=strcat({'Angular displacements when simulated '},num2str(numsim),{' times'}); 138 | title(titlestr4) 139 | xlabel('time (s)') 140 | ylabel('radians') 141 | hold on 142 | plot(times,theta, 'b', 'Linewidth',1.2) 143 | hold on 144 | plot(times,psi, 'g', 'Linewidth',1.2) 145 | 146 | 147 | %PATH PLOT 148 | xmax=max(meanx)+10; 149 | xmin=min(meanx); 150 | ymax=max(meany)+10; 151 | ymin=min(meany); 152 | zmax=max(meanz)+10; 153 | zmin=min(meanz); 154 | 155 | figure; 156 | for j=1:numsim 157 | plot3(x(j,:),y(j,:),z(j,:),'r','LineWidth',0.8) 158 | hold on 159 | end 160 | 161 | axis([xmin,xmax,ymin,ymax,zmin,zmax]) 162 | xlabel('x') 163 | ylabel('y') 164 | zlabel('z') 165 | grid on 166 | posstr=strcat('x=',num2str(meanx(length(meanx)))... 167 | ,{', y='},num2str(meany(length(meany))),{', z='},num2str(meanz(length(meanz)))); 168 | text(meanx(length(meanx)),meany(length(meany)),meanz(length(meanz)),posstr); 169 | trise=strcat({'mean rise time = '},num2str(meanrisetime),'s'); 170 | text(meanx(length(meanx)),meany(length(meany)),0.9*meanz(length(meanz)),trise); 171 | titlestr5=strcat({'Paths of quadcopter when simulated '},num2str(numsim),{' times'}); 172 | title(titlestr5) 173 | 174 | 175 | 176 | 177 | -------------------------------------------------------------------------------- /v8 (results)/mean.asv: -------------------------------------------------------------------------------- 1 | %This script is to run the simulation multiple times in order to gather 2 | %mean results. 3 | 4 | function m = mean() 5 | numsim=5; 6 | %numsim is the number of times the simulation is to be run 7 | x=[]; 8 | for n=1:numsim; 9 | result=meansimulate(PID); 10 | x(1,:)=result.x(1, 11 | end 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /v8 (results)/meanresult.m: -------------------------------------------------------------------------------- 1 | %This script is to run the simulation multiple times in order to gather 2 | %mean results. 3 | 4 | function meanrisetime = meanresult(controller,numsim) 5 | %numsim is the number of times the simulation is to be run 6 | 7 | %define arrays: 8 | x=[]; 9 | y=[]; 10 | z=[]; 11 | 12 | xdot=[]; 13 | ydot=[]; 14 | zdot=[]; 15 | 16 | phi=[]; 17 | theta=[]; 18 | psi=[]; 19 | 20 | phidot=[]; 21 | thetadot=[]; 22 | psidot=[]; 23 | 24 | risetime=[]; 25 | 26 | %extract results from simulations: 27 | for n=1:numsim; 28 | if strcmpi(controller,'PID')==1; 29 | result=nographsimulate(PID); 30 | end 31 | if strcmpi(controller,'PD')==1; 32 | result=nographsimulate(PD); 33 | end 34 | 35 | 36 | x(n,:)= result.x(1,:); 37 | y(n,:)= result.x(2,:); 38 | z(n,:)= result.x(3,:); 39 | 40 | xdot(n,:)=result.vel(1,:); 41 | ydot(n,:)=result.vel(2,:); 42 | zdot(n,:)=result.vel(3,:); 43 | 44 | phi(n,:)=result.theta(1,:); 45 | theta(n,:)=result.theta(2,:); 46 | psi(n,:)=result.theta(3,:); 47 | 48 | phidot(n,:)=result.angvel(1,:); 49 | thetadot(n,:)=result.angvel(2,:); 50 | psidot(n,:)=result.angvel(3,:); 51 | 52 | risetime(n)=result.risetime; 53 | 54 | end 55 | 56 | times=result.t; 57 | 58 | %take mean results: 59 | meanx=mean(x); 60 | meany=mean(y); 61 | meanz=mean(z); 62 | 63 | meanxdot=mean(xdot); 64 | meanydot=mean(ydot); 65 | meanzdot=mean(zdot); 66 | 67 | meanphi=mean(phi); 68 | meantheta=mean(theta); 69 | meanpsi=mean(psi); 70 | 71 | meanphidot=mean(phidot); 72 | meanthetadot=mean(thetadot); 73 | meanpsidot=mean(psidot); 74 | 75 | risetime(risetime==0)=[]; 76 | meanrisetime=mean(risetime); 77 | 78 | %PLOT MEAN RESULTS: 79 | figure 80 | plots=[subplot(2,1,1), subplot(2,1,2)]; 81 | 82 | %VELOCITIES 83 | subplot(plots(1)); 84 | plot(times,meanxdot, '--b', 'Linewidth',1.2) 85 | titlestr1=strcat({'Mean velocities when simulated '},num2str(numsim),{' times'}); 86 | title(titlestr1) 87 | xlabel('time (s)') 88 | ylabel('m/s') 89 | hold on 90 | plot(times,meanydot, 'r', 'Linewidth',1.2) 91 | hold on 92 | plot(times,meanzdot, ':g', 'Linewidth',1.2) 93 | legend('x','y','z') 94 | 95 | %DISPLACEMENTS 96 | subplot(plots(2)); 97 | plot(times,meanx, '--b', 'Linewidth',1.2) 98 | titlestr2=strcat({'Mean displacements when simulated '},num2str(numsim),{' times'}); 99 | title(titlestr2) 100 | xlabel('time (s)') 101 | ylabel('metres') 102 | hold on 103 | plot(times,meany, 'r', 'Linewidth',1.2) 104 | hold on 105 | plot(times,meanz, ':g', 'Linewidth',1.2) 106 | 107 | figure 108 | plots=[subplot(2,1,1), subplot(2,1,2)]; 109 | 110 | %ANGULAR VELOCITIES 111 | subplot(plots(1)); 112 | plot(times,meanphidot, 'r', 'Linewidth',1.2) 113 | titlestr3=strcat({'Mean angular velocities when simulated '},num2str(numsim),{' times'}); 114 | title(titlestr3) 115 | xlabel('time (s)') 116 | ylabel('radians/s') 117 | hold on 118 | plot(times,meanthetadot, '--b', 'Linewidth',1.2) 119 | hold on 120 | plot(times,meanpsidot, ':g', 'Linewidth',1.2) 121 | legend('Roll','Pitch','Yaw') 122 | 123 | %ANGULAR DISPLACEMENTS 124 | subplot(plots(2)); 125 | plot(times,meanphi, 'r', 'Linewidth',1.2) 126 | titlestr4=strcat({'Mean angular displacements when simulated '},num2str(numsim),{' times'}); 127 | title(titlestr4) 128 | xlabel('time (s)') 129 | ylabel('radians') 130 | hold on 131 | plot(times,meantheta, '--b', 'Linewidth',1.2) 132 | hold on 133 | plot(times,meanpsi, ':g', 'Linewidth',1.2) 134 | 135 | %PATH PLOT 136 | xmax=max(meanx)+10; 137 | xmin=min(meanx); 138 | ymax=max(meany)+10; 139 | ymin=min(meany); 140 | zmax=max(meanz)+10; 141 | zmin=min(meanz); 142 | 143 | figure; 144 | plot3(meanx,meany,meanz,'r','LineWidth',1.5) 145 | axis([xmin,xmax,ymin,ymax,zmin,zmax]) 146 | xlabel('x') 147 | ylabel('y') 148 | zlabel('z') 149 | grid on 150 | posstr=strcat('x=',num2str(meanx(length(meanx)))... 151 | ,{', y='},num2str(meany(length(meany))),{', z='},num2str(meanz(length(meanz)))); 152 | text(meanx(length(meanx)),meany(length(meany)),meanz(length(meanz)),posstr); 153 | trise=strcat({'mean rise time = '},num2str(meanrisetime),'s'); 154 | text(meanx(length(meanx)),meany(length(meany)),0.9*meanz(length(meanz)),trise); 155 | titlestr5=strcat({'Mean path of quadcopter when simulated '},num2str(numsim),{' times'}); 156 | title(titlestr5) 157 | 158 | 159 | -------------------------------------------------------------------------------- /v8 (results)/nographsimulate.m: -------------------------------------------------------------------------------- 1 | function result = nographsimulate(controller, x_desired,y_desired,z_desired) 2 | %SET DEFAULT DESIRED COORDINATES 3 | if nargin<2 4 | x_desired=100; 5 | y_desired=100; 6 | z_desired=100; 7 | end 8 | 9 | %SIMULATION TIMES 10 | 11 | start_time = 0; 12 | end_time = 120; 13 | dt = 0.02; 14 | times = start_time:dt:end_time; 15 | N = numel(times); 16 | 17 | %DEFINE PHYSICAL CONSTANTS (FOR A HYPOTHETICAL QUADCOPTER) 18 | 19 | g = 9.81; 20 | m = 0.5; 21 | L = 0.25; 22 | k = 3e-6; 23 | b = 1e-7; 24 | I = diag([5e-3, 5e-3, 10e-3]); 25 | kd = 0.25; 26 | 27 | %INITIAL SIMULATION STATE 28 | 29 | %the x vector triple denotes quadcopter position 30 | x=[0 0 10].'; 31 | 32 | %now create empty triples for xdot (time derivate of x) and theta 33 | %(orientation of the quadcopter using the Euler angles). 34 | 35 | xdot=zeros(3,1); 36 | theta=zeros(3,1); 37 | 38 | %thetadot denotes the angular velocities which can be set to an initial 39 | %disturbance. 40 | 41 | if nargin==0 42 | thetadot=zeros(3,1); 43 | else 44 | thetadot=deg2rad(randi([-90,90],3,1)); 45 | %thetadot=zeros(3,1); 46 | %thetadot=[0.6;0.6;0.6]; 47 | end 48 | 49 | %CREATE ARRAYS AND STRUCTURED ARRAY FOR THE OUTPUTS 50 | 51 | xout=zeros(3,N); 52 | xdotout=zeros(3,N); 53 | thetaout=zeros(3,N); 54 | thetadotout=zeros(3,N); 55 | inputout=zeros(4,N); 56 | 57 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 58 | %These arrays are to store data that is not critical for controller 59 | %simulation but is needed for secondary graphs (used in controller 60 | %design). 61 | 62 | phi_desout=zeros(1,N); 63 | theta_desout=zeros(1,N); 64 | errors=zeros(3,N); 65 | thrust=zeros(1,N); 66 | integralerrors=zeros(6,N); 67 | risetime=0; 68 | 69 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 70 | 71 | %PARAMETERS FOR THE CONTROLLER 72 | controller_params = struct('dt',dt,'I',I,'k',k,'L',L,'b',b,'m',m,'g',g,... 73 | 'xdes',x_desired,'ydes',y_desired,'zdes',z_desired); 74 | 75 | %STEP THROUGH SIMULATION, UPDATING STATE OF QUADCOPTER 76 | 77 | count=0; 78 | graphendtime=0; 79 | graphendcount=0; 80 | 81 | for t = times 82 | count=count+1; 83 | 84 | controller_params.xdot=xdot; 85 | controller_params.x=x; 86 | 87 | %Here we need to take an input from the controller, or if no 88 | %controller is specified then we can use some default input. The 89 | %inputs are values of gamma (angular velocity^2) for each propeller 90 | 91 | if nargin==0 92 | in=zeros(4,1); 93 | in(1)=700; 94 | in(2)=750; 95 | in(3)=700; 96 | in(4)=750; 97 | in=in.^2; 98 | else 99 | [in,controller_params] = controller(controller_params,thetadot); 100 | end 101 | 102 | %COMPUTE FORCES, TORQUES AND ACCELERATIONS 103 | 104 | %Define angular velocity, omega 105 | omega = thetadot2omega(thetadot,theta); 106 | %Define acceleration, a 107 | a=acceleration(in, theta, xdot, m, g, k, kd); 108 | %Define anglular acceleration, omegadot 109 | omegadot=angular_acceleration(in,omega,I,L,b,k); 110 | 111 | %STORE RISE TIME 112 | if risetime==0 113 | if x(1)>0.95*x_desired && x(1)<1.05*x_desired... 114 | && x(2)>0.95*y_desired && x(2)<1.05*y_desired... 115 | && x(3)>0.95*z_desired && x(3)<1.05*z_desired 116 | risetime=times(count); 117 | end 118 | end 119 | 120 | %STORE GRAPH END TIME 121 | if risetime ~= 0 122 | graphendtime=risetime+10; 123 | if graphendcount==0 124 | if times(count)>graphendtime 125 | graphendcount=count; 126 | end 127 | end 128 | end 129 | 130 | %ADVANCE SYSTEM STATE 131 | 132 | omega = omega + omegadot*dt; 133 | thetadot = omega2thetadot(omega,theta); 134 | theta = theta + thetadot*dt; 135 | xdot = xdot + a*dt; 136 | x = x + xdot*dt; 137 | 138 | %STORE SIMULATION STATE FOR OUTPUT 139 | xout(:,count)=x; 140 | xdotout(:,count)=xdot; 141 | thetaout(:,count)=theta; 142 | thetadotout(:,count)=thetadot; 143 | inputout(:,count)=in; 144 | phi_desout(:,count)=controller_params.phi_des; 145 | theta_desout(:,count)=controller_params.theta_des; 146 | errors(:,count)=controller_params.errors; 147 | thrust(:,count)=controller_params.thrust; 148 | %integralerrors(:,count)=controller_params.integral; 149 | 150 | end 151 | 152 | 153 | %PUT OUTPUT INFORMATION INTO A STRUCTURED ARRAY: 154 | result = struct('x',xout,'theta',thetaout,'vel',xdotout,'angvel'... 155 | ,thetadotout,'t',times, 'dt',dt,'input',inputout,'phides'... 156 | ,phi_desout,'thetades',theta_desout,'errors',errors,'thrust',thrust... 157 | ,'integralerrors',integralerrors,'risetime',risetime... 158 | ,'gecount',graphendcount'); 159 | 160 | 161 | end -------------------------------------------------------------------------------- /v8 (results)/omega2thetadot.m: -------------------------------------------------------------------------------- 1 | function thetadot = omega2thetadot(omega,angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | A=zeros(3); 7 | A(1,:)=[ 8 | 1 9 | 0 10 | -sin(theta) 11 | ]; 12 | 13 | A(2,:) =[ 14 | 0 15 | cos(phi) 16 | cos(theta)*sin(phi) 17 | ]; 18 | 19 | A(3,:) =[ 20 | 0 21 | -sin(phi) 22 | cos(phi)*cos(theta) 23 | ]; 24 | 25 | W=inv(A); 26 | thetadot=W*omega; 27 | 28 | %This function converts from angular velocity to the time derivatives 29 | %of roll, pitch and yaw. 30 | end 31 | -------------------------------------------------------------------------------- /v8 (results)/rotation.m: -------------------------------------------------------------------------------- 1 | function R = rotation(angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | R=zeros(3); 7 | R(1,:) = [cos(theta)*cos(psi); cos(psi)*sin(phi)*sin(theta)-cos(phi)*sin(psi);sin(phi)*sin(psi)+cos(phi)*cos(psi)*sin(theta)]; 8 | 9 | R(2,:) = [cos(theta)*sin(psi);cos(phi)*cos(psi) + sin(phi)*sin(theta)*sin(psi);cos(phi)*sin(theta)*sin(psi)-cos(psi)*sin(phi)]; 10 | 11 | R(3,:) = [-sin(theta);cos(theta)*sin(phi);cos(phi)*cos(theta)]; 12 | 13 | %This rotation matrix maps between the body-fixed frame and the inertial 14 | %frame. 15 | 16 | end 17 | 18 | -------------------------------------------------------------------------------- /v8 (results)/simulate.m: -------------------------------------------------------------------------------- 1 | function result = simulate(controller, x_desired,y_desired,z_desired) 2 | %SET DEFAULT DESIRED COORDINATES 3 | if nargin<2 4 | x_desired=100; 5 | y_desired=100; 6 | z_desired=100; 7 | end 8 | 9 | %SIMULATION TIMES 10 | 11 | start_time = 0; 12 | end_time = 120; 13 | dt = 0.02; 14 | times = start_time:dt:end_time; 15 | N = numel(times); 16 | 17 | %DEFINE PHYSICAL CONSTANTS (FOR A HYPOTHETICAL QUADCOPTER) 18 | 19 | g = 9.81; 20 | m = 0.5; 21 | L = 0.25; 22 | k = 3e-6; 23 | b = 1e-7; 24 | I = diag([5e-3, 5e-3, 10e-3]); 25 | kd = 0.25; 26 | 27 | %INITIAL SIMULATION STATE 28 | 29 | %the x vector triple denotes quadcopter position 30 | x=[0 0 10].'; 31 | 32 | %now create empty triples for xdot (time derivate of x) and theta 33 | %(orientation of the quadcopter using the Euler angles). 34 | 35 | xdot=zeros(3,1); 36 | theta=zeros(3,1); 37 | 38 | %thetadot denotes the angular velocities which can be set to an initial 39 | %disturbance. 40 | 41 | if nargin==0 42 | thetadot=zeros(3,1); 43 | else 44 | thetadot=deg2rad(randi([-90,90],3,1)); 45 | %thetadot=zeros(3,1); 46 | %thetadot=[0.6;0.6;0.6]; 47 | end 48 | 49 | %CREATE ARRAYS AND STRUCTURED ARRAY FOR THE OUTPUTS 50 | 51 | xout=zeros(3,N); 52 | xdotout=zeros(3,N); 53 | thetaout=zeros(3,N); 54 | thetadotout=zeros(3,N); 55 | inputout=zeros(4,N); 56 | 57 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 58 | %These arrays are to store data that is not critical for controller 59 | %simulation but is needed for secondary graphs (used in controller 60 | %design). 61 | 62 | phi_desout=zeros(1,N); 63 | theta_desout=zeros(1,N); 64 | errors=zeros(3,N); 65 | thrust=zeros(1,N); 66 | integralerrors=zeros(6,N); 67 | risetime=0; 68 | 69 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 70 | 71 | %PARAMETERS FOR THE CONTROLLER 72 | controller_params = struct('dt',dt,'I',I,'k',k,'L',L,'b',b,'m',m,'g',g,... 73 | 'xdes',x_desired,'ydes',y_desired,'zdes',z_desired); 74 | 75 | %STEP THROUGH SIMULATION, UPDATING STATE OF QUADCOPTER 76 | 77 | count=0; 78 | graphendtime=0; 79 | graphendcount=0; 80 | 81 | for t = times 82 | count=count+1; 83 | 84 | controller_params.xdot=xdot; 85 | controller_params.x=x; 86 | 87 | %Here we need to take an input from the controller, or if no 88 | %controller is specified then we can use some default input. The 89 | %inputs are values of gamma (angular velocity^2) for each propeller 90 | 91 | if nargin==0 92 | in=zeros(4,1); 93 | in(1)=700; 94 | in(2)=750; 95 | in(3)=700; 96 | in(4)=750; 97 | in=in.^2; 98 | else 99 | [in,controller_params] = controller(controller_params,thetadot); 100 | end 101 | 102 | %COMPUTE FORCES, TORQUES AND ACCELERATIONS 103 | 104 | %Define angular velocity, omega 105 | omega = thetadot2omega(thetadot,theta); 106 | %Define acceleration, a 107 | a=acceleration(in, theta, xdot, m, g, k, kd); 108 | %Define anglular acceleration, omegadot 109 | omegadot=angular_acceleration(in,omega,I,L,b,k); 110 | 111 | %STORE RISE TIME 112 | if risetime==0 113 | if x(1)>0.95*x_desired && x(1)<1.05*x_desired... 114 | && x(2)>0.95*y_desired && x(2)<1.05*y_desired... 115 | && x(3)>0.95*z_desired && x(3)<1.05*z_desired 116 | risetime=times(count); 117 | end 118 | end 119 | 120 | %STORE GRAPH END TIME 121 | if risetime ~= 0 122 | graphendtime=risetime+10; 123 | if graphendcount==0 124 | if times(count)>graphendtime 125 | graphendcount=count; 126 | end 127 | end 128 | end 129 | 130 | %ADVANCE SYSTEM STATE 131 | 132 | omega = omega + omegadot*dt; 133 | thetadot = omega2thetadot(omega,theta); 134 | theta = theta + thetadot*dt; 135 | xdot = xdot + a*dt; 136 | x = x + xdot*dt; 137 | 138 | %STORE SIMULATION STATE FOR OUTPUT 139 | xout(:,count)=x; 140 | xdotout(:,count)=xdot; 141 | thetaout(:,count)=theta; 142 | thetadotout(:,count)=thetadot; 143 | inputout(:,count)=in; 144 | phi_desout(:,count)=controller_params.phi_des; 145 | theta_desout(:,count)=controller_params.theta_des; 146 | errors(:,count)=controller_params.errors; 147 | thrust(:,count)=controller_params.thrust; 148 | %integralerrors(:,count)=controller_params.integral; 149 | 150 | end 151 | 152 | 153 | %PUT OUTPUT INFORMATION INTO A STRUCTURED ARRAY: 154 | result = struct('x',xout,'theta',thetaout,'vel',xdotout,'angvel'... 155 | ,thetadotout,'t',times, 'dt',dt,'input',inputout,'phides'... 156 | ,phi_desout,'thetades',theta_desout,'errors',errors,'thrust',thrust... 157 | ,'integralerrors',integralerrors,'risetime',risetime... 158 | ,'gecount',graphendcount'); 159 | 160 | %PLOT RESULTS 161 | grapher(result) 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | end -------------------------------------------------------------------------------- /v8 (results)/simulate2.m: -------------------------------------------------------------------------------- 1 | function result = simulate2(controller, x_desired,y_desired,z_desired) 2 | if nargin<2 3 | x_desired=100; 4 | y_desired=100; 5 | z_desired=100; 6 | end 7 | result=nographsimulate(controller, x_desired,y_desired,z_desired); 8 | grapher(result) 9 | end -------------------------------------------------------------------------------- /v8 (results)/thetadot2omega.m: -------------------------------------------------------------------------------- 1 | function omega = thetadot2omega(thetadot,angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | A=zeros(3); 7 | A(1,:)=[ 8 | 1 9 | 0 10 | -sin(theta) 11 | ]; 12 | A(2,:) =[ 13 | 0 14 | cos(phi) 15 | cos(theta)*sin(phi) 16 | ]; 17 | A(3,:) =[ 18 | 0 19 | -sin(phi) 20 | cos(phi)*cos(theta) 21 | ]; 22 | 23 | omega=A*thetadot; 24 | 25 | 26 | %This function converts time derivatives of roll, pitch and yaw into 27 | %angular velocity. A is the necessary rotation matrix for this operation. 28 | %We can define the reverse function omega2thetadot using the inverse of 29 | %this matrix if desired. 30 | 31 | end 32 | 33 | -------------------------------------------------------------------------------- /v8 (results)/totalgrossresult.m: -------------------------------------------------------------------------------- 1 | function meantimes = totalgrossresult(controller) 2 | series=[]; 3 | n=1024; 4 | for i = 1:3 5 | n=n*2; 6 | series(i)=n; 7 | end 8 | 9 | meantimes=[]; 10 | 11 | count=0; 12 | for j = series 13 | count=count+1; 14 | if strcmpi(controller,'PID')==1; 15 | meantimes(count)=grossresult('PID',j); 16 | end 17 | if strcmpi(controller,'PD')==1; 18 | meantimes(count)=grossresult('PD',j); 19 | end 20 | end 21 | 22 | figure 23 | plot(series,meantimes, 'r', 'Linewidth',1.2) 24 | titlestr=strcat({'Mean rise time against number of simulations for a '}... 25 | ,controller,{' controller'}); 26 | title(titlestr) 27 | xlabel('Number of simulations') 28 | ylabel('Mean rise time') -------------------------------------------------------------------------------- /v8 (results)/totalmeanresult.m: -------------------------------------------------------------------------------- 1 | function meantimes = totalmeanresult(controller) 2 | series=[]; 3 | n=1; 4 | for i = 1:12 5 | n=n*2; 6 | series(i)=n; 7 | end 8 | 9 | meantimes=[]; 10 | 11 | count=0; 12 | for j = series 13 | count=count+1; 14 | if strcmpi(controller,'PID')==1; 15 | meantimes(count)=meanresult('PID',j); 16 | end 17 | if strcmpi(controller,'PD')==1; 18 | meantimes(count)=meanresult('PD',j); 19 | end 20 | end 21 | 22 | figure 23 | plot(series,meantimes, 'r', 'Linewidth',1.2) 24 | titlestr=strcat({'Mean rise time against number of simulations for a '}... 25 | ,controller,{' controller'}); 26 | title(titlestr) 27 | xlabel('Number of simulations') 28 | ylabel('Mean rise time') 29 | 30 | 31 | -------------------------------------------------------------------------------- /v9 (mean times)/Final Dissertation.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/samuelgilonis/Quadcopter-PID-controller/5aae0d5a14cb32cb6a54b4c1b8df19d9bffbc83f/v9 (mean times)/Final Dissertation.pdf -------------------------------------------------------------------------------- /v9 (mean times)/PD.m: -------------------------------------------------------------------------------- 1 | function c = PD() 2 | c = @(state, thetadot) pd_controller(state, thetadot); 3 | end 4 | 5 | function [inputs,state] = pd_controller(state, thetadot) 6 | %the "state" argument is the structured array from which the controller 7 | %must access the necessary information. This is the controller_params 8 | %struct in the simulator code. 9 | 10 | %Gain Parameters 11 | Kp_phi = 5; 12 | Kd_phi = 4; 13 | 14 | Kp_theta = Kp_phi; 15 | Kd_theta = Kd_phi; 16 | 17 | Kp_psi = 5; 18 | Kd_psi = 4; 19 | 20 | Kp_x = 0.3*pi/180; 21 | Kd_x = 0.5*pi/180; 22 | 23 | Kp_y = Kp_x; 24 | Kd_y = Kd_x; 25 | 26 | Kp_z = 1.2e6; 27 | Kd_z = -2.4e6; 28 | 29 | %Create the theta (orientation) vector in the structured array if it does 30 | %not yet exist. This will be updated each time this code is run in the 31 | %simulator for-loop. 32 | if ~isfield(state,'orientation') 33 | state.orientation = zeros (3,1); 34 | end 35 | 36 | if ~isfield(state,'errors') 37 | state.errors = zeros (6,1); 38 | end 39 | 40 | 41 | %Now create the inputs that will be sent to the quadcopter. 42 | %There are four inputs (an angular velocity^2 for each prop) 43 | inputs = zeros(4,1); 44 | 45 | %Now extract from the structured array the necessary information to compute 46 | %the inputs. 47 | 48 | x_des=state.xdes; 49 | y_des=state.ydes; 50 | z_des=state.zdes; 51 | 52 | x=state.x(1); 53 | y=state.x(2); 54 | z=state.x(3); 55 | 56 | error_x=x_des-x; 57 | error_y=y-y_des; 58 | error_z=z_des-z; 59 | errors=[error_x;error_y;error_z]; 60 | 61 | vels=state.xdot; 62 | xdot=vels(1); 63 | ydot=vels(2); 64 | zdot=vels(3); 65 | 66 | errordot_x=xdot; 67 | errordot_y=-ydot; 68 | errordot_z=zdot; 69 | 70 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 71 | %The roll and pitch angles give y and x motion respectively. Therefore 72 | %PD controllers governing the desired roll and pitch angles can allow us to control x and y 73 | %positions. 74 | 75 | phi_des= Kp_y*error_y + Kd_y*errordot_y; 76 | if phi_des>pi/4 77 | phi_des=pi/4; 78 | end 79 | if phi_des<-pi/4 80 | phi_des=-pi/4; 81 | end 82 | 83 | theta_des= Kp_x*error_x + Kd_x*errordot_x; 84 | if theta_des>pi/4 85 | theta_des=pi/4; 86 | end 87 | if theta_des<-pi/4 88 | theta_des=-pi/4; 89 | end 90 | 91 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 92 | 93 | % phi_des= 0.0; 94 | % theta_des=0.0; 95 | psi_des=0.0; 96 | 97 | phi=state.orientation(1); 98 | theta=state.orientation(2); 99 | psi=state.orientation(3); 100 | 101 | phidot=thetadot(1); 102 | thetadot2=thetadot(2); 103 | psidot=thetadot(3); 104 | 105 | error_phi=phi_des-phi; 106 | error_theta=theta_des-theta; 107 | error_psi=psi_des-psi; 108 | 109 | e = zeros(3,1); 110 | e(1)=Kp_phi*-error_phi+Kd_phi*phidot; 111 | e(2)=Kp_theta*-error_theta+Kd_theta*thetadot2; 112 | e(3)=Kp_psi*-error_psi+Kd_psi*psidot; 113 | 114 | e_phi=e(1); 115 | e_theta=e(2); 116 | e_psi=e(3); 117 | 118 | Ixx=state.I(1,1); 119 | Iyy=state.I(2,2); 120 | Izz=state.I(3,3); 121 | 122 | b=state.b; 123 | k=state.k; 124 | L=state.L; 125 | m=state.m; 126 | g=state.g; 127 | 128 | 129 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 130 | %Now compute total thrust (to simplify input equations) 131 | 132 | %To have attitude control (z-axis motion) we are going to need a 133 | %controller to govern the thrust. If thrust = m*g/(k*cos(phi)*cos(theta)); 134 | %then the quadcopter will always be kept at a level altitude but we 135 | %want to be able to have the altitude as an input. 136 | 137 | thrust=Kp_z*error_z+Kd_z*errordot_z; 138 | if thrust<0 139 | thrust=0; 140 | end 141 | 142 | % thrust = m*g/(k*cos(phi)*cos(theta)); 143 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 144 | 145 | %Now compute the propeller angular velocities for a given orientation. 146 | inputs(1)= thrust/4 - (2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 147 | inputs(2)= thrust/4 - (2*b*e_theta*Iyy - k*L*e_psi*Izz)/(4*b*L*k); 148 | inputs(3)= thrust/4 - (-2*b*e_phi*Ixx + k*L*e_psi*Izz)/(4*b*L*k); 149 | inputs(4)= thrust/4 + (2*b*e_theta*Iyy + k*L*e_psi*Izz)/(4*b*L*k); 150 | 151 | %Now advance the controller state (this essentially means updating the 152 | %orientation vector: 153 | 154 | state.orientation = state.orientation + state.dt*thetadot; 155 | state.phi_des=phi_des; 156 | state.theta_des=theta_des; 157 | state.errors=errors; 158 | state.thrust=thrust; 159 | end 160 | 161 | 162 | 163 | 164 | 165 | 166 | -------------------------------------------------------------------------------- /v9 (mean times)/acceleration.m: -------------------------------------------------------------------------------- 1 | function a = acceleration(inputs,angles,velocities,m,g,k,kd) 2 | 3 | %The acceleration of the quadcopter is a function of gravity, the thrust 4 | %and drag forces. From our model we have (a is the acceleration vector, v 5 | %is the velocity vector): 6 | 7 | %m*a=[0 0 -mg].' + T_I +F_D 8 | % =[0 0 -mg].' + R*T_B + F_D 9 | % therefore a = [0 0 -g].' + 1/m*(R*[0 0 k*sum(angular accelerations^2)] + 10 | % kd*v) 11 | 12 | %The angular velocities are the inputs, thrust (T is in F_B) is a function 13 | %of angular velocity and k. 14 | 15 | gravity = [0 0 -g].'; 16 | R = rotation(angles); 17 | thrust_FB=[0;0;k*sum(inputs)]; 18 | thrust_FI=R*thrust_FB; 19 | 20 | Fd = -kd*velocities; 21 | 22 | a = gravity +1/m*thrust_FI +1/m*Fd; 23 | 24 | 25 | end 26 | 27 | -------------------------------------------------------------------------------- /v9 (mean times)/angular_acceleration.m: -------------------------------------------------------------------------------- 1 | function omegadot = angular_acceleration(inputs,omega,I,L,b,k) 2 | 3 | i1=inputs(1); 4 | i2=inputs(2); 5 | i3=inputs(3); 6 | i4=inputs(4); 7 | 8 | p=omega(1); 9 | q=omega(2); 10 | r=omega(3); 11 | 12 | Ixx=I(1,1); 13 | Iyy=I(2,2); 14 | Izz=I(3,3); 15 | 16 | actuator_action = [ 17 | L*k*(i1-i3) 18 | L*k*(i2-i4) 19 | b*(i1-i2+i3-i4) 20 | ]; 21 | 22 | dragtorque=[ 23 | 0 24 | 0 25 | b*(i1-i2+i3-i4) 26 | ]; 27 | 28 | gyrotorque=[ 29 | (Iyy-Izz)*q*r 30 | (Izz-Ixx)*p*r 31 | (Ixx-Iyy)*p*q 32 | ]; 33 | 34 | tau = actuator_action+dragtorque+gyrotorque; 35 | 36 | %Now use the Euler equation for rigid body motion to obtain angular 37 | %acceleration: 38 | 39 | omegadot=inv(I)*(actuator_action-cross(omega,I*omega)); 40 | 41 | 42 | end 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /v9 (mean times)/mean.asv: -------------------------------------------------------------------------------- 1 | %This script is to run the simulation multiple times in order to gather 2 | %mean results. 3 | 4 | function m = mean() 5 | numsim=5; 6 | %numsim is the number of times the simulation is to be run 7 | x=[]; 8 | for n=1:numsim; 9 | result=meansimulate(PID); 10 | x(1,:)=result.x(1, 11 | end 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /v9 (mean times)/meanresult.m: -------------------------------------------------------------------------------- 1 | %This script is to run the simulation multiple times in order to gather 2 | %mean results. 3 | 4 | function meanrisetime = meanresult(controller,numsim) 5 | %numsim is the number of times the simulation is to be run 6 | 7 | %define arrays: 8 | x=[]; 9 | y=[]; 10 | z=[]; 11 | 12 | xdot=[]; 13 | ydot=[]; 14 | zdot=[]; 15 | 16 | phi=[]; 17 | theta=[]; 18 | psi=[]; 19 | 20 | phidot=[]; 21 | thetadot=[]; 22 | psidot=[]; 23 | 24 | risetime=[]; 25 | 26 | %extract results from simulations: 27 | for n=1:numsim; 28 | if strcmpi(controller,'PID')==1; 29 | result=nographsimulate(PID); 30 | end 31 | if strcmpi(controller,'PD')==1; 32 | result=nographsimulate(PD); 33 | end 34 | 35 | 36 | x(n,:)= result.x(1,:); 37 | y(n,:)= result.x(2,:); 38 | z(n,:)= result.x(3,:); 39 | 40 | xdot(n,:)=result.vel(1,:); 41 | ydot(n,:)=result.vel(2,:); 42 | zdot(n,:)=result.vel(3,:); 43 | 44 | phi(n,:)=result.theta(1,:); 45 | theta(n,:)=result.theta(2,:); 46 | psi(n,:)=result.theta(3,:); 47 | 48 | phidot(n,:)=result.angvel(1,:); 49 | thetadot(n,:)=result.angvel(2,:); 50 | psidot(n,:)=result.angvel(3,:); 51 | 52 | risetime(n)=result.risetime; 53 | 54 | end 55 | 56 | times=result.t; 57 | 58 | %take mean results: 59 | meanx=mean(x); 60 | meany=mean(y); 61 | meanz=mean(z); 62 | 63 | meanxdot=mean(xdot); 64 | meanydot=mean(ydot); 65 | meanzdot=mean(zdot); 66 | 67 | meanphi=mean(phi); 68 | meantheta=mean(theta); 69 | meanpsi=mean(psi); 70 | 71 | meanphidot=mean(phidot); 72 | meanthetadot=mean(thetadot); 73 | meanpsidot=mean(psidot); 74 | 75 | risetime(risetime==0)=[]; 76 | meanrisetime=mean(risetime); 77 | 78 | % %PLOT MEAN RESULTS: 79 | % figure 80 | % plots=[subplot(2,1,1), subplot(2,1,2)]; 81 | % 82 | % %VELOCITIES 83 | % subplot(plots(1)); 84 | % plot(times,meanxdot, '--b', 'Linewidth',1.2) 85 | % titlestr1=strcat({'Mean velocities when simulated '},num2str(numsim),{' times'}); 86 | % title(titlestr1) 87 | % xlabel('time (s)') 88 | % ylabel('m/s') 89 | % hold on 90 | % plot(times,meanydot, 'r', 'Linewidth',1.2) 91 | % hold on 92 | % plot(times,meanzdot, ':g', 'Linewidth',1.2) 93 | % legend('x','y','z') 94 | % 95 | % %DISPLACEMENTS 96 | % subplot(plots(2)); 97 | % plot(times,meanx, '--b', 'Linewidth',1.2) 98 | % titlestr2=strcat({'Mean displacements when simulated '},num2str(numsim),{' times'}); 99 | % title(titlestr2) 100 | % xlabel('time (s)') 101 | % ylabel('metres') 102 | % hold on 103 | % plot(times,meany, 'r', 'Linewidth',1.2) 104 | % hold on 105 | % plot(times,meanz, ':g', 'Linewidth',1.2) 106 | % 107 | % figure 108 | % plots=[subplot(2,1,1), subplot(2,1,2)]; 109 | % 110 | % %ANGULAR VELOCITIES 111 | % subplot(plots(1)); 112 | % plot(times,meanphidot, 'r', 'Linewidth',1.2) 113 | % titlestr3=strcat({'Mean angular velocities when simulated '},num2str(numsim),{' times'}); 114 | % title(titlestr3) 115 | % xlabel('time (s)') 116 | % ylabel('radians/s') 117 | % hold on 118 | % plot(times,meanthetadot, '--b', 'Linewidth',1.2) 119 | % hold on 120 | % plot(times,meanpsidot, ':g', 'Linewidth',1.2) 121 | % legend('Roll','Pitch','Yaw') 122 | % 123 | % %ANGULAR DISPLACEMENTS 124 | % subplot(plots(2)); 125 | % plot(times,meanphi, 'r', 'Linewidth',1.2) 126 | % titlestr4=strcat({'Mean angular displacements when simulated '},num2str(numsim),{' times'}); 127 | % title(titlestr4) 128 | % xlabel('time (s)') 129 | % ylabel('radians') 130 | % hold on 131 | % plot(times,meantheta, '--b', 'Linewidth',1.2) 132 | % hold on 133 | % plot(times,meanpsi, ':g', 'Linewidth',1.2) 134 | 135 | %PATH PLOT 136 | xmax=max(meanx)+10; 137 | xmin=min(meanx); 138 | ymax=max(meany)+10; 139 | ymin=min(meany); 140 | zmax=max(meanz)+10; 141 | zmin=min(meanz); 142 | 143 | figure; 144 | plot3(meanx,meany,meanz,'r','LineWidth',1.5) 145 | axis([xmin,xmax,ymin,ymax,zmin,zmax]) 146 | xlabel('x') 147 | ylabel('y') 148 | zlabel('z') 149 | grid on 150 | posstr=strcat('x=',num2str(meanx(length(meanx)))... 151 | ,{', y='},num2str(meany(length(meany))),{', z='},num2str(meanz(length(meanz)))); 152 | text(meanx(length(meanx)),meany(length(meany)),meanz(length(meanz)),posstr); 153 | trise=strcat({'mean rise time = '},num2str(meanrisetime),'s'); 154 | text(meanx(length(meanx)),meany(length(meany)),0.9*meanz(length(meanz)),trise); 155 | titlestr5=strcat({'Mean path of quadcopter when simulated '},num2str(numsim),{' times'}); 156 | title(titlestr5) 157 | 158 | 159 | -------------------------------------------------------------------------------- /v9 (mean times)/meantimes.m: -------------------------------------------------------------------------------- 1 | function m = meantimes() 2 | 3 | series=[]; 4 | n=1; 5 | for i = 1:12 6 | n=n*2; 7 | series(i)=n; 8 | end 9 | 10 | %PID 11 | %mean data 12 | % mt=[16.08,15.2,15.88,15.795,15.8361,16.0645,15.6602,16.1424,16.1836,16.1197,16.1841,16.1236]; 13 | %gross data 14 | % mt=[14.93, 15.2, 15.88, 15.795, 15.8631, 16.0645, 15.6602, 16.1424, 16.0877, 16.1607, 16.1872, 16.1442]; 15 | 16 | %PD 17 | %mean data 18 | % mt=[]; 19 | %gross data 20 | % mt=[]; 21 | 22 | 23 | figure 24 | plot(series,mt,'r','LineWidth', 2) 25 | set(gca,'XScale','log') 26 | title('Mean rise time against number of simulations') 27 | 28 | -------------------------------------------------------------------------------- /v9 (mean times)/nographsimulate.m: -------------------------------------------------------------------------------- 1 | function result = nographsimulate(controller, x_desired,y_desired,z_desired) 2 | %SET DEFAULT DESIRED COORDINATES 3 | if nargin<2 4 | x_desired=100.0; 5 | y_desired=100.0; 6 | z_desired=100.0; 7 | end 8 | 9 | %SIMULATION TIMES 10 | 11 | start_time = 0; 12 | end_time = 60; 13 | dt = 0.005; 14 | times = start_time:dt:end_time; 15 | N = numel(times); 16 | 17 | %DEFINE PHYSICAL CONSTANTS (FOR A HYPOTHETICAL QUADCOPTER) 18 | 19 | g = 9.81; 20 | m = 0.5; 21 | L = 0.25; 22 | k = 3e-6; 23 | b = 1e-7; 24 | I = diag([5e-3, 5e-3, 10e-3]); 25 | kd = 0.25; 26 | 27 | %INITIAL SIMULATION STATE 28 | 29 | %the x vector triple denotes quadcopter position 30 | x=[0 0 10].'; 31 | 32 | %now create empty triples for xdot (time derivate of x) and theta 33 | %(orientation of the quadcopter using the Euler angles). 34 | 35 | xdot=zeros(3,1); 36 | theta=zeros(3,1); 37 | 38 | %thetadot denotes the angular velocities which can be set to an initial 39 | %disturbance. 40 | 41 | if nargin==0 42 | thetadot=zeros(3,1); 43 | else 44 | thetadot=deg2rad(randi([-90,90],3,1)); 45 | %thetadot=zeros(3,1); 46 | %thetadot=[0.6;0.6;0.6]; 47 | end 48 | 49 | %CREATE ARRAYS AND STRUCTURED ARRAY FOR THE OUTPUTS 50 | 51 | xout=zeros(3,N); 52 | xdotout=zeros(3,N); 53 | thetaout=zeros(3,N); 54 | thetadotout=zeros(3,N); 55 | inputout=zeros(4,N); 56 | 57 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 58 | %These arrays are to store data that is not critical for controller 59 | %simulation but is needed for secondary graphs (used in controller 60 | %design). 61 | 62 | phi_desout=zeros(1,N); 63 | theta_desout=zeros(1,N); 64 | errors=zeros(3,N); 65 | thrust=zeros(1,N); 66 | integralerrors=zeros(6,N); 67 | risetime=0; 68 | 69 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 70 | 71 | %PARAMETERS FOR THE CONTROLLER 72 | controller_params = struct('dt',dt,'I',I,'k',k,'L',L,'b',b,'m',m,'g',g,... 73 | 'xdes',x_desired,'ydes',y_desired,'zdes',z_desired); 74 | 75 | %STEP THROUGH SIMULATION, UPDATING STATE OF QUADCOPTER 76 | 77 | count=0; 78 | graphendtime=0; 79 | graphendcount=0; 80 | 81 | for t = times 82 | count=count+1; 83 | 84 | controller_params.xdot=xdot; 85 | controller_params.x=x; 86 | 87 | %Here we need to take an input from the controller, or if no 88 | %controller is specified then we can use some default input. The 89 | %inputs are values of gamma (angular velocity^2) for each propeller 90 | 91 | if nargin==0 92 | in=zeros(4,1); 93 | in(1)=700; 94 | in(2)=750; 95 | in(3)=700; 96 | in(4)=750; 97 | in=in.^2; 98 | else 99 | [in,controller_params] = controller(controller_params,thetadot); 100 | end 101 | 102 | %COMPUTE FORCES, TORQUES AND ACCELERATIONS 103 | 104 | %Define angular velocity, omega 105 | omega = thetadot2omega(thetadot,theta); 106 | %Define acceleration, a 107 | a=acceleration(in, theta, xdot, m, g, k, kd); 108 | %Define anglular acceleration, omegadot 109 | omegadot=angular_acceleration(in,omega,I,L,b,k); 110 | 111 | %STORE RISE TIME 112 | if risetime==0 113 | if x(1)>0.95*x_desired && x(1)<1.05*x_desired... 114 | && x(2)>0.95*y_desired && x(2)<1.05*y_desired... 115 | && x(3)>0.95*z_desired && x(3)<1.05*z_desired 116 | risetime=times(count); 117 | end 118 | end 119 | 120 | %STORE GRAPH END TIME 121 | if risetime ~= 0 122 | graphendtime=risetime+10; 123 | if graphendcount==0 124 | if times(count)>graphendtime 125 | graphendcount=count; 126 | end 127 | end 128 | end 129 | 130 | %ADVANCE SYSTEM STATE 131 | 132 | omega = omega + omegadot*dt; 133 | thetadot = omega2thetadot(omega,theta); 134 | theta = theta + thetadot*dt; 135 | xdot = xdot + a*dt; 136 | x = x + xdot*dt; 137 | 138 | %STORE SIMULATION STATE FOR OUTPUT 139 | xout(:,count)=x; 140 | xdotout(:,count)=xdot; 141 | thetaout(:,count)=theta; 142 | thetadotout(:,count)=thetadot; 143 | inputout(:,count)=in; 144 | phi_desout(:,count)=controller_params.phi_des; 145 | theta_desout(:,count)=controller_params.theta_des; 146 | errors(:,count)=controller_params.errors; 147 | thrust(:,count)=controller_params.thrust; 148 | %integralerrors(:,count)=controller_params.integral; 149 | 150 | end 151 | 152 | 153 | %PUT OUTPUT INFORMATION INTO A STRUCTURED ARRAY: 154 | result = struct('x',xout,'theta',thetaout,'vel',xdotout,'angvel'... 155 | ,thetadotout,'t',times, 'dt',dt,'input',inputout,'phides'... 156 | ,phi_desout,'thetades',theta_desout,'errors',errors,'thrust',thrust... 157 | ,'integralerrors',integralerrors,'risetime',risetime... 158 | ,'gecount',graphendcount'); 159 | 160 | 161 | end -------------------------------------------------------------------------------- /v9 (mean times)/omega2thetadot.m: -------------------------------------------------------------------------------- 1 | function thetadot = omega2thetadot(omega,angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | A=zeros(3); 7 | A(1,:)=[ 8 | 1 9 | 0 10 | -sin(theta) 11 | ]; 12 | 13 | A(2,:) =[ 14 | 0 15 | cos(phi) 16 | cos(theta)*sin(phi) 17 | ]; 18 | 19 | A(3,:) =[ 20 | 0 21 | -sin(phi) 22 | cos(phi)*cos(theta) 23 | ]; 24 | 25 | W=inv(A); 26 | thetadot=W*omega; 27 | 28 | %This function converts from angular velocity to the time derivatives 29 | %of roll, pitch and yaw. 30 | end 31 | -------------------------------------------------------------------------------- /v9 (mean times)/rotation.m: -------------------------------------------------------------------------------- 1 | function R = rotation(angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | R=zeros(3); 7 | R(1,:) = [cos(theta)*cos(psi); cos(psi)*sin(phi)*sin(theta)-cos(phi)*sin(psi);sin(phi)*sin(psi)+cos(phi)*cos(psi)*sin(theta)]; 8 | 9 | R(2,:) = [cos(theta)*sin(psi);cos(phi)*cos(psi) + sin(phi)*sin(theta)*sin(psi);cos(phi)*sin(theta)*sin(psi)-cos(psi)*sin(phi)]; 10 | 11 | R(3,:) = [-sin(theta);cos(theta)*sin(phi);cos(phi)*cos(theta)]; 12 | 13 | %This rotation matrix maps between the body-fixed frame and the inertial 14 | %frame. 15 | 16 | end 17 | 18 | -------------------------------------------------------------------------------- /v9 (mean times)/simulate.m: -------------------------------------------------------------------------------- 1 | function result = simulate(controller, x_desired,y_desired,z_desired) 2 | %SET DEFAULT DESIRED COORDINATES 3 | if nargin<2 4 | x_desired=100; 5 | y_desired=100; 6 | z_desired=100; 7 | end 8 | 9 | %SIMULATION TIMES 10 | 11 | start_time = 0; 12 | end_time = 120; 13 | dt = 0.02; 14 | times = start_time:dt:end_time; 15 | N = numel(times); 16 | 17 | %DEFINE PHYSICAL CONSTANTS (FOR A HYPOTHETICAL QUADCOPTER) 18 | 19 | g = 9.81; 20 | m = 0.5; 21 | L = 0.25; 22 | k = 3e-6; 23 | b = 1e-7; 24 | I = diag([5e-3, 5e-3, 10e-3]); 25 | kd = 0.25; 26 | 27 | %INITIAL SIMULATION STATE 28 | 29 | %the x vector triple denotes quadcopter position 30 | x=[0 0 10].'; 31 | 32 | %now create empty triples for xdot (time derivate of x) and theta 33 | %(orientation of the quadcopter using the Euler angles). 34 | 35 | xdot=zeros(3,1); 36 | theta=zeros(3,1); 37 | 38 | %thetadot denotes the angular velocities which can be set to an initial 39 | %disturbance. 40 | 41 | if nargin==0 42 | thetadot=zeros(3,1); 43 | else 44 | % thetadot=deg2rad(randi([-90,90],3,1)); 45 | thetadot=zeros(3,1); 46 | thetadot=[0.6;0.6;0.6]; 47 | end 48 | 49 | %CREATE ARRAYS AND STRUCTURED ARRAY FOR THE OUTPUTS 50 | 51 | xout=zeros(3,N); 52 | xdotout=zeros(3,N); 53 | thetaout=zeros(3,N); 54 | thetadotout=zeros(3,N); 55 | inputout=zeros(4,N); 56 | 57 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 58 | %These arrays are to store data that is not critical for controller 59 | %simulation but is needed for secondary graphs (used in controller 60 | %design). 61 | 62 | phi_desout=zeros(1,N); 63 | theta_desout=zeros(1,N); 64 | errors=zeros(3,N); 65 | thrust=zeros(1,N); 66 | integralerrors=zeros(6,N); 67 | risetime=0; 68 | 69 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 70 | 71 | %PARAMETERS FOR THE CONTROLLER 72 | controller_params = struct('dt',dt,'I',I,'k',k,'L',L,'b',b,'m',m,'g',g,... 73 | 'xdes',x_desired,'ydes',y_desired,'zdes',z_desired); 74 | 75 | %STEP THROUGH SIMULATION, UPDATING STATE OF QUADCOPTER 76 | 77 | count=0; 78 | graphendtime=0; 79 | graphendcount=0; 80 | 81 | for t = times 82 | count=count+1; 83 | 84 | controller_params.xdot=xdot; 85 | controller_params.x=x; 86 | 87 | %Here we need to take an input from the controller, or if no 88 | %controller is specified then we can use some default input. The 89 | %inputs are values of gamma (angular velocity^2) for each propeller 90 | 91 | if nargin==0 92 | in=zeros(4,1); 93 | in(1)=700; 94 | in(2)=750; 95 | in(3)=700; 96 | in(4)=750; 97 | in=in.^2; 98 | else 99 | [in,controller_params] = controller(controller_params,thetadot); 100 | end 101 | 102 | %COMPUTE FORCES, TORQUES AND ACCELERATIONS 103 | 104 | %Define angular velocity, omega 105 | omega = thetadot2omega(thetadot,theta); 106 | %Define acceleration, a 107 | a=acceleration(in, theta, xdot, m, g, k, kd); 108 | %Define anglular acceleration, omegadot 109 | omegadot=angular_acceleration(in,omega,I,L,b,k); 110 | 111 | %STORE RISE TIME 112 | if risetime==0 113 | if x(1)>0.95*x_desired && x(1)<1.05*x_desired... 114 | && x(2)>0.95*y_desired && x(2)<1.05*y_desired... 115 | && x(3)>0.95*z_desired && x(3)<1.05*z_desired 116 | risetime=times(count); 117 | end 118 | end 119 | 120 | %STORE GRAPH END TIME 121 | if risetime ~= 0 122 | graphendtime=risetime+10; 123 | if graphendcount==0 124 | if times(count)>graphendtime 125 | graphendcount=count; 126 | end 127 | end 128 | end 129 | 130 | %ADVANCE SYSTEM STATE 131 | 132 | omega = omega + omegadot*dt; 133 | thetadot = omega2thetadot(omega,theta); 134 | theta = theta + thetadot*dt; 135 | xdot = xdot + a*dt; 136 | x = x + xdot*dt; 137 | 138 | %STORE SIMULATION STATE FOR OUTPUT 139 | xout(:,count)=x; 140 | xdotout(:,count)=xdot; 141 | thetaout(:,count)=theta; 142 | thetadotout(:,count)=thetadot; 143 | inputout(:,count)=in; 144 | phi_desout(:,count)=controller_params.phi_des; 145 | theta_desout(:,count)=controller_params.theta_des; 146 | errors(:,count)=controller_params.errors; 147 | thrust(:,count)=controller_params.thrust; 148 | %integralerrors(:,count)=controller_params.integral; 149 | 150 | end 151 | 152 | 153 | %PUT OUTPUT INFORMATION INTO A STRUCTURED ARRAY: 154 | result = struct('x',xout,'theta',thetaout,'vel',xdotout,'angvel'... 155 | ,thetadotout,'t',times, 'dt',dt,'input',inputout,'phides'... 156 | ,phi_desout,'thetades',theta_desout,'errors',errors,'thrust',thrust... 157 | ,'integralerrors',integralerrors,'risetime',risetime... 158 | ,'gecount',graphendcount'); 159 | 160 | %PLOT RESULTS 161 | grapher(result) 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | end -------------------------------------------------------------------------------- /v9 (mean times)/simulate2.m: -------------------------------------------------------------------------------- 1 | function result = simulate2(controller, x_desired,y_desired,z_desired) 2 | if nargin<2 3 | x_desired=100; 4 | y_desired=100; 5 | z_desired=100; 6 | end 7 | result=nographsimulate(controller, x_desired,y_desired,z_desired); 8 | grapher(result) 9 | end -------------------------------------------------------------------------------- /v9 (mean times)/thetadot2omega.m: -------------------------------------------------------------------------------- 1 | function omega = thetadot2omega(thetadot,angles) 2 | phi=angles(1); 3 | theta=angles(2); 4 | psi=angles(3); 5 | 6 | A=zeros(3); 7 | A(1,:)=[ 8 | 1 9 | 0 10 | -sin(theta) 11 | ]; 12 | A(2,:) =[ 13 | 0 14 | cos(phi) 15 | cos(theta)*sin(phi) 16 | ]; 17 | A(3,:) =[ 18 | 0 19 | -sin(phi) 20 | cos(phi)*cos(theta) 21 | ]; 22 | 23 | omega=A*thetadot; 24 | 25 | 26 | %This function converts time derivatives of roll, pitch and yaw into 27 | %angular velocity. A is the necessary rotation matrix for this operation. 28 | %We can define the reverse function omega2thetadot using the inverse of 29 | %this matrix if desired. 30 | 31 | end 32 | 33 | -------------------------------------------------------------------------------- /v9 (mean times)/totalgrossresult.m: -------------------------------------------------------------------------------- 1 | function meantimes = totalgrossresult(controller) 2 | series=[]; 3 | n=1; 4 | for i = 1:9 5 | n=n*2; 6 | series(i)=n; 7 | end 8 | 9 | meantimes=[]; 10 | 11 | count=0; 12 | for j = series 13 | count=count+1; 14 | if strcmpi(controller,'PID')==1; 15 | meantimes(count)=grossresult('PID',j); 16 | end 17 | if strcmpi(controller,'PD')==1; 18 | meantimes(count)=grossresult('PD',j); 19 | end 20 | end 21 | 22 | figure 23 | plot(series,meantimes, 'r', 'Linewidth',1.2) 24 | titlestr=strcat({'Mean rise time against number of simulations for a '}... 25 | ,controller,{' controller'}); 26 | title(titlestr) 27 | xlabel('Number of simulations') 28 | ylabel('Mean rise time') -------------------------------------------------------------------------------- /v9 (mean times)/totalmeanresult.m: -------------------------------------------------------------------------------- 1 | function meantimes = totalmeanresult(controller) 2 | series=[]; 3 | n=512; 4 | for i = 1:3 5 | n=n*2; 6 | series(i)=n; 7 | end 8 | 9 | meantimes=[]; 10 | 11 | count=0; 12 | for j = series 13 | count=count+1; 14 | if strcmpi(controller,'PID')==1; 15 | meantimes(count)=meanresult('PID',j); 16 | end 17 | if strcmpi(controller,'PD')==1; 18 | meantimes(count)=meanresult('PD',j); 19 | end 20 | end 21 | 22 | figure 23 | plot(series,meantimes, 'r', 'Linewidth',1.2) 24 | titlestr=strcat({'Mean rise time against number of simulations for a '}... 25 | ,controller,{' controller'}); 26 | title(titlestr) 27 | xlabel('Number of simulations') 28 | ylabel('Mean rise time') 29 | 30 | 31 | --------------------------------------------------------------------------------