diff --git a/src/LQR.slx b/src/LQR.slx
deleted file mode 100644
index c2fa0b4..0000000
Binary files a/src/LQR.slx and /dev/null differ
diff --git a/src/LQRSim_Goal_1.slx b/src/LQRSim_Goal_1.slx
new file mode 100644
index 0000000..6b0e09e
Binary files /dev/null and b/src/LQRSim_Goal_1.slx differ
diff --git a/src/LQR_Goal_1.m b/src/LQR_Goal_1.m
new file mode 100644
index 0000000..fb5c6dc
--- /dev/null
+++ b/src/LQR_Goal_1.m
@@ -0,0 +1,156 @@
+% 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.05;
+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
+% Desired position
+x_d = 0;
+y_d = 0;
+z_d = 0;
+
+%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_pitch = [0, 0, 0, ...
+             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
+   
+%Goal 3: Move from position (0,0,0) to within 5 cm of (1,1,1) within 5 seconds.
+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.
+Q = diag([1000, 1000, 1000, ... % x, y, z
+          1, 1, 100, ... % x', y', z'
+          100, 100, 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);
+
+%Set up for propagation
+ulqr = zeros(nInputs, nSteps);
+xlqr = zeros(nStates, nSteps);
+xlqr(:, 1) = x_0_up; 
+
+for i = 1:(nSteps -  1)
+    ulqr(:,i) = K(:,:,i) * xlqr(:,i);
+    xlqr(:,i+1) = (A*xlqr(:, i) - B*ulqr(:, i));
+end
+
+figure(10);
+hold on;
+plot(tSpan, ulqr(1,:), 'b-');
+plot(tSpan, ulqr(2,:), 'r-');
+plot(tSpan, ulqr(3,:), 'g-');
+plot(tSpan, ulqr(4,:), 'y-');
+
+%% 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