diff --git a/.gitmodules b/.gitmodules
index 144d61c..0df926c 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,3 +1,6 @@
 [submodule "libs/googletest"]
 	path = libs/googletest
 	url = https://github.com/google/googletest
+[submodule "libs/benchmark"]
+	path = libs/benchmark
+	url = https://github.com/google/benchmark
diff --git a/build.gradle b/build.gradle
index 20c6df3..254f88a 100644
--- a/build.gradle
+++ b/build.gradle
@@ -25,6 +25,31 @@ model {
         }
     }
     testSuites {
+        falconlibraryBench(GoogleTestTestSuiteSpec) {
+            testing $.components.falconlibrary
+
+            binaries.all {
+                if (toolChain instanceof GccCompatibleToolChain) {
+                    linker.args << '-pthread'
+                } else {
+                    // MSVC
+                    linker.args << 'shlwapi.lib'
+                }
+                if (!project.hasProperty("withBench"))
+                    tasks.withType(RunTestExecutable) { RunTestExecutable task -> 
+                        task.enabled = false
+                    }
+            }
+
+            sources.cpp {
+                source {
+                    srcDir 'src/bench'
+                    include '**/*.cpp'
+                }
+                lib project: ':libs', library: 'googleBench', linkage: 'static'
+            }
+        }
+
         falconlibraryTest(GoogleTestTestSuiteSpec) {
             testing $.components.falconlibrary
             
diff --git a/libs/benchmark b/libs/benchmark
new file mode 160000
index 0000000..04a9343
--- /dev/null
+++ b/libs/benchmark
@@ -0,0 +1 @@
+Subproject commit 04a9343fc9b7886591c65933b94d0cee7a915452
diff --git a/libs/build.gradle b/libs/build.gradle
index bd5be67..92fa5d0 100644
--- a/libs/build.gradle
+++ b/libs/build.gradle
@@ -2,6 +2,7 @@ apply plugin: "cpp"
 
 ext.libroot = new File(rootProject.rootDir, "libs")
 ext.gtest_root = new File(libroot, "googletest/googletest")
+ext.gbench_root = new File(libroot, "benchmark")
 
 model {
     components {
@@ -17,5 +18,30 @@ model {
                 }
             }
         }
+        googleBench(NativeLibrarySpec) {
+            binaries.all {
+                if (toolChain instanceof GccCompatibleToolChain) {
+                    cppCompiler.args << '-std=c++14' << '-Wno-deprecated-declarations'
+                    linker.args << '-pthread'
+                } else {
+                    // MSVC
+                    cppCompiler.args << '/std:c++14'
+                    linker.args << 'shlwapi.lib'
+                }
+            }
+
+            sources.cpp {
+                source {
+                    srcDir new File(gbench_root, "src")
+                    include "**/*.cc"
+                    exclude "**/benchmark_main.cc"
+                }
+                exportedHeaders {
+                    srcDirs new File(gbench_root, "src"), new File(gbench_root, "include")
+                    include "**/*.hpp", "**/*.h"
+                }
+                lib project: ':libs', library: "googleTest", linkage: "static"
+            }
+        }
     }
 }
\ No newline at end of file
diff --git a/src/bench/bench_arc_param.cpp b/src/bench/bench_arc_param.cpp
new file mode 100644
index 0000000..3e07599
--- /dev/null
+++ b/src/bench/bench_arc_param.cpp
@@ -0,0 +1,14 @@
+#include <FalconLibrary.h>
+#include <benchmark/benchmark.h>
+
+static void BM_ArcParam(benchmark::State& state) {
+  const frc5190::Pose2d initial{0.0, 0.0, frc5190::Rotation2d::FromDegrees(0.0)};
+  const frc5190::Pose2d final{10.0, 10.0, frc5190::Rotation2d::FromDegrees(50.0)};
+  const auto            spline = std::make_shared<frc5190::ParametricQuinticHermiteSpline>(initial, final);
+
+  for (auto _ : state) {
+    frc5190::SplineGenerator::ParameterizeSpline(spline, 2.0, 0.05, 0.1);
+  }
+}
+
+BENCHMARK(BM_ArcParam)->Arg(100)->Complexity()->Unit(benchmark::kMillisecond);
\ No newline at end of file
diff --git a/src/bench/bench_trajectory_generation.cpp b/src/bench/bench_trajectory_generation.cpp
new file mode 100644
index 0000000..be57a69
--- /dev/null
+++ b/src/bench/bench_trajectory_generation.cpp
@@ -0,0 +1,15 @@
+#include <FalconLibrary.h>
+#include <benchmark/benchmark.h>
+
+static void BM_TrajectoryGeneration(benchmark::State& state) {
+  const frc5190::Pose2d initial{0.0, 0.0, frc5190::Rotation2d::FromDegrees(0.0)};
+  const frc5190::Pose2d final{10.0, 10.0, frc5190::Rotation2d::FromDegrees(50.0)};
+
+  for (auto _ : state) {
+    frc5190::TrajectoryGenerator::GenerateTrajectory(
+        std::vector<frc5190::Pose2d>{initial, final},
+        std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{}, 0.0, 0.0, 3., 2., false);
+  }
+}
+
+BENCHMARK(BM_TrajectoryGeneration)->Arg(100)->Complexity()->Unit(benchmark::kMillisecond);
\ No newline at end of file
diff --git a/src/bench/main.cpp b/src/bench/main.cpp
new file mode 100644
index 0000000..a10c74f
--- /dev/null
+++ b/src/bench/main.cpp
@@ -0,0 +1,4 @@
+#include <benchmark/benchmark.h>
+
+BENCHMARK_MAIN();
+
diff --git a/src/include/mathematics/control/TrajectoryTracker.h b/src/include/mathematics/control/TrajectoryTracker.h
index ad06120..c123685 100644
--- a/src/include/mathematics/control/TrajectoryTracker.h
+++ b/src/include/mathematics/control/TrajectoryTracker.h
@@ -1,6 +1,6 @@
 #pragma once
-#include "../geometry/Pose2dWithCurvature.h"
-#include "../trajectory/TimedTrajectory.h"
+
+#include "mathematics/trajectory/TimedTrajectory.h"
 
 #include <memory>
 
diff --git a/src/include/mathematics/spline/SplineGenerator.h b/src/include/mathematics/spline/SplineGenerator.h
index dfa2652..6b47137 100644
--- a/src/include/mathematics/spline/SplineGenerator.h
+++ b/src/include/mathematics/spline/SplineGenerator.h
@@ -1,6 +1,8 @@
 #pragma once
 
 #include <vector>
+#include <memory>
+
 #include "../geometry/Pose2dWithCurvature.h"
 #include "ParametricSpline.h"
 
diff --git a/src/include/mathematics/trajectory/TrajectoryGenerator.h b/src/include/mathematics/trajectory/TrajectoryGenerator.h
index 650a894..ef4ba8d 100644
--- a/src/include/mathematics/trajectory/TrajectoryGenerator.h
+++ b/src/include/mathematics/trajectory/TrajectoryGenerator.h
@@ -28,7 +28,7 @@ class TrajectoryGenerator {
       }
     }
 
-    const auto indexed_trajectory = TrajectoryFromSplineWaypoints(waypoints, 0.051, 0.00127, 0.1);
+    const auto indexed_trajectory = TrajectoryFromSplineWaypoints(waypoints, 2.0, 0.05, 0.1);
 
     auto points = indexed_trajectory.Points();