Add Gradients for Cost Function & Nonlinear Constraints

- Set gradient options to true for `fmincon` optimizer
- Play with optimizer limits a bit
- Add gradient computation for cost function
- Add gradient computation for track & obstacle constraints
- Compute gradients offline in `miscellaneous_stuff.m`
- Update `point_to_line` to output signed distance

Experimentation/miscellaneous_stuff.m

@@ -12,7 +12,7 @@ plot(Y_test(:,1), Y_test(:,3), 'k-')
 plot(TestTrack.bl(1,:), TestTrack.bl(2,:), 'r.')
 plot(TestTrack.br(1,:), TestTrack.br(2,:), 'r.')
 
-%% Part 2 Symbolic Jacobians
+%% Part 2 Symbolic Jacobians for Linearized System
 close all;
 clear all;
 clc;
@@ -65,4 +65,47 @@ B_c_symb = [ ...
 ];
 
 A_c = matlabFunction(A_c_symb, 'File', 'A_c_func');
-B_c = matlabFunction(B_c_symb, 'File', 'B_c_func');
+B_c = matlabFunction(B_c_symb, 'File', 'B_c_func');
+
+%% Part 2 Symbolic Jacobians for Track Constraint
+close all;
+clear all;
+clc;
+
+syms x_err y_err x_ref y_ref v1_x v1_y v2_x v2_y
+x = x_err + x_ref;
+y = y_err + y_ref;
+
+pt = [x; y; 0];
+v1 = [v1_x; v1_y; 0];
+v2 = [v2_x; v2_y; 0];
+
+% Compute vectors
+a       = v2 - v1; % vector from v1 to v2
+b       = pt - v1; % vector from v1 to pt
+cross_p = cross(a,b);
+
+% Compute distance & direction
+dist        = norm(cross_p) / norm(a);
+direction   = sign(cross_p(3));
+signed_dist = dist * direction;
+
+temp1 = matlabFunction(jacobian(signed_dist, x_err), 'File', 'signed_dist_x_err');
+temp2 = matlabFunction(jacobian(signed_dist, y_err), 'File', 'signed_dist_y_err');
+
+%% Part 2 Symbolic Jacobians for Obstacle Constraint
+close all;
+clear all;
+clc;
+
+syms x_err y_err x_ref y_ref rad_obstacle cen_obstacle_x cen_obstacle_y
+x = x_err + x_ref;
+y = y_err + y_ref;
+
+obstacle_func = ...
+    (rad_obstacle)^2 ...
+    - (x - cen_obstacle_x)^2 ...
+    - (y - cen_obstacle_y)^2;
+
+temp1 = matlabFunction(jacobian(obstacle_func, x_err), 'File', 'obstacle_x_err');
+temp2 = matlabFunction(jacobian(obstacle_func, y_err), 'File', 'obstacle_y_err');