diff --git a/.gitignore b/.gitignore
index 92061b1..cb187cd 100644
--- a/.gitignore
+++ b/.gitignore
@@ -31,3 +31,9 @@ codegen/
 
 # Cloud based storage dotfile
 .MATLABDriveTag
+
+
+barrier/*
+normal/*
+receding/*
+.DS_Store
diff --git a/full_quadrotor.m b/Barrier_full_quadrotor.m
similarity index 59%
rename from full_quadrotor.m
rename to Barrier_full_quadrotor.m
index fcfef6f..661172e 100644
--- a/full_quadrotor.m
+++ b/Barrier_full_quadrotor.m
@@ -1,4 +1,4 @@
-function [dyn] = full_quadrotor(dt)
+function [dyn] = full_quadrotor_barrier(dt, xf)
 %   Detailed explanation goes here
 
 % Get parameters
@@ -10,15 +10,18 @@
 Iyy = 0.0032;
 Izz = 0.0055;
 
-syms x y z phi theta ps p q r vx vy vz u1 u2 u3 u4
+syms x y z phi theta ps p q r vx vy vz u1 u2 u3 u4 w;  % psi called "ps" for namespace reasons
 
-coord = [x; y; z; phi; theta; ps];
-vel = [vx; vy; vz; p; q; r];
-state = [coord; vel];
+pos = [x; y; z];
+coord_vel = [vx; vy; vz];
+euler_angle = [phi; theta; ps];
+body_rate = [p; q; r];
+state = [pos; coord_vel; euler_angle; body_rate; w];
 controls = [u1; u2; u3; u4];
 vars = [state; controls];
 
 %% Get equations of motion
+% Linear EOM first
 % Starting with R
 % Define individual rotation matrices
 R1_phi = [1, 0, 0;
@@ -37,19 +40,35 @@
 R = R1_phi * R2_theta * R3_psi;
 
 % Linear acceleration
-lin_acc = (1/m)*R*[0;0; u1 + u2 + u3 + u4] + [0;0;-m*g];
+lin_acc = (1/m)*R*[0;0; u1 + u2 + u3 + u4] + [0;0;-g];
+
+%% Rotational EOM
 
 % Rotational acceleration by Euler's eqn
-rot_acc = [(1/Ixx)*(sqrt(2)/2)*(u1+u3-u2-u4)*l - (Izz - Iyy)*q*r;
-    (1/Iyy)*(sqrt(2)/2)*(u3+u4-u1-u2)*l - (Izz - Ixx)*p*r;
+rot_acc = [(1/Ixx)*((sqrt(2)/2)*(u1+u3-u2-u4)*l - (Izz - Iyy)*q*r);
+    (1/Iyy)*((sqrt(2)/2)*(u3+u4-u1-u2)*l - (Izz - Ixx)*p*r);
     (1/Izz)*kt*(u1+u4-u2-u3)];
 
 % Set eom
 eom = [lin_acc; rot_acc];
 
-f = [coord + dt * vel;
-     vel + dt * eom];
+% body rates to euler rates
+P = [1, tan(theta)*sin(phi), tan(theta)*cos(phi);
+     0, cos(phi), -sin(phi);
+     0, sin(phi)/cos(theta), cos(phi)/cos(theta)];
+
+% Euler integrate to get next state
+f = [pos + dt * coord_vel;
+    coord_vel + dt * lin_acc;
+    euler_angle + dt * P * body_rate;
+    body_rate + dt * rot_acc;
+    0];  % placeholder
+
+% now update barrier state based on new state
+barrier_func = get_barrier_func();
+f(13) = barrier_func(f) - barrier_func(xf);
 
+% take derivatives and convert to matlab functions
 fx = jacobian(f, state);
 fu = jacobian(f, controls);
 
@@ -101,18 +120,19 @@
     u3 = clip(u3,-bound,bound);
     u4 = clip(u4,-bound,bound);
 
-    wrapper_f = num_f(x,y,z,phi,theta,psi,vx,vy,vz,p,q,r,u1,u2,u3,u4);
-    wrapper_fx = num_fx(x,y,z,phi,theta,psi,vx,vy,vz,p,q,r,u1,u2,u3,u4);
-    wrapper_fu = num_fu(x,y,z,phi,theta,psi,vx,vy,vz,p,q,r,u1,u2,u3,u4);
+    wrapper_f = num_f(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4);
+    wrapper_fx = num_fx(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4);
+    wrapper_fu = num_fu(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4);
+	%wrapper_f = num_f(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4);
 
     wrapper_fxx = zeros(12, 12, 12);
     wrapper_fxu = zeros(12, 12, 4);
     wrapper_fuu = zeros(12, 4, 4);
 
     for i = 1:12
-        wrapper_fxx(i, :, :) = num_fxx{i}(x,y,z,phi,theta,psi,vx,vy,vz,p,q,r,u1,u2,u3,u4);
-        wrapper_fxu(i, :, :) = num_fxu{i}(x,y,z,phi,theta,psi,vx,vy,vz,p,q,r,u1,u2,u3,u4);
-        wrapper_fuu(i, :, :) = num_fuu{i}(x,y,z,phi,theta,psi,vx,vy,vz,p,q,r,u1,u2,u3,u4);
+        wrapper_fxx(i, :, :) = num_fxx{i}(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4);
+        wrapper_fxu(i, :, :) = num_fxu{i}(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4);
+        wrapper_fuu(i, :, :) = num_fuu{i}(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4);
     end
 end
 
diff --git a/back_pass.m b/back_pass.m
index a868798..ae7462e 100644
--- a/back_pass.m
+++ b/back_pass.m
@@ -1,4 +1,4 @@
- function [controller, V] = back_pass(states, controls, dyn, costfn, term_costfn, regularizer, mode)
+ function [controller, V] = back_pass(states, controls, dyn, costfn, term_costfn, regularizer, mode, xf)
 %BACK_PASS The backward pass of iLQR / DDP.
 %
 %   states: A tf-by-n matrix where the row t contains the state of the
@@ -75,6 +75,14 @@
     regularizer = 0;
 end
 
+initial_mu = 1e-2;
+delta_0 = 2;
+
+mu = initial_mu;
+mu_min = 1e-6;
+delta = delta_0;
+
+
 
 % Some basic setup code for you
 horizon = size(controls, 1);
@@ -89,6 +97,10 @@
 
 %% Fill in your code below
 
+% add w to 13th pos in state vec
+barrier_func = get_barrier_func();
+states(horizon, 13) = barrier_func(states(horizon, :)) - barrier_func(xf);
+
 % Initialize with final values
 [~, cx, ~, Q, ~, ~] = costfn(states(horizon,:).', controls(horizon,:).');
 next_P = Q;
@@ -104,51 +116,90 @@
     % Evaluate cost and function
     [~, cx, cu, cxx, cxu, cuu] = costfn(states(t,:).', controls(t,:).');
     [f, fx, fu, fxx, fxu, fuu] = dyn(states(t,:).', controls(t,:).');
+    reg = regularizer;
 
-    % Get first-order derivatives of Q
-    Qx = cx + (fx.')*Vx_next - regularizer*(fx.')*(states(t+1,:).' - f);
-    Qu = cu + (fu.')*Vx_next - regularizer*(fu.')*(states(t+1,:).' - f);
+	delta = delta_0;
+    
+    % automatically increment regularizer if Quu not invertible
+    for i = 1:1
+        % Get first-order derivatives of Q
+        Qx = cx + (fx.')*Vx_next - reg*(fx.')*(states(t+1,:).' - f);
+        Qu = cu + (fu.')*Vx_next - reg*(fu.')*(states(t+1,:).' - f);
+    
+        if strcmp(mode, 'ddp')
+            % Get second-order derivatives of Q
+            Qxx = cxx + (fx.')*Vxx_next*fx + fake_tensor_prod(Vx_next,fxx);% + reg*(fx.')*fx;
+            Qxu = cxu + (fx.')*(Vxx_next + reg * eye(length(Qxx)))*fu + fake_tensor_prod(Vx_next,fxu);
+            Qux = Qxu.';
+            Quu = cuu + (fu.')*(Vxx_next + reg * eye(length(Qxx)))*fu + fake_tensor_prod(Vx_next,fuu);
+    
+        elseif strcmp(mode,'ilqr')
+            % Get second-order derivatives of Q without tensor prod
+            Qxx = cxx + (fx.')*Vxx_next*fx;
+            Qxu = cxu + (fx.')*Vxx_next*fu;
+            Qux = Qxu.';
+            Quu = cuu + (fu.')*Vxx_next*fu;
+		end
+		
+		Quu = (Quu + Quu.')/2;
+		%Quu = Quu + mu * eye(size(Quu));
+
+        % check if Quu is positive definite
+        if all(eig(Quu) > 1e-6) && rcond(Quu) > 1e-13
+            break
+		end
+
+        % Increase mu if Quu is not positive definite
+        delta = max(delta_0, delta * delta_0);
+        mu = max(mu_min, mu * delta);
+
+        if reg == 0 || isinf(reg)
+            disp('Reg 0 or inf');
+            break
+        end
+        if isnan(rcond(Quu))
+            disp('NaN rcond(Quu)');
+            break
+        end
+        % otherwise, increment regularizer
+        %reg = reg * 2;
 
-    if strcmp(mode, 'ddp')
-        % Get second-order derivatives of Q
-        Qxx = cxx + (fx.')*Vxx_next*fx + fake_tensor_prod(Vx_next,fxx) + regularizer*(fx.')*fx;
-        Qxu = cxu + (fx.')*Vxx_next*fu + fake_tensor_prod(Vx_next,fxu) + regularizer*(fx.')*(fu);
-        Qux = Qxu.';
-        Quu = cuu + (fu.')*Vxx_next*fu + fake_tensor_prod(Vx_next,fuu) + regularizer*(fu.')*(fu);
-
-    elseif strcmp(mode,'ilqr')
-        % Get second-order derivatives of Q without tensor prod
-        Qxx = cxx + (fx.')*Vxx_next*fx;
-        Qxu = cxu + (fx.')*Vxx_next*fu;
-        Qux = Qxu.';
-        Quu = cuu + (fu.')*Vxx_next*fu;
-    end
+    end % regularizer update loop
 
     % Get new controls
-    K = -inv(Quu)*Qux;
-    k = -inv(Quu)*Qu;
+	
+    K = -Quu \ Qux;
+    k = -Quu \ Qu;
 
     % Get new value function
-    Vx_next = Qx - (K.')*Quu*k;
-    Vxx_next = Qxx - (K.')*Quu*K;
+	Vx_next = Qx + K.'*Quu*k + K.'*Qu + Qux.'*k;
+	Vxx_next = Qxx + K.'*Quu*K + K.'*Qux + Qux.'*K;
+
+    % Decrease mu for next iteration if successful
+    delta = min(1 / delta_0, delta / delta_0);
+    if mu * delta > mu_min
+        mu = mu * delta;
+    else
+        mu = 0;
+    end
+
+	%disp(sort(eig(Quu)))
 
     % Assign
     controller.K(t,:,:) = K;
     controller.k(t,:,:) = k;
 end
 
-
-
-
-V = 0; % don't bother defining V
+V = -0.5 * (Qu.' * pinv(Quu) * Qu);  % cost to go
 end
 
 function out = fake_tensor_prod(A,B)
     out = 0;
-    for i = 1:6
+	n = length(A);
+    for i = 1:n
         out = out + squeeze(A(i)*B(i,:,:));
-    end
-    out = out/3;
+	end
+	out = out / 3;
 end
 
 %% Fill in your code below
diff --git a/ddp.m b/ddp.m
index 8519e32..a610947 100644
--- a/ddp.m
+++ b/ddp.m
@@ -1,4 +1,4 @@
-function [controller, total_costs] = ddp(ic, initial_controls, iters, regularizer, dyn, costfn, term_costfn, mode)
+function [controller, total_costs] = ddp(ic, initial_controls, iters, regularizer, dyn, costfn, term_costfn, mode, line_search_iters, xf)
 %DDP Solves for a locally optimal controller using iLQR / DDP.
 %
 %   ic: An n-by-1 state serving as the initial condition for the control
@@ -36,6 +36,11 @@
 %       mode = 'ddp'
 %
 %   The default is to use iLQR.
+% line_search_iters (int): max num of line search iterations. Min 1.
+
+if nargin < 10
+    xf = zeros(12,1);
+end
 
 % Setup variables
 tf = size(initial_controls, 1);
@@ -50,14 +55,27 @@
 
 total_costs = zeros(iters, 1);
 
+% alphas = 1:-1/line_search_iters:0;  % decreasing step size sequence
+alphas = exp(-(0:line_search_iters));
+
 %% Your code below
 
-[states, controls, ~] = fwd_pass(ic, controller, dyn, costfn, term_costfn);
+[states, controls, best_costs] = fwd_pass(ic, controller, dyn, costfn, term_costfn, alphas(1), xf);
 
 for i = 1:iters
-    [controller, ~] = back_pass(states, controls, dyn, costfn, term_costfn, regularizer, mode);
-    [states, controls, costs] = fwd_pass(ic, controller, dyn, costfn, term_costfn);
-    total_costs(i,1) = norm(costs); %maybe sum(costs)
+    [controller, ~] = back_pass(states, controls, dyn, costfn, term_costfn, regularizer, mode, xf);
+    %disp(sum(best_costs))
+    for j = 1:line_search_iters-1
+        % line search logic
+        [states, controls, new_costs] = fwd_pass(ic, controller, dyn, costfn, term_costfn, alphas(j), xf);
+        if sum(new_costs) < sum(best_costs)
+            best_costs = new_costs;
+        end
+	end
+	
+    % update to a newer controller
+    [controller, ~] = back_pass(states, controls, dyn, costfn, term_costfn, regularizer, mode, xf);
+    total_costs(i,1) = sum(best_costs); 
 end
 
 end
diff --git a/full_quadrotor_barrier.m b/full_quadrotor_barrier.m
new file mode 100644
index 0000000..245dd31
--- /dev/null
+++ b/full_quadrotor_barrier.m
@@ -0,0 +1,139 @@
+function [dyn] = full_quadrotor_barrier(dt, xf)
+%   Detailed explanation goes here
+
+% Get parameters
+m = 0.5;
+g = 9.81;
+kt = 0.01691;
+l = 0.17;
+Ixx = 0.0032;
+Iyy = 0.0032;
+Izz = 0.0055;
+
+syms x y z phi theta ps p q r vx vy vz u1 u2 u3 u4 w;  % psi called "ps" for namespace reasons
+
+pos = [x; y; z];
+coord_vel = [vx; vy; vz];
+euler_angle = [phi; theta; ps];
+body_rate = [p; q; r];
+state = [pos; coord_vel; euler_angle; body_rate; w];
+controls = [u1; u2; u3; u4];
+vars = [state; controls];
+
+%% Get equations of motion
+% Linear EOM first
+% Starting with R
+% Define individual rotation matrices
+R1_phi = [1, 0, 0;
+          0, cos(phi), sin(phi);
+          0, -sin(phi), cos(phi)];
+      
+R2_theta = [cos(theta), 0, -sin(theta);
+            0, 1, 0;
+            sin(theta), 0, cos(theta)];
+        
+R3_psi = [cos(ps), sin(ps), 0;
+          -sin(ps), cos(ps), 0;
+          0, 0, 1];
+      
+% Combine rotation matrices
+R = R1_phi * R2_theta * R3_psi;
+
+% Linear acceleration
+lin_acc = (1/m)*R*[0;0; u1 + u2 + u3 + u4] + [0;0;-g];
+
+%% Rotational EOM
+
+% Rotational acceleration by Euler's eqn
+rot_acc = [(1/Ixx)*((sqrt(2)/2)*(u1+u3-u2-u4)*l - (Izz - Iyy)*q*r);
+    (1/Iyy)*((sqrt(2)/2)*(u3+u4-u1-u2)*l - (Izz - Ixx)*p*r);
+    (1/Izz)*kt*(u1+u4-u2-u3)];
+
+% Set eom
+eom = [lin_acc; rot_acc];
+
+% body rates to euler rates
+P = [1, tan(theta)*sin(phi), tan(theta)*cos(phi);
+     0, cos(phi), -sin(phi);
+     0, sin(phi)/cos(theta), cos(phi)/cos(theta)];
+
+% Euler integrate to get next state
+f = [pos + dt * coord_vel;
+    coord_vel + dt * lin_acc;
+    euler_angle + dt * P * body_rate;
+    body_rate + dt * rot_acc;
+    0];  % placeholder
+
+% now update barrier state based on new state
+barrier_func = get_barrier_func();
+f(13) = barrier_func(f) - barrier_func(xf);
+
+% take derivatives and convert to matlab functions
+fx = jacobian(f, state);
+fu = jacobian(f, controls);
+
+numeric_f = matlabFunction(f, Vars=vars);
+numeric_fx = matlabFunction(fx, Vars=vars);
+numeric_fu = matlabFunction(fu, Vars=vars);
+
+numeric_fxx = {};
+numeric_fxu = {};
+numeric_fuu = {};
+
+for i = 1:13
+    numeric_fxx{i} = matlabFunction(hessian(f(i), state), Vars=vars);
+    numeric_fxu{i} = matlabFunction(jacobian(jacobian(f(i), state), controls), Vars=vars);
+    numeric_fuu{i} = matlabFunction(hessian(f(i), controls), Vars=vars);
+end
+
+
+dyn = @(x, u) dynamics_wrapper(x, u, numeric_f, numeric_fx, numeric_fu, numeric_fxx, numeric_fxu, numeric_fuu);
+
+end
+
+
+function [wrapper_f, wrapper_fx, wrapper_fu, wrapper_fxx, wrapper_fxu, wrapper_fuu] = dynamics_wrapper(state, control, num_f, num_fx, num_fu, num_fxx, num_fxu, num_fuu)
+    % Deconstruct state vector
+    x = state(1);  % translational pos
+    y = state(2);
+    z = state(3);
+    vx = state(4);  % translational vel
+    vy = state(5);
+    vz = state(6);
+    phi = state(7);  % euler pitch roll yaw
+    theta = state(8);
+    psi = state(9);
+    p = state(10);  % body roll pitch yaw
+    q = state(11);
+    r = state(12);
+    w = state(13);
+
+    % Deconstruct control vector
+    u1 = control(1);
+    u2 = control(2);
+    u3 = control(3);
+    u4 = control(4);
+
+    % Clamp controls
+    bound = 10; % N
+    u1 = clip(u1,-bound,bound);
+    u2 = clip(u2,-bound,bound);
+    u3 = clip(u3,-bound,bound);
+    u4 = clip(u4,-bound,bound);
+
+    wrapper_f = num_f(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,w,u1,u2,u3,u4);
+    wrapper_fx = num_fx(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,w,u1,u2,u3,u4);
+    wrapper_fu = num_fu(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,w,u1,u2,u3,u4);
+	%wrapper_f = num_f(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,w,u1,u2,u3,u4);
+
+    wrapper_fxx = zeros(13, 13, 13);
+    wrapper_fxu = zeros(13, 13, 4);
+    wrapper_fuu = zeros(13, 4, 4);
+
+    for i = 1:12
+        wrapper_fxx(i, :, :) = num_fxx{i}(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,w,u1,u2,u3,u4);
+        wrapper_fxu(i, :, :) = num_fxu{i}(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,w,u1,u2,u3,u4);
+        wrapper_fuu(i, :, :) = num_fuu{i}(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,w,u1,u2,u3,u4);
+    end
+end
+
diff --git a/fwd_pass.m b/fwd_pass.m
index 6b5afe7..00789c0 100644
--- a/fwd_pass.m
+++ b/fwd_pass.m
@@ -1,4 +1,4 @@
-function [states, controls, costs] = fwd_pass(ic, controller, dynamics_wrapper, costfn, term_costfn)
+function [states, controls, costs] = fwd_pass(ic, controller, dynamics_wrapper, costfn, term_costfn, alpha, xf)
 %FWD_PASS Forward Pass for iLQR / DDP.
 %   This function computes the forward pass for the current control law.
 %
@@ -50,6 +50,8 @@
 %   time step t. Note that costs(end) (i.e., costs(tf)) should contain the
 %   value of the terminal cost function.
 
+% alpha: step size float
+
 % Setup Variables
 tf = size(controller.k, 1);
 n = size(ic, 1);
@@ -60,6 +62,9 @@
 costs = zeros(tf + 1, 1);
 
 states(1, :) = ic.';
+% update w for initial state
+barrier_func = get_barrier_func();
+states(1,13) = barrier_func(ic) - barrier_func(xf);
 
 %% Your Code Below
 
@@ -70,7 +75,7 @@
     xbar = controller.states(t,:);  % current xbar
     ubar = controller.controls(t,:);  % current ubar
     K = squeeze(controller.K(t,:,:));  % current K
-    k = (controller.k(t,:));  % current k
+    k = alpha * (controller.k(t,:));  % current k given step size
     control = ubar + (K * (state.' - xbar.')).' + k;
     controls(t,:) = control;  % append
     
diff --git a/get_barrier_func.m b/get_barrier_func.m
new file mode 100644
index 0000000..22149fc
--- /dev/null
+++ b/get_barrier_func.m
@@ -0,0 +1,10 @@
+function barrier_func = get_barrier_func()
+% x: state vec, rank 12 or 13
+
+h1 = @(x) (x(1) - 2.2)^2 + (x(3) - 1)^2 - 1 + (x(8) - 2.2)^2;
+h2 = @(x) x(1)^2 + (x(2) + 0.2)^2 + x(3)^2 - 1;
+h3 = @(x) (x(1) - 3)^2 + x(2)^2 + (x(3) - 0.5)^2 - 1;
+h = @(x) h1(x) + h2(x) + h3(x);  % yields scalar
+barrier_func = @(x)-log10(h(x));  % scalar
+
+end
\ No newline at end of file
diff --git a/main.m b/main.m
index 2ff7adf..acd33e4 100644
--- a/main.m
+++ b/main.m
@@ -1,28 +1,29 @@
 clear; clc; close all;
 
 % Given parameters
-x0 = [-3, -2, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0].';  % initial state
-xf = [5, 3, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0].';  % final state
+x0 = [-3, -2, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0].';  % initial state
+xf = [5, 3, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0].';  % final state
 tf = 8;  % final time
 dt = 0.01;  % time step
+t_arr = 0:dt:tf;
 
 % set up dynamics
-dyn = full_quadrotor(dt);
-
-% Tune here!
-% Q = 0.244*eye(length(x0));
-% R = 112.1*eye(4);
-% Qf = 37*eye(length(x0));
-Q = 0.244*eye(length(x0));
-R = 112.1*eye(4);
-Qf = 37*eye(length(x0));
-
-iters = 15;
-%regularizer = 1;
-% iters = 5;
-regularizer = 0.1;
-mode = "ddp";
-initial_controls = 0.612*ones(tf / dt, 4);  % initialize to neutral thrust
+dyn = full_quadrotor_barrier(dt, xf);
+
+pos_gain = 1;
+vel_gain = 1;
+ang_gain = 1;
+ang_vel_gain = 1;
+w_gain = 0;  % no barrier state
+Q = diag([pos_gain, pos_gain, pos_gain, vel_gain, vel_gain, vel_gain, ang_gain, ang_gain, ang_gain, ang_vel_gain, ang_vel_gain, ang_vel_gain, w_gain]);
+R = 0.8*eye(4);
+Qf = 180*Q;
+
+iters = 13;
+regularizer = 1;  % initial value. Will increment automatically unless this is 0
+line_search_iters = 2;  % 1 for no line search
+mode = "ilqr";
+initial_controls = 1.225*ones(tf / dt, 4);  % initialize to neutral thrust
 ic = x0;
 
 % get cost functions
@@ -31,42 +32,104 @@
 % form: [cost, cx, cxx] = term_costfn(state)
 
 % run controller
-[controller, total_costs] = ddp(ic, initial_controls, iters, regularizer, dyn, costfn, term_costfn, mode);
+[controller, total_costs] = ddp(ic, initial_controls, iters, regularizer, dyn, costfn, term_costfn, mode, line_search_iters);
 
 total_costs(end)
 final_cost = norm(controller.states(end,:) - xf)
 
 
-% Plot result
+%% Plot result
 xs = controller.states(:,1);
 ys = controller.states(:,2);
 zs = controller.states(:,3);
 
 figure(1)
-title("Position")
+
 plot3(xs,ys,zs)
-xlabel("X")
-ylabel("Y")
-zlabel("Z")
 hold on
 
-plot3(-3,-2,-1,"ro") %Initial
-plot3(5,3,2,"rx") %Final
+plot3(-3,-2,-1,"ro")
+plot3(5,3,2,"rx")
 legend(["Flight path","Start Point","Goal"])
+axis("equal")
+grid("on")
+xlabel('X')
+ylabel('Y')
+zlabel('Z')
+title("Position")
+saveas(gcf, './normal/3d.png')
+
+%% 2D plots
 
 figure(2)
+
+plot(t_arr(1:end-1), controller.controls(:,1))
 title("Controls")
-plot(controller.controls(:,1))
+xlabel("Time (s)")
+ylabel("Contol")
+grid on
 hold on
-plot(controller.controls(:,2))
-plot(controller.controls(:,3))
-plot(controller.controls(:,4))
+plot(t_arr(1:end-1), controller.controls(:,2))
+plot(t_arr(1:end-1), controller.controls(:,3))
+plot(t_arr(1:end-1), controller.controls(:,4))
+
 legend(["u1","u2","u3","u4"])
+xlabel('Time (s)')
+ylabel('Control Input (N)')
+title("Controls")
+saveas(gcf, './normal/controls.png')
 
 figure(3)
+plot(t_arr, controller.states(:,7))
 title("Attitude")
-plot(controller.states(:,4))
+xlabel("Time (s)")
+ylabel("Rad")
+grid on
 hold on
-plot(controller.states(:,5))
-plot(controller.states(:,6))
+plot(t_arr, controller.states(:,8))
+plot(t_arr, controller.states(:,9))
+
 legend(["\phi","\theta","\psi"])
+xlabel('Time (s)')
+ylabel('Angle (rad)')
+title("Attitude")
+saveas(gcf, './normal/attitude.png')
+
+figure(4)
+plot(t_arr, controller.states(:, 10))
+title("Body Rate")
+xlabel("Time (s)")
+ylabel("Rad/sec")
+grid on
+hold on
+plot(t_arr, controller.states(:, 11))
+plot(t_arr, controller.states(:, 12))
+legend(["p","q","r"])
+xlabel('Time (s)')
+ylabel('Angular Velocity (rad/s)')
+title("Body Rate")
+saveas(gcf, './normal/ang_vel.png')
+
+figure(5)
+plot(t_arr, controller.states(:,1))
+grid on
+hold on
+plot(t_arr, controller.states(:,2))
+plot(t_arr, controller.states(:,3))
+legend(["x","y","z"])
+xlabel('Time (s)')
+ylabel('Position (m)')
+title("Position")
+saveas(gcf, './normal/position.png')
+
+figure(6)
+plot(t_arr, controller.states(:,4))
+grid on
+hold on
+plot(t_arr, controller.states(:,5))
+plot(t_arr, controller.states(:,6))
+legend(["vx","vy","vz"])
+xlabel('Time (s)')
+title("Velocity")
+ylabel('Velocity (m/s)')
+saveas(gcf, './normal/velocity.png')
diff --git a/main_barrier.m b/main_barrier.m
new file mode 100644
index 0000000..af35441
--- /dev/null
+++ b/main_barrier.m
@@ -0,0 +1,193 @@
+clear; clc; close all;
+
+% Given parameters
+x0 = [-3, -2, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0].';  % initial state. Added w
+xf = [5, 3, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0].';  % final state. Added w
+tf = 8;  % final time
+dt = 0.01;  % time step
+t_arr = 0:dt:tf;
+
+
+% set up dynamics
+dyn = full_quadrotor_barrier(dt, xf);  % now needs xf too 
+
+% Tune here!
+pos_gain = 1;
+vel_gain = 1;
+ang_gain = 1;
+ang_vel_gain = 1;
+w_gain = 100;
+Q = diag([pos_gain, pos_gain, pos_gain, vel_gain, vel_gain, vel_gain, ang_gain, ang_gain, ang_gain, ang_vel_gain, ang_vel_gain, ang_vel_gain, w_gain]);
+R = 0.8*eye(4);
+Qf = 180*Q;
+
+iters = 13;
+regularizer = 1;  % initial value. Will increment automatically unless this is 0
+line_search_iters = 2;  % 1 for no line search
+mode = "ilqr";
+initial_controls = 1.225*ones(tf / dt, 4);  % initialize to neutral thrust
+ic = x0;
+
+% get cost functions
+[costfn, term_costfn] = quad_cost(Q, R, Qf, xf);
+% form: [cost, cx, cu, cxx, cxu, cuu] = costfn(state, control)
+% form: [cost, cx, cxx] = term_costfn(state)
+
+% run controller
+[controller, total_costs] = ddp(ic, initial_controls, iters, regularizer, dyn, costfn, term_costfn, mode, line_search_iters, xf);
+
+total_costs(end)
+final_cost = norm(controller.states(end,:) - xf)
+
+
+%% Plot result
+xs = controller.states(:,1);
+ys = controller.states(:,2);
+zs = controller.states(:,3);
+
+% Define the grid size and the range
+grid_size = 50;
+x_range = linspace(-5, 5, grid_size);
+y_range = linspace(-5, 5, grid_size);
+z_range = linspace(-5, 5, grid_size);
+
+% Create the 3D grid of points
+[X, Y, Z] = meshgrid(x_range, y_range, z_range);
+
+% Define the implicit functions for the spheres
+h1 = @(x, y, z) (x - 2.2).^2 + (z - 1).^2 - 1;
+h2 = @(x, y, z) x.^2 + (y + 0.2).^2 + z.^2 - 1;
+h3 = @(x, y, z) (x - 3).^2 + y.^2 + (z - 0.5).^2 - 1;
+
+% Evaluate the functions on the grid
+H1 = h1(X, Y, Z);
+H2 = h2(X, Y, Z);
+H3 = h3(X, Y, Z);
+
+%% plot trajectory
+figure(1)
+
+plot3(xs,ys,zs)
+hold on
+plot3(-3,-2,-1,"ro")
+plot3(5,3,2,"rx")
+
+legend(["Flight path","Start Point","Goal"])
+axis("equal")
+grid("on")
+xlabel('X')
+ylabel('Y')
+zlabel('Z')
+title("Position")
+saveas(gcf, './barrier/3d_nobarriers.png')
+
+% now plot with barriers
+figure(2)
+plot3(xs,ys,zs)
+hold on
+plot3(-3,-2,-1,"ro")
+plot3(5,3,2,"rx")
+
+% plot barriers
+
+h1_surf = isosurface(X, Y, Z, H1, 0);
+h2_surf = isosurface(X, Y, Z, H2, 0);
+h3_surf = isosurface(X, Y, Z, H3, 0);
+p1 = patch(h1_surf, 'FaceColor', 'red', 'EdgeColor', 'none', 'FaceAlpha', 0.5);
+p2 = patch(h2_surf, 'FaceColor', 'green', 'EdgeColor', 'none', 'FaceAlpha', 0.5);
+p3 = patch(h3_surf, 'FaceColor', 'blue', 'EdgeColor', 'none', 'FaceAlpha', 0.5);
+
+legend(["Flight path","Start Point","Goal"])
+axis("equal")
+grid("on")
+xlabel('X')
+ylabel('Y')
+zlabel('Z')
+title("Position")
+saveas(gcf, './barrier/3d.png')
+
+%% 2D plots
+
+figure(3)
+
+plot(t_arr(1:end-1), controller.controls(:,1))
+title("Controls")
+xlabel("Time (s)")
+ylabel("Contol")
+grid on
+hold on
+plot(t_arr(1:end-1), controller.controls(:,2))
+plot(t_arr(1:end-1), controller.controls(:,3))
+plot(t_arr(1:end-1), controller.controls(:,4))
+
+legend(["u1","u2","u3","u4"])
+xlabel('Time (s)')
+ylabel('Control Input (N)')
+title("Controls")
+saveas(gcf, './barrier/controls.png')
+
+figure(4)
+plot(t_arr, controller.states(:,7))
+title("Attitude")
+xlabel("Time (s)")
+ylabel("Rad")
+grid on
+hold on
+plot(t_arr, controller.states(:,8))
+plot(t_arr, controller.states(:,9))
+
+legend(["\phi","\theta","\psi"])
+xlabel('Time (s)')
+ylabel('Angle (rad)')
+title("Attitude")
+saveas(gcf, './barrier/attitude.png')
+
+figure(5)
+plot(t_arr, controller.states(:, 10))
+title("Body Rate")
+xlabel("Time (s)")
+ylabel("Rad/sec")
+grid on
+hold on
+plot(t_arr, controller.states(:, 11))
+plot(t_arr, controller.states(:, 12))
+legend(["p","q","r"])
+xlabel('Time (s)')
+ylabel('Angular Velocity (rad/s)')
+title("Body Rate")
+saveas(gcf, './barrier/ang_vel.png')
+
+figure(6)
+plot(t_arr, controller.states(:,1))
+grid on
+hold on
+plot(t_arr, controller.states(:,2))
+plot(t_arr, controller.states(:,3))
+legend(["x","y","z"])
+xlabel('Time (s)')
+ylabel('Position (m)')
+title("Position")
+saveas(gcf, './barrier/position.png')
+
+figure(7)
+plot(t_arr, controller.states(:,4))
+grid on
+hold on
+plot(t_arr, controller.states(:,5))
+plot(t_arr, controller.states(:,6))
+legend(["vx","vy","vz"])
+xlabel('Time (s)')
+title("Velocity")
+ylabel('Velocity (m/s)')
+saveas(gcf, './barrier/velocity.png')
+
+% barrier state plot
+figure(8)
+plot(t_arr, controller.states(:,13))
+grid on
+xlabel('Time (s)')
+ylabel('Barrier State')
+title("Barrier State")
+saveas(gcf, './barrier/barrier_state.png')
+
+
diff --git a/main_receding_mpc.m b/main_receding_mpc.m
new file mode 100644
index 0000000..ed8f687
--- /dev/null
+++ b/main_receding_mpc.m
@@ -0,0 +1,160 @@
+clear; clc; close all;
+
+% Given parameters
+x0 = [-3, -2, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0].';  % initial state
+xf = [5, 3, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0].';  % final state
+dt = 0.01;  % time step
+sim_horizon = 800;   % total sim time (iterations, i.e 50 => 0.5 seconds)
+planning_horizon = 3;  % "look ahead" amount (seconds)
+n = length(x0);
+m = 4;
+
+t_arr = (0:sim_horizon) * dt;
+
+% set up dynamics
+dyn = full_quadrotor_barrier(dt, xf);
+
+% Tune here!
+pos_gain = 1;
+vel_gain = 1;
+ang_gain = 1;
+ang_vel_gain = 1;
+w_gain = 0;  % no barrier state
+Q = diag([pos_gain, pos_gain, pos_gain, vel_gain, vel_gain, vel_gain, ang_gain, ang_gain, ang_gain, ang_vel_gain, ang_vel_gain, ang_vel_gain, w_gain]);
+R = 1*eye(4);
+Qf = 10*Q;
+
+iters = 1;
+regularizer = 10;  % initial value. Will increment automatically
+line_search_iters = 3;
+mode = "ilqr";
+initial_controls = 1.225*ones(planning_horizon / dt, 4);  % initialize to neutral thrust
+ic = x0;
+
+% get cost functions
+[costfn, term_costfn] = quad_cost(Q, R, Qf, xf);
+
+% run controller receding horizon
+controls = initial_controls;
+
+state_hist = zeros(n, sim_horizon);
+contol_hist = zeros(m, sim_horizon);
+
+current_state = ic;
+for t = 1:sim_horizon
+    disp(t)
+    % get controller
+    %if mod(t, 4)
+    [controller, total_costs] = ddp(current_state, controls, iters, regularizer, dyn, costfn, term_costfn, mode, line_search_iters);
+    %end
+    % use difference in current state and desired to get controls
+    K = squeeze(controller.K(1,:,:));
+    k = controller.k(1,:);
+    ubar = controller.controls(1,:);
+    xbar = controller.states(1,:);
+    controls = ubar + (K * (current_state - xbar.')).' + k;
+
+    % increment state
+    [current_state, ~, ~, ~, ~, ~] = dyn(current_state, controls(1, :));
+
+    % warm start
+    controls = controller.controls; %; controller.controls(end,:)];
+
+    % save vars
+    % TODO: save here for plotting purposes
+    states(t,:,:) = controller.states;
+
+    state_hist(:, t) = current_state;
+    contol_hist(:, t) = controls(1, :).';
+end
+
+total_costs(end)
+final_cost = norm(controller.states(end,:) - xf)
+
+
+%% Plot result
+
+xs = state_hist(1, :);
+ys = state_hist(2, :);
+zs = state_hist(3, :);
+
+figure(1)
+plot3(xs,ys,zs)
+xlabel("X")
+ylabel("Y")
+zlabel("Z")
+hold on
+
+plot3(-3,-2,-1,"ro")
+plot3(5,3,2,"rx")
+legend(["Flight path","Start Point","Goal"])
+grid("on")
+axis("equal")
+title("Position")
+saveas(gcf, './receding/3d.png')
+
+
+%% 2D plots
+
+figure(2)
+plot(t_arr(1:end-1), contol_hist(1, :))
+hold on
+grid on
+plot(t_arr(1:end-1), contol_hist(2, :))
+plot(t_arr(1:end-1), contol_hist(3, :))
+plot(t_arr(1:end-1), contol_hist(4, :))
+legend(["u1","u2","u3","u4"])
+xlabel('Time (s)')
+ylabel('Control Input (N)')
+title("Controls")
+saveas(gcf, './receding/controls.png')
+
+figure(3)
+plot(t_arr(1:end-1), state_hist(7, :))
+hold on
+grid on
+plot(t_arr(1:end-1), state_hist(8, :))
+plot(t_arr(1:end-1), state_hist(9, :))
+legend(["\phi","\theta","\psi"])
+xlabel('Time (s)')
+ylabel('Angle (rad)')
+title("Attitude")
+saveas(gcf, './receding/attitude.png')
+
+figure(4)
+
+plot(t_arr(1:end-1), state_hist(10, :))
+grid on
+hold on
+plot(t_arr(1:end-1), state_hist(11, :))
+plot(t_arr(1:end-1), state_hist(12, :))
+legend(["p","q","r"])
+xlabel('Time (s)')
+ylabel('Angular Velocity (rad/s)')
+title("Body Rate")
+saveas(gcf, './receding/ang_vel.png')
+
+figure(5)
+
+plot(t_arr(1:end-1), state_hist(1, :))
+grid on
+hold on
+plot(t_arr(1:end-1), state_hist(2, :))
+plot(t_arr(1:end-1), state_hist(3, :))
+legend(["x","y","z"])
+xlabel('Time (s)')
+ylabel('Position (m)')
+title("Position")
+saveas(gcf, './receding/position.png')
+
+figure(6)
+plot(t_arr(1:end-1), state_hist(4, :))
+grid on
+hold on
+plot(t_arr(1:end-1), state_hist(5, :))
+plot(t_arr(1:end-1), state_hist(6, :))
+legend(["vx","vy","vz"])
+xlabel('Time (s)')
+ylabel('Velocity (m/s)')
+title("Velocity")
+saveas(gcf, './receding/velocity.png')
diff --git a/simmpc.m b/simmpc.m
deleted file mode 100644
index a038213..0000000
--- a/simmpc.m
+++ /dev/null
@@ -1,117 +0,0 @@
-function [states, controls] = simmpc(ic, sim_horizon, initial_controls, ddp_iters, regularizer, dyn, costfn, term_costfn, mode)
-%SIMMPC Simulation of receding-horizon model-predictive control.
-%
-%   This function simulates the execution of iLQR / DDP in a
-%   receding-horizon manner. That is, at each time step, the selected
-%   control algorithm is run for some number of iterations using a planning
-%   horizon of a selected length. The feedback controller returned by the
-%   algorithm for the first time step of the planning horizon is then used
-%   to compute a control input. This control is then used to advance the
-%   system state. This process is repeated using the new system state as
-%   the initial condition for the optimal control algorithm.
-%
-%   The first execution of iLQR / DDP will use a guessed control sequence
-%   to initialize the optimizer. Subsequently, you should warm-start the
-%   process, i.e., use the nominal control sequence (u_bar) returned by the
-%   control algorithm at the previous time step as the initial guess for
-%   the current time step.
-%
-%   ic: An n-by-1 state serving as the initial condition for the control
-%   problem.
-%
-%   sim_horizon: An integer representing the number of time steps for which
-%   the receding-horizon algorithm is implemented. That is, we will run
-%   iLQR / DDP and advance the system state based on the resulting control
-%   law sim_horizon number of times.
-%
-%   initial_controls: The initial guess control sequence for the
-%   simulation. This is fed to iLQR / DDP at the first time. It is an
-%   h-by-m matrix, where (h + 1) is the planning horizon used by iLQR / DDP.
-%
-%   ddp_iters: The number of iterations of iLQR / DDP to run each time
-%   step.
-%
-%   regularizer: The value of the regularizer passed to iLQR / DDP.
-%
-%   dyn: A function defining the robot dynamics and pertinent derivatives.
-%   It has the form:
-%
-%       [next_state, fx, fu, fxx, fxu, fuu] = dynamics_wrapper(state, control)
-%
-%   All derivatives are evaluated at (state, control).
-%
-%   costfn: A function that computes the cost at each time step before the
-%   terminal step. It also evaluates the pertinent derivatives at the
-%   current state and control. It has the form:
-%
-%       [cost, cx, cu, cxx, cxu, cuu] = costfn(state, control).
-%
-%   term_costfn: A function that computes the terminal cost as well as the
-%   derivatives of the terminal cost function at a given state. It has the
-%   form:
-%
-%       [cost, cx, cxx] = term_costfn(state)
-%
-%   mode: A string indicating whether to use iLQR or DDP. To use ddp, set
-%
-%       mode = 'ddp'
-%
-%   The default is to use iLQR.
-%
-%
-%   Returns:
-%
-%   states: A sim_horizon-by-(h + 1)-by-n array. At simulation time step t,
-%   the array states(t, :, :) contains the nominal states planned by iLQR /
-%   DDP (i.e., controller.states).
-%
-%   controls: A sim_horizon-by-h-by-m array. At simulation time step
-%   t, the array controls(t, :, :) contains the nominal controls planned by
-%   iLQR / DDP (i.e., controller.controls).
-
-% Setup Variables
-n = size(ic, 1);
-m = size(initial_controls, 2);
-planning_horizon = size(initial_controls, 1) + 1; % h + 1.
-
-states = zeros(sim_horizon, planning_horizon, n);
-controls = initial_controls;
-
-current_state = ic;
-controls_history = zeros(sim_horizon, m);
-
-% pseudocode to help developing
-% for t = 1:sim_horizon
-% run ddp for planning_horizon timesteps and ddp_iters iters
-% apply controller to get next state
-% current_state = next_state
-
-
-for t = 1:sim_horizon
-    % get controller
-    [controller, ~] = ddp(current_state, controls, ddp_iters, regularizer, dyn, costfn, term_costfn, mode);
-    
-    % add states and controls to the final output
-    % states(t,:,:) = controller.states;
-    % controls = controller.controls(1,:);  % first one
-    
-    % use difference in current state and desired to get controls
-    K = squeeze(controller.K(1,:,:));
-    k = controller.k(1,:);
-    ubar = controller.controls(1,:);
-    xbar = controller.states(1,:);
-    controls = ubar + (K * (current_state - xbar.')).' + k;
-
-    % increment state
-    [current_state, ~, ~, ~, ~, ~] = dyn(current_state, controls);
-
-    % warm start
-    controls = controller.controls; %; controller.controls(end,:)];
-
-    % save vars
-    states(t,:,:) = controller.states;
-
-end
-
-end
-
diff --git a/tuning.m b/tuning.m
index e6a1ff6..112ab37 100644
--- a/tuning.m
+++ b/tuning.m
@@ -1,8 +1,9 @@
 %% For tuning parameters
 clear; close all; clc;
 
-p0 = [0.244;112.1;37;1];
-p0 = [0.1234;246.39;23.0943;1];
+% p0 = [0.244;112.1;37;1];
+% p0 = [0.1234;246.39;23.0943;1];
+p0 = [100; 100; 10; 10; 1; 100];
 jiggle = 0.1;
 
 
@@ -11,9 +12,9 @@
 new_cost = 90;
 costs = [];
 while new_cost < old_cost
-    directions = zeros(1,8);
-    perturbs = [jiggle*p.*eye(4);-jiggle*p.*eye(4)];
-    for j = 1:8
+    directions = zeros(1,12);
+    perturbs = [jiggle*p.*eye(6); -jiggle*p.*eye(6)];
+    for j = 1:12
         perturb = perturbs(j,:).';
         directions(:,j) = one_run(p + perturb);
     end
@@ -37,12 +38,12 @@
     dyn = full_quadrotor(dt);
     
     % Tune here!
-    Q = parameters(1)*eye(length(x0));
-    R = parameters(2)*eye(4);
-    Qf = parameters(3)*eye(length(x0));
+    Q = diag([parameters(1), parameters(1), parameters(1), parameters(2), parameters(2), parameters(2), parameters(3), parameters(3), parameters(3), parameters(4), parameters(4), parameters(4)]);
+    R = parameters(5)*eye(4);
+    Qf = parameters(6)*Q;
     
     iters = 10;
-    regularizer = parameters(4);
+    regularizer = 1;
     mode = "ddp";
     initial_controls = 0.612*ones(tf / dt, 4);  % initialize to neutral thrust
     ic = x0;
@@ -53,7 +54,7 @@
     % form: [cost, cx, cxx] = term_costfn(state)
     
     % run controller
-    [controller, total_costs] = ddp(ic, initial_controls, iters, regularizer, dyn, costfn, term_costfn, mode);
+    [controller, total_costs] = ddp(ic, initial_controls, iters, regularizer, dyn, costfn, term_costfn, mode, 10);
     
     cost = norm(controller.states(end,:) - xf);
 end
\ No newline at end of file