[Discussion] Is there a way to accept pre-integrated gyroscope quaternions without interfering with features of VQF?

[kitlith]

(I'd make this a discussion instead of an issue, but discussions don't appear to be enabled on this repo.)

For context: Joycons have a mode that sends pre-integrated quaternions instead of raw gyro samples for every timestamp (https://github.com/kitlith/joycon-quat). This feels useful to use in situations where you're missing packets because the environment isn't great, and would cause missed packets to not cause instant drift.

Simply setting state.gyrQuat seems like it'd be a simple way to import these pre-integrated quaternion into VQF, but it looks like I'd be interfering with... basically all of the nice features that aren't in BasicVQF.

Taking consecutive quaternions, turning them back into "gyroscope measurements", then running them through VQF seems possible, but I don't know how to do that correctly, if it's possible. Feels like a brittle solution.

Do you know of anything nifty that would help with this (rather weird) usecase? I'd be interested in throwing it into carl-anders/slimevr-wrangler#40, though it's rather low on my priority list. If nothing else, maybe it's an interesting question?

[dlaidig]

I've enabled discussions and converted this issue to a discussion. Thanks for the suggestion. :)

You are right that just overwriting state.gyrQuat would break gyroscope bias estimation.

My first thought would also be to just convert the difference between two pre-integrated quaternions to a gyroscope measurement and then feed this virtual gyroscope measurement to VQF. In fact, I've already proposed a similar approach in a different paper (see Appendix C.1. in https://www.mdpi.com/1424-8220/22/24/9850 -- but note that there the multiplication order is swapped to also transform the coordinate system in the same step).

Here is some untested pseudocode for how to derive the gyroscope measurement from two quaternions (see qmt.quatToGyrStrapdown for a working and tested Python implementation):

q_diff = VQF.quatMultiply(VQF.quatConj(previousGyrQuat), currentGyrQuat)
if q_diff[0] < 0:
    q_diff = -q_diff
axis = [q_diff[1], q_diff[2], q_diff[3]]
axis_norm = VQF.norm(axis)
angle = wrapToPi(2 * atan2(axis_norm, q_diff[0]))
if axis_norm < 1e-10: # or some other low threshold like std::numeric_limits<T>::epsilon
    return [0 0 0]
else:
    axis = axis / axis_norm
return angle * axis / gyrTs

I don't think that this approach is brittle. Personally, I think it's good to add this as a separate encapsulated preprocessing step. This also makes it easier to add further optimizations and deal with edge cases. (For example, if 2 samples are missing, it might be a good idea to do the calculation above with 3*gyrTs and then call updateGyr 3x with the same virtual gyroscope measurement.)

The alternative would be to further split up the internal state representation in order to separate gyroscope strapdown integration and gyroscope bias correction. Right now, the orientation state representation is quat9D = magQuat ⊗ accQuat ⊗ gyrQuat (although magQuat is technically stored as a scalar delta). This could be extended to quat9D = magQuat ⊗ accQuat ⊗ gyrBiasCorrectionQuat ⊗ pureGyrQuat. Then, pureGyrQuat could just be overwritten with the orientation received from the sensor. However, this would require careful adjustments in many parts of the VQF code.

[kitlith]

My main concern was that there would be multiple ways for gyroscope readings to produce the same quaternion such that you would be able to compute a valid set of gyroscope readings, but not necessarily the valid set of gyroscope readings, if that makes any sense. Or if it would align properly with the accelerometer axis, which we're still receiving the raw data for. Because I don't know such things, it felt brittle.

If that is not the case (or is sufficiently unlikely, because it would imply multiple rotations along an axis), then this does seem like a sound approach. I do not have the grounding to determine if it's sound or not, so I'll take your word on it!

Then, pureGyrQuat could just be overwritten with the orientation received from the sensor. However, this would require careful adjustments in many parts of the VQF code.

to be clear, the goal of an alternate approach here would not necessarily be to just be able to set the quaternion, but instead to feed the quaternions through an "alternate updateGyr" that would compute the same types of things as usual, but using quaternion math directly, and gyrQuat would still be used exactly the same way in the rest of VQF. I just used "setting it directly" as a (bad) example of an alternate frontend that might work for BasicVQF. (in reality it'd probably still be taking the difference between consecutive timesteps as input)

[dlaidig]

My main concern was that there would be multiple ways for gyroscope readings to produce the same quaternion such that you would be able to compute a valid set of gyroscope readings, but not necessarily the valid set of gyroscope readings, if that makes any sense.

This should not happen as long as the rotation between sampling times is below 180° because the algorithm I suggested will always pick the shortest rotation between the two orientations. But if that is not the case, you probably have other issues to worry about... ;)

(And even then, the roundtrip from quaternion to gyroscope measurement to quaternion should still work fine.)

Or if it would align properly with the accelerometer axis, which we're still receiving the raw data for.

The virtual gyroscope measurement should be in the local sensor coordinate system, just like the real gyroscope and accelerometer measurements.

There are a few things that could cause issues. For example, the sensor might give you the inverse quaternion or follow some other convention. But all of that should be quite easy to detect with some basic tests because if something is wrong, the data should look totally different. (One simple test would be to have the device flat on the table and then rotate 360° around the vertical axis. This movement should be repeatable enough so that you can just compare the printed gyroscope values from two different trials -- once with the real measurement and once via the pre-integrated quaternions. If you can transfer both data at the same time, it's a lot easier of course. Just print the values side-by-side and see if they look quite close.)

to be clear, the goal of an alternate approach here would not necessarily be to just be able to set the quaternion, but instead to feed the quaternions through an "alternate updateGyr" that would compute the same types of things as usual, but using quaternion math directly, and gyrQuat would still be used exactly the same way in the rest of VQF

This is also what I had in mind. You would still need to store a gyrBiasCorrectionQuat state and update this quaternion in every call of the alternate updateGyr call. (Basically, the idea would be to turn the current gyroscope bias estimate into a quaternion, transform it to the global frame, and then multiply it to the bias correction quaternion. It's not magic but requires some careful thinking in order to get the signs and multiplication orders correct. And if the implementation is wrong, the effects are way more subtle than with the first approach.)

But I think I was too pessimistic in one aspect yesterday: At the end of the alternate updateGyr call, you can just set gyrQuat as gyrBiasCorrectionQuat ⊗ pureGyrQuat and the rest of the VQF code will probably work fine without any adjustments.

[kitlith]

Getting around to actually working on this: If I'm understanding the qmt source code correctly, I think it's equivalent (aside from working with entire sequences at once) to the following rust code using nalgebra?

use nalgebra::{UnitQuaternion, Vector3};

fn compute_gyro_strapdown(last: UnitQuaternion<f64>, current: UnitQuaternion<f64>, gyrTs: f64) -> Vector3<f64> {
    last.rotation_to(&current).scaled_axis() / gyrTs
}

where rotation_to computes the quaternion x such that x * last == current, and scaled_axis computes the axis angle representation, then multiplies the axis by the angle.