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 (see the Class Reference table at the end of this page for Javadoc and source links).

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)

Class Reference

Javadoc and source links for the classes used in the examples above:

Class Javadoc Source

LevelingEstimator

Javadoc

Source

LevelingEstimator2

Javadoc

Source

AttitudeEstimator

Javadoc

Source

BodyMagneticFluxDensityEstimator

Javadoc

Source

BodyMagneticFluxDensity

Javadoc

Source

Where to go next