Rotation acceleration in AngularCoordinates

[luc]

I am working on CCSDS ADM V2 (not published yet). As part of this work, I discovered that several of our implementations of CCSDS attitude modes were flawed. The new standard explains better what was intended for angular rates (which were really Euler rates in some modes) and for spin/nutation (which I would personally have called spin/precession, but this is another story).
Concerning the nutation mode, I needed to consider the rate of change of the spin axis which in CCSDS is forced to be +Z, i.e. not strictly equal to rotation rate, and which moves both with respect to inertial frame (it describes a cone) and with respect to body frame. This was tricky but fun and not that complicated. You will see that in an upcoming PrecessionFinder class. However, I stumbled upon problems while validating this feature, and am now really puzzled. I think we have one flaw in AngularCoordinates that has been there for at least 9 years.

The problem is the rotation acceleration vector. In AngularCoordinates, we have a Rotation instance for the rotation, a Vector3D instance for the rotation rate and another Vector3D instance for the rotation acceleration. The vectors are expressed in the transformed frame, i.e. in the body frame when the AngularCoordinates represents an attitude. We can build AngularCoordinates either from these components or from a FieldRotation<Derivative> (either DerivativeStructure, UnivariateDerivative1 or UnivariateDerivative2) that holds a quaternion and its time derivatives.

When building from a FieldRotation<Derivative>, we rebuild the rotation rate and rotation acceleration from the quaternion derivatives. The formulas are straigthforward. We start from the well-known relation expressing quaternions rates from rotation rate:

\left\{\begin{aligned} \dot{q}_0 &= \frac{-q_1 \omega_x - q_2 \omega_y - q_3 \omega_z}{2}\\ \dot{q}_1 &= \frac{q_0 \omega_x - q_3 \omega_y + q_2 \omega_z}{2}\\ \dot{q}_2 &= \frac{ q_3 \omega_x + q_0 \omega_y - q_1 \omega_z}{2}\\ \dot{q}_3 &= \frac{-q_2 \omega_x + q_1 \omega_y + q_0 \omega_z}{2} \end{aligned}\right.

Then we invert it:

\left\{\begin{aligned} \omega_x &= 2 [-q_1\dot{q}_0 + q_0\dot{q}_1+q_3\dot{q}_2-q_2\dot{q}_3]\\ \omega_y &= 2 [-q_2\dot{q}_0 - q_3\dot{q}_1+q_0\dot{q}_2+q_1\dot{q}_3]\\ \omega_z &= 2 [-q_3\dot{q}_0 + q_2\dot{q}_1-q_1\dot{q}_2+q_0\dot{q}_3] \\ \end{aligned}\right.

The rotation acceleration is computed by differentiating the inverted relation one more time with respect to time and simplifying (all the terms in the form \dot{q}_i\times\dot{q}_j cancel out, so only terms in the form q_i\times\ddot{q}_j remain):

\left\{\begin{aligned} \dot{\omega}_x &= 2 [-q_1\ddot{q}_0 + q_0\ddot{q}_1+q_3\ddot{q}_2-q_2\ddot{q}_3]\\ \dot{\omega}_y &= 2 [-q_2\ddot{q}_0 - q_3\ddot{q}_1+q_0\ddot{q}_2+q_1\ddot{q}_3]\\ \dot{\omega}_z &= 2 [-q_3\ddot{q}_0 + q_2\ddot{q}_1-q_1\ddot{q}_2+q_0\ddot{q}_3] \\ \end{aligned}\right.

What puzzles me is that in all regular cases (i.e. in all cases except when we explicitly set an arbitrary rotation acceleration or in some interpolation cases), the resulting acceleration is almost 0 (order of magnitude between 1.0e-16 and 1.0e-21).

One explanation I found is that this could be normal as the rotation rate represents the motion of the body frame and is expressed in the body frame, so it is locally fixed. Its first derivative (i.e. the acceleration) should be zero with this interpretation. Its second derivative (i.e. the jerk) would not be zero though, so the vector is not always fixed. I tried to prove this and failed, I am also not really sure about my reasoning here.

Another approach I tested was to differentiate the first expressions before inverting instead of inverting and then differentiating. After eliminating the \dot{q}_i terms I got:

\left\{\begin{aligned} \ddot{q}_0 &= \frac{-q_1 \dot{\omega_x} - q_2 \dot{\omega_y} - q_3 \dot{\omega_z}}{2} -\frac{q_0(\omega_x^2 + \omega_y^2 + \omega_z^2)}{4}\\ \ddot{q}_1 &= \frac{q_0 \dot{\omega_x} - q_3 \dot{\omega_y} + q_2 \dot{\omega_z}}{2} -\frac{q_1 (\omega_x^2 +\omega_y^2 + \omega_z^2)}{4}\\ \ddot{q}_2 &= \frac{ q_3 \dot{\omega_x} + q_0 \dot{\omega_y} - q_1 \dot{\omega_z}}{2} - \frac{q_2 (\omega_x^2+ \omega_y^2+\omega_z^2)}{4}\\ \ddot{q}_3 &= \frac{ -q_2 \dot{\omega_x} + q_1 \dot{\omega_y} + q_0 \dot{\omega_z}}{2} -\frac{q_3 (\omega_x^2+\omega_y^2+ \omega_z^2)}{4} \end{aligned}\right.

In order to isolate the acceleration vector components \dot{\omega}_{x,y,z}, I used this kind of combinations:

\left\{\begin{aligned} -q_1\ddot{q}_0 + q_0\ddot{q}_1 + q_3\ddot{q}_2 - q_2\ddot{q}_3 &= \frac{\dot{\omega_x}}{2}\\ -q_2\ddot{q}_0 -q_3\ddot{q}_1 + q_0\ddot{q}_2 + q_1\ddot{q}_3&= \frac{\dot{\omega_y}}{2}\\\ -q_3\ddot{q}_0 +q_2\ddot{q}_1 - q_1\ddot{q}_2 + q_0\ddot{q}_3 &= \frac{\dot{\omega_z}}{2}\ \end{aligned}\right.

So this leads to similar results as the other way round, but fails to prove this acceleration could be 0.

Anyway, the zero acceleration is troublesome. Either it is correct and this means that in AngularCoordinates we compute a zero vector and lose some information about second derivatives, or it is incorrect and we have to fix it.

I am now considering re-implementing AngularCoordinates using a FieldRotation<UnivariateDerivative2> to preserve derivatives information.

What do you think about this?

[luc]

Note that

(\dot{\omega}_x, \dot{\omega}_y,\dot{\omega}_z) = (0, 0, 0) \Leftrightarrow (\ddot{q}_0, \ddot{q}_1, \ddot{q}_2, \ddot{q}_3) = -\frac{\omega_x^2+\omega_y^2+\omega_z^2}{4} (q_0, q_1, q_2, q_3)

Could someone prove the right hand side is true, so we could deduce the left hand side (i.e. zero acceleration) is also true?

Thinking further, this would also mean the quaternion second derivatives could be deduced from the quaternion value and first derivatives, which seems wrong to me.

So back to square one: why are all the accelerations I see so small?

[luc]

I tried to use FieldRotation<UnivariateDerivative2> as the workhorse for AngularCoordinates.

On the positive side, it was quite easy (one Saturday morning work, without the Field version yet) and the code is really simpler and cleaner. On the negative side, it failed dramatically in the shift/interpolation features, with 321 non-regression tests failing with quite large numerical errors. I tried first using simply the quaternion and derivatives for the shift, and then used modified Rodrigues vector for that. Both failed. I have pushed the branch angular-coordinates-with-univariatederivatives2 if someones wants to look at it, including the failing tests (the continuous integration will certainly complain).

I don’t know what to do next.

[Vincent]

Hi @luc,

I took my monday off so I’ll try to take a look at this. Don’t know if i’ll find something if you didn’t though .

Have a good weekend,
Vincent

[MaximeJ]

Hi @luc, @Vincent,

@luc

What puzzles me is that in all regular cases (i.e. in all cases except when we explicitly set an arbitrary rotation acceleration or in some interpolation cases), the resulting acceleration is almost 0 (order of magnitude between 1.0e-16 and 1.0e-21).

It is indeed weird. What do you mean by “regular cases” ?

One way to test this may be to go the other way around:

• Define a simple rotation acceleration in S/C frame (constant around X)
• Integrate it w.r.t to time to get the rotation rate
• Integrate again and form the quaternion

From there use Orekit formulas to get the rotation acceleration back and check if it corresponds to the input ?

[luc]

Thanks @MaximeJ, I tried something along these lines.

Indeed, it seemed to work. Please take a look at the new testAccelerationModeling tests in {Field}AngularCoordinatesTest. The test is close to an existing one, integrating a simple motion.
With this test, I am able to retrieve significant accelerations from angular coordinates built using quaternions second derivatives.

I don’t know why I get zero accelerations in many of my test cases. I’ll have to check again the SPIN/NUTATION model from CCSDS as I use the acceleration to recover the nutation part.

So sorry to have bothered you, the current code is probably fine, I guess it’s just me not understanding some simple motion.

[MaximeJ]

You’re very welcome Luc and no one was bothered at all don’t worry

@luc

I don’t know why I get zero accelerations in many of my test cases.

That’s why I was asking what you meant by “regular cases”. Maybe most of these cases are based on a nearly constant rotation rate ?

[luc]

@MaximeJ

Maybe most of these cases are based on a nearly constant rotation rate ?

I think they should have non-zero acceleration as there is nutation. Imagine a three Euler angles rotation around Z, then X, then Z. The last rotation about Z is what CCSDS calls the spin axis, i.e. it is forced to be exactly Z, but if the first two rotations are non-zero, it is not equal to what Orekit calls spin, i.e. the instantaneous rotation axis that takes all rotations into account. So for CCSDS, the combination of the first two rotations drive the motion of the spin axis with respect to inertial frame. The SPIN/NUTATION model in ADM V1 and the additional new SPIN/NUTATION-MOMENTUM model in upcoming ADM V2 both consider a simplified model for these first two rotations that correspond to the spin axis describing a cone around angular momentum (so I would call that precession, not nutation). My problem is to recover this cone from the quaternion and its derivatives at each date, typically when writing an AEM ephemeris. I use the acceleration to identify this cone. In my tests, I ended up with a zero acceleration even when doing round-trip tests, i.e. when starting from a cone, computing the quaternion, and recovering the cone.

I have not pushed the code of this branch yet, but here it is:

public class PrecessionFinder {

    /** Precession axis (axis of the cone described by spin). */
    private final Vector3D axis;

    /** Precession angle (fixed cone angle between precession axis an spin axis). */
    private final double precessionAngle;

    /** Angular velocity around the precession axis (rad/s). */
    private final double angularVelocity;

    /** Build from spin axis motion.
     * @param spin spin axis
     */
    public PrecessionFinder(final FieldVector3D<UnivariateDerivative2> spin) {

        // using a suitable coordinates frame, the spin axis can be considered to describe
        // a cone of half aperture angle 0 ≤ η ≤ π around k axis, at angular rate ω ≥ 0
        // with an initial position in the (+i,±k) half-plane. In this frame, the normalized
        // direction of spin s = spin/||spin|| and its first and second time derivatives
        // have coordinates:
        //           s(t):        (sin η cos ω(t-t₀), sin η sin ω(t-t₀), cos η)
        //           ds/dt(t):    (-ω sin η sin ω(t-t₀), ω sin η cos ω(t-t₀), 0)
        //           d²s/dt²(t):  (-ω² sin η cos ω(t-t₀), -ω² sin η sin ω(t-t₀), 0)
        // at initial time t = t₀ this leads to:
        //           s₀ = s(t₀):       (sin η, 0, cos η)
        //           s₁ = ds/dt(t₀):   (0, ω sin η, 0)
        //           s₂ = d²s/dt²(t₀): (-ω² sin η, 0, 0)
        // however, only the spin vector itself is provided, we don't initially know the unit
        // vectors (i, j, k) of this "suitable coordinates frame". We can however easily
        // build another frame (u, v, w) from the normalized spin vector s as follows:
        //           u = s₀, v = s₁/||s₁||, w = u ⨯ v
        // the coordinates of vectors u, v and w in the "suitable coordinates frame" are:
        //           u: ( sin η,   0,  cos η)
        //           v: (     0,   1,      0)
        //           w: (-cos η,   0,  sin η)
        // we can then deduce the following relations, which can be computed regardless
        // of frames used to express the various vectors:
        //           s₁ · v = ω  sin η
        //           s₂ · w = ω² sin η cos η
        // these relations can be solved for η and ω (we know that 0 ≤ η ≤ π and ω ≥ 0):
        //           η = atan2([s₁ · v]², s₂ · w)
        //           ω = [s₁ · v] / sin  η
        // then the k axis, which is the precession axis, can be deduced as:
        //           k = cos η u + sin η w

        final UnivariateDerivative2 nS = spin.getNorm();
        if (nS.getValue() == 0) {
            // special case, no motion at all, set up arbitrary values
            axis            = Vector3D.PLUS_K;
            precessionAngle = 0.0;
            angularVelocity = 0.0;
        } else {

            // build the derivatives vectors
            final FieldVector3D<UnivariateDerivative2> s = spin.scalarMultiply(nS.reciprocal());
            final Vector3D s0 = new Vector3D(s.getX().getValue(),
                                             s.getY().getValue(),
                                             s.getZ().getValue());
            final Vector3D s1 = new Vector3D(s.getX().getFirstDerivative(),
                                             s.getY().getFirstDerivative(),
                                             s.getZ().getFirstDerivative());
            final Vector3D s2 = new Vector3D(s.getX().getSecondDerivative(),
                                             s.getY().getSecondDerivative(),
                                             s.getZ().getSecondDerivative());

            final double nV = s1.getNorm();
            if (nV == 0.0) {
                // special case: we have a fixed spin vector
                axis            = s0;
                precessionAngle = 0.0;
                angularVelocity = 0.0;
            } else {
                // check second derivatives were provided ; we do it on spin rather than s2
                // and use test against exact 0.0 because the normalization process changes
                // the derivatives and what we really want to check are missing derivatives
                if (new Vector3D(spin.getX().getSecondDerivative(),
                                 spin.getY().getSecondDerivative(),
                                 spin.getZ().getSecondDerivative()).getNormSq() == 0) {
                    throw new OrekitException(OrekitMessages.CANNOT_ESTIMATE_PRECESSION_WITHOUT_PROPER_DERIVATIVES);
                }
                // build the unit vectors
                final Vector3D v = s1.scalarMultiply(1.0 / nV);
                final Vector3D w = Vector3D.crossProduct(s0, v);

                // compute precession model
                final double s1v = Vector3D.dotProduct(s1, v);
                final double s2w = Vector3D.dotProduct(s2, w);
                precessionAngle  = FastMath.atan2(s1v * s1v, s2w);
                final SinCos sc  = FastMath.sinCos(precessionAngle);
                angularVelocity  = s1v / sc.sin();
                axis             = new Vector3D(sc.cos(), s0, sc.sin(), w);

            }
        }

    }

    /** Get the precession axis.
     * @return precession axis
     */
    public Vector3D getAxis() {
        return axis;
    }

    /** Get the precession angle.
     * @return fixed cone angle between precession axis an spin axis (rad/)
     */
    public double getPrecessionAngle() {
        return precessionAngle;
    }

    /** Get the angular velocity around precession axis.
     * @return angular velocity around precession axis (rad/s)
     */
    public double getAngularVelocity() {
        return angularVelocity;
    }

}

I wonder it the normalization step at the beginning is correct. We want a normalized quaternion not only at current time, but also around this time, so we should probably fiddle around with the first and second derivative too.

[MaximeJ]

@luc

I think they should have non-zero acceleration as there is nutation. Imagine a three Euler angles rotation around Z, then X, then Z. The last rotation about Z is what CCSDS calls the spin axis, i.e. it is forced to be exactly Z, but if the first two rotations are non-zero, it is not equal to what Orekit calls spin, i.e. the instantaneous rotation axis that takes all rotations into account.

Ok I understand better

@luc

In my tests, I ended up with a zero acceleration even when doing round-trip tests, i.e. when starting from a cone, computing the quaternion, and recovering the cone.

It is indeed weird. I haven’t seen errors in your equations and code.

[luc]

OK, I finally nailed it.
The rotation acceleration can be non null and is really independent from other coordinates.
The current implementation with Vector3D instances to compute both rate and acceleration is correct.
My problem came from at least two unrelated causes: one was a copy/paste error between two equations in the test drivers (I forgot to change a sine into a cosine when copying equation from X coordinate to Y coordinate in an intermediate vector) and the second error was simply the test data that was wrong (the precession cone was properly set up, but the spin was specified to move at 0°/s along this cone…).

So I will keep the current implementation of AngularCoordinates, as it handles better shifts and interpolations. I will let the angular-coordinates-with-univariatederivatives2 branch in the source tree if we decide later on to change the implementation (which would imply fixing the shift/interpolation issues), but do not intent to merge it, it will just remain as a stale branch.

[MaximeJ]

Glad to know you nailed it Luc.

@luc

So I will keep the current implementation of AngularCoordinates, as it handles better shifts and interpolations. I will let the angular-coordinates-with-univariatederivatives2 branch in the source tree if we decide later on to change the implementation