[mc_tvm] Include refAccel in OrientationFunction and PositionFunction

jrl-umi3218 · Sep 8, 2023 · 183ed86 · 183ed86
1 parent a77276c
commit 183ed86
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 2 deletions.
diff --git a/src/mc_tvm/OrientationFunction.cpp b/src/mc_tvm/OrientationFunction.cpp
@@ -58,7 +58,7 @@ void OrientationFunction::updateJacobian()
 
 void OrientationFunction::updateNormalAcceleration()
 {
-  normalAcceleration_ = frame_.normalAcceleration().angular();
+  normalAcceleration_ = frame_.normalAcceleration().angular() - refAccel_;
 }
 
 } // namespace mc_tvm
diff --git a/src/mc_tvm/PositionFunction.cpp b/src/mc_tvm/PositionFunction.cpp
@@ -57,7 +57,7 @@ void PositionFunction::updateJacobian()
 
 void PositionFunction::updateNormalAcceleration()
 {
-  normalAcceleration_ = frame_.normalAcceleration().linear();
+  normalAcceleration_ = frame_.normalAcceleration().linear() - refAccel_;
 }
 
 } // namespace mc_tvm