Function navtk::inertial::calc_force_ned(const Matrix3&, double, const Vector3&, const Vector3&)

Function Documentation

Vector3 navtk::inertial::calc_force_ned(const Matrix3 &C_s_to_n, double dt, const Vector3 &dth, const Vector3 &dv)

Converts a delta velocity measurement in the inertial sensor frame into a specific force measurement in the NED frame.

Applies the correction for a rotating frame using delta rotation measurements:

\( f^n_{corr, k + 1} = C_b^n \left( \delta v_{k + 1} + \frac{1}{2}\delta\theta_{k + 1}\times \delta v_{k + 1} + \frac{1}{2}\int^{t_{k + 1}}_{t_k} \delta\theta \times \ f^b - \omega^b \times \delta v \,\mathrm{d}t \right)\)

Which is eq 10.64 (11.64 in 2nd ed) of Titterton and Weston, with adjusted notation. The last term in the parentheses is a ‘high update rate’ term to correct for sculling motion, which is 0 when \( f^b \) and \( \omega^b \) are constant over the interval. We make the assumption that this is true for each measurement, and therefore this term is not implemented.

Obtaining the above equation from the ‘parent’ equation

\( f^n_{corr, k + 1} = C_b^n \left( \int^{t_{k + 1}}_{t_k}\! f^b \,\mathrm{d}t + \int^{t_{k + 1}}_{t_k}\!\delta\theta\times f^b \,\mathrm{d}t\right)\)

has raised questions, so that bit is included here for posterity. Following Savage (Strapdown Analytics, 7.2.2.2-6 through 7.2.2.2.-22), first solve this derivative and re-arrange:

\( \frac{\delta}{dt}(\delta\theta \times \delta v) = \omega \times \delta v + \delta\theta \times f \)

\( \delta\theta \times f = \frac{\delta}{dt}(\delta\theta \times\delta v) - \omega \times \delta v \)

Also,

\( \delta\theta \times f = \frac{1}{2}\delta\theta \times f + \frac{1}{2} \delta\theta \times f \)

Then substitute the first into the second and get

\( \delta\theta \times f = \frac{1}{2}\delta\theta \times f + \frac{1}{2}( \frac{\delta}{dt}(\delta\theta \times\delta v) - \omega \times f )\)

which can then be substituted into the integral in the ‘parent’ equation

\( \int^{t_{k + 1}}_{t_k}\!\delta\theta \times f \,\mathrm{d}t = \frac{1}{2}\int^{t_{k + 1}}_{t_k}\!\delta\theta \times f\,\mathrm{d}t + \frac{1}{2}\int^{t_{k + 1}}_{t_k}\!( \frac{\delta}{dt}(\delta\theta \times\delta v) - \omega \times f)\,\mathrm{d}t\)

and simplified to

\( \int^{t_{k + 1}}_{t_k}\!\delta\theta \times f \,\mathrm{d}t = \frac{1}{2}\delta\theta_{k + 1} \times\delta v_{k + 1} + \frac{1}{2}\int^{t_{k + 1}}_{t_k}\!\delta\theta \times f - \omega \times f\,\mathrm{d}t\)

Parameters

  • C_s_to_n – DCM that would rotate dv from the inertial sensor frame to the NED frame.

  • dt – Delta time over which dv is valid (s).

  • dth – Vector3 of delta rotation values over the dt period (rad), in the inertial sensor frame.

  • dv – Vector3 of delta velocity values over the dt period (m/s), in the inertial sensor frame.

Returns

Vector3 of specific force measurements in the navigation frame (m/s^2).