INNER CODE UNIT · Python
_imu_linear_acc_body
MOLAorg/mola · mola_demos/demos/fake_sensor_publisher.py:103
def _imu_linear_acc_body(yaw, pitch, roll, ax_world=0.0, ay_world=0.0):
"""Specific force in body frame (m/s²): (world_accel - gravity) rotated to body."""
sf = np.array([ax_world, ay_world, 9.81]
) # specific force in world (up = +Z)
cy, sy = math.cos(yaw), math.sin(yaw)
cp, sp = math.cos(pitch), math.sin(pitch)
cr, sr = math.cos(roll), math.sin(roll)
R = np.array([
[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
[sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
[-sp, cp * sr, cp * cr],
])
return R.T @ sf
# ---------------------------------------------------------------------------
# Ground-truth trajectories
# ---------------------------------------------------------------------------