From 28231933028c61bbdf7683713372aa3b02e9daa2 Mon Sep 17 00:00:00 2001
From: Ryan Pavlik <ryan.pavlik@collabora.com>
Date: Tue, 12 Nov 2019 12:02:54 -0600
Subject: [PATCH] t_imu_fusion: Add capability to get world non-gravity
 acceleration

---
 src/xrt/auxiliary/tracking/t_imu_fusion.h | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/src/xrt/auxiliary/tracking/t_imu_fusion.h b/src/xrt/auxiliary/tracking/t_imu_fusion.h
index 43b436af2..3b326f88b 100644
--- a/src/xrt/auxiliary/tracking/t_imu_fusion.h
+++ b/src/xrt/auxiliary/tracking/t_imu_fusion.h
@@ -167,6 +167,18 @@ public:
 		return true;
 	}
 
+	/*!
+	 * Use this to obtain the residual, world-space acceleration not
+	 * associated with gravity, after incorporating a measurement.
+	 */
+	Eigen::Vector3d
+	getCorrectedWorldAccel(Eigen::Vector3d const &accel) const
+	{
+		Eigen::Vector3d adjusted_accel = accel * getAccelScaleFactor();
+		return (quat_ * adjusted_accel) -
+		       (Eigen::Vector3d::UnitY() * MATH_GRAVITY_M_S2);
+	}
+
 	Eigen::Matrix3d
 	getRotationMatrix() const
 	{