diff --git a/dart/utils/AccelerationTrackAndMinimize.cpp b/dart/utils/AccelerationTrackAndMinimize.cpp
new file mode 100644
index 000000000..f2fc408af
--- /dev/null
+++ b/dart/utils/AccelerationTrackAndMinimize.cpp
@@ -0,0 +1,190 @@
+#include "AccelerationTrackAndMinimize.hpp"
+
+#include <iostream>
+
+// #include <Eigen/Core>
+// #include <Eigen/Dense>
+#include <Eigen/IterativeLinearSolvers>
+// #include <unsupported/Eigen/IterativeSolvers>
+
+#include "dart/math/MathTypes.hpp"
+
+namespace dart {
+namespace utils {
+
+AccelerationTrackAndMinimize::AccelerationTrackAndMinimize(
+    int numTimesteps,
+    std::vector<bool> trackAccelerationAtTimesteps,
+    s_t zeroUnobservedAccWeight,
+    s_t trackObservedAccWeight,
+    s_t regularizationWeight,
+    s_t dt,
+    int numIterations)
+  : mTimesteps(numTimesteps),
+    mDt(dt),
+    mTrackAccelerationAtTimesteps(trackAccelerationAtTimesteps),
+    mTrackObservedAccWeight(trackObservedAccWeight),
+    mZeroUnobservedAccWeight(zeroUnobservedAccWeight),
+    mRegularizationWeight(regularizationWeight),
+    mNumIterations(numIterations),
+    mNumIterationsBackoff(6),
+    mDebugIterationBackoff(false),
+    mConvergenceTolerance(1e-10)
+{
+  if (mTimesteps < 3)
+  {
+    std::cout << "AccelerationTrackAndMinimize requires at least 3 timesteps"
+              << std::endl;
+    return;
+  }
+  if (mTrackAccelerationAtTimesteps.size() != mTimesteps)
+  {
+    std::cout << "trackAccelerationAtTimesteps.size() != mTimesteps"
+              << std::endl;
+    return;
+  }
+
+  Eigen::Vector3s stamp;
+  stamp << 1, -2, 1;
+  stamp *= 1.0 / (mDt * mDt);
+
+  typedef Eigen::Triplet<s_t> T;
+  std::vector<T> tripletList;
+  const int accTimesteps = mTimesteps - 2;
+  for (int i = 0; i < accTimesteps; i++)
+  {
+    for (int j = 0; j < 3; j++)
+    {
+      if (trackAccelerationAtTimesteps[i + 1])
+      {
+        tripletList.push_back(T(i, i + j, stamp(j) * mTrackObservedAccWeight));
+      }
+      else
+      {
+        tripletList.push_back(T(i, i + j, stamp(j) * mZeroUnobservedAccWeight));
+      }
+    }
+  }
+  for (int i = 0; i < mTimesteps; i++)
+  {
+    tripletList.push_back(T(accTimesteps + i, i, mRegularizationWeight));
+  }
+  // Add one extra input (to solve for) which applies a linear offset to the
+  // tracked accelerations
+  for (int i = 0; i < accTimesteps; i++)
+  {
+    if (trackAccelerationAtTimesteps[i + 1])
+    {
+      tripletList.push_back(T(i, mTimesteps, 1.0));
+    }
+  }
+  mB_sparse
+      = Eigen::SparseMatrix<s_t>(accTimesteps + mTimesteps, mTimesteps + 1);
+  mB_sparse.setFromTriplets(tripletList.begin(), tripletList.end());
+  mB_sparse.makeCompressed();
+  mB_sparseSolver.analyzePattern(mB_sparse);
+  mB_sparseSolver.factorize(mB_sparse);
+  if (mB_sparseSolver.info() != Eigen::Success)
+  {
+    std::cout << "mB_sparseSolver.factorize(mB_sparse) error: "
+              << mB_sparseSolver.lastErrorMessage() << std::endl;
+  }
+  assert(mB_sparseSolver.info() == Eigen::Success);
+}
+
+AccelerationTrackingResult AccelerationTrackAndMinimize::minimize(
+    Eigen::VectorXs series, Eigen::VectorXs trackAcc)
+{
+  AccelerationTrackingResult result;
+  result.series = series;
+  result.accelerationOffset = 0.0;
+  if (series.size() != mTimesteps)
+  {
+    std::cout << "series.size() != mTimesteps" << std::endl;
+    return result;
+  }
+  if (trackAcc.size() != mTimesteps)
+  {
+    std::cout << "trackAcc.size() != mTimesteps" << std::endl;
+    return result;
+  }
+
+  const int accTimesteps = mTimesteps - 2;
+  Eigen::VectorXs b = Eigen::VectorXs(accTimesteps + mTimesteps);
+  b.segment(0, accTimesteps) = trackAcc.segment(1, accTimesteps);
+  for (int i = 0; i < accTimesteps; i++)
+  {
+    if (!mTrackAccelerationAtTimesteps[i + 1])
+    {
+      if (b(i) != 0)
+      {
+        std::cout << "Warning: trackAcc[" << i << "] is non-zero, but we're "
+                  << "not tracking acceleration at this timestep. Setting it "
+                  << "to zero. Check how you are formatting your input array!"
+                  << std::endl;
+      }
+      b(i) = 0;
+    }
+    else
+    {
+      b(i) *= mTrackObservedAccWeight;
+    }
+  }
+  b.segment(accTimesteps, mTimesteps) = series * mRegularizationWeight;
+
+  Eigen::VectorXs paddedSeries = Eigen::VectorXs::Zero(mTimesteps + 1);
+  paddedSeries.head(mTimesteps) = series;
+
+  int iterations = mNumIterations;
+  for (int i = 0; i < mNumIterationsBackoff; i++)
+  {
+    Eigen::LeastSquaresConjugateGradient<Eigen::SparseMatrix<s_t>> solver;
+    solver.compute(mB_sparse);
+    solver.setTolerance(mConvergenceTolerance);
+    solver.setMaxIterations(iterations);
+    paddedSeries = solver.solveWithGuess(b, paddedSeries);
+    // Check convergence
+    if (solver.info() == Eigen::Success)
+    {
+      // Converged
+      break;
+    }
+    else
+    {
+      if (mDebugIterationBackoff)
+      {
+        std::cout << "[AccelerationTrackAndMinimize] "
+                     "LeastSquaresConjugateGradient did "
+                     "not converge in "
+                  << iterations << ", with error " << solver.error()
+                  << " so doubling iteration count and trying again."
+                  << std::endl;
+      }
+      iterations *= 2;
+    }
+  }
+
+  result.series = paddedSeries.head(mTimesteps);
+  result.accelerationOffset
+      = paddedSeries(mTimesteps) / mTrackObservedAccWeight;
+
+  return result;
+}
+
+void AccelerationTrackAndMinimize::setDebugIterationBackoff(bool debug)
+{
+  mDebugIterationBackoff = debug;
+}
+
+void AccelerationTrackAndMinimize::setNumIterationsBackoff(int numIterations)
+{
+  mNumIterationsBackoff = numIterations;
+}
+
+void AccelerationTrackAndMinimize::setConvergenceTolerance(s_t tolerance)
+{
+  mConvergenceTolerance = tolerance;
+}
+
+} // namespace utils
+} // namespace dart
\ No newline at end of file
diff --git a/dart/utils/AccelerationTrackAndMinimize.hpp b/dart/utils/AccelerationTrackAndMinimize.hpp
new file mode 100644
index 000000000..2e29c669a
--- /dev/null
+++ b/dart/utils/AccelerationTrackAndMinimize.hpp
@@ -0,0 +1,59 @@
+#ifndef UTILS_ACC_TRACK_AND_MINIMIZE
+#define UTILS_ACC_TRACK_AND_MINIMIZE
+
+#include <Eigen/Sparse>
+#include <Eigen/SparseQR>
+
+#include "dart/math/MathTypes.hpp"
+
+namespace dart {
+namespace utils {
+
+typedef struct AccelerationTrackingResult
+{
+  Eigen::VectorXs series;
+  s_t accelerationOffset;
+} AccelerationTrackingResult;
+
+class AccelerationTrackAndMinimize
+{
+public:
+  AccelerationTrackAndMinimize(
+      int numTimesteps,
+      std::vector<bool> trackAccelerationAtTimesteps,
+      s_t zeroUnobservedAccWeight = 1.0,
+      s_t trackObservedAccWeight = 1.0,
+      s_t regularizationWeight = 0.01,
+      s_t dt = 1.0,
+      int numIterations = 10000);
+
+  AccelerationTrackingResult minimize(
+      Eigen::VectorXs series, Eigen::VectorXs trackAcc);
+
+  void setDebugIterationBackoff(bool debug);
+
+  void setNumIterationsBackoff(int numIterations);
+
+  void setConvergenceTolerance(s_t tolerance);
+
+protected:
+  int mTimesteps;
+  s_t mDt;
+  s_t mZeroUnobservedAccWeight;
+  s_t mTrackObservedAccWeight;
+  s_t mRegularizationWeight;
+  std::vector<bool> mTrackAccelerationAtTimesteps;
+
+  int mNumIterations;
+  int mNumIterationsBackoff;
+  bool mDebugIterationBackoff;
+  s_t mConvergenceTolerance;
+  Eigen::SparseMatrix<s_t> mB_sparse;
+  Eigen::SparseQR<Eigen::SparseMatrix<s_t>, Eigen::NaturalOrdering<int>>
+      mB_sparseSolver;
+};
+
+} // namespace utils
+} // namespace dart
+
+#endif
\ No newline at end of file
diff --git a/python/_nimblephysics/utils/AccelerationTrackAndMinimize.cpp b/python/_nimblephysics/utils/AccelerationTrackAndMinimize.cpp
new file mode 100644
index 000000000..4c7fb9721
--- /dev/null
+++ b/python/_nimblephysics/utils/AccelerationTrackAndMinimize.cpp
@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2011-2019, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/master/LICENSE
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <dart/utils/AccelerationTrackAndMinimize.hpp>
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+namespace py = pybind11;
+
+namespace dart {
+namespace python {
+
+void AccelerationTrackAndMinimize(py::module& m)
+{
+  py::class_<dart::utils::AccelerationTrackingResult>(
+      m, "AccelerationTrackingResult")
+      .def_readwrite("series", &dart::utils::AccelerationTrackingResult::series)
+      .def_readwrite(
+          "accelerationOffset",
+          &dart::utils::AccelerationTrackingResult::accelerationOffset);
+
+  ::py::class_<dart::utils::AccelerationTrackAndMinimize>(
+      m, "AccelerationTrackAndMinimize")
+      .def(
+          ::py::init<int, std::vector<bool>, s_t, s_t, s_t, s_t, int>(),
+          ::py::arg("numTimesteps"),
+          ::py::arg("trackAccelerationAtTimesteps"),
+          ::py::arg("zeroUnobservedAccWeight") = 1.0,
+          ::py::arg("trackObservedAccWeight") = 1.0,
+          ::py::arg("regularizationWeight") = 0.01,
+          ::py::arg("dt") = 1.0,
+          ::py::arg("numIterations") = 10000)
+      .def(
+          "minimize",
+          &dart::utils::AccelerationTrackAndMinimize::minimize,
+          ::py::arg("series"),
+          ::py::arg("trackAcc"))
+      .def(
+          "setDebugIterationBackoff",
+          &dart::utils::AccelerationTrackAndMinimize::setDebugIterationBackoff,
+          ::py::arg("iterations"))
+      .def(
+          "setNumIterationsBackoff",
+          &dart::utils::AccelerationTrackAndMinimize::setNumIterationsBackoff,
+          ::py::arg("series"))
+      .def(
+          "setConvergenceTolerance",
+          &dart::utils::AccelerationTrackAndMinimize::setConvergenceTolerance,
+          ::py::arg("tolerance"));
+}
+
+} // namespace python
+} // namespace dart
\ No newline at end of file
diff --git a/python/_nimblephysics/utils/module.cpp b/python/_nimblephysics/utils/module.cpp
index b734b4dac..240d8a887 100644
--- a/python/_nimblephysics/utils/module.cpp
+++ b/python/_nimblephysics/utils/module.cpp
@@ -45,6 +45,7 @@ void MJCFExporter(py::module& sm);
 void UniversalLoader(py::module& sm);
 void AccelerationSmoother(py::module& sm);
 void AccelerationMinimizer(py::module& sm);
+void AccelerationTrackAndMinimize(py::module& sm);
 
 void dart_utils(py::module& m)
 {
@@ -58,6 +59,7 @@ void dart_utils(py::module& m)
   UniversalLoader(sm);
   AccelerationSmoother(sm);
   AccelerationMinimizer(sm);
+  AccelerationTrackAndMinimize(sm);
 }
 
 } // namespace python
diff --git a/unittests/unit/CMakeLists.txt b/unittests/unit/CMakeLists.txt
index 6dfda9cc0..8907c2b9f 100644
--- a/unittests/unit/CMakeLists.txt
+++ b/unittests/unit/CMakeLists.txt
@@ -48,6 +48,9 @@ if(TARGET dart-utils)
   dart_add_test("unit" test_AccelerationMinimizer)
   target_link_libraries(test_AccelerationMinimizer dart-utils)
 
+  dart_add_test("unit" test_AccelerationTrackAndMinimize)
+  target_link_libraries(test_AccelerationTrackAndMinimize dart-utils)
+
   dart_add_test("unit" test_IKInitializer)
   target_link_libraries(test_IKInitializer dart-utils)
 
diff --git a/unittests/unit/test_AccelerationTrackAndMinimize.cpp b/unittests/unit/test_AccelerationTrackAndMinimize.cpp
new file mode 100644
index 000000000..e674c45e2
--- /dev/null
+++ b/unittests/unit/test_AccelerationTrackAndMinimize.cpp
@@ -0,0 +1,172 @@
+#include <iostream>
+#include <memory>
+
+#include <gtest/gtest.h>
+
+#include "dart/math/MathTypes.hpp"
+#include "dart/utils/AccelerationMinimizer.hpp"
+#include "dart/utils/AccelerationTrackAndMinimize.hpp"
+
+#include "TestHelpers.hpp"
+
+using namespace dart;
+using namespace utils;
+
+// #define ALL_TESTS
+
+#ifdef ALL_TESTS
+TEST(ACCEL_TRACK_AND_MINIMIZE, DOES_NOT_CRASH)
+{
+  int timesteps = 50;
+  std::vector<bool> trackAcc = std::vector<bool>(timesteps, false);
+  for (int i = 0; i < timesteps; i++)
+  {
+    if (i % 2 == 0)
+    {
+      trackAcc[i] = true;
+    }
+  }
+  AccelerationTrackAndMinimize minimizer(timesteps, trackAcc, 1.0, 1.0);
+
+  Eigen::VectorXs series = Eigen::VectorXs::Random(timesteps);
+  Eigen::VectorXs track = Eigen::VectorXs::Random(timesteps);
+  for (int i = 0; i < timesteps; i++)
+  {
+    if (!trackAcc[i])
+    {
+      track[i] = 0.0;
+    }
+  }
+
+  Eigen::VectorXs x = minimizer.minimize(series, track).series;
+
+  std::cout << "Finished" << std::endl;
+  std::cout << x << std::endl;
+}
+#endif
+
+#ifdef ALL_TESTS
+TEST(ACCEL_TRACK_AND_MINIMIZE, REDUCES_TO_ACC_MIN)
+{
+  int timesteps = 50;
+  std::vector<bool> trackAcc = std::vector<bool>(timesteps, false);
+  s_t smoothingWeight = 1.0;
+  s_t regularizationWeight = 0.01;
+  s_t dt = 1.0;
+  AccelerationTrackAndMinimize minimizer(
+      timesteps, trackAcc, smoothingWeight, 0.0, regularizationWeight, dt);
+  AccelerationMinimizer minimizer2(timesteps, 1.0, 0.01);
+
+  Eigen::VectorXs series = Eigen::VectorXs::Random(timesteps);
+  Eigen::VectorXs track = Eigen::VectorXs::Random(timesteps);
+  for (int i = 0; i < timesteps; i++)
+  {
+    if (!trackAcc[i])
+    {
+      track[i] = 0.0;
+    }
+  }
+
+  Eigen::VectorXs x = minimizer.minimize(series, track).series;
+  Eigen::VectorXs x2 = minimizer2.minimize(series);
+
+  s_t dist = (x - x2).norm();
+  if (dist > 1e-8)
+  {
+    std::cout << "x: " << x << std::endl;
+    std::cout << "x2: " << x2 << std::endl;
+  }
+  EXPECT_TRUE(dist < 1e-8);
+}
+#endif
+
+#ifdef ALL_TESTS
+TEST(ACCEL_TRACK_AND_MINIMIZE, PERFECTLY_TRACKS_ACC)
+{
+  int timesteps = 10;
+  std::vector<bool> trackAcc = std::vector<bool>(timesteps, true);
+  s_t dt = 0.1;
+  AccelerationTrackAndMinimize minimizer(
+      timesteps, trackAcc, 0.0, 1.0, 0.0, dt);
+  minimizer.setDebugIterationBackoff(true);
+
+  Eigen::VectorXs series = Eigen::VectorXs::Random(timesteps);
+  Eigen::VectorXs acc = Eigen::VectorXs::Zero(timesteps);
+  for (int i = 1; i < timesteps - 1; i++)
+  {
+    acc[i] = (series[i + 1] - 2 * series[i] + series[i - 1]) / (dt * dt);
+  }
+  acc[0] = acc[1];
+  acc[timesteps - 1] = acc[timesteps - 2];
+
+  Eigen::VectorXs initialization = Eigen::VectorXs::Random(timesteps);
+
+  Eigen::VectorXs x = minimizer.minimize(initialization, acc).series;
+
+  Eigen::VectorXs recoveredAcc = Eigen::VectorXs::Zero(timesteps);
+  for (int i = 1; i < timesteps - 1; i++)
+  {
+    recoveredAcc[i] = (x[i + 1] - 2 * x[i] + x[i - 1]) / (dt * dt);
+  }
+  recoveredAcc[0] = recoveredAcc[1];
+  recoveredAcc[timesteps - 1] = recoveredAcc[timesteps - 2];
+  s_t dist = (recoveredAcc - acc).norm();
+  if (dist > 1e-8)
+  {
+    Eigen::MatrixXs compare = Eigen::MatrixXs(timesteps, 3);
+    compare.col(0) = acc;
+    compare.col(1) = recoveredAcc;
+    compare.col(2) = acc - recoveredAcc;
+    std::cout << "acc - recovered acc - diff: " << std::endl;
+    std::cout << compare << std::endl;
+  }
+  EXPECT_TRUE(dist < 1e-8);
+}
+#endif
+
+#ifdef ALL_TESTS
+TEST(ACCEL_TRACK_AND_MINIMIZE, TRACKS_OFFSET_ACCS)
+{
+  int timesteps = 10;
+  std::vector<bool> trackAcc = std::vector<bool>(timesteps, true);
+  s_t dt = 0.1;
+  AccelerationTrackAndMinimize minimizer(
+      timesteps, trackAcc, 1.0, 1.0, 1.0, dt);
+  minimizer.setDebugIterationBackoff(true);
+
+  Eigen::VectorXs series = Eigen::VectorXs::Random(timesteps);
+  Eigen::VectorXs acc = Eigen::VectorXs::Zero(timesteps);
+  for (int i = 1; i < timesteps - 1; i++)
+  {
+    acc[i] = (series[i + 1] - 2 * series[i] + series[i - 1]) / (dt * dt);
+  }
+  acc[0] = acc[1];
+  acc[timesteps - 1] = acc[timesteps - 2];
+
+  Eigen::VectorXs offsetAcc = acc + Eigen::VectorXs::Ones(timesteps) * 0.5;
+
+  AccelerationTrackingResult result = minimizer.minimize(series, offsetAcc);
+  Eigen::VectorXs x = result.series;
+  s_t offset = result.accelerationOffset;
+  EXPECT_NEAR(offset, 0.5, 1e-6);
+
+  Eigen::VectorXs recoveredAcc = Eigen::VectorXs::Zero(timesteps);
+  for (int i = 1; i < timesteps - 1; i++)
+  {
+    recoveredAcc[i] = (x[i + 1] - 2 * x[i] + x[i - 1]) / (dt * dt);
+  }
+  recoveredAcc[0] = recoveredAcc[1];
+  recoveredAcc[timesteps - 1] = recoveredAcc[timesteps - 2];
+  s_t dist = (recoveredAcc - acc).norm();
+  if (dist > 1e-6)
+  {
+    Eigen::MatrixXs compare = Eigen::MatrixXs(timesteps, 3);
+    compare.col(0) = acc;
+    compare.col(1) = recoveredAcc;
+    compare.col(2) = acc - recoveredAcc;
+    std::cout << "acc - recovered acc - diff: " << std::endl;
+    std::cout << compare << std::endl;
+  }
+  EXPECT_TRUE(dist < 1e-6);
+}
+#endif
\ No newline at end of file