Function navtk::inertial::apply_aiding_alt_accel

Function Documentation

double navtk::inertial::apply_aiding_alt_accel(double r_zero, Vector3 *accel_vector, AidingAltData *aiding_alt_data, double alt0, double dt, const Vector3 &g)

Helps mitigate vertical drift by correcting down acceleration/velocity prior to integration.

The correction is based upon the difference between the inertial altitude and the provided aiding altitude. Note that accel_vector is not returned but is modified in place, and will likely have changed after calling this function.

If using altitude aiding, ensure the INS is not reset, as that is incompatible with the aiding algorithm and will produce invalid results. The aiding algorithm is a third-order control loop, taking into account the difference between the INS altitude and that of an external sensor, and adjusting the acceleration, velocity, and position of the INS altitude solution. Note that the algorithm applies its corrections to accelerations with the effects of gravity already removed. Applying our altitude aiding will require conversion from dv’s and dth’s to accelerations with the corresponding dt.

The altitude aiding algorithm is taken from \( \textit{Applied Mathematics in Integrated Navigation Systems, Third Edition} \), by Robert M. Rogers (p 118 - p 120). Vertical channel dynamics are given by the following equations:

\( \dot{h} = v_z^n - C_1 (h-h_B) \) (eq 5.70)

\( \dot{v}_z^n = f_z + g_z + (\rho+2\Omega)_x v_y^n - (\rho+2\Omega)_y x_y^n - C_2(h-h_B) - C_3 \int (h-h_B)dt \) (eq 5.71)

with the coefficients defined as:

\( C_1 = 3 \lambda \) \( C_2 = 4 \lambda^2 + \frac{2g}{R} \) \( C_3 = 2 \lambda^3 \) (eq 5.83)

When choosing a value for \( \lambda \), it is imperative to ensure it is sufficiently small enough for the dynamics of the tracking loop.

Parameters

  • r_zero – Ellipsoidal Earth radius, meters.

  • accel_vector – 3x1 vector of acceleration in the NED frame, m/s^2.

  • aiding_alt_data – Container holding aiding altitude, error, and method.

  • alt0 – Current inertial altitude, m HAE.

  • dt – Delta time that accel_vector is to be integrated over, sec.

  • g – Gravity estimate, m/s^2.

Returns

A scaling term, C1, to be used in correcting the altitude.