INNER CODE UNIT · Python
_enu_to_latlon
MOLAorg/mola · mola_demos/demos/fake_sensor_publisher.py:95
def _enu_to_latlon(east, north, alt,
lat0=_GT_LAT0_DEG, lon0=_GT_LON0_DEG, alt0=_GT_ALT0_M):
m_per_deg_lon = _M_PER_DEG_LAT * math.cos(math.radians(lat0))
return (lat0 + north / _M_PER_DEG_LAT,
lon0 + east / max(m_per_deg_lon, 1e-9),
alt0 + alt)
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],