diff --git a/code/MATLAB/ProjectToSE3.m b/code/MATLAB/ProjectToSE3.m
new file mode 100644
index 0000000..72ec825
--- /dev/null
+++ b/code/MATLAB/ProjectToSE3.m
@@ -0,0 +1,23 @@
+%*** BASIC HELPER FUNCTIONS ***
+
+function T = ProjectToSE3(mat)
+% Takes mat: A matrix near SE(3) to project to SE(3).
+% Returns T representing the closest rotation matrix that is in SE(3)
+% Example Inputs:
+%{
+  clear; clc;
+  mat = [ 0.675, 0.150,  0.720, 1.2;
+          0.370, 0.771, -0.511, 5.4;
+         -0.630, 0.619,  0.472, 3.6;
+          0.003, 0.002,  0.010, 0.9];
+  T = ProjectToSE3(mat)
+%}
+% Output:
+% T =
+%     0.6790    0.1489    0.7189    1.2000
+%     0.3732    0.7732   -0.5127    5.4000
+%    -0.6322    0.6164    0.4694    3.6000
+%          0         0         0    1.0000
+
+T = RpToTrans(ProjectToSO3(mat(1: 3, 1: 3)), mat(1: 3, 4));
+end
\ No newline at end of file
diff --git a/code/MATLAB/ProjectToSO3.m b/code/MATLAB/ProjectToSO3.m
new file mode 100644
index 0000000..dc6ff2e
--- /dev/null
+++ b/code/MATLAB/ProjectToSO3.m
@@ -0,0 +1,25 @@
+%*** BASIC HELPER FUNCTIONS ***
+
+function R = ProjectToSO3(mat)
+% Takes mat: A matrix near SO(3) to project to SO(3).
+% Returns R representing the closest rotation matrix that is in SO(3)
+% Example Inputs:
+%{
+  clear; clc;
+  mat = [ 0.675, 0.150,  0.720;
+          0.370, 0.771, -0.511;
+         -0.630, 0.619,  0.472];
+  R = ProjectToSO3(mat)
+%}
+% Output:
+% R =
+%    0.6790    0.1489    0.7189
+%    0.3732    0.7732   -0.5127
+%   -0.6322    0.6164    0.4694
+
+[U, S, V] = svd(mat);
+R = U * V';
+if det(R) < 0
+    R = [R(:, 1: 2); -R(:, 3)];
+end
+end
\ No newline at end of file