Nonlinear Plots

- Generate all plots via nonlinear system
- Remove extra matlab and simulink files that are no longer used

src/LQR.m

@@ -1,14 +1,9 @@
 % Clear workspace
-clear all;
+clear all; close all; clc;
-close all;
-clc;
 
 % Parameters source: https://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf
-g = 9.81;
+g = 9.81;   m = 0.468;  Ix = 4.856*10^-3;
-m = 0.468;
+Iy = 4.856*10^-3;   Iz = 8.801*10^-3;
-Ix = 4.856*10^-3;
-Iy = 4.856*10^-3;
-Iz = 8.801*10^-3;
 
 % States:
 % X1: x                         X4: x'
@@ -74,46 +69,39 @@ 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,
+%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
-% 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]';
+       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, ...
-             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, ...
        0, 0, 0, ...
-            0, 10, 0, ...
+       0, 10*(pi/180), 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
+x_0_trans = [-1, -1, 0, ...
-% 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]';
+       0, 0, 0]'; %Redefine origin! 
 
 %% Finite-Time Horizon LQR for Goal 1
 
-% Cost matrices
+%Define Q and R for the cost function. Begin with nominal ones for all.
 Q = diag([1000, 1000, 1000, ... % x, y, z
           1, 1, 100, ... % x', y', z'
           200, 200, 1, ... % roll, pitch, yaw
@@ -121,21 +109,32 @@ Q = diag([1000, 1000, 1000, ... % x, y, z
 
 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;
 nSteps = length(tSpan);
 
-% Determine gains
+%Determine gains
 [K, P] = LQR_LTI(discrete.A, discrete.B, Q, R, nSteps);
+FiniteLQR_Goal_1_K = K;
 
-% Propagate
+%Simulate nonlinear model
-[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_up, K, discrete.A, discrete.B);
+[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_up);
-% States are relative to origin, so we need to add the reference to the
+set_param('LQRNonlinearSim', 'StopTime', '2')
-% state to get global coordinates
+simout = sim('LQRNonlinearSim', 'FixedStep', '.01');
-xlqr(3,:) = xlqr(3,:) + 1;
+
-% Plot
+state = [simout.x';
-plot_states(xlqr, tSpan);
+         simout.y';
-zd = diff(xlqr(6,:))./T_s
+         simout.z' + 1;
+         simout.xdot';
+         simout.ydot';
+         simout.zdot';
+         simout.pitch';
+         simout.roll';
+         simout.yaw';
+         simout.dotpitch';
+         simout.dotroll';
+         simout.dotyaw'];
+plot_states(state, tSpan);
 
 %% Infinite-Time Horizon LQR for Goal 1
 
@@ -152,103 +151,142 @@ tSpan = 0:T_s:2;
 nSteps = length(tSpan);
 
 % Determine Gains
-[X, K, L, info] = idare(discrete.A, discrete.B, Q, R, [], []);
+[X, K_const, 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;
+K = zeros(nInputs, nStates, nSteps);
-plot_states(xlqr, tSpan);
+for i = 1:nSteps
-zd = diff(xlqr(6,:))./T_s;
+    K(:,:,i) = K_const;
+end
+
+%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');
+
+state = [simout.x';
+         simout.y';
+         simout.z' + 1;
+         simout.xdot';
+         simout.ydot';
+         simout.zdot';
+         simout.pitch';
+         simout.roll';
+         simout.yaw';
+         simout.dotpitch';
+         simout.dotroll';
+         simout.dotyaw'];
+plot_states(state, tSpan);
 
 %% Finite-Time Horizon LQR for Goal 2
 
-% Cost matrices
+Q = diag([0, 0, 0, ... % x, y, z
-Q = diag([1000, 1000, 1000, ... % x, y, z
+          0, 0, 0, ... % x', y', z'
-          1, 1, 100, ... % x', y', z'
+          1000, 1000, 1, ... % roll, pitch, yaw
-          200, 200, 1, ... % roll, pitch, yaw
+          10, 10, 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.
+%Calculate number of timesteps.
-tSpan = 0:T_s:2;
+tSpan = 0:T_s:4;
 nSteps = length(tSpan);
 
-% Determine gains
+%Determine gains
 [K, P] = LQR_LTI(discrete.A, discrete.B, Q, R, nSteps);
 
-% Pitch Goal
+%Simulate nonlinear model
-% Propagate
+[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_pitchroll);
-[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_pitch, K, discrete.A, discrete.B);
+set_param('LQRNonlinearSim', 'StopTime', '4')
+simout = sim('LQRNonlinearSim', 'FixedStep', '.01');
 
-% Plot
+state = [simout.x';
-plot_states(xlqr, tSpan);
+         simout.y';
-yd = diff(xlqr(5,:))./T_s;
+         simout.z' + 1;
-pd = diff(xlqr(7,:))./T_s;
+         simout.xdot';
-% Propagate
+         simout.ydot';
-[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_roll, K, discrete.A, discrete.B);
+         simout.zdot';
-
+         simout.pitch';
-% Plot
+         simout.roll';
-plot_states(xlqr, tSpan);
+         simout.yaw';
-xd = diff(xlqr(4,:))./T_s;
+         simout.dotpitch';
-rd = diff(xlqr(8,:))./T_s;
+         simout.dotroll';
+         simout.dotyaw'];
+plot_states(state, tSpan);
 
 %% Infinite-Time Horizon LQR for Goal 2
 
 % Cost matrices
-Q = diag([1000, 1000, 1, ... % x, y, z
+Q = diag([0, 0, 0, ... % x, y, z
-          10, 10, 1, ... % x', y', z'
+          0, 0, 0, ... % x', y', z'
-          1000, 1000, 1, ... % roll, pitch, yaw
+          1000, 1000, 0, ... % roll, pitch, yaw
-          1, 1, 1]);   % roll', pitch', yaw'
+          10, 10, 0]);   % 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;
+tSpan = 0:T_s:4;
 nSteps = length(tSpan);
 
 % Determine gains
 [X, K, L, info] = idare(discrete.A, discrete.B, Q, R, [], []);
 
+K = zeros(nInputs, nStates, nSteps);
+for i = 1:nSteps
+    K(:,:,i) = K_const;
+end
 
-% Pitch Goal
+%Simulate nonlinear model
-% Propagate
+[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_pitchroll);
-[ulqr, xlqr] = propagate_inf(nInputs, nStates, nSteps, x_0_pitch, K, discrete.A, discrete.B);
+set_param('LQRNonlinearSim', 'StopTime', '4')
+simout = sim('LQRNonlinearSim', 'FixedStep', '.01');
 
-% Plot
+state = [simout.x';
-plot_states(xlqr, tSpan);
+         simout.y';
-yd = diff(xlqr(5,:))./T_s;
+         simout.z' + 1;
-pd = diff(xlqr(7,:))./T_s;
+         simout.xdot';
-% Propagate
+         simout.ydot';
-[ulqr, xlqr] = propagate_inf(nInputs, nStates, nSteps, x_0_roll, K, discrete.A, discrete.B);
+         simout.zdot';
-
+         simout.pitch';
-% Plot
+         simout.roll';
-plot_states(xlqr, tSpan);
+         simout.yaw';
-xd = diff(xlqr(4,:))./T_s;
+         simout.dotpitch';
-rd = diff(xlqr(8,:))./T_s;
+         simout.dotroll';
+         simout.dotyaw'];
+plot_states(state, tSpan);
 
 %% Finite-Time Horizon For Goal 3
 
-% Cost matrices
 Q = diag([1000, 1000, 1000, ... % x, y, z
-          1, 1, 100, ... % x', y', z'
+          0, 0, 0, ... % x', y', z'
-          200, 200, 1, ... % roll, pitch, yaw
+          1000, 1000, 0, ... % roll, pitch, yaw
-          1, 1, 1]);   % roll', pitch', yaw'
+          0, 0, 0]);   % roll', pitch', yaw'
       
-R = diag([10, 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;
 nSteps = length(tSpan);
 
-% Determine gains
+%Determine gains
 [K, P] = LQR_LTI(discrete.A, discrete.B, Q, R, nSteps);
 
-% Pitch Goal
+%Simulate nonlinear model
-% Propagate
+[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_trans);
-[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_trans, K, discrete.A, discrete.B);
+set_param('LQRNonlinearSim', 'StopTime', '5')
-xlqr(1:3,:) = xlqr(1:3,:) + 1;
+simout = sim('LQRNonlinearSim', 'FixedStep', '.01');
 
-% Plot
+state = [simout.x';
-plot_states(xlqr, tSpan);
+         simout.y';
+         simout.z' + 1;
+         simout.xdot';
+         simout.ydot';
+         simout.zdot';
+         simout.pitch';
+         simout.roll';
+         simout.yaw';
+         simout.dotpitch';
+         simout.dotroll';
+         simout.dotyaw'];
+plot_states(state, tSpan);
 
 %% Infinite-Time Horizon For Goal 3
 
@@ -267,22 +305,38 @@ nSteps = length(tSpan);
 % Determine gains
 [X, K, L, info] = idare(discrete.A, discrete.B, Q, R, [], []);
 
-% Pitch Goal
+K = zeros(nInputs, nStates, nSteps);
-% Propagate
+for i = 1:nSteps
-[ulqr, xlqr] = propagate_inf(nInputs, nStates, nSteps, x_0_trans, K, discrete.A, discrete.B);
+    K(:,:,i) = K_const;
-xlqr(1:3,:) = xlqr(1:3,:) + 1;
+end
 
-% Plot
+%Simulate nonlinear model
-plot_states(xlqr, tSpan);
+[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');
+
+state = [simout.x';
+         simout.y';
+         simout.z' + 1;
+         simout.xdot';
+         simout.ydot';
+         simout.zdot';
+         simout.pitch';
+         simout.roll';
+         simout.yaw';
+         simout.dotpitch';
+         simout.dotroll';
+         simout.dotyaw'];
+plot_states(state, 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
@@ -294,7 +348,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; 
@@ -305,21 +359,9 @@ 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);
+    subplot(2, 1, 1);
     plot(tSpan, xlqr(1, :), '-r', 'LineWidth', 2);
     hold on;
     plot(tSpan, xlqr(2, :), '-g');
@@ -332,7 +374,7 @@ function plot_states(xlqr, tSpan)
     xlabel("Time(s)");
     ylabel("Displacement (m)");
 
-    subplot(1, 2, 2);
+    subplot(2, 1, 2);
     plot(tSpan, xlqr(7, :), '-r');
     hold on;
     plot(tSpan, xlqr(8, :), '-g');
@@ -343,5 +385,9 @@ function plot_states(xlqr, tSpan)
     legend('Pitch (about x)', 'Roll (about y)', 'Yaw (about z)', 'Pitch Rate', 'Roll Rate', 'Yaw Rate');
     title("Angular Displacements(-) and Velocities(--)");
     xlabel("Time(s)");
-    ylabel("Displacement (deg)");
+    ylabel("Displacement (rad)");
+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

src/LQR_Goal_1.m

@@ -1,284 +0,0 @@
-% Clear workspace
-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;
-
-% States:
-% X1: x                         X4: x'
-% X2: y                         X5: y'
-% X3: z                         X6: z'
-% X7: Pitch angle (x-axis)      X10: Pitch rate (x-axis)
-% X8: Roll angle (y-axis)       X11: Roll rate (y-axis)
-% X9: Yaw angle (z-axis)        X12: Yaw rate (z-axis)
-
-% Inputs:                                   Outputs:
-% U1: Total Upward Force (along z-axis)     Y1: Position along x axis
-% U2: Pitch Torque (about x-axis)           Y2: Position along y axis
-% U3: Roll Torque (about y-axis)            Y3: Position along z axis
-% U4: Yaw Torque (about z-axis)             Y4: Pitch (about x-axis)
-%                                           Y5: Roll (about y-axis)
-%                                           Y6: Yaw (about z-axis)
-
-% State Space Source: https://arxiv.org/ftp/arxiv/papers/1908/1908.07401.pdf
-% X' = Ax + Bu
-% Y = Cx
-
-nStates = 12;
-nInputs = 4;
-nOutputs = 6;
-
-A = [0  0  0  1  0  0  0  0  0  0  0  0;
-     0  0  0  0  1  0  0  0  0  0  0  0;
-     0  0  0  0  0  1  0  0  0  0  0  0;
-     0  0  0  0  0  0  0  -g 0  0  0  0;
-     0  0  0  0  0  0  g  0  0  0  0  0;
-     0  0  0  0  0  0  0  0  0  0  0  0;
-     0  0  0  0  0  0  0  0  0  1  0  0;
-     0  0  0  0  0  0  0  0  0  0  1  0;
-     0  0  0  0  0  0  0  0  0  0  0  1;
-     0  0  0  0  0  0  0  0  0  0  0  0;
-     0  0  0  0  0  0  0  0  0  0  0  0;
-     0  0  0  0  0  0  0  0  0  0  0  0];
-
-% Note: In paper, 1/m is in wrong spot
-B = [0      0       0       0;
-     0      0       0       0;
-     0      0       0       0;
-     0      0       0       0;
-     0      0       0       0;
-     1/m    0       0       0;
-     0      0       0       0;
-     0      0       0       0;
-     0      0       0       0;
-     0      1/Ix    0       0;
-     0      0       1/Iy    0;
-     0      0       0       1/Iz];
-
-C = [1 0 0 0 0 0 0 0 0 0 0 0;
-     0 1 0 0 0 0 0 0 0 0 0 0;
-     0 0 1 0 0 0 0 0 0 0 0 0;
-     0 0 0 0 0 0 1 0 0 0 0 0;
-     0 0 0 0 0 0 0 1 0 0 0 0;
-     0 0 0 0 0 0 0 0 1 0 0 0];
-
-D = zeros(6,4);
-
-continuous = ss(A, B, C, D);
-T_s = 0.01;
-discrete = c2d(continuous, T_s);
-
-%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
-
-%% Define goals
-%Goal 1: settle at 1m height <2s
-x_0_up = [0, 0, -1, ...
-       0, 0, 0, ...
-       0, 0, 0, ...
-       0, 0, 0]'; %Redefine origin! 
-
-%Goal 2: Stabilize from a 10-degree roll and pitch with <3deg overshoot
-x_0_pitchroll = [0, 0, 0, ...
-       0, 0, 0, ...
-       10*(pi/180), 10*(pi/180), 0, ...
-       0, 0, 0]'; %Pitch and roll of 10 degrees
-
-x_0_roll = [0, 0, 0, ...
-       0, 0, 0, ...
-       0, 10*(pi/180), 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.
-x_0_trans = [-1, -1, 0, ...
-       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.
-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;
-nSteps = length(tSpan);
-
-%Determine gains
-[K, P] = LQR_LTI(discrete.A, discrete.B, Q, R, nSteps);
-FiniteLQR_Goal_1_K = K;
-save('FiniteLQRGoal_1_K.mat', 'K');
-
-%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
-xlqr(3,:) = xlqr(3,:) + 1;
-%Plot
-plot_states(xlqr, tSpan);
-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
-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.
-tSpan = 0:T_s:4;
-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_pitchroll, 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(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
-
-%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
-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.
-tSpan = 0:T_s:5;
-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_trans, K, discrete.A, discrete.B);
-xlqr(1:2,:) = xlqr(1:2,:) + 1;
-
-%Plot
-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
-
-function [K, P] = LQR_LTI(A, B, Q, R, nSteps)
-    %Set P up
-    P = zeros(size(Q, 1), size(Q, 2), nSteps);
-    %Initial value of P
-    P(:, :, nSteps) = 1/2 * Q;
-    %Set K up, initial K is 0, so this is fine.
-    K = zeros(length(R), length(Q), nSteps);
-        
-    for i = nSteps-1:-1:1
-        P_ = P(:,:, i+1);
-        
-        K(:, :, i) = ( 1/2 * R + B' * P_ * B )^(-1) * B' * P_ * A;
-        P(:, :, i) = A' * P_ * ( A - B * K(:, :, i) ) + Q * 1/2;
-    end
-end
-
-function [ulqr, xlqr] = propagate(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(:,:,i) * xlqr(:,i);
-        xlqr(:,i+1) = (A*xlqr(:, i) - B*ulqr(:, i));
-    end
-end
-
-function plot_states(xlqr, tSpan)
-    figure();
-    subplot(1, 2, 1);
-    plot(tSpan, xlqr(1, :), '-r', 'LineWidth', 2);
-    hold on;
-    plot(tSpan, xlqr(2, :), '-g');
-    plot(tSpan, xlqr(3, :), '-b');
-    plot(tSpan, xlqr(4, :), '--r', 'LineWidth', 2);
-    plot(tSpan, xlqr(5, :), '--g');
-    plot(tSpan, xlqr(6, :), '--b');
-    legend('x', 'y', 'z', 'x`', 'y`', 'z`');
-    title("Translations(-) and Velocities (--)");
-    xlabel("Time(s)");
-    ylabel("Displacement (m)");
-
-    subplot(1, 2, 2);
-    plot(tSpan, xlqr(7, :), '-r');
-    hold on;
-    plot(tSpan, xlqr(8, :), '-g');
-    plot(tSpan, xlqr(9, :), '-b');
-    plot(tSpan, xlqr(10, :), '--r');
-    plot(tSpan, xlqr(11, :), '--g');
-    plot(tSpan, xlqr(12, :), '--b');
-    legend('Pitch (about x)', 'Roll (about y)', 'Yaw (about z)', 'Pitch Rate', 'Roll Rate', 'Yaw Rate');
-    title("Angular Displacements(-) and Velocities(--)");
-    xlabel("Time(s)");
-    ylabel("Displacement (rad)");
-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