Use positional data to improve commutator precision

[aacuevas]

Adding an optional way to use not only rotational information but also posicional data could improve the precision of the algorithm in corner cases.

The issue

Algorithm explanation

The current algorithm, introduced in #14 does the following:

1. Extracts the current rotation from the Quaternion input $\Omega_n$
2. Calculates the delta rotation $\Delta\Omega = \Omega_n - \Omega_{n-1}$ (in quaternion form $Q_{\Delta\Omega} = Q_{\Omega_n}\times Q_{\Omega_{n-1}}^*$ )
3. Uses $\Omega_{n-1}$ to get a normalized vector $\vec{V_L}$ that represents the local "up" of the headstage in global coordinates. i.e.: the vector that represents the direction the tether is going out of the headstage
4. Gets a static $V_G$ which is a normalized vector that represents the direction the tether enters the commutator
5. Gets the projections of $\Delta\Omega$ over $\vec{V_L}$ and $\vec{V_G}$, getting the local and global twist $T_L$ and $T_G$
6. Gets the angle $\Theta$ between $\vec{V_L}$ and $\vec{V_G}$
7. Returns a twist $\tau = \frac{T_L + T_G}{1+cos(\Theta)}$

Mathematical explanation

The algorithm assumes that $\Delta\Omega$ is comprised of:
a) an orbit around the commutator axis $\vec{V_G}$: $\omega_G$
b) a twist of the cable over the headstage axis $\vec{V_L}$: $\omega_L$
projected

and also that the total twist is a combination of both $\tau = \omega_L + \omega_G$

Since we are dealing with small incremental rotations and a quaternion is defined as $q_{(\vec{u},\alpha)} = (cos(\alpha/2), sin(\alpha/2)\vec{u})$
we can assume that for a small incremental rotation $\Delta\Omega \rightarrow 0, cos(\Delta\Omega/2) \approx 1,sin(\Delta\Omega/2) \approx \Delta\Omega/2 \longrightarrow q_{(\vec{u},\Delta\Omega)} \approx (1,\frac{\Delta\Omega}{2}\vec{u})$
We are defining the projections as $P_{(q,\vec{V})} = atan2((\vec{q.v}\cdot\vec{V}),q.w)$ which, for a small incremental rotation $\Delta\Omega \rightarrow 0$ can be approximated as $P_{(q_{(\vec{u},\Delta\Omega)})} \approx atan2(\frac{\Delta\Omega}{2}(\vec{u}\cdot\vec{v}),1) \approx \vec{u}\cdot\vec{v}$

We can then assume that the rotation increment is caused by a lineal combination of both of its elements
$\Delta\Omega = \omega_L \cdot \vec{V_L} + \omega_G \cdot \vec{V_G}$
The projections can be approximated as:
$T_L = \Delta\Omega \cdot V_L = (\omega_L \cdot \vec{V_L} + \omega_G \cdot \vec{V_G}) \cdot \vec{V_L}) = \omega_L + k \cdot \omega_G$
$T_G = \Delta\Omega \cdot V_G = (\omega_L \cdot \vec{V_L} + \omega_G \cdot \vec{V_G}) \cdot \vec{V_G}) = \omega_G + k \cdot \omega_L$
where $k = \vec{V_L}\cdot\vec{V_G}$ which, for unit vectors $k = cos(\Theta)$ where $\Theta$ is the angle between the headstage local vector and the commutator vector
and, again, the total twist
$\tau = \omega_L + \omega_G$

Solving this equation system gives us $\tau = \frac{T_L + T_G}{1+k} = \frac{T_L + T_G}{1+cos(\Theta)}$
(note that the actual solution is $\tau = \frac{(T_L + T_G)(1-k)}{(1+k)(1-k)}$. We'll see the effects of the indetermination in the next section)

The issue

In truth, this system is an approximation from incomplete information. Pure rotational information lacks information about the physical reality of the headstage-tether-commutator system.
One clear effect of this can be seen on the indeterminations on the general solution seen above:
$\tau = \frac{(T_L + T_G)(1-k)}{(1+k)(1-k)}$

Let's split this in two cases:

1. k = 1: Vectors are aligned

In this case, it is impossible to tell apart when a rotation $\Delta\Omega$ from the IMU represents an orbit or a twist. Fortunately, in this case the rotations are additive and have the same effect: A counter-clockwise twist in the headstage will induce a counter-clockwise rotation in the commutator and a counter-clockwise rotation orbit will also cause a counter-clockwise rotation. So, the effect is additive
In fact
$\lim_{k\to 1}\frac{(T_L + T_G)(1-k)}{(1+k)(1-k)} = \frac{T_L + T_G}{2}$
In this case, since both projections will be equal $T_L = T_G$ the equation will trim the duplicate effects. For a given $\Delta\Omega$ the commutator needs to turn in a clear direction, by half of the compound measured projections.
This case is mostly safe and will follow the animal movements.

2. k = -1: Vectors are aligned but opposite

In this case, rotations are opposed. Likewise, the algorithm has no way of knowing if a given rotation is produced by a twist of an orbit. However, in this case, the sign of the twist is inverted: A counter-clockwise rotation on the headstage will produce a clockwise rotation on the commutator while a counter-clockwise orbit will still produce a counter-clockwise rotation. In this case, for a given $\Delta\Omega$, the algorithm has no means to determine which direction should the commutator turn.

Mathematically, there is a non-convergent pole.

For this reason, the current algorithm has condition of the form

if (cos_angle > FallbackThreshold)
    twist = (localTwist + globalTwist) / (1.0 + cos_angle);
else
    twist = FallbackRotation == FallbackRotationModes.Global ? globalTwist : localTwist;

So when approaching a threshold, the user can select which rotations is most likely to be the cause of the movement.

However, this threshold can not be too close to -1. Since the equation approaches an indetermination, for small denominators, small noise variations can be greatly amplified, resulting in instability.

Refining twist with positional data

With positional data, it would not be necessary to estimate the orbit motion from the IMU quaternion.

It would be possible to just calculate the orbit angle by knowing the position of the headstage in relationship to the commutator. In this scenario, it would be possible to extract two twist components:

• The global orbit $\omega_G$ would be extracted from positional data
• The local twist $\omega_L$ would be extraced by the current projection of the IMU rotation over the headstage local axis

The impossible rotation

There is a case that is extremely challenging, if not outright impossible, to solve. This is if the animal where to coil the tether along an axis orthogonal or near-orthogonal to the commutator.

With IMU data only, there is no way if the coiling is done, for example, in the +X or -X direction. A same winding sense (CW or CCW) would result in opposite twists, depending on the direction.

With positional data, it could be possible to solve this, but would be challenging. Very precise movement information would be required.

But, physically, it would probably be an impossible situation. In such a situation, there would be actual windings over the animal or knots in the cable, which are situations which a simple rotating commutator can not solve. So care is still required in the tether physical attachment so these kind of twists are not possible.