diff --git a/.gitignore b/.gitignore index 926ed9f..a854fba 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,4 @@ -*.slxc -*.slx.autosave -*.asv -slprj +*.slxc +*.slx.autosave +*.asv +slprj diff --git a/LICENSE b/LICENSE index f30218f..d8b0106 100644 --- a/LICENSE +++ b/LICENSE @@ -1,21 +1,21 @@ -MIT License - -Copyright (c) 2020 ME-561-W20-Quadcopter-Project - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. +MIT License + +Copyright (c) 2020 ME-561-W20-Quadcopter-Project + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/docs/home.md b/docs/home.md index 993c163..9970bf5 100644 --- a/docs/home.md +++ b/docs/home.md @@ -1,25 +1,25 @@ -# Full State Feedback and Control of a Quadcopter Drone - -## Table of Contents -- [Contributors](#contributors) -- [Documents](#documents) - -## Contributors - -### Project Team - -- Sravan Balaji ([balajsra@umich.edu](mailto:balajsra@umich.edu)) -- Aditya Iyer ([adiyer@umich.edu](mailto:adiyer@umich.edu)) -- Lakshmanan Periakaruppan ([lperiaka@umich.edu](mailto:lperiaka@umich.edu)) -- Naman Shah ([namanvs@umich.edu](mailto:namanvs@umich.edu)) -- Sumedh Vaishampayan ([sumi@umich.edu](mailto:sumi@umich.edu)) - -### EECS / MECHENG 561 W20 Course Staff - -- Ram Vasudevan ([ramv@umich.edu](mailto:ramv@umich.edu)) -- Sid Dey ([siddey@umich.edu](mailto:siddey@umich.edu)) - -## Documents - -1. [Project Proposal](1.%20ME%20561%20Project%20Proposal.pdf) -2. [Final Report - Overleaf (Read-Only)](https://www.overleaf.com/read/kyjvdsxkfnmg) +# Full State Feedback and Control of a Quadcopter Drone + +## Table of Contents +- [Contributors](#contributors) +- [Documents](#documents) + +## Contributors + +### Project Team + +- Sravan Balaji ([balajsra@umich.edu](mailto:balajsra@umich.edu)) +- Aditya Iyer ([adiyer@umich.edu](mailto:adiyer@umich.edu)) +- Lakshmanan Periakaruppan ([lperiaka@umich.edu](mailto:lperiaka@umich.edu)) +- Naman Shah ([namanvs@umich.edu](mailto:namanvs@umich.edu)) +- Sumedh Vaishampayan ([sumi@umich.edu](mailto:sumi@umich.edu)) + +### EECS / MECHENG 561 W20 Course Staff + +- Ram Vasudevan ([ramv@umich.edu](mailto:ramv@umich.edu)) +- Sid Dey ([siddey@umich.edu](mailto:siddey@umich.edu)) + +## Documents + +1. [Project Proposal](1.%20ME%20561%20Project%20Proposal.pdf) +2. [Final Report - Overleaf (Read-Only)](https://www.overleaf.com/read/kyjvdsxkfnmg) diff --git a/src/Naman_LQR_Working.m b/src/Naman_LQR_Working.m index e7d439e..34baaa8 100644 --- a/src/Naman_LQR_Working.m +++ b/src/Naman_LQR_Working.m @@ -83,18 +83,18 @@ x_0_up = [0, 0, -1, ... 0, 0, 0]'; %Redefine origin! %Goal 2: Stabilize from a 10-degree roll and pitch with <3deg overshoot -x_0_pitch = [0, 0, 0, ... +x_0_pitchroll = [0, 0, 0, ... 0, 0, 0, ... - 10, 0, 0, ... - 0, 0, 0]'; %Pitch of 10 degrees + 10*(pi/180), 10*(pi/180), 0, ... + 0, 0, 0]'; %Pitch and roll of 10 degrees x_0_roll = [0, 0, 0, ... 0, 0, 0, ... - 0, 10, 0, ... + 0, 10*(pi/180), 0, ... 0, 0, 0]'; %Roll of 10 degrees %Goal 3: Move from position (0,0,0) to within 5 cm of (1,1,1) within 5 seconds. -x_0_trans = [-1, -1, -1, ... +x_0_trans = [-1, -1, 0, ... 0, 0, 0, ... 0, 0, 0, ... 0, 0, 0]'; %Redefine origin! @@ -114,6 +114,8 @@ nSteps = length(tSpan); %Determine gains [K, P] = LQR_LTI(discrete.A, discrete.B, Q, R, nSteps); +FiniteLQR_Goal_1_K = K; +save('FiniteLQRGoal_1_K.mat', 'K'); %Propagate [ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_up, K, discrete.A, discrete.B); @@ -122,12 +124,31 @@ nSteps = length(tSpan); xlqr(3,:) = xlqr(3,:) + 1; %Plot plot_states(xlqr, tSpan); -zd = diff(xlqr(6,:))./T_s +zd = diff(xlqr(6,:))./T_s; + +%Simulate nonlinear model +[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_up); +set_param('LQRNonlinearSim', 'StopTime', '2') +simout = sim('LQRNonlinearSim', 'FixedStep', '.01'); + +time = simout.allVals.Time(:,1); +figure(); +plot(time, simout.z+1, 'LineStyle', '--', 'color',[0 0.5 0], 'LineWidth', 2); hold on; +plot(time, xlqr(3,:), '-b','LineWidth', 1); +xlabel('Time (s)'); +ylabel('Z (m)'); +legend({'Nonlinear Model', 'Linear Model'}); + + %% Finite-Time Horizon LQR for Goal 2 +Q = diag([0, 0, 0, ... % x, y, z + 0, 0, 0, ... % x', y', z' + 200, 200, 1, ... % roll, pitch, yaw + 10, 10, 1]); % roll', pitch', yaw' %Calculate number of timesteps. -tSpan = 0:T_s:2; +tSpan = 0:T_s:4; nSteps = length(tSpan); %Determine gains @@ -136,22 +157,40 @@ nSteps = length(tSpan); %Pitch Goal %Propagate -[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_pitch, K, discrete.A, discrete.B); +[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_pitchroll, K, discrete.A, discrete.B); %Plot plot_states(xlqr, tSpan); yd = diff(xlqr(5,:))./T_s pd = diff(xlqr(7,:))./T_s + %Propagate -[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_roll, K, discrete.A, discrete.B); +%[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_roll, K, discrete.A, discrete.B); %Plot -plot_states(xlqr, tSpan); -xd = diff(xlqr(4,:))./T_s -rd = diff(xlqr(8,:))./T_s +%plot_states(xlqr, tSpan); +%xd = diff(xlqr(4,:))./T_s +%rd = diff(xlqr(8,:))./T_s + +%Simulate nonlinear model +[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_pitchroll); +set_param('LQRNonlinearSim', 'StopTime', '4') +simout = sim('LQRNonlinearSim', 'FixedStep', '.01'); + +time = simout.allVals.Time(:,1); +figure(); +plot(time, simout.roll, 'LineStyle', '--', 'color',[0 0.5 0], 'LineWidth', 2); hold on; +plot(time, xlqr(8,:), '-b','LineWidth', 1); +%plot(time, simout.roll, 'LineStyle', '--', 'color',[0 0.5 0], 'LineWidth', 2); hold on; +xlabel('Time (s)'); +ylabel('Pitch/Roll Angle (rad)'); +legend({'Nonlinear Model', 'Linear Model'}); %% Finite-Time Horizon For Goal 3 - +Q = diag([1000, 1000, 0, ... % x, y, z + 0, 0, 0, ... % x', y', z' + 200, 200, 1, ... % roll, pitch, yaw + 0, 0, 1]); % roll', pitch', yaw' %Calculate number of timesteps. tSpan = 0:T_s:5; nSteps = length(tSpan); @@ -162,11 +201,25 @@ nSteps = length(tSpan); %Pitch Goal %Propagate [ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_trans, K, discrete.A, discrete.B); -xlqr(1:3,:) = xlqr(1:3,:) + 1; +xlqr(1:2,:) = xlqr(1:2,:) + 1; %Plot plot_states(xlqr, tSpan); +%Simulate nonlinear model +[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_trans); +set_param('LQRNonlinearSim', 'StopTime', '5') +simout = sim('LQRNonlinearSim', 'FixedStep', '.01'); + +time = simout.allVals.Time(:,1); +figure(); +plot(time, simout.x+1, 'LineStyle', '--', 'color',[0 0.5 0], 'LineWidth', 2); hold on; +plot(time, xlqr(1,:), '-b','LineWidth', 1); +%plot(time, simout.roll, 'LineStyle', '--', 'color',[0 0.5 0], 'LineWidth', 2); hold on; +xlabel('Time (s)'); +ylabel('x (m)'); +legend({'Nonlinear Model', 'Linear Model'}); + %% Helper Functions function [K, P] = LQR_LTI(A, B, Q, R, nSteps) @@ -223,5 +276,9 @@ function plot_states(xlqr, tSpan) legend('Pitch (about x)', 'Roll (about y)', 'Yaw (about z)', 'Pitch Rate', 'Roll Rate', 'Yaw Rate'); title("Angular Displacements(-) and Velocities(--)"); xlabel("Time(s)"); - ylabel("Displacement (deg)"); -end \ No newline at end of file + ylabel("Displacement (rad)"); +end + +function [x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(X0) +x0=X0(1); y0=X0(2); z0=X0(3); xdot0=X0(4); ydot0=X0(5); zdot0=X0(6); phi0=X0(7); theta0=X0(8); psi0=X0(9); phidot0=X0(10); thetadot0=X0(11); psidot0=X0(12); +end diff --git a/src/PlantModel.m b/src/PlantModel.m index ae22b4c..a798175 100644 --- a/src/PlantModel.m +++ b/src/PlantModel.m @@ -79,7 +79,6 @@ kdps = 0.0486; kpz = 4.0670; kdz = 2.9031; -Q = zeros(12); Q(7:9, 7:9) = eye(3); -R = 1; -%K = lqr(A,B,Q,R); +%% Load LQR gains +LQRGoal1 = matfile('FiniteLQRGoal_1_K.mat'); diff --git a/src/PlantModelSim.slx b/src/PlantModelSim.slx index dba551d..8e78b03 100644 Binary files a/src/PlantModelSim.slx and b/src/PlantModelSim.slx differ