eayvali / Computing-Motion-Derivatives-from-6DoF-Noisy-Pose-Measurements

Angular&linear velocity and acceleration estimation from quaternion and position measurements

Computing Motion Derivatives from 6DoF Noisy Pose Measurements

Generating ground truth 6DoF pose, velocity, and acceleration data is crucial to verify many algorithms in robotics. This repository implements [1] with some modification to estimate translational and angular velocity and acceleration from a given sequence of pose measurements. The rotation is represented as a unit quaternion. The implementation assumes constant rotational and translational acceleration in body coordinates. The estimation of translational and rotational derivatives are decoupled, and the motion curve is approximated as a three step motion: half rotation, translation and half rotation as shown below.

Pseudocode: The pseudocode is provided below.𝑀𝑜𝑡𝑖𝑜𝑛𝑅𝑒𝑔𝑟𝑒𝑠𝑠𝑖𝑜𝑛1𝐷 implementation is identical to [1].

There are ambiguities with the quaternion notation in [1], which inherits the notation from one of its references. The implementation of 𝑆𝑝𝑎𝑡𝑖𝑎𝑙𝑅𝑜𝑡𝑎𝑡𝑖𝑜𝑛𝐷𝑒𝑟𝑖𝑣𝑎𝑡𝑖𝑣𝑒𝑠 and 𝐵𝑜𝑑𝑦𝐷𝑒𝑟𝑖𝑣𝑎𝑡𝑖𝑣𝑒𝑠 follows the notation below. Superscript B denotes body frame, S denotes spatial(world) frame. Additionally, I included an option to preprocess the pose measurements. If you set PREPROCESSING=True, the position and quaternion measurements are averaged over a sliding window. The implementation of quaternion averaging is described here[2]. For smoothing position, it's better to use this to get a smooth interpolation.

Notes: A substantial amount of papers have ambiguous rotation/altitude representations especially when using quaternions. A rotation operation can be active and rotate a vector or be passive and rotate only point of view via frame transformation. The equations can differ depending on order of the components and the right/left handedness of the coordinate systems. I recommend reading [3].

Example:

Below are some of the plots from the example in the code. The position input was generated from a sinusoidal. Note that if we integrate the incremental rtr motion and transform it to the world frame, it matches with the original input as expected. The quaternion and ground truth for angular velocity in body frame were obtained using the code in this repository. You can also see that the estimated angular velocity aligns with the ground truth.

Files:

• /code/test_data.mat : Test data containing quaternion and position measurements
• /Vel_Acc_Estimation_from_Pose.py : standalone main python script

References:

[1] Sittel, Florian, Joerg Mueller, and Wolfram Burgard.Computing velocities and accelerations from a pose time sequence in three-dimensional space. Technical Report 272, University of Freiburg, Department of Computer Science, 2013.

[2] Markley, F. Landis, et al. "Averaging quaternions." Journal of Guidance, Control, and Dynamics 30.4 (2007): 1193-1197.

[3] Sola, Joan. "Quaternion kinematics for the error-state Kalman filter." arXiv preprint arXiv:1711.02508 (2017).