diff --git a/include/bodydynamics/math/force_torque_inertial_delta_tools.h b/include/bodydynamics/math/force_torque_inertial_delta_tools.h
index e5a87d1bf0c6898ecbb5b5a10b3a5a4b4ad709f1..4f771ceb0d39ca5f1b299b4d887bba9603775091 100644
--- a/include/bodydynamics/math/force_torque_inertial_delta_tools.h
+++ b/include/bodydynamics/math/force_torque_inertial_delta_tools.h
@@ -644,22 +644,24 @@ template <typename D1,
           typename D16,
           typename D17,
           typename D18,
+          typename D19,
           class T>
-inline void betweenStates(const MatrixBase<D1>&      p1,
-                          const MatrixBase<D2>&      v1,
-                          const MatrixBase<D5>&      L1,
-                          const QuaternionBase<D6>&  q1,
-                          const MatrixBase<D7>&      p2,
-                          const MatrixBase<D8>&      v2,
-                          const MatrixBase<D11>&     L2,
-                          const QuaternionBase<D12>& q2,
-                          const T                    dt,
-                          MatrixBase<D13>&           dpi,
-                          MatrixBase<D14>&           dvi,
-                          MatrixBase<D15>&           dpd,
-                          MatrixBase<D16>&           dvd,
-                          MatrixBase<D17>&           dL,
-                          QuaternionBase<D18>&       dq)
+inline void betweenStatesGlobalAcceleration(const MatrixBase<D1>&      p1,
+                                            const MatrixBase<D2>&      v1,
+                                            const MatrixBase<D5>&      L1,
+                                            const QuaternionBase<D6>&  q1,
+                                            const MatrixBase<D7>&      p2,
+                                            const MatrixBase<D8>&      v2,
+                                            const MatrixBase<D11>&     L2,
+                                            const QuaternionBase<D12>& q2,
+                                            const MatrixBase<D13>&     g,
+                                            const T                    dt,
+                                            MatrixBase<D14>&           dpi,
+                                            MatrixBase<D15>&           dvi,
+                                            MatrixBase<D16>&           dpd,
+                                            MatrixBase<D17>&           dvd,
+                                            MatrixBase<D18>&           dL,
+                                            QuaternionBase<D19>&       dq)
 {
     MatrixSizeCheck<3, 1>::check(p1);
     MatrixSizeCheck<3, 1>::check(v1);
@@ -667,25 +669,31 @@ inline void betweenStates(const MatrixBase<D1>&      p1,
     MatrixSizeCheck<3, 1>::check(p2);
     MatrixSizeCheck<3, 1>::check(v2);
     MatrixSizeCheck<3, 1>::check(L2);
+    MatrixSizeCheck<3, 1>::check(g);
     MatrixSizeCheck<3, 1>::check(dpi);
     MatrixSizeCheck<3, 1>::check(dvi);
     MatrixSizeCheck<3, 1>::check(dpd);
     MatrixSizeCheck<3, 1>::check(dvd);
     MatrixSizeCheck<3, 1>::check(dL);
 
-    dpi = q1.conjugate() * (p2 - p1 - v1 * dt - 0.5 * gravity() * dt * dt);
-    dvi = q1.conjugate() * (v2 - v1 - gravity() * dt);
+    dpi = q1.conjugate() * (p2 - p1 - v1 * dt - 0.5 * g * dt * dt);
+    dvi = q1.conjugate() * (v2 - v1 - g * dt);
     dpd = dpi;
     dvd = dvi;
     dL  = q1.conjugate() * (L2 - L1);
     dq  = q1.conjugate() * q2;
 }
 
-template <typename D1, typename D2, typename D3, class T>
-inline void betweenStates(const MatrixBase<D1>& x1, const MatrixBase<D2>& x2, T dt, MatrixBase<D3>& x2_minus_x1)
+template <typename D1, typename D2, typename D3, typename D4, class T>
+inline void betweenStatesGlobalAcceleration(const MatrixBase<D1>& x1, 
+                                            const MatrixBase<D2>& x2, 
+                                            const MatrixBase<D3>& g, 
+                                            T dt, 
+                                            MatrixBase<D4>& x2_minus_x1)
 {
     MatrixSizeCheck<13, 1>::check(x1);
     MatrixSizeCheck<13, 1>::check(x2);
+    MatrixSizeCheck<3,  1>::check(g);
     MatrixSizeCheck<19, 1>::check(x2_minus_x1);
 
     Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&x1(0));
@@ -703,37 +711,120 @@ inline void betweenStates(const MatrixBase<D1>& x1, const MatrixBase<D2>& x2, T
     Map<Matrix<typename D3::Scalar, 3, 1>>       dL(&x2_minus_x1(12));
     Map<Quaternion<typename D3::Scalar>>         dq(&x2_minus_x1(15));
 
-    betweenStates(p1, v1, L1, q1, p2, v2, L2, q2, dt, dpi, dvi, dpd, dvd, dL, dq);
+    betweenStatesGlobalAcceleration(p1, v1, L1, q1, p2, v2, L2, q2, g, dt, dpi, dvi, dpd, dvd, dL, dq);
 }
 
-template <typename D1, typename D2, class T>
-inline Matrix<typename D1::Scalar, 19, 1> betweenStates(const MatrixBase<D1>& x1, const MatrixBase<D2>& x2, T dt)
+template <typename D1, typename D2, typename D3, class T>
+inline Matrix<typename D1::Scalar, 19, 1> betweenStatesGlobalAcceleration(const MatrixBase<D1>& x1, 
+                                                                          const MatrixBase<D2>& x2,
+                                                                          const MatrixBase<D3>& g,
+                                                                          T dt)
 {
     Matrix<typename D1::Scalar, 19, 1> ret;
-    betweenStates(x1, x2, dt, ret);
+    betweenStatesGlobalAcceleration(x1, x2, g, dt, ret);
     return ret;
 }
 
-inline void betweenStates(const VectorComposite& x1, const VectorComposite& x2, double dt, VectorComposite& ret)
+inline void betweenStatesGlobalAcceleration(const VectorComposite& x1, 
+                                            const VectorComposite& x2, 
+                                            const Vector3d& g,
+                                            double dt, VectorComposite& ret)
 {
     Map<const Quaterniond> q1(x1.at('O').data());
     Map<const Quaterniond> q2(x2.at('O').data());
-
-    ret['P'] = q1.conjugate() * (x2.at('P') - x1.at('P') - x1.at('V') * dt - 0.5 * gravity() * dt * dt);
-    ret['V'] = q1.conjugate() * (x2.at('V') - x1.at('V') - gravity() * dt);
-    ret['p'] = ret.at('P');
-    ret['v'] = ret.at('V');
-    ret['L'] = q1.conjugate() * (x2.at('L') - x1.at('L'));
-    ret['O'] = (q1.conjugate() * q2).coeffs();
+    Map<Quaterniond> dq(ret.at('O').data());
+
+    betweenStatesGlobalAcceleration(x1.at('P'), 
+                                    x1.at('V'), 
+                                    x1.at('L'), 
+                                    q1, 
+                                    x2.at('P'), 
+                                    x2.at('V'), 
+                                    x2.at('L'), 
+                                    q2, 
+                                    g, 
+                                    dt, 
+                                    ret.at('P'), 
+                                    ret.at('V'), 
+                                    ret.at('p'), 
+                                    ret.at('v'), 
+                                    ret.at('L'), 
+                                    dq);
+
+    // ret['P'] = q1.conjugate() * (x2.at('P') - x1.at('P') - x1.at('V') * dt - 0.5 * g * dt * dt);
+    // ret['V'] = q1.conjugate() * (x2.at('V') - x1.at('V') - g * dt);
+    // ret['p'] = ret.at('P');
+    // ret['v'] = ret.at('V');
+    // ret['L'] = q1.conjugate() * (x2.at('L') - x1.at('L'));
+    // ret['O'] = (q1.conjugate() * q2).coeffs();
 }
 
-inline VectorComposite betweenStates(const VectorComposite& x1, const VectorComposite& x2, double dt)
+inline VectorComposite betweenStatesGlobalAcceleration(const VectorComposite& x1, 
+                                                       const VectorComposite& x2, 
+                                                       const Vector3d& g,
+                                                       double dt)
 {
     VectorComposite ret;
-    betweenStates(x1, x2, dt, ret);
+    betweenStatesGlobalAcceleration(x1, x2, g, dt, ret);
     return ret;
 }
 
+template <typename D1,
+          typename D2,
+          typename D5,
+          typename D6,
+          typename D7,
+          typename D8,
+          typename D11,
+          typename D12,
+          typename D13,
+          typename D14,
+          typename D15,
+          typename D16,
+          typename D17,
+          typename D18,
+          class T>
+inline void betweenStates(const MatrixBase<D1>&      p1,
+                          const MatrixBase<D2>&      v1,
+                          const MatrixBase<D5>&      L1,
+                          const QuaternionBase<D6>&  q1,
+                          const MatrixBase<D7>&      p2,
+                          const MatrixBase<D8>&      v2,
+                          const MatrixBase<D11>&     L2,
+                          const QuaternionBase<D12>& q2,
+                          const T                    dt,
+                          MatrixBase<D13>&           dpi,
+                          MatrixBase<D14>&           dvi,
+                          MatrixBase<D15>&           dpd,
+                          MatrixBase<D16>&           dvd,
+                          MatrixBase<D17>&           dL,
+                          QuaternionBase<D18>&       dq)
+{
+    betweenStatesGlobalAcceleration(p1, v1, L1, q1, p2, v2, L2, q2, gravity(), dt, dpi, dvi, dpd, dvd, dL, dq);
+}
+
+template <typename D1, typename D2, typename D3, class T>
+inline void betweenStates(const MatrixBase<D1>& x1, const MatrixBase<D2>& x2, T dt, MatrixBase<D3>& x2_minus_x1)
+{
+    betweenStatesGlobalAcceleration(x1, x2, gravity(), dt, x2_minus_x1);
+}
+
+template <typename D1, typename D2, class T>
+inline Matrix<typename D1::Scalar, 19, 1> betweenStates(const MatrixBase<D1>& x1, const MatrixBase<D2>& x2, T dt)
+{
+    return betweenStatesGlobalAcceleration(x1, x2, gravity(), dt);;
+}
+
+inline void betweenStates(const VectorComposite& x1, const VectorComposite& x2, double dt, VectorComposite& ret)
+{
+    betweenStatesGlobalAcceleration(x1, x2, gravity(), dt, ret);
+}
+
+inline VectorComposite betweenStates(const VectorComposite& x1, const VectorComposite& x2, double dt)
+{
+    return betweenStatesGlobalAcceleration(x1, x2, gravity(), dt);
+}
+
 template <typename Derived>
 Matrix<typename Derived::Scalar, 18, 1> log_fti(const MatrixBase<Derived>& delta_in)
 {