diff --git a/Deliverables/MPC_Class.m b/Deliverables/MPC_Class.m
index 1456c69..15bddc2 100644
--- a/Deliverables/MPC_Class.m
+++ b/Deliverables/MPC_Class.m
@@ -54,8 +54,9 @@ classdef MPC_Class
         ];
 
         % Simulation Parameters
-        T_s = 0.01; % Step Size [s]
-        T_p = 0.5;  % Prediction Horizon [s]
+        T_s = 0.01;         % Step Size [s]
+        T_p = 0.5;          % Prediction Horizon [s]
+        sim_stop_idx = 2e3; % Index in reference trajectory to stop sim
 
         % Decision Variables
         nstates = 6; % Number of states per prediction
@@ -126,10 +127,9 @@ classdef MPC_Class
             obj.ndecinputs  = obj.npredinputs * obj.ninputs;
             obj.ndec        = obj.ndecstates + obj.ndecinputs;
 
-            % Check if `Y_curr` is within prediction horizon of
-            % the end of `Y_ref`
+            % Stop simulation when `sim_stop_idx` is reached
             obj.FLAG_terminate = 0;
-            if size(obj.Y_ref, 1) - obj.ref_idx < obj.npred
+            if obj.ref_idx > obj.sim_stop_idx
                 obj.FLAG_terminate = 1;
             end
         end
diff --git a/Experimentation/part2_test_controller.m b/Experimentation/part2_test_controller.m
index f8e816c..3eaebed 100644
--- a/Experimentation/part2_test_controller.m
+++ b/Experimentation/part2_test_controller.m
@@ -4,7 +4,7 @@ clear;
 clc;
 
 %% Call simulation functions
-[Y,U,t_total,t_update] = forwardIntegrate();
+[Y,U,t_total,t_update,Xobs] = forwardIntegrate();
 info = getTrajectoryInfo(Y,U)
 
 %% Figures
@@ -17,9 +17,15 @@ grid on;
 load('TestTrack.mat')
 load('ROB535_ControlProject_part1_Team3.mat')
 
-plot(Y(:,1), Y(:,3), 'b-');
-plot(ROB535_ControlProject_part1_state(:,1), ROB535_ControlProject_part1_state(:,3), 'k-');
-
 plot(TestTrack.bl(1,:), TestTrack.bl(2,:), 'r.-');
 plot(TestTrack.br(1,:), TestTrack.br(2,:), 'r.-');
-plot(TestTrack.cline(1,:), TestTrack.cline(2,:), 'g.-');
\ No newline at end of file
+plot(TestTrack.cline(1,:), TestTrack.cline(2,:), 'g.-');
+
+for i = 1:length(Xobs)
+    xvals = Xobs{i}(:,1);
+    yvals = Xobs{i}(:,2);
+    fill(xvals, yvals, 'm');
+end
+
+plot(ROB535_ControlProject_part1_state(:,1), ROB535_ControlProject_part1_state(:,3), 'k--');
+plot(Y(:,1), Y(:,3), 'b-');
\ No newline at end of file
diff --git a/Simulation/forwardIntegrate.m b/Simulation/forwardIntegrate.m
index e4de4b9..faa69c8 100644
--- a/Simulation/forwardIntegrate.m
+++ b/Simulation/forwardIntegrate.m
@@ -1,4 +1,4 @@
-function [Y,U,t_total,t_update] = forwardIntegrate()
+function [Y,U,t_total,t_update,Xobs] = forwardIntegrate()
 % [Y,U,t_total,t_update] = forwardIntegrate
 % 
 % This script returns the vehicle trajectory with control input being