A graph optimization approach for motion estimation using inertial measurement unit data

Abstract

This study presents a novel approach for processing motion data from a six-degree-of-freedom inertial measurement unit (IMU). Trajectory estimation through double integration of acceleration measurements results in the generation and accumulation of multiple errors. Existing IMU-based measurement methods often use constrained initial and final states to resolve these errors. The constraints on the initial and final states lead to a uniform distribution of the accumulated errors throughout the calculated trajectory so that they cancel each other. We develop a generalized method that can incorporate the constraints from the measurements of intermediate states. The proposed approach is inspired by graph-based simultaneous localization and mapping processes from robotics research. We tested the proposed method with simulated and actual IMU data and found that our method estimates trajectories more accurately than conventional methods with acceptably higher computational costs.

Introduction

Background

Measuring athlete motion is an important aspect of sports science. The typical instruments employed for this purpose include high-speed video cameras and infrared motion capture cameras. However, these instruments are expensive and require time and effort for installation.

Recently, small and inexpensive inertial measurement units (IMUs) have become available, making sports motion measurements accessible to a wide range of people. For example, measurement devices that can be attached to a golf club [1, 2] or a tennis racket [3] have been developed.

Considering these applications, this study seeks to improve the data-processing algorithm for extracting trajectories from IMU data. The IMU employed herein is assumed to comprise a three-axis accelerometer and a three-axis gyroscope. We aim to estimate a time series of the attitude, velocity, and position values with a time series of acceleration and angular velocity measurements.

Main issues to be addressed

The integration of IMU measurements is the simplest way to extract a trajectory from IMU data. Theoretically, attitude can be calculated by integrating the angular velocity, velocity can be calculated by integrating the acceleration, and position can be calculated by the double integration of the acceleration. However, position estimation by simple double integration amplifies the error [4]. Even a small error in the integration of the acceleration can lead to a bias drift in the velocity values. Additionally, the integration of biased velocity causes rapid generation of the positional errors. In addition, if the measurement is performed for a long period, the attitude and velocity estimations are affected by the bias drift in the angular velocity and acceleration values. Thus, it is necessary to correct the accumulated errors.

Related work

Human gait analysis is a well-studied topic in IMU-based motion estimation. Several methods that can precisely track human gait have been developed [5, 6]. Error correction methods for gait data rely on the assumption that the foot velocity is zero during the stance phase, which is the period during which the foot is in contact with the ground. The errors accumulated in the velocity data can be eliminated using this assumption [6, 7].

A method to correct motion errors without such zero-velocity assumptions needs to be developed because the assumptions are not always applicable to various sports motions. Sagawa et al. developed a method for measuring the pitching motion used in baseball [8]. They constrained the target motion with a known initial state and a known final state. The accumulated errors are corrected with regard to these constraints.

However, this method is limited as error correction is performed only with one constraint, thereby returning inaccurate trajectories for motions that extend over a long period of time. Therefore, a more powerful error correction method is needed to measure longer sets of motions, such as a rally in a tennis match.

Contribution

Herein, we propose a novel error correction method that can incorporate multiple constraints. While the relation between the initial and final states is the only constraint employed in the conventional motion estimation method, the proposed method employs generalized constraints that can represent the differences between any two known points in time. The problem is formulated as a least-squares fit of the constraint errors. Steps reducing the computational complexity of the algorithm are also introduced to make computations practical. We quantify the performance of the proposed algorithm with simulated and actual data.

IMU-based motion estimation: review

Problem definition

We consider the problem of estimating a time series of the states of an IMU.

$$\begin{aligned} \{ \varvec{x}_i \}_{i=1}^n, \mathrm {\ \ } \varvec{x}_i := \begin{bmatrix}\varvec{u}_i\\ \varvec{v}_i\\ \varvec{t}_i \end{bmatrix}. \end{aligned}$$

(1)

Here, $\varvec{u}, \varvec{v},$ and $\varvec{t}$ denote the attitude, velocity, and position, respectively. We assume that the initial state of the IMU, $\varvec{x}_0$, is known and that the IMU collects three-dimensional (3D) acceleration and 3D angular velocity over time $\{\varvec{a}_i, \varvec{\omega }_i \}_{i=1}^{n}$.

Herein, we employ a rotation vector [9] to represent the 3D attitude $\varvec{u}$. The definition of the rotation vector representation and the operations performed with it are available in Appendix.

Naive integration

The simplest method for IMU state estimation is the naive integration of the measured data. We estimate the attitude by integrating 3D angular velocity data obtained by the IMU. Assuming that the time step $\Delta t$ is small, the attitude at each time step can be incrementally calculated as follows:

$$\begin{aligned} \varvec{u}_{i+1} = \varvec{u}_{i} * (\varvec{\omega }_i \Delta t), \end{aligned}$$

(2)

where the $*$ operator represents the concatenation of two rotation vectors.

Velocity can be estimated through the integration of the 3D acceleration data. Note that we need to compensate for the gravitational acceleration, which is denoted by $\varvec{g}$, as follows:

$$\begin{aligned} \varvec{v}_{i+1} = \varvec{v}_{i} + (\varvec{R}_i\varvec{a}_i - \varvec{g})\Delta t, \end{aligned}$$

(3)

where $\varvec{R}_i$ represents the rotation matrix representation of the attitude $\varvec{u}_{i}$ and $\varvec{R}_i\varvec{a}_i$ represents the acceleration transformed into the world frame.

Position can be estimated through the integration of velocity data as follows:

$$\begin{aligned} \varvec{t}_{i+1} = \varvec{t}_{i} + \varvec{v}_i \Delta t. \end{aligned}$$

(4)

Sagawa et al.’s error correction method

The aforementioned integration typically leads to significant error accumulation. We now review a method proposed by Sagawa et al. [8] for correcting such errors.

This method assumes that the initial and final states are known, so the relative difference between the initial and final states is used as a constraint on trajectory reconstruction. The estimated time-series states are corrected by uniformly distributing the accumulated errors along the time sequence so that the error at the final state is canceled out.

This method uses the following steps:

• 1. Initial estimates of attitude are calculated via simple integration. The accumulated errors in these attitude values are corrected using the constraint.

• 2. Velocities are estimated via integration using the acceleration data and the corrected attitudes. The accumulated errors are again corrected using the constraint.

• 3. Positions are estimated through the integration of the corrected velocities, and these estimations are corrected with respect to the constraint.

Although the algorithm in the original paper [8] was described using the rotation matrix representation, we will detail the steps in rotation vector notation for easy comparison with the proposed algorithm.

Attitude correction

The attitudes estimated via simple integration are denoted as $\{ \tilde{\varvec{u}}_i \}^n_{i=1}$, and the known final attitude is denoted by ${\varvec{u}}_n^{\mathrm {GT}}$. The accumulated attitude error can be expressed as a single rotation:

$$\begin{aligned} \varvec{u}^{\mathrm {E}} = (-\tilde{\varvec{u}}_n) *{\varvec{u}}_n^{\mathrm {GT}}. \end{aligned}$$

Attitude error correction is accomplished by uniformly distributing the error according to the time sequence as follows:

$$\begin{aligned} \hat{\varvec{u}}_i = \tilde{\varvec{u}}_i * (\frac{i}{n}\varvec{u}^{\mathrm {E}}). \end{aligned}$$

Velocity correction

The velocity error is corrected so that the estimated final velocity $\tilde{\varvec{v}}_n$ matches the given final velocity ${\varvec{v}}_n^{\mathrm {GT}}$. The accumulated error is assumed to be caused by a constant acceleration bias, ${\varvec{a}}^{\mathrm {E}}$, which is calculated as follows:

$$\begin{aligned} {\varvec{a}}^{\mathrm {E}} n\Delta t = {\varvec{v}}_n^{\mathrm {GT}} - \tilde{\varvec{v}}_n. \end{aligned}$$

The corrected velocity sequence $\{\hat{\varvec{v}}_i\}_{i=1}^n$ is then calculated as follows:

$$\begin{aligned} \hat{\varvec{v}}_i = \tilde{\varvec{v}}_i + {\varvec{a}}^{\mathrm {E}} i\Delta t. \end{aligned}$$

Position correction

Although the position correction step was not detailed in the original paper [8], we assume that Sagawa et al. repeated the above process. The difference between the final position estimated via simple integration $\tilde{\varvec{t}}_n$ and the given final position ${\varvec{t}}_n^{\mathrm {GT}}$ assumed to be caused by a constant velocity bias, ${\varvec{v}}^E$, which satisfies the following relation:

$$\begin{aligned} {\varvec{v}}^{\mathrm {E}} n\Delta t = {\varvec{t}}_n^{\mathrm {GT}} - \tilde{\varvec{t}}_n. \end{aligned}$$

The corrected position sequence $\{\hat{\varvec{t}}_i\}_{i=1}^n$ is then calculated as follows:

$$\begin{aligned} \hat{\varvec{t}}_i = \tilde{\varvec{t}}_i + {\varvec{v}}^{\mathrm {E}} i\Delta t. \end{aligned}$$

Proposed method

The aforementioned error correction employed a single constraint. Thus, we extend the problem to multiple constraints.

We treat the problem as an optimization of a graphical model shown in Fig. 1. The state at each time is modeled as a graph node. The constraints representing a relation between two nodes are modeled as graph edges.

Objective function

We define a vector containing all time-series state parameters as follows:

$$\begin{aligned} \varvec{x} := [\varvec{x}_1^\top , \ldots , \varvec{x}_n^\top ]^\top . \end{aligned}$$

This vector is used as an objective variable in our optimization problem.

Next, given a set of constraints, we aim to find the optimal vector $\varvec{x}^{*}$. Constraint $\varvec{c}_{ij}$ is a nine-dimensional vector representing the state of $\varvec{x}_j$ relative to $\varvec{x}_i$, which is obtained from IMU measurements and prior knowledge about the target motion. The problem is formulated as the maximization of the posterior density as follows:

$$\begin{aligned} \varvec{x}^{*} = \mathop {\text{arg max}}\limits _{\varvec{x}} p(\varvec{x} | \{ \varvec{c}_{ij} \}_{\langle i,j \rangle \in {C}}). \end{aligned}$$

(5)

Assuming that the errors in the constraints are statistically independent of each other and Gaussian, we can express the posterior density as follows:

$$\begin{aligned} p(\varvec{x} | \{ \varvec{c}_{ij} \}_{\langle i,j \rangle \in {C}})&= \prod _{\langle i,j \rangle \in {C}} f( \varvec{e}_{ij}(\varvec{x}) | \varvec{0} , \varvec{\Sigma }_{ij}) , \end{aligned}$$

(6)

where $f(\varvec{x} | \varvec{\mu }, \varvec{\Sigma })$ is the probability density function of the normal distribution $\mathcal {N}(\varvec{\mu }, \varvec{\Sigma })$ and $\varvec{e}_{ij}(\varvec{x})$ denotes the error of the constraint $\varvec{c}_{ij}$.

By taking the logarithm of Eq. (6), the optimization problem can be rewritten as follows:

$$\begin{aligned} \varvec{x}^{*}& = \mathop {\text{arg min}}\limits _{\varvec{x}} F(\varvec{x}), \end{aligned}$$

(7)

$$\begin{aligned} F(\varvec{x})&:= \sum _{\langle i,j \rangle \in C} \varvec{e}_{ij}(\varvec{x})^{\top } \varvec{\Omega }_{ij} \varvec{e}_{ij}(\varvec{x}) ,\end{aligned}$$

(8)

$$\begin{aligned} \varvec{\Omega }_{ij}&:= \varvec{\Sigma }_{ij}^{-1}. \end{aligned}$$

(9)

Constraints

Each component (attitude, velocity, and position) in the constraint $\varvec{c}_{ij}$ is denoted as $\varvec{u}^{(ij)}, \varvec{v}^{(ij)}$, and $\varvec{t}^{(ij)}$, respectively. Two types of constraints are employed: a constraint between consecutive nodes ($j = i + 1$) and that between non-sequential nodes (loop-closing constraints). The former type of constraints can be calculated using IMU measurements as follows:

$$\begin{aligned} \varvec{c}_{ij} = \begin{bmatrix} \varvec{\omega }_i \Delta t\\ (\varvec{R}_i\varvec{a}_i - \varvec{g})\Delta t\\ \varvec{v}_i\Delta t \end{bmatrix}. \end{aligned}$$

In contrast, the non-sequential constraints can be obtained from prior knowledge about the motion or observations with external sensors. The single constraint used in Sagawa et al.’s method is given in this form. For example, a constraint that the initial (time $t = 0$) and final (time $t = n$) states are identical can be expressed as follows:

$$\begin{aligned} \varvec{c}_{0,n} = [0, 0, 0, 0, 0, 0]^{\top }. \end{aligned}$$

Error function

We define the error function as follows:

$$\begin{aligned} \varvec{e}_{ij}(\varvec{x}) := \begin{bmatrix} (-\varvec{u}^{(ij)})*(-\varvec{u}_i)*(\varvec{u}_j)\\ -\varvec{v}^{(ij)} + \varvec{v}_j - \varvec{v}_i\\ -\varvec{t}^{(ij)} + \varvec{t}_j - \varvec{t}_i \end{bmatrix}. \end{aligned}$$

The error function returns a zero vector when the state of node j relative to node i matches $\varvec{c}_{ij}$.

Optimization

We employ the Gauss–Newton method to minimize function F in Eq. (8). This method is a gradient descent method that can be used for nonlinear least-squares problems. We initiate the objective variable with an initial estimate, $\varvec{\hat{x}}$, and iterate over the following three steps until convergence:

• 1. Linearization: Calculate $\bar{F}_{\varvec{\hat{x}}}(\varvec{d})$, i.e., the linear approximation of $F(\varvec{x})$ at $\varvec{x} = \varvec{\hat{x}}$.

• 2. Minimization: Determine the update vector $\varvec{d}$, which minimizes $\bar{F}_{\varvec{\hat{x}}}(\varvec{d})$.

• 3. Update: Update the current estimate $\varvec{\hat{x}}$ using $\varvec{d}.$

Linearization

In the first step, we obtain the linearized objective function $\bar{F}_{\varvec{\hat{x}}}(\varvec{d})$ as follows:

$$\begin{aligned}&F(\varvec{\hat{x}} + \varvec{d}) = \sum _{\langle i,j \rangle \in C} \varvec{e}_{ij}(\varvec{\hat{x}}+\varvec{d})^\top \varvec{\Omega }_{ij}\varvec{e}_{ij}(\varvec{\hat{x}}+\varvec{d})\\&\approx \sum _{\langle i,j \rangle \in C} (\varvec{e}_{ij}(\varvec{\hat{x}}) + \varvec{J}_{ij}\varvec{d})^\top \varvec{\Omega }_{ij}(\varvec{e}_{ij}(\varvec{\hat{x}}) + \varvec{J}_{ij}\varvec{d})\\&:= \bar{F}_{\varvec{\hat{x}}}(\varvec{d}). \\ \end{aligned}$$

Here, $\varvec{J}_{ij}$ is the Jacobian matrix of $\varvec{e}_{ij}(\varvec{x})$ at $\varvec{x} = \varvec{\hat{x}}$.

The linearized objective function $\bar{F}_{\varvec{\hat{x}}}(\varvec{d})$ can be expanded into a quadratic function as follows:

$$\begin{aligned} \bar{F}_{\varvec{\hat{x}}}(\varvec{d})&= \varvec{d}^\top \varvec{H} \varvec{d} + 2\varvec{b}^\top \varvec{d} + \mathrm{const.},\end{aligned}$$

(10)

$$\begin{aligned} \mathrm { where} \nonumber \\ \varvec{H}&:= \sum _{\langle i,j \rangle \in C} \varvec{J}_{ij}^\top \varvec{\Omega }_{ij}\varvec{J}_{ij},\nonumber \\ \varvec{b}&:= \sum _{\langle i,j \rangle \in C} \varvec{e}_{ij}(\varvec{\hat{x}})^\top \varvec{\Omega }_{ij}\varvec{J}_{ij}. \end{aligned}$$

(11)

Minimization

By setting the derivative of Eq. (10) to zero, the minimizer $\hat{\varvec{d}}$ is obtained by solving the following linear equation:

$$\begin{aligned} \varvec{H}\varvec{d} = -\varvec{b}. \end{aligned}$$

(12)

$\varvec{H}$ is a $9n \ \times \ 9n$ matrix, and it grows rapidly as the number of nodes increases. Therefore, naive methods, including Gauss–Seidel minimization, require a large amount of time for computations. Since $\varvec{H}$ is symmetric, positive-definite, and sparse (i.e., the number of loop-closing constraints is small), the linear system can be efficiently solved via sparse Cholesky decomposition or the preconditioned conjugate gradient method [10, 11].

Update

Finally, using the obtained minimizer $\hat{\varvec{d}}$, we update the current estimates using the following equation:

$$\begin{aligned} \varvec{\hat{x}} \leftarrow \varvec{\hat{x}} + \hat{\varvec{d}}. \end{aligned}$$

(13)

Relevance to graph-based simultaneous localization and mapping

The formulation can be considered as an extension of graph-based simultaneous localization and mapping (SLAM) [12, 13]. Graph-based SLAM produces attitude and position estimates, and the proposed method extends the state vector with velocity as an extra degree of freedom.

Improving initial estimates

The computational cost of the proposed method is higher than those of the graph-based SLAM methods for the same number of nodes and constraints as $\varvec{H}$ becomes larger. Therefore, to achieve practical processing time, we employ an initialization technique, which is described below.

The positions estimated via simple integration are typically far from the optimum value. If we begin optimization with such poor initial estimates, the optimization will require a large amount of time to converge.

Fig. 1

Sample graphical model

Therefore, we improve the initial estimates by solving a sub-problem. The sub-problem employs the same graph-based formulation as the main problem and only optimize the attitude and velocity estimates. More specifically,

$$\begin{aligned} \varvec{x'} := \left[\varvec{u}_0^\top , \varvec{v}_0^\top , \varvec{u}_1^\top , \varvec{v}_1^\top , \ldots , \varvec{u}_n^\top , \varvec{v}_n^\top \right]^\top \end{aligned}$$

is used as an objective variable. The attitude and velocity components of the constraints were also extracted and employed.

We initialize the position estimates for the main problem by integrating the attitudes and velocities returned by sub-problem optimization, thereby reducing the iterations required for convergence in the main optimization problem.

Results

Simulation experiments

Experiments with simulated data were conducted as follows. IMUSim [14] was employed to generate 100 different random trajectories with ideal 100-Hz IMU measurements. The duration of each generated trajectory was 3 s.

To improve the realism of the simulation, artificial white noise was added to the ideal measurements. The standard deviations of the noise terms were 0.1 $\mathrm {m/s^2}$ for acceleration and 0.3°/s for angular velocity. Bias errors of 0.2°/s were also added to the angular velocity data.

First, we tested the processing methods for motions with only one constraint. The estimation accuracy of the proposed method was compared with those of the naive integration and Sagawa et al.’s conventional method [8].

The examples of estimated trajectories and RMS errors of each method are shown in Fig. 2 and summarized in Fig. 3. Regarding position and velocity estimations, the proposed method returned errors that were almost same as those returned by the other methods, which makes sense because both methods used the same information.

Fig. 2

Sample trajectory estimation result

Fig. 3

Motion estimation errors using simulated data with one loop-closing constraint. Error bars indicate standard errors

However, the proposed method exhibited significantly smaller attitude errors in comparison with Sagawa et al.’s method ($p < 0.001$ using the Wilcoxon signed-rank test). The typical attitude estimation results are plotted in Fig. 4, which is generated using the same motion as shown in Fig. 2. This plot shows that both correction methods accurately estimated the final attitude; however, Sagawa et al.’s method exhibited larger errors for intermediate states. This was because the method uniformly distributed the attitude errors along a single rotation axis. Thus, the correction was not expected to necessarily match with the direction of the errors.

Next, we tested the method with multiple constraints. Several observation points were chosen in the simulated data sequences, and the true IMU states at these points were used as additional constraints. The relation between the number of constraints used and the estimation accuracy is summarized in Fig. 5. This figure clearly shows that the estimation accuracy improves as the number of constraints increases.

Trajectory estimation using real IMU data

The accuracy of trajectory estimation was evaluated using real IMU data collected with an Xsense MTi-3 IMU at 100 Hz. The motion measured is shown in Fig. 6. The IMU was moved to four vertices of a 0.3 m $\times$ 0.2 m rectangle and returned to the initial position and orientation. The motion lasted approximately 7 s. To perform the calibration of the IMU’s acceleration and angular velocity bias, approximately 3.0 s of motionless time was included at the beginning of the motion.

Fig. 4

Attitude estimation errors for comparison

Fig. 5

RMS errors with different number of constraints (simulated data)

Fig. 6

IMU data collection procedure of trajectory estimation experiment

We again compared the proposed method and Sagawa et al.’s method [8] in terms of estimation accuracy. Both methods were provided the same constraint, i.e., the initial and final states were identical. Extra constraints were employed in the proposed method, which are defined by the relative positions of three vertices and the assumption that the device stops moving at these vertices. The times when the IMU visited the vertices was calculated based on the acceleration spikes caused by collisions with the table surface.

The estimated 3D trajectories were projected into a two dimensional plane (Fig. 7) for comparison with the reference trajectory. The Fréchet distance (a similarity measure) between the estimated trajectory and reference trajectory was employed for measuring errors. The errors were 0.062 m from Sagawa et al.’s method and 0.022 m from the proposed method.

Fig. 7

Two dimensional projection of the estimated trajectories using real IMU data

The changes in the estimation accuracy for different number of constraints are plotted in Fig. 8. This figure shows that the estimation accuracy improves as the number of constraints increases.

Fig. 8

Trajectory estimation errors with different number of constraints (real IMU data)

The processing times for the proposed method and Sagawa et al.’s method were 0.58 and 0.022 s, respectively, on a laptop equipped with a Core i7-7600U processor (mean of 10 trials). Although the proposed method performed slower than Sagawa et al.’s method, the performance of our method does not appear to be a significant problem for the purpose of offline motion analysis.

Attitude estimation using real IMU data

The attitude estimation accuracy was also evaluated using the MTi-3 IMU. In this evaluation, the IMU was rotated randomly and returned to the initial state, as depicted in Fig. 9. The constraint that the initial and final states are identical was employed. The mean attitude errors for the collected 53 sequences of the IMU data are shown in Fig. 10. As can be seen from the figure, the estimation error of the proposed method was smaller than that of Sagawa et al.’s method ($p = 0.0376$).

Analysis of the effect of data frequency and length

To evaluate how the data acquisition rate affects the performance of the proposed method, we conducted an additional experiment using simulated IMU data of 1000 Hz from the same 100 random motions employed in the abovementioned simulation experiments. The estimation results are summarized in Fig. 11. The graph shows similar trend with that of 100 Hz (Fig. 3). The mean processing time of the proposed method was 3.4 s for 1000 Hz data and it is 15 times slower than that for 100 Hz (0.23 s).

Fig. 9

IMU data collection procedure of attitude estimation experiment

Fig. 10

Results of attitude estimation experiments using real IMU data. Error bars indicate standard errors

Fig. 11

Estimation errors using 1000 Hz simulated data

The estimation accuracy for longer motions was evaluated using the same settings as the trajectory estimation experiments using the MTi-3 IMU. Two additional data sequences were collected by slowly moving along the rectangle (same procedure as shown in Fig. 6); the duration of the data was approximately 14 and 21 s. Trajectory estimation results are summarized in Fig. 12. It was observed that the estimation accuracy of both the proposed method and Sagawa et al.’s method degraded rapidly as motion length increased.

Fig. 12

Trajectory estimation results for long motions (real IMU data)

Conclusions and future work

Using IMU data, the proposed motion estimation method corrects the accumulated errors via least-squares fitting with constraints that are obtained from IMU measurements and from prior knowledge of the motion. This method can incorporate more than one constraint in error correction steps, thereby improving the estimation accuracy. The effect of accuracy improvement was verified by experiments using simulated and real data. When using a single constraint, the proposed method estimated attitude more accurately than the conventional methods. The computation time of the proposed method was practical for motions with duration of several seconds, thanks to the use of an initialization technique. Nevertheless, the computational cost will be large on data with long duration or high frequency. One way to alleviate the problem would be node thinning.

In future work, we plan to apply this method to actual sports data. An analysis of table tennis rallies is in progress.

The proposed method has a limitation: it assumes that the constraints are independent; however, biased measurements can violate this assumption. A mathematical model that considers gyroscope and accelerometer biases can compensate for this limitation.

Appendix

Rotation vector definition

A rotation vector is a 3D vector representing a 3D rotation. It can be calculated from a 3D unit vector representing the rotation axis $\varvec{a} = [a_x, a_y, a_z]^\top$ and the angle of rotation $\theta$[rad] as follows:

$$\begin{aligned} \varvec{v} := \theta \varvec{a}. \end{aligned}$$

(14)

The inverse rotation can be calculated as follows:

$$\begin{aligned} \varvec{v}^{-1} = -\varvec{v}. \end{aligned}$$

(15)

For brevity, the norm of $\varvec{v}$ is denoted as v.

Conversion between unit quaternion

Conversion from a rotation vector to unit quaternion $\varvec{q} = [q_w, q_x, q_y, q_z]^\top$ can be expressed as follows:

$$\begin{aligned} \varvec{q}_v(\varvec{v}) := \begin{bmatrix} \cos \frac{v}{2} \\ \frac{v_1}{v}\sin \frac{v}{2} \\ \frac{v_2}{v}\sin \frac{v}{2} \\ \frac{v_3}{v}\sin \frac{v}{2} \end{bmatrix}. \end{aligned}$$

(16)

Conversion from unit quaternion to rotation vector can be expressed as follows:

$$\begin{aligned} \varvec{v}_q(\varvec{q}) := \frac{2\arccos (q_w)}{\sqrt{1-q_w^2}} \begin{bmatrix} q_x\\ q_y\\ q_z \end{bmatrix}. \end{aligned}$$

(17)

Rotation vector to rotation matrix

A rotation vector can be converted to a rotation matrix as follows (Rodrigues’s formula):

$$\begin{aligned} \varvec{R}&= \varvec{I}_3 + \sin {v}\varvec{K} + (1-\cos {v})\varvec{K}^2, \end{aligned}$$

(18)

$$\begin{aligned} \varvec{K}&:= \frac{1}{v} \begin{bmatrix} 0&-v_{3}&v_{2}\\ v_{3}&0&-v_{1}\\ -v_{2}&v_{1}&0 \end{bmatrix}. \end{aligned}$$

(19)

Rotation vector multiplication

The product of the two rotation vectors $\varvec{v}$ and $\varvec{u}$ is represented by $\varvec{v} * \varvec{u}$, which is calculated in terms of the quaternion product.

$$\begin{aligned} \varvec{v} * \varvec{u} := \varvec{v}_q( \varvec{q}_v(\varvec{v}) \varvec{q}_v(\varvec{u})) \end{aligned}$$

(20)

This operation represents the concatenation of two rotations (first rotation by $\varvec{v}$, and the next rotation by $\varvec{u}$).

Derivatives of rotation vector multiplication

Derivatives of three rotation vector multiplication are calculated as follows:

$$\begin{aligned} \frac{\partial (\varvec{u}*\varvec{v}*\varvec{w})}{\partial \varvec{v}}&= H(\varvec{q}_v(\varvec{u}*\varvec{v}*\varvec{w})){Q}(\varvec{q}^{\varvec{u}})\bar{Q}(\varvec{q}^{\varvec{w}})G(\varvec{v}) \nonumber \\&:= U_2(\varvec{u},\varvec{v},\varvec{w}),\end{aligned}$$

(21)

$$\begin{aligned} \frac{\partial (\varvec{u}*\varvec{v}*\varvec{w})}{\partial \varvec{w}}&= H(\varvec{q}_v(\varvec{u}*\varvec{v}*\varvec{w})){Q}(\varvec{q}^{\varvec{u}}){Q}(\varvec{q}^{\varvec{v}})G(\varvec{w}) \nonumber \\&:= U_3(\varvec{u},\varvec{v},\varvec{w}). \end{aligned}$$

(22)

Here, H, G, Q,  and $\bar{Q}$ are given as follows [9]:

$$\begin{aligned} H(\varvec{q})&:= \frac{\partial \varvec{v}_q (\varvec{q})}{\partial \varvec{q}} \nonumber \\&= \begin{bmatrix} 2cq_x(dq_w-1)&2d&0&0 \\ 2cq_y(dq_w-1)&0&2d&0 \\ 2cq_z(dq_w-1)&0&0&2d \end{bmatrix},\end{aligned}$$

(23)

$\mathrm {where\ } c:=\frac{1}{1-q_w^2}, \ \ d:=\frac{ \arccos (q_w)}{\sqrt{1-q_w^2}},$

$$\begin{aligned} G(\varvec{v})&:= \frac{\partial \varvec{q}_v(\varvec{v})}{\partial \varvec{v}} \nonumber \\&= \frac{S}{2v} \begin{bmatrix} -v_1&-v_2&-v_3\\ 2&0&0\\ 0&2&0\\ 0&0&2 \end{bmatrix} \nonumber \\&\ \ \ \ \ +\frac{a}{2v^3} \begin{bmatrix} 0&0&0\\ v_1^2&v_1v_2&v_1v_3 \\ v_1v_2&v_2^2&v_2v_3 \\ v_1v_3&v_2v_3&v_3^2 \end{bmatrix}, \end{aligned}$$

(24)

$\mathrm {where\ } S:=\sin \frac{v}{2}, \ a:= v\cos \frac{v}{2}-2S,$

$$\begin{aligned} {Q}(\varvec{q})&:= \begin{bmatrix} q_w&-q_x&-q_y&-q_z\\ q_x&q_w&-q_z&q_y \\ q_y&q_z&q_w&-q_x\\ q_z&-q_y&q_x&q_w \end{bmatrix},\end{aligned}$$

(25)

$$\begin{aligned} \bar{Q}(\varvec{q})&:= \begin{bmatrix} q_w&-q_x&-q_y&-q_z\\ q_x&q_w&q_z&-q_y \\ q_y&-q_z&q_w&q_x\\ q_z&q_y&-q_x&q_w \end{bmatrix}. \end{aligned}$$

(26)

Jacobian matrix calculation

The Jacobian matrix of $\varvec{e}_{ij}(\varvec{x})$ at $\varvec{x}=\hat{\varvec{x}}$ has the following sparse structure:

$$\begin{aligned} \varvec{J}_{ij} = \left( \cdots \mathbf{\large 0} \cdots \varvec{J}_{ij}^{(i)} \cdots \mathbf{\large 0} \cdots \varvec{J}_{ij}^{(j)} \cdots \mathbf{\large 0} \cdots \right) . \end{aligned}$$

Here, $\varvec{J}_{ij}^{(i)}$ and $\varvec{J}_{ij}^{(j)}$ are the partial derivatives of $\varvec{e}_{ij}(\varvec{x})$ by $\varvec{x}_i$ and $\varvec{x}_j$, respectively, and they can be calculated as:

$$\begin{aligned} \varvec{J}_{ij}^{(j)}&:= \frac{\partial \varvec{e}_{ij}(\varvec{x})}{\partial \varvec{x}_j} \Bigr |_{\varvec{x}=\hat{\varvec{x}}} \nonumber \\&= \frac{\partial }{\partial \varvec{x}_j} \begin{bmatrix} (-\varvec{u}^{(ij)})*(-\varvec{u}_i)*(\varvec{u}_j)\\ -\varvec{v}^{(ij)} + \varvec{v}_j - \varvec{v}_i\\ -\varvec{t}^{(ij)} + \varvec{t}_j - \varvec{t}_i \end{bmatrix} \Biggr |_{\varvec{x}=\hat{\varvec{x}}} \nonumber \\&= \begin{bmatrix} \frac{\partial }{\partial \varvec{x}_j} (-\varvec{u}^{(ij)})*(-\varvec{u}_i)*(\varvec{u}_j)\bigr |_{\varvec{x}=\hat{\varvec{x}}}&\varvec{0}&\varvec{0}\\ \varvec{0}&\varvec{I}_3&\varvec{0}\\ \varvec{0}&\varvec{0}&\varvec{I}_3 \end{bmatrix} \nonumber \\&= \begin{bmatrix} U_3(-\varvec{u}^{(ij)}, -\hat{\varvec{u}}_i, \hat{\varvec{u}}_j)&\varvec{0}&\varvec{0}\\ \varvec{0}&\varvec{I}_3&\varvec{0}\\ \varvec{0}&\varvec{0}&\varvec{I}_3 \end{bmatrix}, \end{aligned}$$

(27)

$$\begin{aligned} \varvec{J}_{ij}^{(i)}&:= \frac{\partial \varvec{e}_{ij}(\varvec{x})}{\partial \varvec{x}_i} \Bigr |_{\varvec{x}=\hat{\varvec{x}}} \nonumber \\&= \frac{\partial }{\partial \varvec{x}_i} \begin{bmatrix} (-\varvec{u}^{(ij)})*(-\varvec{u}_i)*(\varvec{u}_j)\\ -\varvec{v}^{(ij)} + \varvec{v}_j - \varvec{v}_i\\ -\varvec{t}^{(ij)} + \varvec{t}_j - \varvec{t}_i \end{bmatrix} \Biggr |_{\varvec{x}=\hat{\varvec{x}}} \nonumber \\&= \begin{bmatrix} \frac{\partial }{\partial \varvec{x}_i} (-\varvec{u}^{(ij)})*(-\varvec{u}_i)*(\varvec{u}_j)\bigr |_{\varvec{x}=\hat{\varvec{x}}}&\varvec{0}&\varvec{0}\\ \varvec{0}&-\varvec{I}_3&\varvec{0}\\ \varvec{0}&\varvec{0}&-\varvec{I}_3 \end{bmatrix} \nonumber \\&= -\begin{bmatrix} U_2(-\varvec{u}^{(ij)}, -\hat{\varvec{u}}_i, \hat{\varvec{u}}_j)&\varvec{0}&\varvec{0}\\ \varvec{0}&\varvec{I}_3&\varvec{0}\\ \varvec{0}&\varvec{0}&\varvec{I}_3 \end{bmatrix}. \end{aligned}$$

(28)