imu_orientation_estimator

Estimates the orientation quaternion from a gyro and an accelerometer, optionally with a magnetometer, without GNSS.

Processing

Each gyro sample moves the orientation by the mean of the last two samples, then the latest acceleration corrects roll and pitch.
filter_mode selects the correction: Mahony (complementary filter, also estimates the gyro bias) or Madgwick (gradient descent).
With use_magnetometer the latest field also corrects the yaw, which then does not drift.
The first gyro sample gives the first orientation from the acceleration (and the field); without a magnetometer the yaw starts at 0.
gyro, acc and mag need not have the same timestamps; the output carries the timestamp of gyro.

gyro, acc and mag have 3 elements; other sizes stop the component with an error. Sensors with one output per axis: gather them with the Vectorizer of RTMaps (Sampling mode = Synchronized).
acc is the specific force: +Z when level at rest. acc and mag are used for their direction only, in any unit.
quaternion is [w, x, y, z], body to world. With body axes X forward, Y left, Z up, connect it to imu_to_pose6D with wxyz and ENU.

Without a magnetometer the yaw is relative and drifts. With one, the yaw is referenced to the magnetic north; magnetic_declination_in_deg (east positive) makes it the true north. See the topic Magnetic declination.
Accelerations of the motion are taken as gravity: roll and pitch have an error during strong acceleration.
The magnetometer must be calibrated beforehand; motors, steel and batteries disturb it.
A sample of acc or mag that is not finite, or with a norm below 1e-6, is ignored: the last valid one is used. A gyro sample that is not finite is skipped. A warning is reported once.
With GNSS, use an EKF: this component does not use a position.

[1] R. Mahony, T. Hamel and J.-M. Pflimlin, "Nonlinear Complementary Filters on the Special Orthogonal Group," IEEE Transactions on Automatic Control, vol. 53, no. 5, pp. 1203-1218, 2008. https://doi.org/10.1109/TAC.2008.923738 — Used for: Mahony update.
[2] S. O. H. Madgwick, A. J. L. Harrison and R. Vaidyanathan, "Estimation of IMU and MARG orientation using a gradient descent algorithm," 2011 IEEE International Conference on Rehabilitation Robotics, pp. 1-7, 2011. https://doi.org/10.1109/ICORR.2011.5975346 — Used for: Madgwick update.
[3] T. Foote and M. Purvis, REP 103: Standard Units of Measure and Coordinate Conventions, ROS Enhancement Proposals. https://www.ros.org/reps/rep-0103.html — Used for: axes and units.

Name	Default	Description
`filter_mode`	`Mahony`	Mahony or Madgwick. Options: `Mahony`, `Madgwick`.
`gyro_unit`	`rad_per_s`	Unit of gyro: rad_per_s or deg_per_s. Options: `rad_per_s`, `deg_per_s`.
`use_magnetometer`	`false`	Corrects the yaw with the input mag.
`max_gap_in_ms`	`500`	When the time between two gyro samples is longer than this, or not positive, the orientation is held for that sample.
`mahony_kp`	`1.0`	Strength of the correction (1/s). The time constant is 1/mahony_kp. Must be greater than 0.
`mahony_ki`	`0.01`	Strength of the estimation of the gyro bias (1/s^2). 0 disables it. Must not be negative.
`madgwick_beta`	`0.1`	Speed of the correction (rad/s). Must be greater than 0.
`magnetic_declination_in_deg`	`0.0`	Magnetic declination, east positive, -180 to 180. With 0 the yaw is referenced to the magnetic north.

Name	Description
`quaternion`	Orientation [w, x, y, z], body to world. Type: `Float64`

Name	Description
`gyro`	Angular rate [x, y, z] in gyro_unit. Each sample updates the orientation and gives one output. Type: `Float64`
`acc`	Acceleration [x, y, z], any unit. The latest valid sample is used. Type: `Float64`
`mag`	Magnetic field [x, y, z], any unit, calibrated. Only with use_magnetometer. The latest valid sample is used. Type: `Float64`