From 82636165f20df272e7878452b394725626470722 Mon Sep 17 00:00:00 2001
From: Elliot Kantor <74844563+elliotkantor@users.noreply.github.com>
Date: Sun, 8 Dec 2024 14:22:47 -0500
Subject: [PATCH 01/21] Autoupdate regularizer term

---
 back_pass.m | 45 ++++++++++++++++++++++++++++-----------------
 1 file changed, 28 insertions(+), 17 deletions(-)

diff --git a/back_pass.m b/back_pass.m
index a868798..862e5af 100644
--- a/back_pass.m
+++ b/back_pass.m
@@ -104,25 +104,36 @@
     % 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,:).');
+    
+    % automatically increment regularizer if Quu not invertible
+    while true
+        % 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);
+    
+        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
 
-    % 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);
+        % check if Quu is positive definite
+        if all(eig(Quu) > 0)
+            break
+        end
+        % otherwise, increment regularizer
+        regularizer = regularizer * 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 % while true loop
 
     % Get new controls
     K = -inv(Quu)*Qux;

From 87e55157e7d4a302c43895ea86f9dae6ca082f2d Mon Sep 17 00:00:00 2001
From: Elliot Kantor <74844563+elliotkantor@users.noreply.github.com>
Date: Sun, 8 Dec 2024 14:51:02 -0500
Subject: [PATCH 02/21] Implement line search

---
 ddp.m      | 20 ++++++++++++++++----
 fwd_pass.m |  6 ++++--
 2 files changed, 20 insertions(+), 6 deletions(-)

diff --git a/ddp.m b/ddp.m
index 8519e32..62c637c 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)
 %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,7 @@
 %       mode = 'ddp'
 %
 %   The default is to use iLQR.
+% line_search_iters (int): max num of line search iterations. Min 1.
 
 % Setup variables
 tf = size(initial_controls, 1);
@@ -50,14 +51,25 @@
 
 total_costs = zeros(iters, 1);
 
+alphas = 1:-1/line_search_iters:0;  % decreasing step size sequence
+
 %% Your code below
 
-[states, controls, ~] = fwd_pass(ic, controller, dyn, costfn, term_costfn);
+[states, controls, last_costs] = fwd_pass(ic, controller, dyn, costfn, term_costfn, alphas(1));
 
 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)
+    
+    for j = 1:line_search_iters
+        % line search logic
+        [states, controls, new_costs] = fwd_pass(ic, controller, dyn, costfn, term_costfn, alphas(j+1));
+        if sum(new_costs) < sum(last_costs)
+            break
+        end
+        last_costs = new_costs;
+    end
+
+    total_costs(i,1) = sum(new_costs); 
 end
 
 end
diff --git a/fwd_pass.m b/fwd_pass.m
index 6b5afe7..9bd17be 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)
 %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);
@@ -70,7 +72,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
     

From bdfad659992a2a8a3191472017d33dd4836844bb Mon Sep 17 00:00:00 2001
From: Elliot Kantor <74844563+elliotkantor@users.noreply.github.com>
Date: Sun, 8 Dec 2024 15:14:44 -0500
Subject: [PATCH 03/21] Mess with tuning and autotune code

---
 back_pass.m |  2 +-
 main.m      | 27 ++++++++++++++-------------
 tuning.m    | 21 +++++++++++----------
 3 files changed, 26 insertions(+), 24 deletions(-)

diff --git a/back_pass.m b/back_pass.m
index 862e5af..65737bd 100644
--- a/back_pass.m
+++ b/back_pass.m
@@ -131,7 +131,7 @@
             break
         end
         % otherwise, increment regularizer
-        regularizer = regularizer * 2
+        regularizer = regularizer+0.01 * 2
 
     end % while true loop
 
diff --git a/main.m b/main.m
index 2ff7adf..6629804 100644
--- a/main.m
+++ b/main.m
@@ -13,14 +13,18 @@
 % 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;
+pos_gain = 100;
+vel_gain = 100;
+ang_gain = 10;
+ang_vel_gain = 10;
+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]);
+R = 15*eye(4);
+Qf = 100*Q;
+
+iters = 10;
+regularizer = 1;  % initial value. Will increment automatically unless this is 0
+line_search_iters = 10;
 mode = "ddp";
 initial_controls = 0.612*ones(tf / dt, 4);  % initialize to neutral thrust
 ic = x0;
@@ -31,7 +35,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, line_search_iters);
 
 total_costs(end)
 final_cost = norm(controller.states(end,:) - xf)
@@ -45,13 +49,10 @@
 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"])
 
 figure(2)
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

From 9c5179e85d7054f0aabf9c06abdf2890f8f2ebf9 Mon Sep 17 00:00:00 2001
From: Elliot Kantor <74844563+elliotkantor@users.noreply.github.com>
Date: Sun, 8 Dec 2024 15:34:43 -0500
Subject: [PATCH 04/21] Start WIP receding horizon code

---
 main_receding_mpc.m | 110 ++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 110 insertions(+)
 create mode 100644 main_receding_mpc.m

diff --git a/main_receding_mpc.m b/main_receding_mpc.m
new file mode 100644
index 0000000..a960476
--- /dev/null
+++ b/main_receding_mpc.m
@@ -0,0 +1,110 @@
+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
+dt = 0.01;  % time step
+sim_horizon = 8;   % total sim time
+planning_horizon = 3;  % "look ahead" amount
+
+% set up dynamics
+dyn = full_quadrotor(dt);
+
+% Tune here!
+pos_gain = 100;
+vel_gain = 100;
+ang_gain = 10;
+ang_vel_gain = 10;
+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]);
+R = 15*eye(4);
+Qf = 100*Q;
+
+iters = 10;
+regularizer = 1;  % initial value. Will increment automatically
+line_search_iters = 10;
+mode = "ddp";
+initial_controls = 0.612*ones(planning_horizon / dt, 4);  % initialize to neutral thrust
+ic = x0;
+
+% get cost functions
+[costfn, term_costfn] = quad_cost(Q, R, Qf, xf);
+
+%% MPC BELOW
+% PASTED FROM HW2 CODE:
+
+% 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
+
+% run controller receding horizon
+controls = initial_controls;
+current_state = ic;
+for t = 1:sim_horizon
+    % get controller
+    [controller, total_costs] = ddp(current_state, controls, iters, regularizer, dyn, costfn, term_costfn, mode, line_search_iters);
+
+    % 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
+    % TODO: save here for plotting purposes
+    states(t,:,:) = controller.states;
+end
+
+total_costs(end)
+final_cost = norm(controller.states(end,:) - xf)
+
+
+%% Plot result
+xs = controller.states(:,1);
+ys = controller.states(:,2);
+zs = controller.states(:,3);
+
+figure(1)
+title("Position")
+plot3(xs,ys,zs)
+hold on
+
+plot3(-3,-2,-1,"ro")
+plot3(5,3,2,"rx")
+legend(["Flight path","Start Point","Goal"])
+
+figure(2)
+title("Controls")
+plot(controller.controls(:,1))
+hold on
+plot(controller.controls(:,2))
+plot(controller.controls(:,3))
+plot(controller.controls(:,4))
+legend(["u1","u2","u3","u4"])
+
+figure(3)
+title("Attitude")
+plot(controller.states(:,4))
+hold on
+plot(controller.states(:,5))
+plot(controller.states(:,6))
+legend(["\phi","\theta","\psi"])

From 90f044186d6ffd28fb0858744d02c56ddd786668 Mon Sep 17 00:00:00 2001
From: Elliot Kantor <74844563+elliotkantor@users.noreply.github.com>
Date: Wed, 11 Dec 2024 12:34:19 -0500
Subject: [PATCH 05/21] Fix dynamics, break Quu mat

---
 full_quadrotor.m | 32 +++++++++++++++++++++++---------
 1 file changed, 23 insertions(+), 9 deletions(-)

diff --git a/full_quadrotor.m b/full_quadrotor.m
index fcfef6f..99aaf3f 100644
--- a/full_quadrotor.m
+++ b/full_quadrotor.m
@@ -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;  % 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];
 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,30 @@
 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]; 
 
+% take derivatives and convert to matlab functions
 fx = jacobian(f, state);
 fu = jacobian(f, controls);
 

From b2989c4c6ebbb3ea57a5d32549aaa6529c1e5b02 Mon Sep 17 00:00:00 2001
From: Elliot Kantor <74844563+elliotkantor@users.noreply.github.com>
Date: Wed, 11 Dec 2024 13:25:14 -0500
Subject: [PATCH 06/21] Improve line search, reg update

---
 back_pass.m | 28 +++++++++++++---------------
 ddp.m       | 14 +++++++-------
 main.m      | 12 ++++++------
 3 files changed, 26 insertions(+), 28 deletions(-)

diff --git a/back_pass.m b/back_pass.m
index 65737bd..1980714 100644
--- a/back_pass.m
+++ b/back_pass.m
@@ -104,19 +104,20 @@
     % 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;
     
     % automatically increment regularizer if Quu not invertible
-    while true
+    for i = 1:5
         % 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);
+        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) + regularizer*(fx.')*fx;
-            Qxu = cxu + (fx.')*Vxx_next*fu + fake_tensor_prod(Vx_next,fxu) + regularizer*(fx.')*(fu);
+            Qxx = cxx + (fx.')*Vxx_next*fx + fake_tensor_prod(Vx_next,fxx) + reg*(fx.')*fx;
+            Qxu = cxu + (fx.')*Vxx_next*fu + fake_tensor_prod(Vx_next,fxu) + reg*(fx.')*(fu);
             Qux = Qxu.';
-            Quu = cuu + (fu.')*Vxx_next*fu + fake_tensor_prod(Vx_next,fuu) + regularizer*(fu.')*(fu);
+            Quu = cuu + (fu.')*Vxx_next*fu + fake_tensor_prod(Vx_next,fuu) + reg*(fu.')*(fu);
     
         elseif strcmp(mode,'ilqr')
             % Get second-order derivatives of Q without tensor prod
@@ -127,17 +128,17 @@
         end
 
         % check if Quu is positive definite
-        if all(eig(Quu) > 0)
+        if all(eig(Quu) > 1e-6) && rcond(Quu) > 1e-8
             break
         end
         % otherwise, increment regularizer
-        regularizer = regularizer+0.01 * 2
+        reg = (reg+0.01) * 2
 
-    end % while true loop
+    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;
@@ -148,10 +149,7 @@
     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)
diff --git a/ddp.m b/ddp.m
index 62c637c..dda8170 100644
--- a/ddp.m
+++ b/ddp.m
@@ -51,25 +51,25 @@
 
 total_costs = zeros(iters, 1);
 
-alphas = 1:-1/line_search_iters:0;  % decreasing step size sequence
+% alphas = 1:-1/line_search_iters:0;  % decreasing step size sequence
+alphas = exp(-(0:line_search_iters));
 
 %% Your code below
 
-[states, controls, last_costs] = fwd_pass(ic, controller, dyn, costfn, term_costfn, alphas(1));
+[states, controls, best_costs] = fwd_pass(ic, controller, dyn, costfn, term_costfn, alphas(1));
 
 for i = 1:iters
     [controller, ~] = back_pass(states, controls, dyn, costfn, term_costfn, regularizer, mode);
     
-    for j = 1:line_search_iters
+    for j = 1:line_search_iters-1
         % line search logic
         [states, controls, new_costs] = fwd_pass(ic, controller, dyn, costfn, term_costfn, alphas(j+1));
-        if sum(new_costs) < sum(last_costs)
-            break
+        if sum(new_costs) < sum(best_costs)
+            best_costs = new_costs;
         end
-        last_costs = new_costs;
     end
 
-    total_costs(i,1) = sum(new_costs); 
+    total_costs(i,1) = sum(best_costs); 
 end
 
 end
diff --git a/main.m b/main.m
index 6629804..3925420 100644
--- a/main.m
+++ b/main.m
@@ -10,8 +10,8 @@
 dyn = full_quadrotor(dt);
 
 % Tune here!
-% Q = 0.244*eye(length(x0));
-% R = 112.1*eye(4);
+% Q = 100.244*eye(length(x0));
+% R = 12.1*eye(4);
 % Qf = 37*eye(length(x0));
 
 pos_gain = 100;
@@ -19,12 +19,12 @@
 ang_gain = 10;
 ang_vel_gain = 10;
 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]);
-R = 15*eye(4);
-Qf = 100*Q;
+R = 1*eye(4);
+Qf = 10*Q;
 
 iters = 10;
 regularizer = 1;  % initial value. Will increment automatically unless this is 0
-line_search_iters = 10;
+line_search_iters = 10;  % don't line search
 mode = "ddp";
 initial_controls = 0.612*ones(tf / dt, 4);  % initialize to neutral thrust
 ic = x0;
@@ -41,7 +41,7 @@
 final_cost = norm(controller.states(end,:) - xf)
 
 
-% Plot result
+%% Plot result
 xs = controller.states(:,1);
 ys = controller.states(:,2);
 zs = controller.states(:,3);

From 8763077a7e66cb6f4fcfb7a21ae6b0c5cbbd2570 Mon Sep 17 00:00:00 2001
From: Elliot Kantor <74844563+elliotkantor@users.noreply.github.com>
Date: Wed, 11 Dec 2024 14:11:00 -0500
Subject: [PATCH 07/21] Improve line search, play w params

---
 back_pass.m | 16 ++++++++++++----
 ddp.m       |  2 ++
 main.m      |  8 ++++----
 3 files changed, 18 insertions(+), 8 deletions(-)

diff --git a/back_pass.m b/back_pass.m
index 1980714..dbed9c9 100644
--- a/back_pass.m
+++ b/back_pass.m
@@ -114,10 +114,10 @@
     
         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*fu + fake_tensor_prod(Vx_next,fxu) + reg*(fx.')*(fu);
+            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*fu + fake_tensor_prod(Vx_next,fuu) + reg*(fu.')*(fu);
+            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
@@ -131,8 +131,16 @@
         if all(eig(Quu) > 1e-6) && rcond(Quu) > 1e-8
             break
         end
+        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+0.01) * 2
+        reg = reg * 2
 
     end % regularizer update loop
 
diff --git a/ddp.m b/ddp.m
index dda8170..5fcd393 100644
--- a/ddp.m
+++ b/ddp.m
@@ -69,6 +69,8 @@
         end
     end
 
+    % update to a newer controller
+    [controller, ~] = back_pass(states, controls, dyn, costfn, term_costfn, regularizer, mode);
     total_costs(i,1) = sum(best_costs); 
 end
 
diff --git a/main.m b/main.m
index 3925420..5cc1ce6 100644
--- a/main.m
+++ b/main.m
@@ -19,12 +19,12 @@
 ang_gain = 10;
 ang_vel_gain = 10;
 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]);
-R = 1*eye(4);
-Qf = 10*Q;
+R = .01*eye(4);
+Qf = 100*Q;
 
 iters = 10;
-regularizer = 1;  % initial value. Will increment automatically unless this is 0
-line_search_iters = 10;  % don't line search
+regularizer = 0;  % initial value. Will increment automatically unless this is 0
+line_search_iters = 1;  % 1 for no line search
 mode = "ddp";
 initial_controls = 0.612*ones(tf / dt, 4);  % initialize to neutral thrust
 ic = x0;

From 7d10b7bde4fd5488704154e13868b8c94a5b40f6 Mon Sep 17 00:00:00 2001
From: David-Rey <71460537+David-Rey@users.noreply.github.com>
Date: Wed, 11 Dec 2024 16:12:38 -0500
Subject: [PATCH 08/21] fixed dynamics issue, still not working. getting nans
 in Quu

---
 full_quadrotor.m | 13 +++++++------
 main.m           |  8 ++++----
 2 files changed, 11 insertions(+), 10 deletions(-)

diff --git a/full_quadrotor.m b/full_quadrotor.m
index 99aaf3f..fe7767e 100644
--- a/full_quadrotor.m
+++ b/full_quadrotor.m
@@ -115,18 +115,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/main.m b/main.m
index 5cc1ce6..e500b1a 100644
--- a/main.m
+++ b/main.m
@@ -19,14 +19,14 @@
 ang_gain = 10;
 ang_vel_gain = 10;
 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]);
-R = .01*eye(4);
+R = 1*eye(4);
 Qf = 100*Q;
 
 iters = 10;
-regularizer = 0;  % initial value. Will increment automatically unless this is 0
-line_search_iters = 1;  % 1 for no line search
+regularizer = 1;  % initial value. Will increment automatically unless this is 0
+line_search_iters = 10;  % 1 for no line search
 mode = "ddp";
-initial_controls = 0.612*ones(tf / dt, 4);  % initialize to neutral thrust
+initial_controls = 1.225*ones(tf / dt, 4);  % initialize to neutral thrust
 ic = x0;
 
 % get cost functions

From df3c92048b4b21af42a22b3d66e9c173840153d2 Mon Sep 17 00:00:00 2001
From: David-Rey <71460537+David-Rey@users.noreply.github.com>
Date: Wed, 11 Dec 2024 17:43:45 -0500
Subject: [PATCH 09/21] Working DDP :)

---
 back_pass.m | 50 +++++++++++++++++++++++++++++++++++++++++---------
 ddp.m       |  8 ++++----
 main.m      | 18 ++++++++++--------
 3 files changed, 55 insertions(+), 21 deletions(-)

diff --git a/back_pass.m b/back_pass.m
index dbed9c9..7b821f1 100644
--- a/back_pass.m
+++ b/back_pass.m
@@ -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);
@@ -105,9 +113,11 @@
     [~, cx, cu, cxx, cxu, cuu] = costfn(states(t,:).', controls(t,:).');
     [f, fx, fu, fxx, fxu, fuu] = dyn(states(t,:).', controls(t,:).');
     reg = regularizer;
+
+	delta = delta_0;
     
     % automatically increment regularizer if Quu not invertible
-    for i = 1:5
+    for i = 1:50
         % 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);
@@ -125,12 +135,20 @@
             Qxu = cxu + (fx.')*Vxx_next*fu;
             Qux = Qxu.';
             Quu = cuu + (fu.')*Vxx_next*fu;
-        end
+		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-8
             break
-        end
+		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
@@ -140,17 +158,30 @@
             break
         end
         % otherwise, increment regularizer
-        reg = reg * 2
+        %reg = reg * 2;
 
     end % regularizer update loop
 
     % Get new controls
+	
     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
+	Vx_next = Qx + K.'*Quu*k + K.'*Qu + Qux.'*k;
+	Vxx_next = Qxx + K.'*Quu*K + K.'*Qux + Qux.'*K;
+    %Vxx_next = Qxx - (K.')*Quu*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;
@@ -162,10 +193,11 @@
 
 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 5fcd393..1b0c9aa 100644
--- a/ddp.m
+++ b/ddp.m
@@ -60,15 +60,15 @@
 
 for i = 1:iters
     [controller, ~] = back_pass(states, controls, dyn, costfn, term_costfn, regularizer, mode);
-    
+    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+1));
+        [states, controls, new_costs] = fwd_pass(ic, controller, dyn, costfn, term_costfn, alphas(j));
         if sum(new_costs) < sum(best_costs)
             best_costs = new_costs;
         end
-    end
-
+	end
+	
     % update to a newer controller
     [controller, ~] = back_pass(states, controls, dyn, costfn, term_costfn, regularizer, mode);
     total_costs(i,1) = sum(best_costs); 
diff --git a/main.m b/main.m
index e500b1a..9bbfe70 100644
--- a/main.m
+++ b/main.m
@@ -14,17 +14,17 @@
 % R = 12.1*eye(4);
 % Qf = 37*eye(length(x0));
 
-pos_gain = 100;
-vel_gain = 100;
-ang_gain = 10;
-ang_vel_gain = 10;
+pos_gain = 1;
+vel_gain = 1;
+ang_gain = 1;
+ang_vel_gain = 1;
 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]);
-R = 1*eye(4);
-Qf = 100*Q;
+R = 0.8*eye(4);
+Qf = 180*Q;
 
-iters = 10;
+iters = 13;
 regularizer = 1;  % initial value. Will increment automatically unless this is 0
-line_search_iters = 10;  % 1 for no line search
+line_search_iters = 3;  % 1 for no line search
 mode = "ddp";
 initial_controls = 1.225*ones(tf / dt, 4);  % initialize to neutral thrust
 ic = x0;
@@ -54,6 +54,8 @@
 plot3(-3,-2,-1,"ro")
 plot3(5,3,2,"rx")
 legend(["Flight path","Start Point","Goal"])
+axis("equal")
+grid("on")
 
 figure(2)
 title("Controls")

From b47048d80284c0859d5898dd3d757ddf34579079 Mon Sep 17 00:00:00 2001
From: David Reynolds <davidreynolds2277@gmail.com>
Date: Thu, 12 Dec 2024 14:05:21 -0500
Subject: [PATCH 10/21] working but slow mpc

---
 back_pass.m         |  6 ++----
 ddp.m               |  2 +-
 main.m              |  4 ++--
 main_receding_mpc.m | 31 +++++++++++++++++++++----------
 simmpc.m            |  5 +++--
 5 files changed, 29 insertions(+), 19 deletions(-)

diff --git a/back_pass.m b/back_pass.m
index 7b821f1..6c371ec 100644
--- a/back_pass.m
+++ b/back_pass.m
@@ -117,7 +117,7 @@
 	delta = delta_0;
     
     % automatically increment regularizer if Quu not invertible
-    for i = 1:50
+    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);
@@ -141,7 +141,7 @@
 		%Quu = Quu + mu * eye(size(Quu));
 
         % check if Quu is positive definite
-        if all(eig(Quu) > 1e-6) && rcond(Quu) > 1e-8
+        if all(eig(Quu) > 1e-6) && rcond(Quu) > 1e-13
             break
 		end
 
@@ -168,10 +168,8 @@
     k = -Quu \ Qu;
 
     % Get new value function
-    %Vx_next = Qx - (K.')*Quu*k
 	Vx_next = Qx + K.'*Quu*k + K.'*Qu + Qux.'*k;
 	Vxx_next = Qxx + K.'*Quu*K + K.'*Qux + Qux.'*K;
-    %Vxx_next = Qxx - (K.')*Quu*K;
 
     % Decrease mu for next iteration if successful
     delta = min(1 / delta_0, delta / delta_0);
diff --git a/ddp.m b/ddp.m
index 1b0c9aa..3967ca8 100644
--- a/ddp.m
+++ b/ddp.m
@@ -60,7 +60,7 @@
 
 for i = 1:iters
     [controller, ~] = back_pass(states, controls, dyn, costfn, term_costfn, regularizer, mode);
-    disp(sum(best_costs))
+    %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));
diff --git a/main.m b/main.m
index 9bbfe70..78ddb9f 100644
--- a/main.m
+++ b/main.m
@@ -24,8 +24,8 @@
 
 iters = 13;
 regularizer = 1;  % initial value. Will increment automatically unless this is 0
-line_search_iters = 3;  % 1 for no line search
-mode = "ddp";
+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;
 
diff --git a/main_receding_mpc.m b/main_receding_mpc.m
index a960476..7bd483f 100644
--- a/main_receding_mpc.m
+++ b/main_receding_mpc.m
@@ -4,31 +4,33 @@
 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
 dt = 0.01;  % time step
-sim_horizon = 8;   % total sim time
+sim_horizon = 50;   % total sim time
 planning_horizon = 3;  % "look ahead" amount
 
 % set up dynamics
 dyn = full_quadrotor(dt);
 
 % Tune here!
-pos_gain = 100;
-vel_gain = 100;
-ang_gain = 10;
-ang_vel_gain = 10;
+pos_gain = 1;
+vel_gain = 1;
+ang_gain = 1;
+ang_vel_gain = 1;
 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]);
-R = 15*eye(4);
+R = 2*eye(4);
 Qf = 100*Q;
 
 iters = 10;
 regularizer = 1;  % initial value. Will increment automatically
-line_search_iters = 10;
-mode = "ddp";
-initial_controls = 0.612*ones(planning_horizon / dt, 4);  % initialize to neutral thrust
+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);
 
+
+
 %% MPC BELOW
 % PASTED FROM HW2 CODE:
 
@@ -51,11 +53,20 @@
 
 % run controller receding horizon
 controls = initial_controls;
+
+%[states, controls] = simmpc(ic, sim_horizon, initial_controls, iters, regularizer, dyn, costfn, term_costfn, 'ilqr', line_search_iters);
+%function [states, controls] = simmpc(ic, sim_horizon, initial_controls, ddp_iters, regularizer, dyn, costfn, term_costfn, mode)
+
+%plot_states = squeeze(states(:, 1, :));
+
+
 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,:);
diff --git a/simmpc.m b/simmpc.m
index a038213..8848c9e 100644
--- a/simmpc.m
+++ b/simmpc.m
@@ -1,4 +1,4 @@
-function [states, controls] = simmpc(ic, sim_horizon, initial_controls, ddp_iters, regularizer, dyn, costfn, term_costfn, mode)
+function [states, controls] = simmpc(ic, sim_horizon, initial_controls, ddp_iters, regularizer, dyn, costfn, term_costfn, mode, line_search_iters)
 %SIMMPC Simulation of receding-horizon model-predictive control.
 %
 %   This function simulates the execution of iLQR / DDP in a
@@ -89,7 +89,8 @@
 
 for t = 1:sim_horizon
     % get controller
-    [controller, ~] = ddp(current_state, controls, ddp_iters, regularizer, dyn, costfn, term_costfn, mode);
+    disp(t)
+    [controller, ~] = ddp(current_state, controls, ddp_iters, regularizer, dyn, costfn, term_costfn, mode, line_search_iters);
     
     % add states and controls to the final output
     % states(t,:,:) = controller.states;

From cc87e7bcc0bc41b6f9c5d6b6c74cc81d05036012 Mon Sep 17 00:00:00 2001
From: Kevin Maranto <marantokevin@gmail.com>
Date: Thu, 12 Dec 2024 14:38:22 -0500
Subject: [PATCH 11/21] Add barrier dynamics

---
 Barrier_full_quadrotor.m | 138 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 138 insertions(+)
 create mode 100644 Barrier_full_quadrotor.m

diff --git a/Barrier_full_quadrotor.m b/Barrier_full_quadrotor.m
new file mode 100644
index 0000000..385d3df
--- /dev/null
+++ b/Barrier_full_quadrotor.m
@@ -0,0 +1,138 @@
+function [dyn] = barrier_full_quadrotor(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:12
+    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);
+
+    % 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,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,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
+

From 44071a25eb80966f5da3898696fdaef41037e156 Mon Sep 17 00:00:00 2001
From: Elliot Kantor <74844563+elliotkantor@users.noreply.github.com>
Date: Thu, 12 Dec 2024 14:17:10 -0500
Subject: [PATCH 12/21] Copy main.m and add barrier funcs

---
 get_barrier_func.m  | 10 +++++++
 main_barrier.m      | 71 +++++++++++++++++++++++++++++++++++++++++++++
 quad_cost_barrier.m | 33 +++++++++++++++++++++
 3 files changed, 114 insertions(+)
 create mode 100644 get_barrier_func.m
 create mode 100644 main_barrier.m
 create mode 100644 quad_cost_barrier.m

diff --git a/get_barrier_func.m b/get_barrier_func.m
new file mode 100644
index 0000000..50c5881
--- /dev/null
+++ b/get_barrier_func.m
@@ -0,0 +1,10 @@
+function barrier_func = get_barrier_func()
+% x: state vec 
+
+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_barrier.m b/main_barrier.m
new file mode 100644
index 0000000..1ad4aaf
--- /dev/null
+++ b/main_barrier.m
@@ -0,0 +1,71 @@
+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
+tf = 8;  % final time
+dt = 0.01;  % time step
+
+% set up dynamics
+dyn = full_quadrotor(dt);
+
+% Tune here!
+pos_gain = 1;
+vel_gain = 1;
+ang_gain = 1;
+ang_vel_gain = 1;
+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]);
+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);
+
+total_costs(end)
+final_cost = norm(controller.states(end,:) - xf)
+
+
+%% Plot result
+xs = controller.states(:,1);
+ys = controller.states(:,2);
+zs = controller.states(:,3);
+
+figure(1)
+title("Position")
+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")
+
+figure(2)
+title("Controls")
+plot(controller.controls(:,1))
+hold on
+plot(controller.controls(:,2))
+plot(controller.controls(:,3))
+plot(controller.controls(:,4))
+legend(["u1","u2","u3","u4"])
+
+figure(3)
+title("Attitude")
+plot(controller.states(:,4))
+hold on
+plot(controller.states(:,5))
+plot(controller.states(:,6))
+legend(["\phi","\theta","\psi"])
diff --git a/quad_cost_barrier.m b/quad_cost_barrier.m
new file mode 100644
index 0000000..5d1a4df
--- /dev/null
+++ b/quad_cost_barrier.m
@@ -0,0 +1,33 @@
+function [c, c_term] = quad_cost_barrier(Q, R, Qf, target_state)
+% yield cost functions for the quadrotor
+
+% Q, R, and Qf are all square
+% target_state: 1-by-12 target state vector (in HW2 this was a column
+% vector)
+
+    function [c, cx, cu, cxx, cxu, cuu] = costfn(x, u)
+        % x: state vector with barrier state at end 
+        w = x(13);  % get barrier state
+        x = x(1:12);  % cut off barrier state for error calcs
+        err = (x - target_state);
+        % added +w^2 term to both fns
+        c = 0.5 * (err.' * Q * err + u.' * R * u) + w^2;
+        cx = Q * err;
+        cxx = Q;
+        cxu = 0;
+        cu = R * u;
+        cuu = R;
+    end
+
+    function [c, cx, cxx] = term_costfn(x)
+        w = x(13);
+        x = x(1:12);
+        err = (x - target_state); 
+        c = 0.5 * (err' * Qf * err) + w^2;
+        cx = Qf * err;
+        cxx = Qf;
+    end
+
+c = @costfn;
+c_term = @term_costfn;
+end

From 28d9b2d244f00b5976443fb33bd50b128f960102 Mon Sep 17 00:00:00 2001
From: David Reynolds <davidreynolds2277@gmail.com>
Date: Thu, 12 Dec 2024 14:43:16 -0500
Subject: [PATCH 13/21] updated plots

---
 main.m              | 17 ++++++++++---
 main_receding_mpc.m | 58 +++++++++++++++++++++++++++++++++------------
 2 files changed, 57 insertions(+), 18 deletions(-)

diff --git a/main.m b/main.m
index 78ddb9f..1b9b5f6 100644
--- a/main.m
+++ b/main.m
@@ -60,6 +60,7 @@
 figure(2)
 title("Controls")
 plot(controller.controls(:,1))
+grid on
 hold on
 plot(controller.controls(:,2))
 plot(controller.controls(:,3))
@@ -68,8 +69,18 @@
 
 figure(3)
 title("Attitude")
-plot(controller.states(:,4))
+plot(controller.states(:,7))
+grid on
 hold on
-plot(controller.states(:,5))
-plot(controller.states(:,6))
+plot(controller.states(:,8))
+plot(controller.states(:,9))
 legend(["\phi","\theta","\psi"])
+
+figure(4)
+title("Body Rate")
+plot(controller.states(:, 10))
+grid on
+hold on
+plot(controller.states(:, 11))
+plot(controller.states(:, 12))
+legend(["p","q","r"])
diff --git a/main_receding_mpc.m b/main_receding_mpc.m
index 7bd483f..837d524 100644
--- a/main_receding_mpc.m
+++ b/main_receding_mpc.m
@@ -4,8 +4,10 @@
 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
 dt = 0.01;  % time step
-sim_horizon = 50;   % total sim time
-planning_horizon = 3;  % "look ahead" amount
+sim_horizon = 300;   % total sim time (iterations, i.e 50 => 0.5 seconds)
+planning_horizon = 3;  % "look ahead" amount (seconds)
+n = length(x0);
+m = 4;
 
 % set up dynamics
 dyn = full_quadrotor(dt);
@@ -17,9 +19,9 @@
 ang_vel_gain = 1;
 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]);
 R = 2*eye(4);
-Qf = 100*Q;
+Qf = 10*Q;
 
-iters = 10;
+iters = 1;
 regularizer = 1;  % initial value. Will increment automatically
 line_search_iters = 3;
 mode = "ilqr";
@@ -59,6 +61,8 @@
 
 %plot_states = squeeze(states(:, 1, :));
 
+state_hist = zeros(n, sim_horizon);
+contol_hist = zeros(m, sim_horizon);
 
 current_state = ic;
 for t = 1:sim_horizon
@@ -75,7 +79,7 @@
     controls = ubar + (K * (current_state - xbar.')).' + k;
 
     % increment state
-    [current_state, ~, ~, ~, ~, ~] = dyn(current_state, controls);
+    [current_state, ~, ~, ~, ~, ~] = dyn(current_state, controls(1, :));
 
     % warm start
     controls = controller.controls; %; controller.controls(end,:)];
@@ -83,6 +87,9 @@
     % 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)
@@ -90,9 +97,16 @@
 
 
 %% Plot result
-xs = controller.states(:,1);
-ys = controller.states(:,2);
-zs = controller.states(:,3);
+
+%disp(states(1, :, 1))
+
+%xs = controller.states(:,1);
+%ys = controller.states(:,2);
+%zs = controller.states(:,3);
+
+xs = state_hist(1, :);
+ys = state_hist(2, :);
+zs = state_hist(3, :);
 
 figure(1)
 title("Position")
@@ -102,20 +116,34 @@
 plot3(-3,-2,-1,"ro")
 plot3(5,3,2,"rx")
 legend(["Flight path","Start Point","Goal"])
+grid("on")
+axis("equal")
 
 figure(2)
 title("Controls")
-plot(controller.controls(:,1))
+plot(contol_hist(1, :))
+grid on
 hold on
-plot(controller.controls(:,2))
-plot(controller.controls(:,3))
-plot(controller.controls(:,4))
+
+plot(contol_hist(2, :))
+plot(contol_hist(3, :))
+plot(contol_hist(4, :))
 legend(["u1","u2","u3","u4"])
 
 figure(3)
 title("Attitude")
-plot(controller.states(:,4))
+plot(state_hist(7, :))
+grid on
 hold on
-plot(controller.states(:,5))
-plot(controller.states(:,6))
+plot(state_hist(8, :))
+plot(state_hist(9, :))
 legend(["\phi","\theta","\psi"])
+
+figure(4)
+title("Body Rate")
+plot(state_hist(10, :))
+grid on
+hold on
+plot(state_hist(11, :))
+plot(state_hist(12, :))
+legend(["p","q","r"])
\ No newline at end of file

From b113e667d7fdaf8ac6e4a9e387b5ecb6a2ebae68 Mon Sep 17 00:00:00 2001
From: Elliot Kantor <74844563+elliotkantor@users.noreply.github.com>
Date: Thu, 12 Dec 2024 14:50:40 -0500
Subject: [PATCH 14/21] Working barrier states

---
 Barrier_full_quadrotor.m |   2 +-
 back_pass.m              |   6 +-
 ddp.m                    |  10 +--
 full_quadrotor_barrier.m | 139 +++++++++++++++++++++++++++++++++++++++
 fwd_pass.m               |   5 +-
 get_barrier_func.m       |   2 +-
 main_barrier.m           |  11 ++--
 quad_cost_barrier.m      |  33 ----------
 8 files changed, 161 insertions(+), 47 deletions(-)
 create mode 100644 full_quadrotor_barrier.m
 delete mode 100644 quad_cost_barrier.m

diff --git a/Barrier_full_quadrotor.m b/Barrier_full_quadrotor.m
index 385d3df..661172e 100644
--- a/Barrier_full_quadrotor.m
+++ b/Barrier_full_quadrotor.m
@@ -1,4 +1,4 @@
-function [dyn] = barrier_full_quadrotor(dt, xf)
+function [dyn] = full_quadrotor_barrier(dt, xf)
 %   Detailed explanation goes here
 
 % Get parameters
diff --git a/back_pass.m b/back_pass.m
index 6c371ec..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
@@ -97,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;
diff --git a/ddp.m b/ddp.m
index 3967ca8..2020fdd 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, line_search_iters)
+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
@@ -56,21 +56,21 @@
 
 %% Your code below
 
-[states, controls, best_costs] = fwd_pass(ic, controller, dyn, costfn, term_costfn, alphas(1));
+[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);
+    [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));
+        [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);
+    [controller, ~] = back_pass(states, controls, dyn, costfn, term_costfn, regularizer, mode, xf);
     total_costs(i,1) = sum(best_costs); 
 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 9bd17be..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, alpha)
+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.
 %
@@ -62,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
 
diff --git a/get_barrier_func.m b/get_barrier_func.m
index 50c5881..22149fc 100644
--- a/get_barrier_func.m
+++ b/get_barrier_func.m
@@ -1,5 +1,5 @@
 function barrier_func = get_barrier_func()
-% x: state vec 
+% 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;
diff --git a/main_barrier.m b/main_barrier.m
index 1ad4aaf..394bd74 100644
--- a/main_barrier.m
+++ b/main_barrier.m
@@ -1,20 +1,21 @@
 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. 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
 
 % set up dynamics
-dyn = full_quadrotor(dt);
+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;
-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 = 10;
+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;
 
@@ -31,7 +32,7 @@
 % 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);
+[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)
diff --git a/quad_cost_barrier.m b/quad_cost_barrier.m
deleted file mode 100644
index 5d1a4df..0000000
--- a/quad_cost_barrier.m
+++ /dev/null
@@ -1,33 +0,0 @@
-function [c, c_term] = quad_cost_barrier(Q, R, Qf, target_state)
-% yield cost functions for the quadrotor
-
-% Q, R, and Qf are all square
-% target_state: 1-by-12 target state vector (in HW2 this was a column
-% vector)
-
-    function [c, cx, cu, cxx, cxu, cuu] = costfn(x, u)
-        % x: state vector with barrier state at end 
-        w = x(13);  % get barrier state
-        x = x(1:12);  % cut off barrier state for error calcs
-        err = (x - target_state);
-        % added +w^2 term to both fns
-        c = 0.5 * (err.' * Q * err + u.' * R * u) + w^2;
-        cx = Q * err;
-        cxx = Q;
-        cxu = 0;
-        cu = R * u;
-        cuu = R;
-    end
-
-    function [c, cx, cxx] = term_costfn(x)
-        w = x(13);
-        x = x(1:12);
-        err = (x - target_state); 
-        c = 0.5 * (err' * Qf * err) + w^2;
-        cx = Qf * err;
-        cxx = Qf;
-    end
-
-c = @costfn;
-c_term = @term_costfn;
-end

From 2b8f2046a12e2f0e40d59513e4811db8158e0965 Mon Sep 17 00:00:00 2001
From: Elliot Kantor <74844563+elliotkantor@users.noreply.github.com>
Date: Thu, 12 Dec 2024 15:11:06 -0500
Subject: [PATCH 15/21] Fix bugs, plot barriers

---
 ddp.m               |  4 ++++
 main.m              |  9 +++++----
 main_barrier.m      | 34 +++++++++++++++++++++++++++++++++-
 main_receding_mpc.m |  9 +++++----
 plot_barriers.m     | 38 ++++++++++++++++++++++++++++++++++++++
 5 files changed, 85 insertions(+), 9 deletions(-)
 create mode 100644 plot_barriers.m

diff --git a/ddp.m b/ddp.m
index 2020fdd..a610947 100644
--- a/ddp.m
+++ b/ddp.m
@@ -38,6 +38,10 @@
 %   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);
 n = size(ic, 1);
diff --git a/main.m b/main.m
index 1b9b5f6..59c6d46 100644
--- a/main.m
+++ b/main.m
@@ -1,13 +1,13 @@
 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
 
 % set up dynamics
-dyn = full_quadrotor(dt);
+dyn = full_quadrotor_barrier(dt, xf);
 
 % Tune here!
 % Q = 100.244*eye(length(x0));
@@ -18,7 +18,8 @@
 vel_gain = 1;
 ang_gain = 1;
 ang_vel_gain = 1;
-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 = 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;
 
diff --git a/main_barrier.m b/main_barrier.m
index 394bd74..ab0bc1b 100644
--- a/main_barrier.m
+++ b/main_barrier.m
@@ -14,7 +14,7 @@
 vel_gain = 1;
 ang_gain = 1;
 ang_vel_gain = 1;
-w_gain = 10;
+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;
@@ -43,16 +43,48 @@
 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)
 title("Position")
 plot3(xs,ys,zs)
 hold on
 
+% plot barriers
+isosurface(X, Y, Z, H1, 0);
+isosurface(X, Y, Z, H2, 0);
+isosurface(X, Y, Z, H3, 0);
+
 plot3(-3,-2,-1,"ro")
 plot3(5,3,2,"rx")
 legend(["Flight path","Start Point","Goal"])
 axis("equal")
 grid("on")
+xlabel('X Axis')
+ylabel('Y Axis')
+zlabel('Z Axis')
+
+
+
+
 
 figure(2)
 title("Controls")
diff --git a/main_receding_mpc.m b/main_receding_mpc.m
index 837d524..0808823 100644
--- a/main_receding_mpc.m
+++ b/main_receding_mpc.m
@@ -1,8 +1,8 @@
 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
 dt = 0.01;  % time step
 sim_horizon = 300;   % total sim time (iterations, i.e 50 => 0.5 seconds)
 planning_horizon = 3;  % "look ahead" amount (seconds)
@@ -10,14 +10,15 @@
 m = 4;
 
 % set up dynamics
-dyn = full_quadrotor(dt);
+dyn = full_quadrotor_barrier(dt, xf);
 
 % Tune here!
 pos_gain = 1;
 vel_gain = 1;
 ang_gain = 1;
 ang_vel_gain = 1;
-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 = 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 = 2*eye(4);
 Qf = 10*Q;
 
diff --git a/plot_barriers.m b/plot_barriers.m
new file mode 100644
index 0000000..2381705
--- /dev/null
+++ b/plot_barriers.m
@@ -0,0 +1,38 @@
+function plot_barriers()
+% 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 + (2.2 - 2.2).^2;
+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 the spheres using isosurface
+figure;
+hold on;
+isosurface(X, Y, Z, H1, 0);
+isosurface(X, Y, Z, H2, 0);
+isosurface(X, Y, Z, H3, 0);
+
+% Set the plot properties
+xlabel('X-axis');
+ylabel('Y-axis');
+zlabel('Z-axis');
+title('Spheres representing h1, h2, and h3');
+axis equal;
+grid on;
+legend({'h1', 'h2', 'h3'});
+view(3);
+hold off;
+end
\ No newline at end of file

From 033d8629b80fe829a6babbac4f3549a974b6c44b Mon Sep 17 00:00:00 2001
From: Elliot Kantor <74844563+elliotkantor@users.noreply.github.com>
Date: Thu, 12 Dec 2024 15:37:20 -0500
Subject: [PATCH 16/21] Fix all plots

---
 .gitignore          |  6 ++++
 main.m              | 63 ++++++++++++++++++++++++++++-----
 main_barrier.m      | 86 ++++++++++++++++++++++++++++++++++++++-------
 main_receding_mpc.m | 76 ++++++++++++++++++++++++++++++---------
 plot_barriers.m     | 38 --------------------
 5 files changed, 193 insertions(+), 76 deletions(-)
 delete mode 100644 plot_barriers.m

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/main.m b/main.m
index 59c6d46..12c251f 100644
--- a/main.m
+++ b/main.m
@@ -48,7 +48,7 @@
 zs = controller.states(:,3);
 
 figure(1)
-title("Position")
+
 plot3(xs,ys,zs)
 hold on
 
@@ -57,31 +57,76 @@
 legend(["Flight path","Start Point","Goal"])
 axis("equal")
 grid("on")
+xlabel('X Axis')
+ylabel('Y Axis')
+zlabel('Z Axis')
+title("Position")
+saveas(gcf, './normal/3d.png')
+
+%% 2D plots
 
 figure(2)
-title("Controls")
+
 plot(controller.controls(:,1))
-grid on
 hold on
+grid on
 plot(controller.controls(:,2))
 plot(controller.controls(:,3))
 plot(controller.controls(:,4))
 legend(["u1","u2","u3","u4"])
+xlabel('Time (s)')
+ylabel('Control Input (N)')
+title("Controls")
+saveas(gcf, './normal/controls.png')
 
 figure(3)
-title("Attitude")
-plot(controller.states(:,7))
-grid on
+
+plot(controller.states(:,4))
 hold on
-plot(controller.states(:,8))
-plot(controller.states(:,9))
+grid on
+plot(controller.states(:,5))
+plot(controller.states(:,6))
 legend(["\phi","\theta","\psi"])
+xlabel('Time (s)')
+ylabel('Angle (rad)')
+title("Attitude")
+saveas(gcf, './normal/attitude.png')
 
 figure(4)
-title("Body Rate")
+
 plot(controller.states(:, 10))
 grid on
 hold on
 plot(controller.states(:, 11))
 plot(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(controller.states(:,1))
+grid on
+hold on
+plot(controller.states(:,2))
+plot(controller.states(:,3))
+legend(["x","y","z"])
+xlabel('Time (s)')
+ylabel('Position (m)')
+title("Position")
+saveas(gcf, './normal/position.png')
+
+figure(6)
+
+plot(controller.states(:,4))
+grid on
+hold on
+plot(controller.states(:,5))
+plot(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
index ab0bc1b..e766351 100644
--- a/main_barrier.m
+++ b/main_barrier.m
@@ -62,43 +62,105 @@
 H2 = h2(X, Y, Z);
 H3 = h3(X, Y, Z);
 
-% plot trajectory
+%% plot trajectory
 figure(1)
-title("Position")
+
 plot3(xs,ys,zs)
 hold on
+plot3(-3,-2,-1,"ro")
+plot3(5,3,2,"rx")
 
 % plot barriers
-isosurface(X, Y, Z, H1, 0);
-isosurface(X, Y, Z, H2, 0);
-isosurface(X, Y, Z, H3, 0);
 
-plot3(-3,-2,-1,"ro")
-plot3(5,3,2,"rx")
+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 Axis')
 ylabel('Y Axis')
 zlabel('Z Axis')
+title("Position")
+saveas(gcf, './barrier/3d.png')
 
-
-
-
+%% 2D plots
 
 figure(2)
-title("Controls")
+
 plot(controller.controls(:,1))
 hold on
+grid on
 plot(controller.controls(:,2))
 plot(controller.controls(:,3))
 plot(controller.controls(:,4))
 legend(["u1","u2","u3","u4"])
+xlabel('Time (s)')
+ylabel('Control Input (N)')
+title("Controls")
+saveas(gcf, './barrier/controls.png')
 
 figure(3)
-title("Attitude")
+
 plot(controller.states(:,4))
 hold on
+grid on
 plot(controller.states(:,5))
 plot(controller.states(:,6))
 legend(["\phi","\theta","\psi"])
+xlabel('Time (s)')
+ylabel('Angle (rad)')
+title("Attitude")
+saveas(gcf, './barrier/attitude.png')
+
+figure(4)
+
+plot(controller.states(:, 10))
+grid on
+hold on
+plot(controller.states(:, 11))
+plot(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(5)
+
+plot(controller.states(:,1))
+grid on
+hold on
+plot(controller.states(:,2))
+plot(controller.states(:,3))
+legend(["x","y","z"])
+xlabel('Time (s)')
+title("Position")
+ylabel('Position (m)')
+saveas(gcf, './barrier/position.png')
+
+figure(6)
+
+plot(controller.states(:,4))
+grid on
+hold on
+plot(controller.states(:,5))
+plot(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(7)
+plot(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
index 0808823..a0f9b1f 100644
--- a/main_receding_mpc.m
+++ b/main_receding_mpc.m
@@ -110,7 +110,7 @@
 zs = state_hist(3, :);
 
 figure(1)
-title("Position")
+
 plot3(xs,ys,zs)
 hold on
 
@@ -119,32 +119,74 @@
 legend(["Flight path","Start Point","Goal"])
 grid("on")
 axis("equal")
+title("Position")
+saveas(gcf, './receding/3d.png')
+
+
+%% 2D plots
 
 figure(2)
-title("Controls")
-plot(contol_hist(1, :))
-grid on
-hold on
 
-plot(contol_hist(2, :))
-plot(contol_hist(3, :))
-plot(contol_hist(4, :))
+plot(controller.controls(:,1))
+hold on
+grid on
+plot(controller.controls(:,2))
+plot(controller.controls(:,3))
+plot(controller.controls(:,4))
 legend(["u1","u2","u3","u4"])
+xlabel('Time (s)')
+ylabel('Control Input (N)')
+title("Controls")
+saveas(gcf, './receding/controls.png')
 
 figure(3)
-title("Attitude")
-plot(state_hist(7, :))
-grid on
+
+plot(controller.states(:,4))
 hold on
-plot(state_hist(8, :))
-plot(state_hist(9, :))
+grid on
+plot(controller.states(:,5))
+plot(controller.states(:,6))
 legend(["\phi","\theta","\psi"])
+xlabel('Time (s)')
+ylabel('Angle (rad)')
+title("Attitude")
+saveas(gcf, './receding/attitude.png')
 
 figure(4)
+
+plot(controller.states(:, 10))
+grid on
+hold on
+plot(controller.states(:, 11))
+plot(controller.states(:, 12))
+legend(["p","q","r"])
+xlabel('Time (s)')
+ylabel('Angular Velocity (rad/s)')
 title("Body Rate")
-plot(state_hist(10, :))
+saveas(gcf, './receding/ang_vel.png')
+
+figure(5)
+
+plot(controller.states(:,1))
+grid on
+hold on
+plot(controller.states(:,2))
+plot(controller.states(:,3))
+legend(["x","y","z"])
+xlabel('Time (s)')
+ylabel('Position (m)')
+title("Position")
+saveas(gcf, './receding/position.png')
+
+figure(6)
+
+plot(controller.states(:,4))
 grid on
 hold on
-plot(state_hist(11, :))
-plot(state_hist(12, :))
-legend(["p","q","r"])
\ No newline at end of file
+plot(controller.states(:,5))
+plot(controller.states(:,6))
+legend(["vx","vy","vz"])
+xlabel('Time (s)')
+ylabel('Velocity (m/s)')
+title("Velocity")
+saveas(gcf, './receding/velocity.png')
diff --git a/plot_barriers.m b/plot_barriers.m
deleted file mode 100644
index 2381705..0000000
--- a/plot_barriers.m
+++ /dev/null
@@ -1,38 +0,0 @@
-function plot_barriers()
-% 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 + (2.2 - 2.2).^2;
-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 the spheres using isosurface
-figure;
-hold on;
-isosurface(X, Y, Z, H1, 0);
-isosurface(X, Y, Z, H2, 0);
-isosurface(X, Y, Z, H3, 0);
-
-% Set the plot properties
-xlabel('X-axis');
-ylabel('Y-axis');
-zlabel('Z-axis');
-title('Spheres representing h1, h2, and h3');
-axis equal;
-grid on;
-legend({'h1', 'h2', 'h3'});
-view(3);
-hold off;
-end
\ No newline at end of file

From 717ab7a937b0405ea4a407c3edadd9256f4b9359 Mon Sep 17 00:00:00 2001
From: David Reynolds <davidreynolds2277@gmail.com>
Date: Thu, 12 Dec 2024 15:44:29 -0500
Subject: [PATCH 17/21] updated plots for main and mpc

---
 main.m              | 31 ++++++++++++++++++++-----------
 main_receding_mpc.m | 41 +++++++++++++++++++++++------------------
 2 files changed, 43 insertions(+), 29 deletions(-)

diff --git a/main.m b/main.m
index 1b9b5f6..3ea9f99 100644
--- a/main.m
+++ b/main.m
@@ -5,6 +5,7 @@
 xf = [5, 3, 2, 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);
@@ -47,8 +48,10 @@
 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")
@@ -58,29 +61,35 @@
 grid("on")
 
 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"])
 
 figure(3)
+plot(t_arr, controller.states(:,7))
 title("Attitude")
-plot(controller.states(:,7))
+xlabel("Time (s)")
+ylabel("Rad")
 grid on
 hold on
-plot(controller.states(:,8))
-plot(controller.states(:,9))
+plot(t_arr, controller.states(:,8))
+plot(t_arr, controller.states(:,9))
 legend(["\phi","\theta","\psi"])
 
 figure(4)
+plot(t_arr, controller.states(:, 10))
 title("Body Rate")
-plot(controller.states(:, 10))
+xlabel("Time (s)")
+ylabel("Rad/sec")
 grid on
 hold on
-plot(controller.states(:, 11))
-plot(controller.states(:, 12))
+plot(t_arr, controller.states(:, 11))
+plot(t_arr, controller.states(:, 12))
 legend(["p","q","r"])
diff --git a/main_receding_mpc.m b/main_receding_mpc.m
index 837d524..f83c860 100644
--- a/main_receding_mpc.m
+++ b/main_receding_mpc.m
@@ -4,11 +4,14 @@
 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
 dt = 0.01;  % time step
-sim_horizon = 300;   % total sim time (iterations, i.e 50 => 0.5 seconds)
+sim_horizon = 200;   % 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(dt);
 
@@ -98,19 +101,15 @@
 
 %% Plot result
 
-%disp(states(1, :, 1))
-
-%xs = controller.states(:,1);
-%ys = controller.states(:,2);
-%zs = controller.states(:,3);
-
 xs = state_hist(1, :);
 ys = state_hist(2, :);
 zs = state_hist(3, :);
 
 figure(1)
-title("Position")
 plot3(xs,ys,zs)
+xlabel("X")
+ylabel("Y")
+zlabel("Z")
 hold on
 
 plot3(-3,-2,-1,"ro")
@@ -120,30 +119,36 @@
 axis("equal")
 
 figure(2)
+plot(t_arr(1:end-1), contol_hist(1, :))
 title("Controls")
-plot(contol_hist(1, :))
+xlabel("Time (s)")
+ylabel("Contol")
 grid on
 hold on
 
-plot(contol_hist(2, :))
-plot(contol_hist(3, :))
-plot(contol_hist(4, :))
+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"])
 
 figure(3)
+plot(t_arr(1:end-1), state_hist(7, :))
 title("Attitude")
-plot(state_hist(7, :))
+xlabel("Time (s)")
+ylabel("Rad")
 grid on
 hold on
-plot(state_hist(8, :))
-plot(state_hist(9, :))
+plot(t_arr(1:end-1), state_hist(8, :))
+plot(t_arr(1:end-1), state_hist(9, :))
 legend(["\phi","\theta","\psi"])
 
 figure(4)
+plot(t_arr(1:end-1), state_hist(10, :))
 title("Body Rate")
-plot(state_hist(10, :))
+xlabel("Time (s)")
+ylabel("Rad/sec")
 grid on
 hold on
-plot(state_hist(11, :))
-plot(state_hist(12, :))
+plot(t_arr(1:end-1), state_hist(11, :))
+plot(t_arr(1:end-1), state_hist(12, :))
 legend(["p","q","r"])
\ No newline at end of file

From 68d50a47f991d591f79717e2cc1e034023e21e81 Mon Sep 17 00:00:00 2001
From: Elliot Kantor <74844563+elliotkantor@users.noreply.github.com>
Date: Thu, 12 Dec 2024 15:54:28 -0500
Subject: [PATCH 18/21] Clean up repo

---
 full_quadrotor.m    | 133 --------------------------------------------
 main.m              |   5 --
 main_barrier.m      |  28 ++++++++--
 main_receding_mpc.m |  31 +----------
 simmpc.m            | 118 ---------------------------------------
 5 files changed, 24 insertions(+), 291 deletions(-)
 delete mode 100644 full_quadrotor.m
 delete mode 100644 simmpc.m

diff --git a/full_quadrotor.m b/full_quadrotor.m
deleted file mode 100644
index fe7767e..0000000
--- a/full_quadrotor.m
+++ /dev/null
@@ -1,133 +0,0 @@
-function [dyn] = full_quadrotor(dt)
-%   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;  % 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];
-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]; 
-
-% 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:12
-    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);
-
-    % 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,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,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/main.m b/main.m
index 12c251f..d7d41e4 100644
--- a/main.m
+++ b/main.m
@@ -9,11 +9,6 @@
 % set up dynamics
 dyn = full_quadrotor_barrier(dt, xf);
 
-% Tune here!
-% Q = 100.244*eye(length(x0));
-% R = 12.1*eye(4);
-% Qf = 37*eye(length(x0));
-
 pos_gain = 1;
 vel_gain = 1;
 ang_gain = 1;
diff --git a/main_barrier.m b/main_barrier.m
index e766351..378171c 100644
--- a/main_barrier.m
+++ b/main_barrier.m
@@ -70,6 +70,22 @@
 plot3(-3,-2,-1,"ro")
 plot3(5,3,2,"rx")
 
+legend(["Flight path","Start Point","Goal"])
+axis("equal")
+grid("on")
+xlabel('X Axis')
+ylabel('Y Axis')
+zlabel('Z Axis')
+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);
@@ -90,7 +106,7 @@
 
 %% 2D plots
 
-figure(2)
+figure(3)
 
 plot(controller.controls(:,1))
 hold on
@@ -104,7 +120,7 @@
 title("Controls")
 saveas(gcf, './barrier/controls.png')
 
-figure(3)
+figure(4)
 
 plot(controller.states(:,4))
 hold on
@@ -117,7 +133,7 @@
 title("Attitude")
 saveas(gcf, './barrier/attitude.png')
 
-figure(4)
+figure(5)
 
 plot(controller.states(:, 10))
 grid on
@@ -130,7 +146,7 @@
 title("Body Rate")
 saveas(gcf, './barrier/ang_vel.png')
 
-figure(5)
+figure(6)
 
 plot(controller.states(:,1))
 grid on
@@ -143,7 +159,7 @@
 ylabel('Position (m)')
 saveas(gcf, './barrier/position.png')
 
-figure(6)
+figure(7)
 
 plot(controller.states(:,4))
 grid on
@@ -157,7 +173,7 @@
 saveas(gcf, './barrier/velocity.png')
 
 % barrier state plot
-figure(7)
+figure(8)
 plot(controller.states(:,13))
 grid on
 xlabel('Time (s)')
diff --git a/main_receding_mpc.m b/main_receding_mpc.m
index a0f9b1f..25437f1 100644
--- a/main_receding_mpc.m
+++ b/main_receding_mpc.m
@@ -4,7 +4,7 @@
 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 = 300;   % total sim time (iterations, i.e 50 => 0.5 seconds)
+sim_horizon = 100;   % total sim time (iterations, i.e 50 => 0.5 seconds)
 planning_horizon = 3;  % "look ahead" amount (seconds)
 n = length(x0);
 m = 4;
@@ -23,7 +23,7 @@
 Qf = 10*Q;
 
 iters = 1;
-regularizer = 1;  % initial value. Will increment automatically
+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
@@ -32,36 +32,9 @@
 % get cost functions
 [costfn, term_costfn] = quad_cost(Q, R, Qf, xf);
 
-
-
-%% MPC BELOW
-% PASTED FROM HW2 CODE:
-
-% 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
-
 % run controller receding horizon
 controls = initial_controls;
 
-%[states, controls] = simmpc(ic, sim_horizon, initial_controls, iters, regularizer, dyn, costfn, term_costfn, 'ilqr', line_search_iters);
-%function [states, controls] = simmpc(ic, sim_horizon, initial_controls, ddp_iters, regularizer, dyn, costfn, term_costfn, mode)
-
-%plot_states = squeeze(states(:, 1, :));
-
 state_hist = zeros(n, sim_horizon);
 contol_hist = zeros(m, sim_horizon);
 
diff --git a/simmpc.m b/simmpc.m
deleted file mode 100644
index 8848c9e..0000000
--- a/simmpc.m
+++ /dev/null
@@ -1,118 +0,0 @@
-function [states, controls] = simmpc(ic, sim_horizon, initial_controls, ddp_iters, regularizer, dyn, costfn, term_costfn, mode, line_search_iters)
-%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
-    disp(t)
-    [controller, ~] = ddp(current_state, controls, ddp_iters, regularizer, dyn, costfn, term_costfn, mode, line_search_iters);
-    
-    % 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
-

From 4a9b70aa53057b9a2ec764da30ae3748310ada7a Mon Sep 17 00:00:00 2001
From: Kevin Maranto <marantokevin@gmail.com>
Date: Thu, 12 Dec 2024 16:02:14 -0500
Subject: [PATCH 19/21] Resolve merge conflict

---
 main_receding_mpc.m | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/main_receding_mpc.m b/main_receding_mpc.m
index 25437f1..d7a08a6 100644
--- a/main_receding_mpc.m
+++ b/main_receding_mpc.m
@@ -4,7 +4,7 @@
 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 = 100;   % total sim time (iterations, i.e 50 => 0.5 seconds)
+sim_horizon = 600;   % total sim time (iterations, i.e 50 => 0.5 seconds)
 planning_horizon = 3;  % "look ahead" amount (seconds)
 n = length(x0);
 m = 4;

From 42efa1b98faa95a06bb1296ef42ba97d8a354c34 Mon Sep 17 00:00:00 2001
From: David Reynolds <davidreynolds2277@gmail.com>
Date: Thu, 12 Dec 2024 16:28:47 -0500
Subject: [PATCH 20/21] made plotting consistent

---
 main.m              | 10 +++++-----
 main_receding_mpc.m |  3 +--
 2 files changed, 6 insertions(+), 7 deletions(-)

diff --git a/main.m b/main.m
index b74b92e..acd33e4 100644
--- a/main.m
+++ b/main.m
@@ -53,9 +53,9 @@
 legend(["Flight path","Start Point","Goal"])
 axis("equal")
 grid("on")
-xlabel('X Axis')
-ylabel('Y Axis')
-zlabel('Z Axis')
+xlabel('X')
+ylabel('Y')
+zlabel('Z')
 title("Position")
 saveas(gcf, './normal/3d.png')
 
@@ -126,8 +126,8 @@
 plot(t_arr, controller.states(:,4))
 grid on
 hold on
-plot(controller.states(:,5))
-plot(controller.states(:,6))
+plot(t_arr, controller.states(:,5))
+plot(t_arr, controller.states(:,6))
 legend(["vx","vy","vz"])
 xlabel('Time (s)')
 title("Velocity")
diff --git a/main_receding_mpc.m b/main_receding_mpc.m
index ec36b42..ed8f687 100644
--- a/main_receding_mpc.m
+++ b/main_receding_mpc.m
@@ -4,14 +4,13 @@
 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 = 500;   % total sim time (iterations, i.e 50 => 0.5 seconds)
+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);
 

From bb42583ffc7140195e754a952264d0060c479fbb Mon Sep 17 00:00:00 2001
From: David Reynolds <davidreynolds2277@gmail.com>
Date: Thu, 12 Dec 2024 16:38:53 -0500
Subject: [PATCH 21/21] made plots consitent

---
 main_barrier.m | 71 +++++++++++++++++++++++++++++---------------------
 1 file changed, 41 insertions(+), 30 deletions(-)

diff --git a/main_barrier.m b/main_barrier.m
index 378171c..af35441 100644
--- a/main_barrier.m
+++ b/main_barrier.m
@@ -5,6 +5,8 @@
 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 
@@ -73,9 +75,9 @@
 legend(["Flight path","Start Point","Goal"])
 axis("equal")
 grid("on")
-xlabel('X Axis')
-ylabel('Y Axis')
-zlabel('Z Axis')
+xlabel('X')
+ylabel('Y')
+zlabel('Z')
 title("Position")
 saveas(gcf, './barrier/3d_nobarriers.png')
 
@@ -98,9 +100,9 @@
 legend(["Flight path","Start Point","Goal"])
 axis("equal")
 grid("on")
-xlabel('X Axis')
-ylabel('Y Axis')
-zlabel('Z Axis')
+xlabel('X')
+ylabel('Y')
+zlabel('Z')
 title("Position")
 saveas(gcf, './barrier/3d.png')
 
@@ -108,12 +110,16 @@
 
 figure(3)
 
-plot(controller.controls(:,1))
-hold on
+plot(t_arr(1:end-1), controller.controls(:,1))
+title("Controls")
+xlabel("Time (s)")
+ylabel("Contol")
 grid on
-plot(controller.controls(:,2))
-plot(controller.controls(:,3))
-plot(controller.controls(:,4))
+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)')
@@ -121,12 +127,15 @@
 saveas(gcf, './barrier/controls.png')
 
 figure(4)
-
-plot(controller.states(:,4))
-hold on
+plot(t_arr, controller.states(:,7))
+title("Attitude")
+xlabel("Time (s)")
+ylabel("Rad")
 grid on
-plot(controller.states(:,5))
-plot(controller.states(:,6))
+hold on
+plot(t_arr, controller.states(:,8))
+plot(t_arr, controller.states(:,9))
+
 legend(["\phi","\theta","\psi"])
 xlabel('Time (s)')
 ylabel('Angle (rad)')
@@ -134,12 +143,14 @@
 saveas(gcf, './barrier/attitude.png')
 
 figure(5)
-
-plot(controller.states(:, 10))
+plot(t_arr, controller.states(:, 10))
+title("Body Rate")
+xlabel("Time (s)")
+ylabel("Rad/sec")
 grid on
 hold on
-plot(controller.states(:, 11))
-plot(controller.states(:, 12))
+plot(t_arr, controller.states(:, 11))
+plot(t_arr, controller.states(:, 12))
 legend(["p","q","r"])
 xlabel('Time (s)')
 ylabel('Angular Velocity (rad/s)')
@@ -147,25 +158,23 @@
 saveas(gcf, './barrier/ang_vel.png')
 
 figure(6)
-
-plot(controller.states(:,1))
+plot(t_arr, controller.states(:,1))
 grid on
 hold on
-plot(controller.states(:,2))
-plot(controller.states(:,3))
+plot(t_arr, controller.states(:,2))
+plot(t_arr, controller.states(:,3))
 legend(["x","y","z"])
 xlabel('Time (s)')
-title("Position")
 ylabel('Position (m)')
+title("Position")
 saveas(gcf, './barrier/position.png')
 
 figure(7)
-
-plot(controller.states(:,4))
+plot(t_arr, controller.states(:,4))
 grid on
 hold on
-plot(controller.states(:,5))
-plot(controller.states(:,6))
+plot(t_arr, controller.states(:,5))
+plot(t_arr, controller.states(:,6))
 legend(["vx","vy","vz"])
 xlabel('Time (s)')
 title("Velocity")
@@ -174,9 +183,11 @@
 
 % barrier state plot
 figure(8)
-plot(controller.states(:,13))
+plot(t_arr, controller.states(:,13))
 grid on
 xlabel('Time (s)')
 ylabel('Barrier State')
 title("Barrier State")
 saveas(gcf, './barrier/barrier_state.png')
+
+