diff --git a/common/interpolation/src/spline_interpolation.cpp b/common/interpolation/src/spline_interpolation.cpp
index f8d14ff7bba37..3fa8ad17474bc 100644
--- a/common/interpolation/src/spline_interpolation.cpp
+++ b/common/interpolation/src/spline_interpolation.cpp
@@ -72,7 +72,7 @@ inline std::vector<double> solveTridiagonalMatrixAlgorithm(const TDMACoef & tdma
       x[j] = p[j] * x[j + 1] + q[j];
     }
   } else {
-    x.push_back(d[0] / b[0]);
+    x[0] = (d[0] / b[0]);
   }
 
   return x;
diff --git a/common/interpolation/test/src/test_spline_interpolation.cpp b/common/interpolation/test/src/test_spline_interpolation.cpp
index 9d9869903b614..e3e65c966cc4c 100644
--- a/common/interpolation/test/src/test_spline_interpolation.cpp
+++ b/common/interpolation/test/src/test_spline_interpolation.cpp
@@ -61,7 +61,7 @@ TEST(spline_interpolation, slerp)
     }
   }
 
-  {  // curve: query_keys is same as random
+  {  // curve: query_keys is random
     const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
     const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
     const std::vector<double> query_keys{0.0, 8.0, 18.0};
@@ -96,6 +96,18 @@ TEST(spline_interpolation, slerp)
       EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
     }
   }
+
+  {  // curve: query_keys is random. size of base_keys is 3 (edge case in the implementation)
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0};
+    const std::vector<double> query_keys{-1.0, 0.0, 4.0};
+    const std::vector<double> ans{-0.808769, -0.077539, 1.035096};
+
+    const auto query_values = interpolation::slerp(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
 }
 
 TEST(spline_interpolation, slerpYawFromPoints)