# Integrating 3D controls to propagate desired state

Not-quite-inertial navigation

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 $\Delta \theta $ are the integrated body rates, then we simply rotate ${R}_{i}$ by $\Delta \theta $. Expressed as quaternions,

$${R}_{i+1}={R}_{i}\cdot \left(cos(\theta /2)+sin(\theta /2)\hat{\Delta \theta}\right)$$(here, $\theta =\Vert \Delta \theta \Vert $ and $\hat{\Delta \theta}=\Delta \theta /\theta $).

For translation (by a body-integrated amount $\Delta 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(\left(\hat{\Delta \theta}\cdot \Delta x\right)\hat{\Delta \theta}+\frac{sin(\theta )}{\theta}\left(\Delta x-\left(\hat{\Delta \theta}\cdot \Delta x\right)\hat{\Delta \theta}\right)+\frac{1-cos(\theta )}{\theta}\left(\hat{\Delta \theta}\times \Delta x\right)\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 $\theta $ is 0 (avoid numerical division by zero and optimize for the purely translational case)
- Compute $sin(\theta )$ and $1-cos(\theta )$ in terms of $sin(\theta /2)$ and $cos(\theta /2)$ via double-angle identities (avoid unnecessary trig calls and poor precision of $1-cos(\theta )$ for small $\theta $)

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:

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) { auto const dtheta_hat = dtheta/theta; auto const dr = Eigen::Quaternionf(Eigen::AngleAxisf(theta, dtheta_hat)); // OPT: These should be eligible for common subexpression elimination with the AngleAxis // constructor above. auto const cos_theta_2 = 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_cos_theta = 2.0f*sin_theta_2*sin_theta_2; auto const dot_term = dtheta_hat.dot(dx)*dtheta_hat; auto const dx_body0 = dot_term + (sin_theta*(dx - dot_term) + om_cos_theta*dtheta_hat.cross(dx))/theta; return std::make_pair(x + r._transformVector(dx_body0), 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.