Integrating 3D controls to propagate desired state

Suppose you have a sensor that integrates body rates and velocities and reports these measurements over arbitrary intervals. And let’s further suppose that it’s reasonable to assume that those body rates and velocities are constant during each interval. How should we propagate the position and attitude of this sensor?

First, it’s fair to ask what real-life situations this could reflect. Constant body rates would apply to inertial motion with rotation about stable principal axes and could be integrated by a gyro. But constant body velocities? In two dimensions this assumption could apply to a vehicle driven by wheels or treads moving at a constant speed, and twice-integrated accelerator data would produce the desired signal, but in most situations it probably makes more sense to assume constant inertial velocity or perhaps constant body-frame acceleration. Thus applicability seems questionable from a navigation point of view, but what about (pilot) control? A 6-DOF hand controller or SpaceMouse provides exactly this kind of signal.

As an example, consider piloting a vehicle (or, more mundane, a camera view in a 3D application) and commanding a left translation simultaneously with a clockwise rotation. You would expect the resulting motion to circle a fixed point at some distance in front of you. This will be the test of our propagation scheme.

One approach would simply be to divide the measurements by the duration of the interval (recovering the rates and velocities), then numerically integrate them to advance the state. But this integration can be done analytically, allowing the propagation to be performed in a single operation (with no need to choose between integrators, step sizes, etc.). Integrating the rotation is straightforward: if $R_{i}$ represents the initial attitude (world-to-body-frame quaternion, for example) and $Δ θ$ are the integrated body rates, then we simply rotate $R_{i}$ by $Δ θ$ . Expressed as quaternions,

R_{i + 1} = R_{i} \cdot (cos (θ / 2) + sin (θ / 2) \hat{Δ θ})

(here, $θ = ‖ Δ θ ‖$ and $\hat{Δ θ} = Δ θ / θ$ ).

For translation (by a body-integrated amount $Δ x$ ), I don’t know of an elegant way to derive the result, but brute force gives us

x_{i+1}=x_i + R_i^{-1}\cdot\left( \frac{\sin(\theta)}{\theta}\mathbf{\Delta x} + \left( 1 - \frac{\sin(\theta)}{\theta} \right) \left( \hat{\mathbf{\Delta \theta}}\cdot\mathbf{\Delta x} \right) \hat{\mathbf{\Delta \theta}} + \frac{1 - \cos(\theta)}{\theta} \hat{\mathbf{\Delta \theta}} \times \mathbf{\Delta x} \right)

(here, $R_{i}^{- 1}$ is treated as a rotation operator, like a DCM, so this should not be interpreted as multiplying a quaternion by a promoted vector).

For practical implementation, there are a few numerical edge cases that need to be addressed. Additionally, since this will be called for every measurement (125 Hz for a USB SpaceMouse, for example), we should take advantage of some related performance optimizations as well:

Special-case the situation in which $θ$ is 0 (avoid numerical division by zero and optimize for the purely translational case)
Compute $sin (θ)$ and $1 - cos (θ)$ in terms of $sin (θ / 2)$ and $cos (θ / 2)$ via double-angle identities (avoid unnecessary trig calls and poor precision of $1 - cos (θ)$ for small $θ$ )
Compute $1 - \frac{sin (θ)}{θ}$ via Taylor series for small $θ$ (to again avoid loss of precision from naïve subtraction)

One could consider optimizing for the purely rotational case as well. Also, be aware of any normalization requirements when using quaternion libraries (my personal view is that quaternions should be left unnormalized, with division by the norm taking place in any rotation operations, but that’s a subject for another article). A production-ready version of this procedure might thus look like:

/**
 * Compute `1 - sin(x)/x`, maintaining precision even when `x` is small.
 * Computation is "assisted" by taking `sin(x)` as an argument; this avoids
 * recomputation if `sin(x)` is used elsewhere by the caller (in case common
 * subexpression elimination can't see into this function's implementation).
 */
float om_sinc_assist(float const x, float const sin_x) {
  float const x2 = x*x;
  if (x2 > 0.7857809f) {
    // `x` is large enough that we don't lose significant precision in subtraction
    return 1.0f - sin_x / x;
  } else {
    // Sum Taylor series, stopping when next term won't affect result
    float ans = (x2 > 0.15294082f) ? (x2*2.5052108e-8f) : 0.0f;
    if (x2 > 0.15294082f) {
      ans = x2*(ans - 2.7557319e-6f);
    }
    if (x2 > 0.007074649f) {
      ans = x2*(ans + 0.00019841270f);
    }
    if (x2 > 1.2043186e-6f) {
      ans = x2*(ans - 0.0083333333f);
    }
    return x2*(ans + 0.16666667f);
  }
}

/**
 * Propagate a position and attitude given integrated linear and angular
 * velocities.
 */
std::pair<Eigen::Vector3f, Eigen::Quaternionf>
propagate(Eigen::Vector3f const & x, Eigen::Quaternionf const & r,
    Eigen::Vector3f const & dx, Eigen::Vector3f const & dtheta) {
  auto const theta = dtheta.stableNorm();

  if (theta > 0.0f) {
    // Don't use `auto` to store the results of Eigen expressions -- they return temporaries
    Eigen::Vector3f const dtheta_hat = dtheta/theta;
    auto const dr = Eigen::Quaternionf(Eigen::AngleAxisf(theta, dtheta_hat));

    if (!dx.isZero()) {
      // OPT: These should be eligible for common subexpression elimination with the AngleAxis
      //   constructor above.
      auto const cos_theta_2 = dr.w();  // std::cos(0.5f*theta);
      auto const sin_theta_2 = std::sin(0.5f*theta);

      auto const sin_theta = 2.0f*cos_theta_2*sin_theta_2;
      auto const om_sinc_theta = om_sinc_assist(theta, sin_theta);
      auto const om_cos_theta = 2.0f*sin_theta_2*sin_theta_2;
      Eigen::Vector3f const dx_body0 = (sin_theta/theta)*dx +
          om_sinc_theta*dtheta_hat.dot(dx)*dtheta_hat +
          (om_cos_theta/theta)*dtheta_hat.cross(dx);
      return std::make_pair(x + r._transformVector(dx_body0), r*dr);
    } else {
      // Pure rotation
      return std::make_pair(x, r*dr);
    }
  } else {
    // Pure translation
    return std::make_pair(x + r._transformVector(dx), r);
  }
}

This propagation can be applied for each new measurement and is independent of the durations of the intervals they are made over. Compared to an inertial navigation system, it takes no account of sensor noise and provides a low order of accuracy with respect to measurement interval. But for tracking a desired state based on input from a 6-DOF controller, especially if updates are irregular, it’s probably the ideal solution.