From 2943d7a4f814ee8fb6b2d6dd7662cbc23bc626a5 Mon Sep 17 00:00:00 2001 From: Sravan Balaji Date: Mon, 20 Apr 2020 01:18:40 -0400 Subject: [PATCH] Implement Infinite Horizon LQR - Code and comment formatting cleanup - Add additional comments for clarity - Add cost matrices to each section - Use idare to solve for infinite horizon gains --- src/LQR.m | 224 +++++++++++++++++++++++++++++++++++++++++------------- 1 file changed, 172 insertions(+), 52 deletions(-) diff --git a/src/LQR.m b/src/LQR.m index 6ac1c99..b4cfd2d 100644 --- a/src/LQR.m +++ b/src/LQR.m @@ -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' @@ -69,112 +74,215 @@ continuous = ss(A, B, C, D); T_s = 0.01; discrete = c2d(continuous, T_s); -%Check if this works +% Check if this works impulse(discrete, 0:T_s:1); -%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 +% 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 %% Define goals -%Goal 1: settle at 1m height <2s +% 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, ... + 0, 0, 0, ... + 0, 0, 0]'; -%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, ... - 0, 0, 0, ... - 10, 0, 0, ... - 0, 0, 0]'; %Pitch of 10 degrees + 0, 0, 0, ... + 10, 0, 0, ... + 0, 0, 0]'; %Pitch of 10 degrees x_0_roll = [0, 0, 0, ... - 0, 0, 0, ... - 0, 10, 0, ... - 0, 0, 0]'; %Roll of 10 degrees + 0, 0, 0, ... + 0, 10, 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. +% 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! - -%Define Q and R for the cost function. Begin with nominal ones for all. + 0, 0, 0, ... + 0, 0, 0, ... + 0, 0, 0]'; + +%% 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. +% Calculate number of timesteps. tSpan = 0:T_s:2; nSteps = length(tSpan); -%Determine gains +% Determine gains [K, P] = LQR_LTI(discrete.A, discrete.B, Q, R, nSteps); -%Propagate +% Propagate [ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_up, K, discrete.A, discrete.B); -%States are relative to origin, so we need to add the reference to the -%state to get global coordinates +% States are relative to origin, so we need to add the reference to the +% state to get global coordinates xlqr(3,:) = xlqr(3,:) + 1; -%Plot +% Plot plot_states(xlqr, tSpan); zd = diff(xlqr(6,:))./T_s -%% Finite-Time Horizon LQR for Goal 2 +%% Infinite-Time Horizon LQR for Goal 1 -%Calculate number of timesteps. +% 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 +% 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); + +% Determine gains [K, P] = LQR_LTI(discrete.A, discrete.B, Q, R, nSteps); - -%Pitch Goal -%Propagate +% Pitch Goal +% Propagate [ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_pitch, K, discrete.A, discrete.B); -%Plot +% Plot plot_states(xlqr, tSpan); -yd = diff(xlqr(5,:))./T_s -pd = diff(xlqr(7,:))./T_s -%Propagate +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 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 -%Calculate number of timesteps. +% 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); -%Determine gains +% Determine gains [K, P] = LQR_LTI(discrete.A, discrete.B, Q, R, nSteps); -%Pitch Goal -%Propagate +% Pitch Goal +% Propagate [ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_trans, K, discrete.A, discrete.B); xlqr(1:3,:) = xlqr(1:3,:) + 1; -%Plot +% 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) - %Set P up + % Set P up P = zeros(size(Q, 1), size(Q, 2), nSteps); - %Initial value of P + % Initial value of P 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); for i = nSteps-1:-1:1 @@ -186,7 +294,7 @@ function [K, P] = LQR_LTI(A, B, Q, R, nSteps) end function [ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0, K, A, B) - %Set up for propagation + % Set up for propagation ulqr = zeros(nInputs, nSteps); xlqr = zeros(nStates, nSteps); xlqr(:, 1) = x_0; @@ -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);