diff --git a/include/gtsamutils.h b/include/gtsamutils.h
index e0e845b..405c43b 100755
--- a/include/gtsamutils.h
+++ b/include/gtsamutils.h
@@ -30,7 +30,6 @@ namespace gtsamutils{
     gtsam::Vector3 gyros_Vector3(const imu& myImu, const int& idx);
     Eigen::MatrixXd gyroMatrix(const imu& myImu);
     Eigen::MatrixXd accelMatrix(const imu& myImu);
-    double median(std::vector<double> v);
     uint nearestIdxToVal(std::vector<double> v, double val);
     void saveMatrixToFile(const gtsam::Matrix& A, const std::string &s, const std::string& filename);
     void writeEigenMatrixToCsvFile(const std::string& name, const Eigen::MatrixXd& matrix, const Eigen::IOFormat& CSVFormat=Eigen::IOFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "\n"));
diff --git a/src/gtsamutils.cpp b/src/gtsamutils.cpp
index 749faaa..c8d27bd 100755
--- a/src/gtsamutils.cpp
+++ b/src/gtsamutils.cpp
@@ -154,27 +154,7 @@ namespace gtsamutils{
         }
         return accels;
     }
-
-    double median(std::vector<double> len){
-        assert(!len.empty());
-        if (len.size() % 2 == 0) {
-            const auto median_it1 = len.begin() + len.size() / 2 - 1;
-            const auto median_it2 = len.begin() + len.size() / 2;
-
-            std::nth_element(len.begin(), median_it1 , len.end());
-            const auto e1 = *median_it1;
-
-            std::nth_element(len.begin(), median_it2 , len.end());
-            const auto e2 = *median_it2;
-
-            return (e1 + e2) / 2;
-
-        } else {
-            const auto median_it = len.begin() + len.size() / 2;
-            std::nth_element(len.begin(), median_it , len.end());
-            return *median_it;
-        }
-    }
+    
 
     uint nearestIdxToVal(std::vector<double> v, double val){
         // this is gonna be ugly. copies entire vector and brute force searches for index nearest to value val.
diff --git a/src/lowerBodyPoseEstimator.cpp b/src/lowerBodyPoseEstimator.cpp
index 5f84716..1cc8068 100755
--- a/src/lowerBodyPoseEstimator.cpp
+++ b/src/lowerBodyPoseEstimator.cpp
@@ -1897,7 +1897,7 @@ void lowerBodyPoseEstimator::adjustDistalImuTrajectoryForInboundJointAngles(cons
     if(zeroMedianCenterIntExtRot){
         Eigen::VectorXd intExtRotAngs=origJointAngles.col(2);
         std::vector<double> vec(intExtRotAngs.data(), intExtRotAngs.data() + intExtRotAngs.rows() * intExtRotAngs.cols()); // cast as std::vector
-        double myMedian=gtsamutils::median(vec);
+        double myMedian=mathutils::median(vec);
         uint medIdx=gtsamutils::nearestIdxToVal(vec, myMedian);
         //std::cout<<std::endl<<"int/ext rot median is "<<myMedian<<" and occurs at index "<<medIdx<<" (true val in array = "<<vec[medIdx]<<")"<<std::endl;
         R_B_to_B2 = lowerBodyPoseEstimator::getDistalImuOrientationAdjustmentGivenDesiredIntExtRotAngle(R_ProxSeg_to_N[medIdx],distalImuPose[medIdx].rotation(),0.0,rightAxisDist,distalImuToProxJointCtr,distalImuToDistalJointCtr); // R[B->B2]
@@ -1911,7 +1911,7 @@ void lowerBodyPoseEstimator::adjustDistalImuTrajectoryForInboundJointAngles(cons
     if(zeroMedianCenterAbAd){
         Eigen::VectorXd abAdAngs=origJointAngles.col(1);
         std::vector<double> vec(abAdAngs.data(), abAdAngs.data() + abAdAngs.rows() * abAdAngs.cols()); // cast as std::vector
-        double myMedian=gtsamutils::median(vec);
+        double myMedian=mathutils::median(vec);
         uint medIdx=gtsamutils::nearestIdxToVal(vec, myMedian);
         //std::cout<<std::endl<<"ab/ad rot median is "<<myMedian<<" and occurs at index "<<medIdx<<" (true val in array = "<<vec[medIdx]<<")"<<std::endl;
         R_B_to_B2 = lowerBodyPoseEstimator::getDistalImuOrientationAdjustmentGivenDesiredAbAdAngle(R_ProxSeg_to_N[medIdx],distalImuPose[medIdx].rotation(),0.0,rightAxisDist,distalImuToProxJointCtr,distalImuToDistalJointCtr); // R[B->B2]