sravan/Quadcopter-Control
1
0
Fork 0
mirror of https://github.com/ME-561-W20-Quadcopter-Project/Quadcopter-Control.git synced 2025-09-02 21:53:13 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add nonlinear system

Browse Source
This commit is contained in:
Lakshu Periakaruppan
2020-04-20 21:02:53 -05:00
parent 80c686c517
commit bf612c0dfc
6 changed files with 125 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
.gitignore vendored
View File

@@ -1,4 +1,4 @@
*.slxc *.slxc
*.slx.autosave *.slx.autosave
*.asv *.asv
slprj slprj

42
LICENSE
View File

@@ -1,21 +1,21 @@
MIT License MIT License
Copyright (c) 2020 ME-561-W20-Quadcopter-Project Copyright (c) 2020 ME-561-W20-Quadcopter-Project
Permission is hereby granted, free of charge, to any person obtaining a copy Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions: furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software. copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 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 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE. SOFTWARE.

50
docs/home.md
View File

@@ -1,25 +1,25 @@
# Full State Feedback and Control of a Quadcopter Drone <!-- omit in toc --> # Full State Feedback and Control of a Quadcopter Drone <!-- omit in toc -->
## Table of Contents <!-- omit in toc --> ## Table of Contents <!-- omit in toc -->
- [Contributors](#contributors) - [Contributors](#contributors)
- [Documents](#documents) - [Documents](#documents)
## Contributors ## Contributors
### Project Team <!-- omit in toc --> ### Project Team <!-- omit in toc -->
- Sravan Balaji ([balajsra@umich.edu](mailto:balajsra@umich.edu)) - Sravan Balaji ([balajsra@umich.edu](mailto:balajsra@umich.edu))
- Aditya Iyer ([adiyer@umich.edu](mailto:adiyer@umich.edu)) - Aditya Iyer ([adiyer@umich.edu](mailto:adiyer@umich.edu))
- Lakshmanan Periakaruppan ([lperiaka@umich.edu](mailto:lperiaka@umich.edu)) - Lakshmanan Periakaruppan ([lperiaka@umich.edu](mailto:lperiaka@umich.edu))
- Naman Shah ([namanvs@umich.edu](mailto:namanvs@umich.edu)) - Naman Shah ([namanvs@umich.edu](mailto:namanvs@umich.edu))
- Sumedh Vaishampayan ([sumi@umich.edu](mailto:sumi@umich.edu)) - Sumedh Vaishampayan ([sumi@umich.edu](mailto:sumi@umich.edu))
### EECS / MECHENG 561 W20 Course Staff <!-- omit in toc --> ### EECS / MECHENG 561 W20 Course Staff <!-- omit in toc -->
- Ram Vasudevan ([ramv@umich.edu](mailto:ramv@umich.edu)) - Ram Vasudevan ([ramv@umich.edu](mailto:ramv@umich.edu))
- Sid Dey ([siddey@umich.edu](mailto:siddey@umich.edu)) - Sid Dey ([siddey@umich.edu](mailto:siddey@umich.edu))
## Documents ## Documents
1. [Project Proposal](1.%20ME%20561%20Project%20Proposal.pdf) 1. [Project Proposal](1.%20ME%20561%20Project%20Proposal.pdf)
2. [Final Report - Overleaf (Read-Only)](https://www.overleaf.com/read/kyjvdsxkfnmg) 2. [Final Report - Overleaf (Read-Only)](https://www.overleaf.com/read/kyjvdsxkfnmg)

89
src/Naman_LQR_Working.m
View File

@@ -83,18 +83,18 @@ x_0_up = [0, 0, -1, ...
0, 0, 0]'; %Redefine origin! 0, 0, 0]'; %Redefine origin!
%Goal 2: Stabilize from a 10-degree roll and pitch with <3deg overshoot %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, ... 0, 0, 0, ...
10, 0, 0, ... 10*(pi/180), 10*(pi/180), 0, ...
0, 0, 0]'; %Pitch of 10 degrees 0, 0, 0]'; %Pitch and roll of 10 degrees
x_0_roll = [0, 0, 0, ... x_0_roll = [0, 0, 0, ...
0, 0, 0, ... 0, 0, 0, ...
0, 10, 0, ... 0, 10*(pi/180), 0, ...
0, 0, 0]'; %Roll of 10 degrees 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. %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, ... 0, 0, 0, ...
0, 0, 0]'; %Redefine origin! 0, 0, 0]'; %Redefine origin!
@@ -114,6 +114,8 @@ nSteps = length(tSpan);
%Determine gains %Determine gains
[K, P] = LQR_LTI(discrete.A, discrete.B, Q, R, nSteps); [K, P] = LQR_LTI(discrete.A, discrete.B, Q, R, nSteps);
FiniteLQR_Goal_1_K = K;
save('FiniteLQRGoal_1_K.mat', 'K');
%Propagate %Propagate
[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_up, K, discrete.A, discrete.B); [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; xlqr(3,:) = xlqr(3,:) + 1;
%Plot %Plot
plot_states(xlqr, tSpan); 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 %% 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. %Calculate number of timesteps.
tSpan = 0:T_s:2; tSpan = 0:T_s:4;
nSteps = length(tSpan); nSteps = length(tSpan);
%Determine gains %Determine gains
@@ -136,22 +157,40 @@ nSteps = length(tSpan);
%Pitch Goal %Pitch Goal
%Propagate %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
plot_states(xlqr, tSpan); plot_states(xlqr, tSpan);
yd = diff(xlqr(5,:))./T_s yd = diff(xlqr(5,:))./T_s
pd = diff(xlqr(7,:))./T_s pd = diff(xlqr(7,:))./T_s
%Propagate %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
plot_states(xlqr, tSpan); %plot_states(xlqr, tSpan);
xd = diff(xlqr(4,:))./T_s %xd = diff(xlqr(4,:))./T_s
rd = diff(xlqr(8,:))./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 %% 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. %Calculate number of timesteps.
tSpan = 0:T_s:5; tSpan = 0:T_s:5;
nSteps = length(tSpan); nSteps = length(tSpan);
@@ -162,11 +201,25 @@ nSteps = length(tSpan);
%Pitch Goal %Pitch Goal
%Propagate %Propagate
[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_trans, K, discrete.A, discrete.B); [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
plot_states(xlqr, tSpan); 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 %% Helper Functions
function [K, P] = LQR_LTI(A, B, Q, R, nSteps) 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'); legend('Pitch (about x)', 'Roll (about y)', 'Yaw (about z)', 'Pitch Rate', 'Roll Rate', 'Yaw Rate');
title("Angular Displacements(-) and Velocities(--)"); title("Angular Displacements(-) and Velocities(--)");
xlabel("Time(s)"); xlabel("Time(s)");
ylabel("Displacement (deg)"); ylabel("Displacement (rad)");
end 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

5
src/PlantModel.m
View File

@@ -79,7 +79,6 @@ kdps = 0.0486;
kpz = 4.0670; kpz = 4.0670;
kdz = 2.9031; kdz = 2.9031;
Q = zeros(12); Q(7:9, 7:9) = eye(3); %% Load LQR gains
R = 1; LQRGoal1 = matfile('FiniteLQRGoal_1_K.mat');
%K = lqr(A,B,Q,R);

BIN
src/PlantModelSim.slx
View File

Binary file not shown.