Wrong acceleration when converting angular coordinates
The following snippet shows accelerations are not converted properly. The coordinates given as a quaternion with second order derivatives are converted to quaternion, rotation rate and rotation acceleration, however the acceleration is not consistent with the derivative of the rotation rate. It should be 1°/s² along Z, but is almost 0.
@Test
public void testAccelerationIssue() {
final FieldRotation<UnivariateDerivative2> r =
new FieldRotation<>(new UnivariateDerivative2( 0.3812272063696537,
-0.00803169011561382,
-2.9032111989204744E-5),
new UnivariateDerivative2( 0.08052140686538142,
2.9106034570476104E-4,
-6.132055799232064E-6),
new UnivariateDerivative2( 0.03335305878500302,
-7.026818340694362E-4,
-2.5399806772705075E-6),
new UnivariateDerivative2( 0.9203638919632241,
0.003326834974665178,
-7.008971850867259E-5),
false);
final Vector3D acceleration = new AngularCoordinates(r).getRotationAcceleration();
System.out.println(acceleration.getX() + " " + acceleration.getY() + " " + acceleration.getZ());
double dt = 0.1;
final Vector3D rM4H = new AngularCoordinates(shift(r, -4 * dt)).getRotationRate();
final Vector3D rM3H = new AngularCoordinates(shift(r, -3 * dt)).getRotationRate();
final Vector3D rM2H = new AngularCoordinates(shift(r, -2 * dt)).getRotationRate();
final Vector3D rM1H = new AngularCoordinates(shift(r, -1 * dt)).getRotationRate();
final Vector3D rP1H = new AngularCoordinates(shift(r, +1 * dt)).getRotationRate();
final Vector3D rP2H = new AngularCoordinates(shift(r, +2 * dt)).getRotationRate();
final Vector3D rP3H = new AngularCoordinates(shift(r, +3 * dt)).getRotationRate();
final Vector3D rP4H = new AngularCoordinates(shift(r, +4 * dt)).getRotationRate();
final double c = 1.0 / (840 * dt);
final Vector3D finiteDifferencesAcceleration =
new Vector3D( -3 * c, rP4H.subtract(rM4H),
32 * c, rP3H.subtract(rM3H),
-168 * c, rP2H.subtract(rM2H),
673 * c, rP1H.subtract(rM1H));
System.out.println(finiteDifferencesAcceleration.getX() + " " +
finiteDifferencesAcceleration.getY() + " " +
finiteDifferencesAcceleration.getZ());
}
private FieldRotation<UnivariateDerivative2> shift(final FieldRotation<UnivariateDerivative2> r, final double dt) {
return new FieldRotation<>(shift(r.getQ0(), dt),
shift(r.getQ1(), dt),
shift(r.getQ2(), dt),
shift(r.getQ3(), dt),
false);
}
private UnivariateDerivative2 shift(final UnivariateDerivative2 ud, final double dt) {
return new UnivariateDerivative2(ud.taylor(dt),
ud.getFirstDerivative() + dt * ud.getFirstDerivative(),
ud.getSecondDerivative());
}
Output:
7.446913854026183E-22 1.5286356600290326E-21 -5.2983159964800145E-21
-3.90922985423032E-18 7.938201917924147E-19 0.017494847962500955
Edited by Luc Maisonobe