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
2025-08-31 21:03:13 +00:00 · 2020-04-20 01:18:40 -04:00
parent 5252eeb140
commit 2943d7a4f8
1 changed files with 172 additions and 52 deletions
--- 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);