diff --git a/terrain_navigation/include/terrain_navigation/path.h b/terrain_navigation/include/terrain_navigation/path.h
index 41f660d3..92791ca2 100644
--- a/terrain_navigation/include/terrain_navigation/path.h
+++ b/terrain_navigation/include/terrain_navigation/path.h
@@ -84,10 +84,9 @@ class Path {
    * @param closest_point
    * @param tangent
    * @param curvature
-   * @param epsilon
    */
   void getClosestPoint(const Eigen::Vector3d &position, Eigen::Vector3d &closest_point, Eigen::Vector3d &tangent,
-                       double &curvature, double epsilon = 0.001) {
+                       double &curvature) {
     closest_point = segments.front().states.front().position;
 
     // Iterate through all segments
@@ -121,7 +120,6 @@ class Path {
   }
 
   PathSegment &getCurrentSegment(const Eigen::Vector3d &position) {
-    double theta{-std::numeric_limits<double>::infinity()};
     Eigen::Vector3d closest_point;
     Eigen::Vector3d tangent;
     double curvature;
@@ -150,7 +148,7 @@ class Path {
     }
   }
 
-  int getCurrentSegmentIndex(const Eigen::Vector3d &position, double epsilon = 0.1) {
+  int getCurrentSegmentIndex(const Eigen::Vector3d &position) {
     Eigen::Vector3d closest_point;
     Eigen::Vector3d tangent;
     double curvature;
diff --git a/terrain_navigation/include/terrain_navigation/path_segment.h b/terrain_navigation/include/terrain_navigation/path_segment.h
index cc777fdb..43c79315 100644
--- a/terrain_navigation/include/terrain_navigation/path_segment.h
+++ b/terrain_navigation/include/terrain_navigation/path_segment.h
@@ -48,7 +48,7 @@ struct State {
   Eigen::Vector4d attitude;
 };
 
-static void wrap_2pi(double &angle) {
+inline void wrap_2pi(double &angle) {
   while ((angle < 0.0) || (angle > 2 * M_PI)) {
     if (angle < 0.0) {
       angle += 2 * M_PI;
@@ -58,7 +58,7 @@ static void wrap_2pi(double &angle) {
   }
 }
 
-static void wrap_pi(double &angle) {
+inline void wrap_pi(double &angle) {
   while (std::abs(angle) > M_PI) {
     if (angle > 0)
       angle = angle - 2 * M_PI;
@@ -207,7 +207,7 @@ class PathSegment {
     if (states.size() == 1) {
       // Segment only contains a single state, meaning that it is nor a line or a arc
       theta = 1.0;
-    } else if (std::abs(curvature) < 0.0001) {
+    } else if (std::abs(curvature) < epsilon) {
       // Compute closest point on a line segment
       // Get Path Progress
       theta = getLineProgress(position, segment_start, segment_end);
diff --git a/terrain_navigation/src/data_logger.cpp b/terrain_navigation/src/data_logger.cpp
index 777c8571..1f57e0e1 100644
--- a/terrain_navigation/src/data_logger.cpp
+++ b/terrain_navigation/src/data_logger.cpp
@@ -62,9 +62,9 @@ void DataLogger::writeToFile(const std::string path) {
 
   for (auto data : data_list_) {
     for (auto key : keys_) {
-      if (std::string* s = std::any_cast<std::string>(&data.at(key))) {
+      if (std::any_cast<std::string>(&data.at(key))) {
         output_file << std::any_cast<std::string&>(data.at(key)) << field_seperator;
-      } else if (double* i = std::any_cast<double>(&data.at(key))) {
+      } else if (std::any_cast<double>(&data.at(key))) {
         output_file << std::any_cast<double&>(data.at(key)) << field_seperator;
       }
     }
diff --git a/terrain_planner/src/DubinsAirplane.cpp b/terrain_planner/src/DubinsAirplane.cpp
index a5c2e341..635bd443 100644
--- a/terrain_planner/src/DubinsAirplane.cpp
+++ b/terrain_planner/src/DubinsAirplane.cpp
@@ -1256,7 +1256,6 @@ void DubinsAirplaneStateSpace::getStateOnCircle(const ob::State* from, int rl /*
 }
 
 unsigned int DubinsAirplaneStateSpace::convert_idx(unsigned int i) const {
-  assert(i >= 0u && "In convert_idx, i < 0");
   assert(i < 6u && "In convert_idx, i > 5");
   switch (i) {
     case 0:  // start helix