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

Implement Infinite Horizon LQR

Browse Source 
- Code and comment formatting cleanup
- Add additional comments for clarity
- Add cost matrices to each section
- Use idare to solve for infinite horizon gains
This commit is contained in:
Sravan Balaji
2020-04-20 01:18:40 -04:00
parent 5252eeb140
commit 2943d7a4f8
1 changed files with 172 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

144
src/LQR.m
View File

@@ -1,9 +1,14 @@
% Clear workspace
clear all; close all; clc;
clear all;
close all;
clc;
% Parameters source: https://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf
g = 9.81; m = 0.468; Ix = 4.856*10^-3;
Iy = 4.856*10^-3; Iz = 8.801*10^-3;
g = 9.81;
m = 0.468;
Ix = 4.856*10^-3;
Iy = 4.856*10^-3;
Iz = 8.801*10^-3;
% States:
% X1: x X4: x'
@@ -77,10 +82,14 @@ impulse(discrete, 0:T_s:1);
%% Define goals
% Goal 1: settle at 1m height <2s
% LQR drives states to 0, so we redefine initial
% condition to be -1 in z direction such that
% controller gives a positive z input as if
% quadcopter drives from origin up 1 m in z direction
x_0_up = [0, 0, -1, ...
0, 0, 0, ...
0, 0, 0, ...
0, 0, 0]'; %Redefine origin!
0, 0, 0]';
% Goal 2: Stabilize from a 10-degree roll and pitch with <3deg overshoot
x_0_pitch = [0, 0, 0, ...
@@ -94,19 +103,23 @@ x_0_roll = [0, 0, 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.
% Redefine initial condition to be -1 in x, y, and z direction so
% when LQR drives states to 0, it is as if quadcopter drives from
% origin to (1,1,1)
x_0_trans = [-1, -1, -1, ...
0, 0, 0, ...
0, 0, 0, ...
0, 0, 0]'; %Redefine origin!
0, 0, 0]';
%Define Q and R for the cost function. Begin with nominal ones for all.
%% Finite-Time Horizon LQR for Goal 1
% Cost matrices
Q = diag([1000, 1000, 1000, ... % x, y, z
1, 1, 100, ... % x', y', z'
200, 200, 1, ... % roll, pitch, yaw
1, 1, 1]); % roll', pitch', yaw'
R = diag([10, 20, 20, 1]); % upward force, pitch torque, roll torque, yaw torque
%% Finite-Time Horizon LQR for Goal 1
% Calculate number of timesteps.
tSpan = 0:T_s:2;
@@ -124,8 +137,37 @@ xlqr(3,:) = xlqr(3,:) + 1;
plot_states(xlqr, tSpan);
zd = diff(xlqr(6,:))./T_s
%% Infinite-Time Horizon LQR for Goal 1
% Cost matrices
Q = diag([1, 1, 1000, ... % x, y, z
1, 1, 100, ... % x', y', z'
1, 1, 1, ... % roll, pitch, yaw
1, 1, 1]); % roll', pitch', yaw'
R = diag([10, 20, 20, 1]); % upward force, pitch torque, roll torque, yaw torque
% Calculate number of timesteps
tSpan = 0:T_s:2;
nSteps = length(tSpan);
% Determine Gains
[X, K, L, info] = idare(discrete.A, discrete.B, Q, R, [], []);
[ulqr, xlqr] = propagate_inf(nInputs, nStates, nSteps, x_0_up, K, discrete.A, discrete.B);
xlqr(3,:) = xlqr(3,:) + 1;
plot_states(xlqr, tSpan);
zd = diff(xlqr(6,:))./T_s;
%% Finite-Time Horizon LQR for Goal 2
% Cost matrices
Q = diag([1000, 1000, 1000, ... % x, y, z
1, 1, 100, ... % x', y', z'
200, 200, 1, ... % roll, pitch, yaw
1, 1, 1]); % roll', pitch', yaw'
R = diag([10, 20, 20, 1]); % upward force, pitch torque, roll torque, yaw torque
% Calculate number of timesteps.
tSpan = 0:T_s:2;
nSteps = length(tSpan);
@@ -133,25 +175,66 @@ nSteps = length(tSpan);
% Determine gains
[K, P] = LQR_LTI(discrete.A, discrete.B, Q, R, nSteps);
% Pitch Goal
% Propagate
[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_pitch, K, discrete.A, discrete.B);
% Plot
plot_states(xlqr, tSpan);
yd = diff(xlqr(5,:))./T_s
pd = diff(xlqr(7,:))./T_s
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);
% Plot
plot_states(xlqr, tSpan);
xd = diff(xlqr(4,:))./T_s
rd = diff(xlqr(8,:))./T_s
xd = diff(xlqr(4,:))./T_s;
rd = diff(xlqr(8,:))./T_s;
%% Infinite-Time Horizon LQR for Goal 2
% Cost matrices
Q = diag([1000, 1000, 1, ... % x, y, z
10, 10, 1, ... % x', y', z'
1000, 1000, 1, ... % roll, pitch, yaw
1, 1, 1]); % roll', pitch', yaw'
R = diag([10, 20, 20, 1]); % upward force, pitch torque, roll torque, yaw torque
% Calculate number of timesteps.
tSpan = 0:T_s:2;
nSteps = length(tSpan);
% Determine gains
[X, K, L, info] = idare(discrete.A, discrete.B, Q, R, [], []);
% Pitch Goal
% Propagate
[ulqr, xlqr] = propagate_inf(nInputs, nStates, nSteps, x_0_pitch, 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_inf(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;
%% Finite-Time Horizon For Goal 3
% Cost matrices
Q = diag([1000, 1000, 1000, ... % x, y, z
1, 1, 100, ... % x', y', z'
200, 200, 1, ... % roll, pitch, yaw
1, 1, 1]); % roll', pitch', yaw'
R = diag([10, 20, 20, 1]); % upward force, pitch torque, roll torque, yaw torque
% Calculate number of timesteps.
tSpan = 0:T_s:5;
nSteps = length(tSpan);
@@ -167,6 +250,31 @@ xlqr(1:3,:) = xlqr(1:3,:) + 1;
% Plot
plot_states(xlqr, tSpan);
%% Infinite-Time Horizon For Goal 3
% Cost matrices
Q = diag([1000, 1000, 1000, ... % x, y, z
10, 10, 10, ... % x', y', z'
1000, 1000, 1, ... % roll, pitch, yaw
1, 1, 1]); % roll', pitch', yaw'
R = diag([10, 20, 20, 1]); % upward force, pitch torque, roll torque, yaw torque
% Calculate number of timesteps.
tSpan = 0:T_s:5;
nSteps = length(tSpan);
% Determine gains
[X, K, L, info] = idare(discrete.A, discrete.B, Q, R, [], []);
% Pitch Goal
% Propagate
[ulqr, xlqr] = propagate_inf(nInputs, nStates, nSteps, x_0_trans, K, discrete.A, discrete.B);
xlqr(1:3,:) = xlqr(1:3,:) + 1;
% Plot
plot_states(xlqr, tSpan);
%% Helper Functions
function [K, P] = LQR_LTI(A, B, Q, R, nSteps)
@@ -197,6 +305,18 @@ function [ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0, K, A, B)
end
end
function [ulqr, xlqr] = propagate_inf(nInputs, nStates, nSteps, x_0, K, A, B)
% Set up for propagation
ulqr = zeros(nInputs, nSteps);
xlqr = zeros(nStates, nSteps);
xlqr(:, 1) = x_0;
for i = 1:(nSteps - 1)
ulqr(:,i) = K * xlqr(:,i);
xlqr(:,i+1) = (A*xlqr(:, i) - B*ulqr(:, i));
end
end
function plot_states(xlqr, tSpan)
figure();
subplot(1, 2, 1);