Stateless mathematical utilities shared across the AHRS filters.
All inputs and outputs are in SI units (rad/s for angular velocity,
m/s² for linear acceleration, radians for angles). Unit conversion is
the responsibility of the sensor driver populating the inbound
BB.Message.Sensor.Imu payload.
Ported from gworkman/ahrs'
Ahrs.Math.
Summary
Functions
Calculates roll and pitch in radians from a 3-axis acceleration vector. Yaw is unobservable from acceleration alone.
Converts Euler angles in radians to a quaternion (Z-Y-X Tait-Bryan).
Standard gravity in m/s².
Returns true if the magnitude of an accel vector in m/s² is within
threshold of standard gravity, where threshold is a fractional
value (e.g. 0.1 means ±10% around 1 g).
Calculates q̇, the time-derivative of a quaternion under angular
velocity (gx, gy, gz) in rad/s. Returns a {ẇ, ẋ, ẏ, ż} 4-tuple.
Normalises an angle to the range (-π, π].
Converts a quaternion to Euler angles using the Z-Y-X Tait-Bryan
convention. Returns {roll, pitch, yaw} in radians, unless :degrees
is passed.
Rotates a 3D {x, y, z} vector by a quaternion. Equivalent to
q * v * conjugate(q).
Functions
Calculates roll and pitch in radians from a 3-axis acceleration vector. Yaw is unobservable from acceleration alone.
Returns {roll, pitch}. The accel components are in any unit; only
their relative magnitudes matter.
@spec euler_to_quaternion(roll :: float(), pitch :: float(), yaw :: float()) :: BB.Math.Quaternion.t()
Converts Euler angles in radians to a quaternion (Z-Y-X Tait-Bryan).
@spec gravity() :: float()
Standard gravity in m/s².
Returns true if the magnitude of an accel vector in m/s² is within
threshold of standard gravity, where threshold is a fractional
value (e.g. 0.1 means ±10% around 1 g).
Used by Madgwick and Mahony to reject readings dominated by linear acceleration rather than gravity.
@spec gyro_derivative(BB.Math.Quaternion.t(), float(), float(), float()) :: {float(), float(), float(), float()}
Calculates q̇, the time-derivative of a quaternion under angular
velocity (gx, gy, gz) in rad/s. Returns a {ẇ, ẋ, ẏ, ż} 4-tuple.
A derivative is not itself a rotation, so it is returned as a bare tuple
rather than a BB.Math.Quaternion - that type is always a unit quaternion and
would normalise the magnitude away. Callers scale it by dt, add it to the
current orientation, and normalise the result.
Normalises an angle to the range (-π, π].
@spec quaternion_to_euler( BB.Math.Quaternion.t(), keyword() ) :: {roll :: float(), pitch :: float(), yaw :: float()}
Converts a quaternion to Euler angles using the Z-Y-X Tait-Bryan
convention. Returns {roll, pitch, yaw} in radians, unless :degrees
is passed.
@spec rotate_vector({float(), float(), float()}, BB.Math.Quaternion.t()) :: {float(), float(), float()}
Rotates a 3D {x, y, z} vector by a quaternion. Equivalent to
q * v * conjugate(q).