Radii of Curvature, Position and Velocity Estimators

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

Classes: RadiiOfCurvatureEstimator, NEDPositionEstimator, NEDVelocityEstimator.

The WGS84 ellipsoid and curvilinear position

Position on the Earth’s surface (or above/below it) can be described either as a Cartesian ECEF vector or, more usefully for navigation output, as curvilinear position: geodetic latitude , longitude , and height above the WGS84 reference ellipsoid.

Geodetic height and latitude of a body above the ellipsoid

RadiiOfCurvatureEstimator

Latitude and longitude do not change at a uniform linear rate per meter travelled, because the WGS84 ellipsoid is not a sphere: it is flattened at the poles. The meridian radius of curvature, , governs north-south motion; the transverse radius of curvature, , governs east-west motion. Both are smallest at the equator and largest at the poles:


where is the WGS84 equatorial radius and is the WGS84 eccentricity. See [book-groves], §2.3.2.

double latitude = Math.toRadians(41.3851);

RadiiOfCurvature radii = RadiiOfCurvatureEstimator.estimateRadiiOfCurvatureAndReturnNew(latitude);

double meridianRadius = radii.getRn();   // R_N(L), meters
double transverseRadius = radii.getRe(); // R_E(L), meters
Geocentric vs. geodetic latitude on the WGS84 ellipsoid

Both NEDPositionEstimator and NEDVelocityEstimator use these radii to convert between the NED velocity and the rate of change of curvilinear position:



NEDPositionEstimator

Integrates curvilinear position forward over one time step from the old and new NED velocity, using the trapezoidal (average of old/new rate) form of the classic 4th-order Runge-Kutta integration scheme described in Groves §5.4.1 for the general state equation :

Height, then latitude, then longitude must be updated in that order, since each subsequent equation depends on the previous result (the radii of curvature are re-evaluated at the newly updated latitude).

double timeInterval = 1.0; // seconds
double oldLatitude = Math.toRadians(41.3851);
double oldLongitude = Math.toRadians(2.1734);
double oldHeight = 0.0;
double oldVn = 1.0, oldVe = 0.0, oldVd = 0.0; // previous NED velocity (m/s)
double vn = 1.0, ve = 0.0, vd = 0.0;          // current NED velocity (m/s)

NEDPosition newPosition = NEDPositionEstimator.estimatePositionAndReturnNew(
        timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd, vn, ve, vd);

double newLatitude = newPosition.getLatitude();
double newHeight = newPosition.getHeight();

NEDVelocityEstimator

The inverse operation: given curvilinear position at two epochs, estimate the average NED velocity between them by finite-differencing latitude/longitude/height and then inverting the same trapezoidal relation used above (solving for the "new" velocity component given the "old" one and the average rate):

This is mainly useful for turning a sequence of positions (e.g., from a reference trajectory or a GNSS log) into the velocity time series needed as input to kinematics estimators or a navigator initialization step.

double timeInterval = 1.0; // seconds
double oldLatitude = Math.toRadians(41.3851);
double oldLongitude = Math.toRadians(2.1734);
double oldHeight = 0.0;
double oldVn = 1.0, oldVe = 0.0, oldVd = 0.0; // previous NED velocity (m/s)

// current position, e.g. the next GNSS fix in a log
double latitude = Math.toRadians(41.38511);
double longitude = oldLongitude;
double height = 0.0;

NEDVelocity velocity = NEDVelocityEstimator.estimateVelocityAndReturnNew(
        timeInterval, oldLatitude, oldLongitude, oldHeight, oldVn, oldVe, oldVd,
        latitude, longitude, height);

double vn = velocity.getVn();

Where to go next