Attitude Estimators: Leveling, Gyrocompassing, and Magnetic Heading

This documentation was generated with the assistance of AI. Please report any inaccuracies.

Classes: LevelingEstimator, LevelingEstimator2, AttitudeEstimator, BodyMagneticFluxDensityEstimator.

The alignment problem

Before an INS can start integrating IMU measurements, it needs an initial attitude. Self-alignment (Groves §5.5.2, see [book-groves]) splits this into two steps: leveling (roll and pitch, from the sensed reaction to gravity) and gyrocompassing/magnetic heading (yaw, from either the Earth’s rotation sensed by high-grade gyros, or a magnetometer).

Leveling principle: sensed specific force is the reaction to gravity when stationary

Every formula on this page assumes (and the angular rate) follow Groves' body-frame convention: = forward, = right, = down (FRD — see the estimators overview for the full explanation and the conversion to apply if your IMU instead reports an ENU-aligned body convention). Getting this wrong will not throw an exception — it will silently produce a wrong roll/pitch/heading, since e.g. swapping / or flipping the sign of changes which formula below computes which angle.

LevelingEstimator

The simplest form. When the body is stationary, the only specific force sensed is the reaction to gravity, ; roll and pitch follow directly (equation 5.88/5.89 — this implementation neglects Earth rotation and the small north component of gravity, so it is accurate to about rad / 0.05°):


If sufficiently accurate (aviation-grade) gyroscope measurements are also available, LevelingEstimator can additionally derive yaw through direct gyrocompassing, sensing the Earth-rotation vector itself (equation 5.90/5.91):


Because rad/s is tiny, this only works with aviation- or marine-grade gyros (drift well under about 0.01°/hr); consumer-grade MEMS gyros cannot gyrocompass at all, which is why AttitudeEstimator exists as a magnetometer-based alternative.

// specific force sensed while stationary (m/s^2)
double fx = 0.1, fy = -0.2, fz = -9.81;

// roll/pitch only
CoordinateTransformation attitude = LevelingEstimator.getAttitude(fx, fy, fz, 0.0, 0.0, 0.0);

// roll/pitch + gyrocompassed yaw (requires aviation-grade angular rate, rad/s)
double angularRateX = 1e-7, angularRateY = -3e-8, angularRateZ = 5e-8;
CoordinateTransformation attitudeWithYaw = LevelingEstimator.getAttitude(
        fx, fy, fz, angularRateX, angularRateY, angularRateZ);

double roll = attitudeWithYaw.getRollEulerAngle();
double yaw = attitudeWithYaw.getYawEulerAngle();

LevelingEstimator2

A refinement of LevelingEstimator that no longer assumes the Earth is spherical: it uses NEDGravityEstimator to obtain the true NED gravity vector at the device’s actual latitude and height (which has a small north component because the Earth is flattened), then computes the rotation that aligns the normalized measured specific force with the normalized true gravity direction, via the axis-angle (Rodrigues) form:



The resulting quaternion only fixes roll and pitch — yaw is left arbitrary and must be resolved separately (e.g. by AttitudeEstimator or gyrocompassing), exactly as with LevelingEstimator.

double latitude = Math.toRadians(41.3851);
double height = 0.0;
double fx = 0.1, fy = -0.2, fz = -9.81;

// yaw in the result is arbitrary; only roll/pitch are meaningful here
CoordinateTransformation attitude = LevelingEstimator2.getAttitude(
        latitude, height, fx, fy, fz, 0.0, 0.0, 0.0);

AttitudeEstimator

Combines a leveling solution (roll/pitch, via LevelingEstimator2) with a magnetometer measurement to resolve yaw as well — the practical option for consumer-grade IMUs where the gyroscopes are not accurate enough for gyrocompassing:

flowchart TD A["Accelerometer specific force f_ib^b"] --> B["LevelingEstimator2:\nroll, pitch (yaw arbitrary)"] C["Magnetometer measurement b^b\n+ declination angle"] --> D["Magnetic heading"] B --> D D --> E["Full attitude:\nroll, pitch, yaw"]

Magnetic heading and yaw (true heading = magnetic heading + declination):

Declination and dip can come from the WMM package (WMMEarthMagneticFluxDensityEstimator) or from a directly measured NEDMagneticFluxDensity (EarthMagneticFluxDensityEstimator).

double latitude = Math.toRadians(41.3851);
double height = 0.0;
double fx = 0.1, fy = -0.2, fz = -9.81;   // measured specific force (m/s^2)
double bx = 2.3e-5, by = 1.1e-5, bz = 4.2e-5; // measured magnetic flux density (T)

// declination for this position/date, e.g. from WMMEarthMagneticFluxDensityEstimator
double declination = Math.toRadians(1.2);

CoordinateTransformation attitude = AttitudeEstimator.getAttitude(
        latitude, height, fx, fy, fz, bx, by, bz, declination);

double yaw = attitude.getYawEulerAngle(); // true heading (rad)

BodyMagneticFluxDensityEstimator

The inverse of the attitude problem above: given the Earth’s magnetic flux density (magnitude, declination, dip — typically from the WMM package) and a known body attitude, it predicts what a magnetometer would measure in body-frame axes. This is used both to generate synthetic magnetometer test data and, internally, wherever a NED-to-body magnetic-field transform is needed:




where is yaw (true heading), is declination, is the field magnitude, and is the NED-to-body coordinate transformation matrix built from roll/pitch/yaw.

double magnitude = 4.8e-5;             // Earth field magnitude (T)
double declination = Math.toRadians(1.2);
double dip = Math.toRadians(58.0);
double roll = 0.0, pitch = 0.0, yaw = Math.toRadians(30.0); // known body attitude

BodyMagneticFluxDensity b = new BodyMagneticFluxDensity();
BodyMagneticFluxDensityEstimator.estimate(magnitude, declination, dip, roll, pitch, yaw, b);

double bx = b.getBx(); // predicted magnetometer reading, body x-axis (T)

Where to go next