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

Code Cleanup & Project Name Update

Browse Source 
- Cleanup LQR.m and add whitespaces
- Change project name in README and home
- Add Source Code section to home.md describing each file
This commit is contained in:
Sravan Balaji
2020-04-21 02:00:05 -04:00
parent 81426af0ce
commit 691da16e93
3 changed files with 64 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
README.md
View File

@@ -2,7 +2,7 @@
UMICH EECS / MECHENG 561: Design of Digital Control Systems WN 2020 UMICH EECS / MECHENG 561: Design of Digital Control Systems WN 2020
Full State Feedback and Control of a Quadcopter Drone A Comparison of Quadcopter Drone Control Methods
## Documentation ## Documentation

21
docs/home.md
View File

@@ -1,8 +1,9 @@
# Full State Feedback and Control of a Quadcopter Drone <!-- omit in toc --> # A Comparison of Quadcopter Drone Control Methods <!-- omit in toc -->
## Table of Contents <!-- omit in toc --> ## Table of Contents <!-- omit in toc -->
- [Contributors](#contributors) - [Contributors](#contributors)
- [Documents](#documents) - [Documents](#documents)
- [Source Code](#source-code)
## Contributors ## Contributors
@@ -23,3 +24,21 @@
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)
## Source Code
### [LQR.m](../src/LQR.m) <!-- omit in toc -->
Finite and infinite time horizon LQR implementation in MATLAB. Gains determined for linearized discrete time system, then simulated on nonlinear system (see LQRNonlinearSim.slx below).
### [LQRNonlinearSim.slx](../src/LQRNonlinearSim.slx) <!-- omit in toc -->
Simulink nonlinear model that is run from within LQR.m. Takes gain matrix **K** and initial condition **x_0** as input.
### [PlantModel.m](../src/PlantModel.m) <!-- omit in toc -->
Parameters for PID controller (see PlantModelSim.slx below).
### [PlantModelSim.slx](../src/PlantModelSim.slx) <!-- omit in toc -->
PID control of nonlinear and linear system.

61
src/LQR.m
View File

@@ -1,9 +1,14 @@
% Clear workspace % Clear workspace
clear all; close all; clc; clear all;
close all;
clc;
% Parameters source: https://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf % Parameters source: https://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf
g = 9.81; m = 0.468; Ix = 4.856*10^-3; g = 9.81;
Iy = 4.856*10^-3; Iz = 8.801*10^-3; m = 0.468;
Ix = 4.856*10^-3;
Iy = 4.856*10^-3;
Iz = 8.801*10^-3;
% States: % States:
% X1: x X4: x' % X1: x X4: x'
@@ -69,31 +74,31 @@ continuous = ss(A, B, C, D);
T_s = 0.01; T_s = 0.01;
discrete = c2d(continuous, T_s); discrete = c2d(continuous, T_s);
%Check if this works % Check if this works
impulse(discrete, 0:T_s:1); impulse(discrete, 0:T_s:1);
%We should see that U1 gets us only translation in z, U2 couples Y2 and Y4, % We should see that U1 gets us only translation in z, U2 couples Y2 and Y4,
%U3 couples Y1 and Y5, and U4 gets us Y6 % U3 couples Y1 and Y5, and U4 gets us Y6
%% Define goals %% Define goals
%Goal 1: settle at 1m height <2s % Goal 1: settle at 1m height <2s
x_0_up = [0, 0, -1, ... x_0_up = [0, 0, -1, ...
0, 0, 0, ... 0, 0, 0, ...
0, 0, 0, ... 0, 0, 0, ...
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_pitchroll = [0, 0, 0, ... x_0_pitchroll = [0, 0, 0, ...
0, 0, 0, ... 0, 0, 0, ...
10*(pi/180), 10*(pi/180), 0, ... 10*(pi/180), 10*(pi/180), 0, ...
0, 0, 0]'; %Pitch and roll of 10 degrees 0, 0, 0]'; %Pitch and roll of 10 degrees (convert to radians)
x_0_roll = [0, 0, 0, ... x_0_roll = [0, 0, 0, ...
0, 0, 0, ... 0, 0, 0, ...
0, 10*(pi/180), 0, ... 0, 10*(pi/180), 0, ...
0, 0, 0]'; %Roll of 10 degrees 0, 0, 0]'; %Roll of 10 degrees (convert to radians)
%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, 0, ... x_0_trans = [-1, -1, 0, ...
0, 0, 0, ... 0, 0, 0, ...
0, 0, 0, ... 0, 0, 0, ...
@@ -101,7 +106,7 @@ x_0_trans = [-1, -1, 0, ...
%% Finite-Time Horizon LQR for Goal 1 %% Finite-Time Horizon LQR for Goal 1
%Define Q and R for the cost function. Begin with nominal ones for all. % Define Q and R for the cost function. Begin with nominal ones for all.
Q = diag([1000, 1000, 1000, ... % x, y, z Q = diag([1000, 1000, 1000, ... % x, y, z
1, 1, 100, ... % x', y', z' 1, 1, 100, ... % x', y', z'
200, 200, 1, ... % roll, pitch, yaw 200, 200, 1, ... % roll, pitch, yaw
@@ -109,15 +114,15 @@ Q = diag([1000, 1000, 1000, ... % x, y, z
R = diag([10, 20, 20, 1]); % upward force, pitch torque, roll torque, yaw torque R = diag([10, 20, 20, 1]); % upward force, pitch torque, roll torque, yaw torque
%Calculate number of timesteps. % Calculate number of timesteps.
tSpan = 0:T_s:2; tSpan = 0:T_s:2;
nSteps = length(tSpan); 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; FiniteLQR_Goal_1_K = K;
%Simulate nonlinear model % Simulate nonlinear model
[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_up); [x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_up);
set_param('LQRNonlinearSim', 'StopTime', '2') set_param('LQRNonlinearSim', 'StopTime', '2')
simout = sim('LQRNonlinearSim', 'FixedStep', '.01'); simout = sim('LQRNonlinearSim', 'FixedStep', '.01');
@@ -158,7 +163,7 @@ for i = 1:nSteps
K(:,:,i) = K_const; K(:,:,i) = K_const;
end end
%Simulate nonlinear model % Simulate nonlinear model
[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_up); [x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_up);
set_param('LQRNonlinearSim', 'StopTime', '2') set_param('LQRNonlinearSim', 'StopTime', '2')
simout = sim('LQRNonlinearSim', 'FixedStep', '.01'); simout = sim('LQRNonlinearSim', 'FixedStep', '.01');
@@ -186,14 +191,14 @@ Q = diag([0, 0, 0, ... % x, y, z
R = diag([10, 20, 20, 1]); % upward force, pitch torque, roll torque, yaw torque R = diag([10, 20, 20, 1]); % upward force, pitch torque, roll torque, yaw torque
%Calculate number of timesteps. % Calculate number of timesteps.
tSpan = 0:T_s:4; tSpan = 0:T_s:4;
nSteps = length(tSpan); 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);
%Simulate nonlinear model % Simulate nonlinear model
[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_pitchroll); [x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_pitchroll);
set_param('LQRNonlinearSim', 'StopTime', '4') set_param('LQRNonlinearSim', 'StopTime', '4')
simout = sim('LQRNonlinearSim', 'FixedStep', '.01'); simout = sim('LQRNonlinearSim', 'FixedStep', '.01');
@@ -234,7 +239,7 @@ for i = 1:nSteps
K(:,:,i) = K_const; K(:,:,i) = K_const;
end end
%Simulate nonlinear model % Simulate nonlinear model
[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_pitchroll); [x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_pitchroll);
set_param('LQRNonlinearSim', 'StopTime', '4') set_param('LQRNonlinearSim', 'StopTime', '4')
simout = sim('LQRNonlinearSim', 'FixedStep', '.01'); simout = sim('LQRNonlinearSim', 'FixedStep', '.01');
@@ -262,14 +267,14 @@ Q = diag([1000, 1000, 1000, ... % x, y, z
R = diag([1, 20, 20, 1]); % upward force, pitch torque, roll torque, yaw torque R = diag([1, 20, 20, 1]); % upward force, pitch torque, roll torque, yaw torque
%Calculate number of timesteps. % Calculate number of timesteps.
tSpan = 0:T_s:5; tSpan = 0:T_s:5;
nSteps = length(tSpan); 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);
%Simulate nonlinear model % Simulate nonlinear model
[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_trans); [x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_trans);
set_param('LQRNonlinearSim', 'StopTime', '5') set_param('LQRNonlinearSim', 'StopTime', '5')
simout = sim('LQRNonlinearSim', 'FixedStep', '.01'); simout = sim('LQRNonlinearSim', 'FixedStep', '.01');
@@ -310,7 +315,7 @@ for i = 1:nSteps
K(:,:,i) = K_const; K(:,:,i) = K_const;
end end
%Simulate nonlinear model % Simulate nonlinear model
[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_trans); [x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_trans);
set_param('LQRNonlinearSim', 'StopTime', '5') set_param('LQRNonlinearSim', 'StopTime', '5')
simout = sim('LQRNonlinearSim', 'FixedStep', '.01'); simout = sim('LQRNonlinearSim', 'FixedStep', '.01');
@@ -332,11 +337,11 @@ plot_states(state, tSpan);
%% Helper Functions %% Helper Functions
function [K, P] = LQR_LTI(A, B, Q, R, nSteps) function [K, P] = LQR_LTI(A, B, Q, R, nSteps)
%Set P up % Set P up
P = zeros(size(Q, 1), size(Q, 2), nSteps); P = zeros(size(Q, 1), size(Q, 2), nSteps);
%Initial value of P % Initial value of P
P(:, :, nSteps) = 1/2 * Q; P(:, :, nSteps) = 1/2 * Q;
%Set K up, initial K is 0, so this is fine. % Set K up, initial K is 0, so this is fine.
K = zeros(length(R), length(Q), nSteps); K = zeros(length(R), length(Q), nSteps);
for i = nSteps-1:-1:1 for i = nSteps-1:-1:1
@@ -348,7 +353,7 @@ function [K, P] = LQR_LTI(A, B, Q, R, nSteps)
end end
function [ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0, K, A, B) function [ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0, K, A, B)
%Set up for propagation % Set up for propagation
ulqr = zeros(nInputs, nSteps); ulqr = zeros(nInputs, nSteps);
xlqr = zeros(nStates, nSteps); xlqr = zeros(nStates, nSteps);
xlqr(:, 1) = x_0; xlqr(:, 1) = x_0;