Robust Plug-and-Play Joint Axis Estimation Using Inertial Sensors

Abstract

Inertial motion capture relies on accurate sensor-to-segment calibration. When two segments are connected by a hinge joint, for example in human knee or finger joints as well as in many robotic limbs, then the joint axis vector must be identified in the intrinsic sensor coordinate systems. Methods for estimating the joint axis using accelerations and angular rates of arbitrary motion have been proposed, but the user must perform sufficiently informative motion in a predefined initial time window to accomplish complete identifiability. Another drawback of state of the art methods is that the user has no way of knowing if the calibration was successful or not. To achieve plug-and-play calibration, it is therefore important that 1) sufficiently informative data can be extracted even if large portions of the data set consist of non-informative motions, and 2) the user knows when the calibration has reached a sufficient level of accuracy. In the current paper, we propose a novel method that achieves both of these goals. The method combines acceleration- and angular rate information and finds a globally optimal estimate of the joint axis. Methods for sample selection, that overcome the limitation of a dedicated initial calibration time window, are proposed. The sample selection allows estimation to be performed using only a small subset of samples from a larger data set as it deselects non-informative and redundant measurements. Finally, an uncertainty quantification method that assures validity of the estimated joint axis parameters, is proposed. Experimental validation of the method is provided using a mechanical joint performing a large range of motions. Angular errors in the order of $2^{\circ }$ were achieved using 125–1000 selected samples. The proposed method is the first truly plug-and-play method that overcome the need for a specific calibration phase and, regardless of the user’s motions, it provides an accurate estimate of the joint axis as soon as possible.

1. Introduction

Wearable inertial measurement units (IMUs) have become a key technology for a range of applications, from performance assessment and optimization in sports [1], to objective measurements and progress monitoring in health care [2], as well as real-time motion tracking for feedback-controlled robotic or neuroprosthetic systems [3]. In all these application domains, IMUs are used to track or capture the motion of mechatronic or biological joint systems such as robotic or human limbs. In this work we consider such systems where the joint is a hinge joint with one degree of freedom. Examples of hinge joints include the knee and finger joints, which are essential in applications targeting lower limb [4] and hand [5] kinematics.

In contrast to stationary optical motion tracking systems, miniature IMU networks can be used in ambulatory settings and facilitate motion tracking outside lab environments. While this is an important step towards ubiquitous sensing, one major limitation of the technology is that the IMUs’ local coordinate systems must be aligned with the anatomical axes of the joints and body segments to which they are attached. This sensor-to-segment calibration is a crucial step that establishes the connection between the motion of the IMUs and the motion of the joint system to which they are attached.

Several different approaches have been proposed for sensor-to-segment calibration of inertial sensor networks, from trying to align the sensor axes with body axes by precise attachment to predefined calibration poses and motions; see, e.g., [6,7,8,9]. However, in all of these cases, the calibration crucially depends on the knowledge and skills of the person who attaches the sensors or the person who performs the calibration procedure. This might be acceptable in supervised settings with trained and able-bodied users, but it represents a major limitation of IMU-based motion tracking and capture in clinical applications and in motion assessment of elderly and children. Finding solutions for these application domains and enabling ubiquitous sensing in daily life requires the development of less restrictive methods for sensor-to-segment calibration.

Ideally, wearable IMU networks should be plug-and-play, and the sensor-to-segment calibration should be performed by the network autonomously, which means without additional effort or requirements on the user’s knowledge or on the performed motion. An important step towards this goal was the development of methods that exploit the kinematic constraints of the joints to identify sensor-to-segment calibration parameters from almost arbitrary motions [10,11]. For joints with one degree of freedom (DOF), the feasibility of this approach has been demonstrated [12,13,14,15]. Methods have been proposed that require the user to perform a sufficiently informative but otherwise arbitrary motion during an initial calibration time window and determine the functional joint axis in intrinsic coordinates of both IMUs, cf. Figure 1. It was recently shown that almost every motion, including purely sequential motions and simultaneous planar motions, is informative enough to render the joint axis identifiable unless the joint remains stiff throughout the motion [16].

Figure 1.

The hinge joint system that we consider. The two segments rotate independently with respect to each other only along the joint axis j. The sensor frames $S_i$ are rigidly fixed to their respective segments and their relative orientation can be described by one joint angle, that corresponds to a rotation about the joint axis. The joint axis expressed in local sensor coordinates is an important sensor-to-segment calibration parameter in joint systems with one degree of freedom (DOF).

Several methods targeting different types of joints or sensor-to-segment calibration parameters have been developed. In [17], a method for identifying the joint axes of a joint with two DOF was proposed. Methods for identifying the position of the joint center relative to sensors attached to adjacent segments have been proposed in [12,18,19]. A method enabling automatic pairing of sensors to lower limb segments have been proposed in [20].

The published kinematic-constraint-based methods constitute an important step forward but still impose undesirable and unnecessary limitations. If the user does not move during the initial calibration time window or if the motion is not sufficiently informative, the calibration will be wrong and all subsequently derived motion parameters will be subject to unpredictable errors. For a truly plug-and-play system, it is therefore crucial that the IMU network is able to

• Recognize how informative motions are and whether they render the joint axis identifiable;

• Wait for sufficiently informative data to be generated and combine useful data even if it is spread and intermitted by useless data;

• Determine how accurate the current estimate of the joint axis is and provide only sufficiently reliable estimates.

An IMU network with such properties can be used without the aforementioned limitations. Once it is installed, it will autonomously gather all available useful information and provide reliable calibration parameters as soon as possible, which immediately enable calculation of accurate motion parameters from the incoming raw data as well as from already recorded data. To explain the practical value of the proposed concept of plug-and-play calibration, we briefly compare this concept to the aforementioned existing calibration concepts that use predefined motions [6,7,8,9] or arbitrary motions [10,11,12,13,14,15]:

Predefined-Motions: The calibration is based on the assumption that the user performs a sequence of predefined motions and poses with sufficient precision within a predefined initial time interval. The approach fails and provides inaccurate calibration without warning if

• (a)The user performs the sequence of predefined motions and poses without sufficient precision;
• (b)The user performs the sequence with sufficient precision but not within the predefined initial time interval;
• (c)The user performs sufficiently informative but otherwise arbitrary motions;
• (d)The user performs no sufficiently informative motion at all, e.g., he/she moves with a stiff joint.

Arbitrary-Motions: The calibration is based on the assumption that the user performs sufficiently informative but otherwise arbitrary motions within a predefined initial time interval. The motion does not need to be precise, and it has been shown that sufficient excitation is provided by almost every motion for which the joint does not remain stiff [16]. However, the approach fails and provides inaccurate calibration without warning if

• (a)The user performs a sequence of predefined motions but not within the predefined initial time interval;
• (b)The user performs sufficiently informative arbitrary motions but not within the predefined initial time interval;
• (c)The user performs no sufficiently informative motion at all, e.g., he/she moves with a stiff joint.

Plug-and-Play: The proposed sensor-to-segment calibration approach. It works well for all mentioned cases and exceptions in the sense that it always provides accurate calibration parameters as soon as the user’s motions are sufficiently informative, and it clearly indicates at all times whether the desired calibration accuracy has yet been reached.

It is important to note that the cases without warning are very dangerous, because inaccurate information is provided and claimed as accurate. In many applications, this leads to unacceptable risks. This and the other listed differences between the two existing approaches and the proposed new method have large implications for the way wearable IMU networks can be used in offline and online applications.

Offline Applications include motion capture for ergonomic workplace assessment [21], for monitoring of movement disorders [2] and for sport performance analysis [1]. In state-of-the-art solutions, the user performs an initial calibration procedure before (or after) recording data from the motions to be analyzed. The user can only hope that the calibration was accurate enough. If the calibration was inaccurate, then all recorded data is corrupted and might lead to false interpretation and conclusions. In contrast, when the calibration is plug-and-play, the user starts recording data from motions that should be analyzed immediately after attaching the sensors. Calibration automatically takes place as soon as sufficiently informative data has been gathered. The system indicates that calibration has been successful, and the user can be sure that all obtained measurements are valid and accurate. The identified calibration parameters are used to evaluate the data that was recorded before and after the moment at which accurate calibration was achieved.

Online Applications include real-time motion tracking for wearable biofeedback systems [22] as well as robotic and neuroprosthetic motion support systems [23]. In state-of-the-art solutions, the user first performs an initial calibration procedure before the sensor system is connected to an assistive device that uses the measurements to provide e.g., biofeedback or motion support. The user can only hope that the calibration was accurate enough. If the calibration was inaccurate, then the provided biofeedback or motion support might be wrong and dangerous. In contrast, when the calibration is plug-and-play, the user instead attaches the sensors and starts moving. As soon as the desired calibration accuracy has been achieved, the sensor system automatically provides measurements to the assistive device. The user can be sure that all provided biofeedback and motion support is based on valid and accurate measurements.

In the present contribution we propose the first joint axis identification method for one-dimensional joints that is plug-and-play in the aforementioned sense. The main contributions of the present work are the following:

1. We leverage recent results on joint axis identifiability [16] to develop a sample selection method that overcomes the limitation of a dedicated initial calibration time window.

2. To assure that the motion needs to fulfill only the minimum required conditions, we combine accelerometer-based and gyroscope-based joint constraints and weight them according to the information contained in both signals.

3. We propose an uncertainty quantification method that assures validity of the estimated joint axis parameters and thereby eradicates the risk of false calibration.

4. We provide an experimental validation in a mechanical joint performing a large range of different motions with different identifiability properties.

In the proposed system, successful calibration no longer depends on performing certain motions in a predefined manner or time window but only on fulfilling the minimum required conditions at some point. Moreover, the system knows when these conditions are fulfilled and provides only reliable calibration parameters.

2. Inertial Measurement Models

Inertial sensors collectively refers to accelerometers and gyroscopes, which are sensors used to measure linear acceleration and angular velocity, respectively. When the sensors have three sensitive axes which are orthogonal to each other, the inertial sensors can measure these quantities in three dimensions. Such sensors are referred to as triaxial. An IMU is a single sensor that contains one triaxial accelerometer and one triaxial gyroscope. The measurements from the IMU are obtained with respect to (w.r.t.) a reference frame, referred to as the sensor frame (S), its axes and origin corresponding to those of the accelerometer triad. The axes of the gyroscope is assumed to be aligned with the axes of the accelerometer. The measured quantities describe the motion of the sensor frame w.r.t. a global frame (G) that is fixed w.r.t. the environment.

The accelerometer measurements at time $t_k$, where the integer k is used as a sample index, can be modeled as

$$\begin{array}{cc}\hfill y_a^S(t_k)& =R^{SG}(t_k)\left(a^G(t_k)+g^G\right)+b_a^S+e_a^S(t_k),\hfill \end{array}$$ (1)

where $a^G\in {\mathbb{R}}^3$ is the acceleration of the sensor w.r.t. the global frame and $g^G\in {\mathbb{R}}^3$ is the gravitational acceleration, which is assumed to be constant in the environment. The measurements are corrupted by a constant additive bias $b_a^S$ and noise $e_a^S(t_k)\in {\mathbb{R}}^3$, which is assumed to be Gaussian $e_a^S(t_k)\sim \mathcal{N}(0,{\mathsf{\Sigma }}_a)$, with zero mean and covariance matrix ${\mathsf{\Sigma }}_a$. The superscript S and G are used to denote in which reference frame a quantity is expressed in, and the rotation matrix $R^{SG}$ describes the rotation from the global frame to the sensor frame, i.e., we have that

$$\begin{array}{cc}\hfill R^{SG}(t_k)\left(a^G(t_k)+g^G\right)& =a^S(t_k)+g^S(t_k).\hfill \end{array}$$ (2)

The multiplication between a rotation matrix and a vector is equivalent to a change of orthonormal basis.

The gyroscope measurements are modeled as

$$\begin{array}{cc}\hfill y_{\omega }^S(t_k)& =R^{SG}(t_k){\omega }^G(t_k)+b_{\omega }^S+e_{\omega }^S(t_k),\hfill \end{array}$$ (3)

where ${\omega }^G\in {\mathbb{R}}^3$ is the angular velocity of the sensor frame in the global frame. Similar to the accelerometer, the measurements are corrupted by constant additive bias $b_{\omega }^S$ and noise $e_{\omega }^S(t_k)\in {\mathbb{R}}^3$, which is assumed to be zero-mean Gaussian $e_{\omega }^S(t_k)\sim \mathcal{N}(0,{\mathsf{\Sigma }}_{\omega })$. Note that the same rotation matrix $R^{SG}$ as in (1) is used to rotate quantities from the global frame to the sensor frame because the accelerometer and the gyroscope are contained in the same IMU and their axes are assumed to be aligned. The gyroscope bias term $b_{\omega }^S$ can be compensated for through pre-calibration of the gyroscopes [24]. In Section 7.5, we will evaluate the effect of uncompensated biases on the proposed method.

Biases and Gaussian measurement noise have been shown to be the dominating error sources, even for low-cost IMUs [25]. However, for longer experiments or for low-quality IMUs, there are other types of errors that may need to be considered. These errors can still be well compensated for by pre-calibration or by online auto-calibration methods. Therefore, we only consider biases in our models, as these are the dominating systematic errors. The bias terms $b_a$ and $b_{\omega }$ are not constant, but drift slowly over time [26]. Sensor manufacturers typically provide a bias stability metric for their sensors, which tells the user the expected rate of the bias drift. Bias instability in inertial sensors is primarily caused by low-frequency flicker noise in the electronics and temperature fluctuations [27]. If the bias drift is significant enough that it needs to be compensated for, there are methods that model the biases as time or temperature dependent, enabling continuous estimation of drifting biases (see, e.g., [28,29]). Such methods can be used in combination with the method proposed in this paper. Low-quality IMUs may be affected by other systematic errors such as non-unit scale factors and misalignments/non-orthogonalities in the sensor axes. If the effect from these types of errors are non-negligible, it is advised to perform a more sophisticated pre-calibration of the sensors to compensate for these errors. Methods for in-field pre-calibration of such errors exist; see, e.g., [30,31,32,33].

3. Kinematics

The kinematic model of the hinge joint system has been described in previous works [12,15,16], and is recapitulated here in Section 3.1 and Section 3.2 for completeness.

3.1. Kinematic Constraints of Two Segments in a Kinematic Chain

Consider the kinematic chain model where we have two rigid body segments connected by a joint. The joint can have 1, 2 or 3 degrees of freedom (DOF). Furthermore, consider the case where each segment has one IMU rigidly attached to it in an arbitrary position and orientation. We therefore have two sensor frames, denoted by $S_1$ and $S_2$, that are fixed in the center of the accelerometer triad of each IMU. The DOF of the joint determines how many angles that are required to describe the orientation of $S_2$ w.r.t. $S_1$ and vice versa. We let subscripts $i\in \{1,2\}$ denote quantities belonging to a specific sensor frame. Rigid body kinematics gives

$$\begin{array}{cc}\hfill a_i^{S_i}(t)& =a_0^{S_i}(t)+{\omega }_i^{S_i}(t)\times ({\omega }_i^{S_i}(t)\times r_i^{S_i})+{\dot{\omega }}_i^{S_i}(t)\times r_i^{S_i},\hfill \end{array}$$ (4)

where $a_i$ are the accelerations of the sensor frames with $i\in \{1,2\}$, $a_0$ is the acceleration of the joint center, ${\omega }_i$ and ${\dot{\omega }}_i$ are the angular velocities and angular accelerations of the sensor frames and t is used to denote time-dependence of the kinematic variables. The positions of the joint center with respect to each sensor frame are denoted by $r_i$, which we assume to be unknown and constant for each sensor. All quantities in (4) are vectors in ${\mathbb{R}}^3$ since they describe 3D motion. The acceleration of the joint center expressed in either of the sensor frames has the same magnitude but a different orientation. We have that

$$\begin{array}{cc}\hfill a_0^G(t)& =R^{G{S}_1}(t)a_0^{S_1}(t)=R^{G{S}_2}(t)a_0^{S_2}(t)\hfill \end{array}$$ (5)

where $R^{G{S}_i}$ are the rotation matrices that maps a vector expressed in $S_i$ into the global frame.

For convenience we shall for the remainder of this document drop the use of the superscripts except for where it’s needed. Hence, the sensor frame of a kinematic variable will be given by subscript $i\in \{1,2\}$. We will also drop the use of t to denote time-dependence unless we want to refer to the kinematic variables at specific time instances. The relationship in (4) is linear in $a_0$ and $r_i$ and can equivalently be formulated as

$$\begin{array}{cc}\hfill a_i& =a_0^{S_i}+K({\omega }_i,{\dot{\omega }}_i)r_i,\hfill \end{array}$$ (6)

where

$$\begin{array}{c}\hfill K(\omega ,\dot{\omega })=\left[\begin{array}{ccc}-{\omega }_y^2-{\omega }_z^2& {\omega }_x{\omega }_y-{\dot{\omega }}_z& {\omega }_x{\omega }_z+{\dot{\omega }}_y\\ {\omega }_x{\omega }_y+{\dot{\omega }}_z& -{\omega }_x^2-{\omega }_z^2& {\omega }_y{\omega }_z-{\dot{\omega }}_x\\ {\omega }_x{\omega }_z-{\dot{\omega }}_y& {\omega }_y{\omega }_z+{\dot{\omega }}_x& -{\omega }_x^2-{\omega }_y^2\end{array}\right],\end{array}$$ (7)

and where subscripts $x,y,z$ denote the elements of the three-dimensional vectors. For convenience of notation we will write $K_i=K({\omega }_i,{\dot{\omega }}_i)$.

3.2. Kinematic Constraints of a Hinge Joint System

For a 1-DOF joint, the two segments can only rotate independently with respect to each other along the joint axis. We let $\parallel ·\parallel$ denote the Euclidean vector norm, then the joint axis is defined by the unit vector $j\in {\mathbb{R}}^3,\parallel j\parallel =1$. We refer to such a joint as a hinge joint. We let $j_1$ and $j_2$ denote the direction of the joint axis in the respective sensor frames. Since the two IMUs are assumed to be rigidly attached to the segments, $j_1$ and $j_2$ are constant. The joint axis j expressed in the global frame must then satisfy

$$\begin{array}{cc}\hfill j^G(t)& =R^{G{S}_1}(t)j_1^{S_1}=R^{G{S}_2}(t)j_2^{S_2},\hfill \end{array}$$ (8)

meaning that the vectors $j_i$ expressed in the two sensor frame has the same direction as j in the global frame, see Figure 1, and time-dependence is only caused by the rotations of the sensor frames in the global frame. We can decompose the angular velocities into one component that is parallel to the joint axis and one that is perpendicular to the joint axis

$$\begin{array}{cc}\hfill {\omega }_i& ={\omega }_{j_i}+{\omega }_{j_i^{\perp }},\hfill \end{array}$$ (9)

$$\begin{array}{cc}\hfill {\omega }_{j_i}& =j_i^{\top }{\omega }_i{j}_i,\hfill \end{array}$$ (10)

$$\begin{array}{cc}\hfill {\omega }_{j_i^{\perp }}& ={\omega }_i-{\omega }_{j_i}={\omega }_i-j_i^{\top }{\omega }_i{j}_i.\hfill \end{array}$$ (11)

Since the two segments can only rotate independently along the joint axis, it follows that the perpendicular components must have the same magnitude regardless of reference frame

$$\begin{array}{cc}\hfill \parallel {\omega }_{j_1^{\perp }}\parallel & =\parallel {\omega }_{j_2^{\perp }}\parallel .\hfill \end{array}$$ (12)

The magnitude of the perpendicular component can also be computed from the cross product between the angular velocity and the joint axis

$$\begin{array}{cc}\hfill \parallel {\omega }_i-j_i^{\top }{\omega }_i{j}_i\parallel & =\parallel {\omega }_i\times j_i\parallel .\hfill \end{array}$$ (13)

Combining (12) and (13) we formulate the angular velocity constraint

$$\begin{array}{c}\hfill \parallel {\omega }_1\times j_1\parallel -\parallel {\omega }_2\times j_2\parallel =0,\end{array}$$ (14)

which must be satisfied by hinge joint systems.

Looking at the projection of the accelerations onto the joint axis, from (6) we have that

$$\begin{array}{cc}\hfill j_i^{\top }{a}_i{j}_i& =j_i^{\top }{a}_0^{S_i}{j}_i+j_i^{\top }{K}_i{r}_i{j}_i.\hfill \end{array}$$ (15)

Because $j_i$ has the same direction as j in the global frame, it must also be the same for the projection of $a_0$ onto $j_i$, it follows from (5) and (8) that

$$\begin{array}{cc}\hfill {j^G}^{\top }{a}_0^G{j}^G& =R^{G{S}_1}{j}_1{j}_1^{\top }{a}_0^{S_1}=R^{G{S}_2}{j}_2{j}_2^{\top }{a}_0^{S_2}\hfill \\ \hfill & \Rightarrow j_1^{\top }{a}_0^{S_1}=j_2^{\top }{a}_0^{S_2}.\hfill \end{array}$$ (16)

By projecting the accelerations onto the joint axis and subtracting one from the other we get

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill j_1^{\top }{a}_1-j_2^{\top }{a}_2& =j_1^{\top }{a}_0^{S_1}-j_2^{\top }{a}_0^{S_2}+j_1^{\top }{K}_1{r}_1-j_2^{\top }{K}_2{r}_2\hfill \\ \hfill & =j_1^{\top }{K}_1{r}_1-j_2^{\top }{K}_2{r}_2,\hfill \end{array}\end{array}$$ (17)

where we see that only the rotational components of the accelerations remain on the right hand side. The relationship (17) is the exact acceleration constraint of the hinge joint system. The right hand side (r.h.s.) of (17) is zero if and only if either $K_i{r}_i\perp j_i$ or $K_i=0$ are satified for all $i\in \{1,2\}$. It is clear that if the rotational acceleration components along the direction of the joint axis are small ($j_i^{\top }{K}_i{r}_i\approx 0,\forall i$), the r.h.s. will vanish

$$\begin{array}{cc}\hfill j_1^{\top }{a}_1-j_2^{\top }{a}_2& \approx 0,\hfill \end{array}$$ (18)

which forms the approximate acceleration constraint for the hinge joint system.

4. Joint Axis Estimation

We assume that we have two IMUs, one attached to each segment of a hinge joint system. Measurements from a completely unspecified motion has been collected. We will use $y_{\omega ,i}$ to refer to the gyroscope measurements (3) and $y_{a,i}$ to refer to the accelerometer measurements (1) from Sensor $i\in \{1,2\}$. We will use the non-indexed $y_{\omega }$ and $y_a$ to refer to measurements from both sensors as

$$\begin{array}{cc}\hfill y_{\omega }& ={\left[\begin{array}{cc}{y}_{\omega ,1}^{\top }& y_{\omega ,2}^{\top }\end{array}\right]}^{\top },\hfill \end{array}$$ (19)

and similarly for $y_a$. We let ${\mathcal{D}}^N=\{y_{\omega }^N,y_a^N\}$ denote our data, which consists of N samples of recorded motion. Each sample in the data set is assigned a sample index $k\in \{1,\dots ,N\}$, such that $t_k$ refers to the sampling time of the kth measurement relative to the beginning of the recorded motion.

Given the data ${\mathcal{D}}^N$ from the two IMUs, the variables we want to estimate are the unit vectors $j_i$ which corresponds to the directions of the joint axis j in the two sensor frames. We let ${\widehat{j}}_i$ denote the estimate of $j_i$. Note that the joint axis in one sensor frame can be described by either $\pm j_i$ since a clockwise rotation w.r.t. the positive axis is equivalent to a counter-clockwise rotation w.r.t. the negative axis. However, we require both $j_1$ and $j_2$ to have the same sign (direction) to correspond to either $\pm j$ in the global frame, otherwise a clockwise rotation for one sensor might be considered a counter-clockwise rotation for the other sensor and vice versa. That is, the sign pairing of the joint axes in the sensor coordinate frames is important. Consequently, $(\pm j_1,\pm j_2)$ is the correct sign pairing and $(\pm j_1,\mp j_2)$ is the wrong sign pairing.

4.1. Formulating the Optimization Problem

We parametrize $j_i$ using spherical coordinates to enforce the unit vector constraint

$$\begin{array}{cc}\hfill x& ={\left[\begin{array}{cccc}{\theta }_1& {\varphi }_1& {\theta }_2& {\varphi }_2\end{array}\right]}^{\top },\hfill \end{array}$$ (20)

$$\begin{array}{cc}\hfill j_i(x)& =\left[\begin{array}{c}cos{\theta }_{i}cos{\varphi }_i\\ cos{\theta }_{i}sin{\varphi }_i\\ sin{\theta }_i\end{array}\right],\hfill \end{array}$$ (21)

which then become the unknown parameters to estimate. The estimation problem for the joint axis is formulated as

$$\begin{array}{cc}\hfill \widehat{x}& =\underset{x}{arg\; min}V(x),\hfill \end{array}$$ (22)

$$\begin{array}{cc}\hfill V(x)& =\sum _{k=1}^N{\left[e_{\omega }(k,x)\right]}^2+{\left[e_a(k,x)\right]}^2,\hfill \end{array}$$ (23)

where $e_{\omega }(k,x)$ and $e_a(k,x)$ are scalar residual terms, based on the angular velocity constraint (14) and acceleration constraints (18) of the hinge joint system

$$\begin{array}{cc}\hfill e_{\omega }(k,x)& =w_{\omega }[\parallel y_{\omega ,1}(t_k)\times j_1(x)\parallel -\parallel y_{\omega ,2}(t_k)\times j_2(x)\parallel ],\hfill \end{array}$$ (24)

$$\begin{array}{cc}\hfill e_a(k,x)& =w_a[j_1^{\top }(x)y_{a,1}(t_k)-j_2^{\top }(x)y_{a,1}(t_k)].\hfill \end{array}$$ (25)

Two scalars $w_{\omega }$ and $w_a$ are used to change the relative weighting of the residuals.

4.2. Identifiability and Local Minima

For the gyroscope measurements to contain information about the joint axis, they have to be recorded from motions where the joint angle is excited, i.e., when the two segments rotate independently. These motions should contain either simultaneous planar rotations, where the segments rotate simultaneously in the plane perpendicular to the joint axis, or sequential rotations of the segments. However, stiff joint motions, which can have a significant angular rate but no independent rotation of the segments, do not facilitate identifiability of the joint axis [16]. For the non-informative stiff joint motions, the relative rotation of the two sensors can be described by a time-invariant rotation matrix R and we have that

$$\begin{array}{cc}\hfill \parallel {\omega }_2(t_k)\times j_2\parallel & =\parallel R({\omega }_1(t_k)\times j_1)\parallel =\parallel {\omega }_1(t_k)\times j_1\parallel ,\hfill \end{array}$$ (26)

where we see that for any choice of $j_1$, the vector $j_2=R{j}_1$ will minimize the gyroscope residual (24). Therefore, we want motions where $\parallel {\omega }_1(t_k)\parallel \ne \parallel {\omega }_2(t_k)\parallel$, which implies that the segments are rotating independently and we require motions where $\parallel {\omega }_i(t_k)\parallel >0$ for at least some time, since $\parallel {\omega }_i(t_k)\parallel =0\Rightarrow \parallel {\omega }_i(t_k)\times j_i\parallel =0,\forall j_i$.

If only acceleration information is considered, we get the following over-determined system of linear equations

$$\begin{array}{cc}\hfill \underset{=A}{\underbrace{\left[\begin{array}{cc}{a}_1^{\top }(t_1)& -a_2^{\top }(t_1)\\ ⋮& ⋮\\ a_1^{\top }(t_M)& -a_2^{\top }(t_M)\end{array}\right]}}\left[\begin{array}{c}{j}_1\\ j_2\end{array}\right]& =0,\hfill \end{array}$$ (27)

which has a unique solution if $\mathrm{rank}(A)=5$, in which case ${\left[\begin{array}{cc}{j}_1^{\top }& j_2^{\top }\end{array}\right]}^{\top }$ lies in the null-space of A. This holds when the acceleration constraint holds exactly for all $t_k$, the accelerations measured are exact and the angular rate and angular accelerations of the sensors are parallel with j [16]. Therefore, for the accelerometer, we want measurements that increase the separation between the column-space and the null-space of A.

The proposed method uses both gyroscope and accelerometer information, and their relative contribution to the cost function is controlled by the weight parameters $w_{\omega }$ and $w_a$. Figure 2 shows how the weights affect the cost function in the case that $w_a=1$ and $w_{\omega }$ is allowed to vary. For small $w_{\omega }$, the local minima corresponds to the correct sign pairing $(\pm j_1,\pm j_2)$, whereas the local maxima corresponds to the wrong sign pairing $(\pm j_1,\mp j_2)$. Note that each local minimum is equally valid for small $w_{\omega }$ because of the periodicity of the spherical coordinates. The acceleration residuals are relatively large whereas the gyroscope residuals are relatively small at the locations corresponding to the wrong sign pairing. Therefore, as $w_{\omega }$ increases the gyroscope residuals will contribute more to the cost function. The peaks associated with the wrong sign pairing are flattened and new local minima will eventually appear at these locations. Therefore, for large $w_{\omega }$ an optimization method (solver) can end up in the wrong local minimum. However, regardless of which sign pairing the solver finds, the opposite sign pairing can always be obtained at $x={\left[\begin{array}{cccc}{\theta }_1& {\varphi }_1& -{\theta }_2& {\varphi }_2+\pi \end{array}\right]}^{\top }$. Therefore, if our solver finds the estimate ${\widehat{x}}^{(1)}$ we can reinitialize at

$$\begin{array}{c}\hfill {\left[\begin{array}{cccc}{\widehat{\theta }}_1^{(1)}& {\widehat{\varphi }}_1^{(1)}& -{\widehat{\theta }}_2^{(1)}& {\widehat{\varphi }}_2^{(1)}+\pi \end{array}\right]}^{\top },\end{array}$$ (28)

and obtain a new estimate ${\widehat{x}}^{(2)}$. Then we select the local minimum with the smallest value of the cost function as our estimate

$$\begin{array}{cc}\hfill \widehat{x}& =\underset{x\in \{{\widehat{x}}^{(1)},{\widehat{x}}^{(2)}\}}{arg\; min}V(x).\hfill \end{array}$$ (29)

Figure 2.

Shape of the cost function $V(x)$ for a motion with simultaneous planar rotations of the segments. The parameters ${\theta }_1$ and ${\varphi }_1$ are fixed near their true values and ${\theta }_2$ and ${\varphi }_2$ are allowed to vary. From left to right, we see how the geometry changes as $w_{\omega }$ increases while $w_a=1$ is constant. As $w_{\omega }$ increases, new local minima appear near the locations at ${\varphi }_2+\pi$ from the previously existing local minima. These new local minima correspond to the wrong sign pairing $(\pm j_1,\mp j_2)$.

Therefore, it is possible to find the correct sign pairing as long as $V({\widehat{x}}^{(2)})$ is numerically distinguishable from $V({\widehat{x}}^{(1)})$. As discussed in this section and shown in Figure 2, the relative weighting of the residuals determines how easy it is to distinguish a correct local minimum from a wrong one. If $w_{\omega }$ is set to be significantly larger than $w_a$, we expect the acceleration residuals to eventually become so small relative to the gyroscope residuals, that the solver is no longer sensitive enough to detect the difference between correct and wrong local minima.

4.3. Solving the Optimization Problem

The optimization problem (22) is a nonlinear least-squares problem. An efficient solver for such problems is the Gauss–Newton method [34]. Given an initial estimate $\widehat{x}(0)$ the Gauss–Newton method iteratively updates the estimate according to    

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \widehat{x}(k+1)& =\widehat{x}(k)-\alpha {\left(J^{\top }(\widehat{x}(k))J(\widehat{x}(k))\right)}^{-1}{J}^{\top }(\widehat{x}(k))e(\widehat{x}(k))\hfill \\ \hfill & =\widehat{x}(k)-\alpha {\mathsf{\Delta }}_x(k)\hfill \end{array},\end{array}$$ (30)

where k is only used here as an integer index denoting the iterations of the method and is not to be confused with the sample index. The method uses the Jacobian matrix $J(x)\in {\mathbb{R}}^{2N\times 4}$, which contains all first-order partial derivatives of $e_{\omega }$ and $e_a$

$$\begin{array}{cc}\hfill J(x)& =\left[\begin{array}{cccc}\frac{\partial e_{\omega }(1,x)}{\partial {\theta }_1}& \frac{\partial e_{\omega }(1,x)}{\partial {\varphi }_1}& \frac{\partial e_{\omega }(1,x)}{\partial {\theta }_2}& \frac{\partial e_{\omega }(1,x)}{\partial {\varphi }_1}\\ ⋮& ⋮& ⋮& ⋮\\ \frac{\partial e_{\omega }(N,x)}{\partial {\theta }_1}& \frac{\partial e_{\omega }(N,x)}{\partial {\varphi }_1}& \frac{\partial e_{\omega }(N,x)}{\partial {\theta }_2}& \frac{\partial e_{\omega }(N,x)}{\partial {\varphi }_1}\\ \frac{\partial e_a(1,x)}{\partial {\theta }_1}& \frac{\partial e_a(1,x)}{\partial {\varphi }_1}& \frac{\partial e_a(1,x)}{\partial {\theta }_2}& \frac{\partial e_a(1,x)}{\partial {\varphi }_1}\\ ⋮& ⋮& ⋮& ⋮\\ \frac{\partial e_a(N,x)}{\partial {\theta }_1}& \frac{\partial e_a(N,x)}{\partial {\varphi }_1}& \frac{\partial e_a(N,x)}{\partial {\theta }_2}& \frac{\partial e_a(N,x)}{\partial {\varphi }_1}\end{array}\right],\hfill \end{array}$$ (31)

and $e(x)\in {\mathbb{R}}^{2N}$ is the residual vector

$$\begin{array}{cc}\hfill e(x)& ={\left[\begin{array}{cccccc}{e}_{\omega }(1,x)& \dots & e_{\omega }(N,x)& e_a(1,x)& \dots & e_a(N,x)\end{array}\right]}^{\top }.\hfill \end{array}$$ (32)

The term ${\left(J^{\top }(x)J(x)\right)}^{-1}$ is an approximation of the Hessian of $V(x)$, which is given by

$$\begin{array}{c}\hfill \frac{d^{2}V(x)}{d{x}^2}=J^{\top }(x)J(x)+\sum _{k=1}^N{e}_{\omega }(k,x)\frac{d^2{e}_{\omega }(k,x)}{d{x}^2}\\ \hfill +\sum _{k=1}^N{e}_a(k,x)\frac{d^2{e}_a(k,x)}{d{x}^2}\hfill \end{array},$$ (33)

where the higher-order terms are ignored, yielding

$$\begin{array}{c}\hfill \frac{d^{2}V(x)}{d{x}^2}\approx J^{\top }(x)J(x).\end{array}$$ (34)

The partial derivatives of the residuals (24) and (25) in the Jacobian (31) are computed in the following way using the chain rule

$$\begin{array}{cc}\hfill \frac{\partial e_{\omega }(k,x)}{\partial x}& =\frac{\partial j}{\partial x}\frac{\partial e_{\omega }(k,x)}{\partial j}{w}_{\omega }(k),\hfill \end{array}$$ (35)

$$\begin{array}{cc}\hfill \frac{\partial e_{\omega }(k,x)}{\partial j}& =\left[\begin{array}{c}\frac{\partial (\parallel y_{\omega ,1}(t_k)\times j_1(x)\parallel )}{\partial j_1}\\ -\frac{\partial (\parallel y_{\omega ,2}(t_k)\times j_2(x)\parallel )}{\partial j_2}\end{array}\right],\hfill \end{array}$$ (36)

$$\begin{array}{cc}\hfill \frac{\partial (\parallel y_{\omega ,i}(t_k)\times j_i(x)\parallel )}{\partial j_i}& =\frac{(y_{\omega ,i}(t_k)\times j_i)\times y_{\omega ,i}(t_k)}{\parallel y_{\omega ,i}(t_k)\times j_i(x)\parallel },\hfill \end{array}$$ (37)

$$\begin{array}{cc}\hfill \frac{\partial e_a(k,x)}{\partial x}& =\frac{\partial j}{\partial x}\frac{\partial e_a(k,x)}{\partial j},\hfill \end{array}$$ (38)

$$\begin{array}{cc}\hfill \frac{\partial e_a(k,x)}{\partial j}& =\left[\begin{array}{c}{y}_{a,1}(t_k)\\ -y_{a,2}(t_k)\end{array}\right]w_a(k),\hfill \end{array}$$ (39)

$$\begin{array}{cc}\hfill \frac{\partial j}{\partial x}& =\left[\begin{array}{cc}\frac{\partial j_1}{\partial x_1}& 0\\ 0& \frac{\partial j_2}{\partial x_2}\end{array}\right],\hfill \end{array}$$ (40)

$$\begin{array}{cc}\hfill \frac{\partial j_i}{\partial x_i}& ={\left[\begin{array}{cc}-sin{\theta }_{i}cos{\varphi }_i& -cos{\theta }_{i}sin{\varphi }_i\\ -sin{\theta }_{i}sin{\varphi }_i& cos{\theta }_{i}cos{\varphi }_i\\ cos{\theta }_i& 0\end{array}\right]}^{\top }.\hfill \end{array}$$ (41)

The term ${\mathsf{\Delta }}_x$ in (30) defines the search direction, and $-{\mathsf{\Delta }}_x$ is a descent direction, meaning that moving our estimate in that direction will decrease the value of the cost function. The scalar $0<\alpha \le 1$ is known as the step length, which controls how far our estimates move in the descent direction. By using a method known as backtracking line search [35], we find a value for $\alpha$ that is guaranteed to lower the value of the cost function. If no such $\alpha$ is found or the change in the value of $V(x)$ is too small, below a set tolerance level $V_{\mathrm{tol}}$, the Gauss–Newton method terminates and returns the estimate corresponding to the current iteration $\widehat{x}=\widehat{x}(k)$.

The complete joint axis estimation method, including the steps of the Gauss–Newton method and the re-initialization step (28) required to identify the minimum corresponding to the correct sign pairing, is described in Algorithm 1.

Algorithm 1 Joint axis estimation

Require: Data ${\mathcal{D}}^N=\{y_{\omega }^N,y_a^N\}$, initial estimate $\widehat{x}(0)$, tolerance $V_{\mathrm{tol}}$, residual weights $w_{\omega }$ and $w_a$. 1:for $i\in \{1,2\}$ do 2:    $k\leftarrow 0$.                      ▹ Begin Gauss–Newton. 3:    ${\mathsf{\Delta }}_V\leftarrow V_{\mathrm{tol}}$. 4:    $V(0)\leftarrow V(\widehat{x}(0))$.                 ▹$V(x)$ defined by (23). 5:    while ${\mathsf{\Delta }}_V\ge V_{\mathrm{tol}}$ do 6:        Compute the Jacobian $J(\widehat{x}(k))$ and the residuals $e(\widehat{x}(k))$ according to (31) and (32). 7:        ${\mathsf{\Delta }}_x(k)\leftarrow {\left(J^{\top }(\widehat{x}(k))J(\widehat{x}(k))\right)}^{-1}{J}^{\top }(\widehat{x}(k))e(\widehat{x}(k))$. 8:        Obtain step length $\alpha$ using backtracking line search. 9:        $\widehat{x}(k+1)\leftarrow \widehat{x}(k)-\alpha {\mathsf{\Delta }}_x(k)$. 10:        $k\leftarrow k+1$. 11:        $V(k)\leftarrow V(\widehat{x}(k))$. 12:        ${\mathsf{\Delta }}_V\leftarrow |V(k-1)-V(k)|$. 13:    end while 14:    $\widehat{x}\leftarrow \widehat{x}(k)$.                     ▹ End Gauss–Newton. 15:    ${\widehat{x}}^{(i)}={\left[\begin{array}{cccc}{\widehat{\theta }}_1^{(i)}& {\widehat{\varphi }}_1^{(i)}& {\widehat{\theta }}_2^{(i)}& {\widehat{\varphi }}_2^{(i)}\end{array}\right]}^{\top }\leftarrow \widehat{x}$. 16:    $\widehat{x}(0)\leftarrow {\left[\begin{array}{cccc}{\widehat{\theta }}_1^{(i)}& {\widehat{\varphi }}_1^{(i)}& -{\widehat{\theta }}_2^{(i)}& {\widehat{\varphi }}_2^{(i)}+\pi \end{array}\right]}^{\top }$.        ▹ Initialize at $-{\widehat{j}}_2$. 17:end for 18:$\widehat{x}\leftarrow {arg\; min}_{x\in \{{\widehat{x}}^{(1)},{\widehat{x}}^{(2)}\}}V(x)$.               ▹Correct sign pairing. 19:return $j(\widehat{x})$.

5. Sample Selection

A key feature of plug-and-play estimation is that it should not require specific calibration data, recorded from predetermined motions. Rather, such plug-and-play methods should be able to use data recorded from arbitrary motions. Such data sets could be very large, and using all available data for identification is often unnecessary and resource-demanding. It is also possible that very few samples in the data set contain information about the joint axis. In a sense, too much bad information might ruin the good information. To handle this, we propose a method for selecting samples to use for estimation.

In the following sections we assume that we want a maximum of $N_{max}$ gyroscope and accelerometer measurements can be used to identify the joint axis, but that we have $N>N_{max}$ measurements available to us to choose from.

5.1. Gyroscope

To distinguish between informative and non-informative motions, we use the difference in angular velocity magnitude measured by the two gyroscopes    

$$\begin{array}{cc}\hfill {\mathsf{\Delta }}_{\omega }(k)& =\parallel y_{\omega ,1}(t_k)\parallel -\parallel y_{\omega ,2}(t_k)\parallel ,\hfill \end{array}$$ (42)

which is a sufficient metric for detecting independent rotations of the sensors, and hence the two segments. For stationary segments ${\mathsf{\Delta }}_{\omega }(k)=0$. One thing to note is that ${\mathsf{\Delta }}_{\omega }(k)$ cannot differentiate between informative motions where $\parallel {\omega }_1\parallel \approx \parallel {\omega }_2\parallel$ and non-informative stiff joint rotations. For example, the two segments can undergo simultaneous planar rotations, where the two segments rotate in different directions but with approximately the same magnitude. However, for realistic motions, especially for motions performed by humans, it is unlikely that independent rotations will have the same magnitude, even for short moments.

Each gyroscope measurement is given a score

$$\begin{array}{cc}\hfill s_{\omega }(k)& ={\mathsf{\Delta }}_{\omega }(l^{\prime })\hfill \end{array}$$ (43)

$$\begin{array}{cc}\hfill l^{\prime }& =\underset{l}{arg\; min}\left|{\mathsf{\Delta }}_{\omega }(l)\right|,l\in (k-n,k+n)\hfill \end{array}$$ (44)

that is equal to the ${\mathsf{\Delta }}_{\omega }$ with smallest magnitude in a window of $2n+1$ samples. This is to avoid selecting large outliers of ${\mathsf{\Delta }}_{\omega }$. For example, if the system is not completely rigid or the sensors are not rigidly attached, the kinematic constraints are violated, and some samples of stiff joint motion can obtain a large ${\mathsf{\Delta }}_{\omega }$ value. However, if the outliers are relatively few, there should be ${\mathsf{\Delta }}_{\omega }$ with smaller magnitude among neighboring samples. In some sense, $s_{\omega }(k)$ assumes a conservative score for each sample.

When the score $s_{\omega }$ has been computed for all measurements, the list of measurements is sorted in descending order such that $s_{\omega }(k^{\prime })\ge s_{\omega }(k^{\prime }+1),\forall k^{\prime }\in (1,N-1)$, where $k^{\prime }$ is a new index variable used to denote the sorted order. The first and last $N_{max}/2$ of the sorted gyroscope measurements are selected, or, equivalently, the measurements corresponding to the middle of the list, i.e., with index $k^{\prime }\in (N_{max}/2+1,N-N_{max}/2)$ are removed from the set of measurements. By doing this, the algorithm will make sure that measurements with excitation in both sensors are selected, since ${\mathsf{\Delta }}_{\omega }>0$ means that Sensor 1 has larger angular rate than Sensor 2 and vice versa for ${\mathsf{\Delta }}_{\omega }<0$. The gyroscope sample selection method is described in Algorithm 2. In essence, the algorithm picks half the required points from either end of the sorted list.

Algorithm 2 Gyroscope sample selection

Require: Gyroscope data $y_{\omega }^N$, number of allowed measurements, $N_{max}$, window size n. 1:if $N>N_{max}$ then 2:    Compute $s_{\omega }(k),\forall k$ according to (43). 3:    Obtain the sorted order $k^{\prime }$ such that $s_{\omega }(k^{\prime })\le s_{\omega }(k^{\prime }+1),\forall k^{\prime }\in (1,N-1)$. 4:    Remove the $N-N_{max}$ samples $y_{\omega }(t_k),\forall k^{\prime }\in (N_{max}/2+1,N-N_{max}/2)$ from $y_{\omega }$. 5:end if 6:return $y_{\omega }^{N_{max}}$

5.2. Accelerometer

The acceleration constraint is accurate when the angular rate and angular accelerations are small, since that makes the right hand side of (17) vanish. Note that linear acceleration terms in (17), which are collected in $a_0$, always cancel out. Therefore, we do not use the energy of the accelerometer measurements to determine if the acceleration constraint is valid. Instead, we give each acceleration measurement a penalty based on the average angular rate energy

$$\begin{array}{cc}\hfill E_i(k)& =\left\{\begin{array}{cc}\frac{1}{2n+1}\sum _{l=k-n}^{k+n}{\parallel y_{\omega ,i}(t_l)\parallel }^2,\hfill & n<k\le N-n\hfill \\ \infty ,\hfill & \mathrm{otherwise}\hfill \end{array}\right.,\hfill \end{array}$$ (45)

where the average is calculated from a window of size $2n+1$, centered around each sample. This angular rate energy statistic has been shown to be an effective detector of stationarity in foot-mounted inertial navigation [36], so-called zero-velocity detection.

Small $E_i(k)$ indicate that Sensor i is stationary. For the hinge joint system, it is sufficient for one sensor to be stationary since the acceleration components in the plane normal to the joint axis does not change the r.h.s of (17). If one sensor is stationary, then the other sensor can only have accelerations that are induced by independent rotation, which has to be in the plane. For this reason, the penalty given to each pair of acceleration measurements is chosen as

$$\begin{array}{cc}\hfill s_a(k)& =min\{E_1(t_k),E_2(t_k)\}.\hfill \end{array}$$ (46)

As a first step of the accelerometer sample selection, measurements with $s_a(k)>E_{\mathrm{th}}$ are removed, where $E_{\mathrm{th}}$ is a scalar threshold parameter, which should be chosen to remove measurements for which it is likely that the motion violates the acceleration constraint.

We also need to consider the conditions for identifiability of the joint axis. That is, we want our measurements to increase the separation between the column-space and the null-space of the matrix A in (27). In practice, A will have full rank regardless of the motion, since the measurements are corrupted by noise and bias and the acceleration constraint does not hold for arbitrary motions. However, if A has one singular value that is relatively small compared to the other singular values, it can be considered to be approximately rank 5. Consider the singular value decomposition (SVD) of A

$$\begin{array}{c}\hfill A=U\mathsf{\Sigma }{W}^{\top },\end{array}$$ (47)

where the diagonal elements ${\sigma }_1$ to ${\sigma }_6$ of $\mathsf{\Sigma }\in {\mathbb{R}}^{M\times 6}$ are the singular values and the columns of U and W represents orthonormal bases in ${\mathbb{R}}^N$ and ${\mathbb{R}}^6$, respectively. The columns of W are known as the right-singular vectors of A, and each is associated with a corresponding singular value, i.e., if

$$\begin{array}{cc}\hfill \mathrm{diag}(\mathsf{\Sigma })& =\left[\begin{array}{cccccc}{\sigma }_1& {\sigma }_2& {\sigma }_3& {\sigma }_4& {\sigma }_5& {\sigma }_6\end{array}\right],\hfill \end{array}$$ (48)

$$\begin{array}{cc}\hfill W& =\left[\begin{array}{cccccc}{w}_1& w_2& w_3& w_4& w_5& w_6\end{array}\right],\hfill \end{array}$$ (49)

the right-singular vector $w_1$ is associated with ${\sigma }_1$. The singular values are ordered ${\sigma }_1\ge {\sigma }_2\ge \dots \ge {\sigma }_6\ge 0$. We have that $w_1$ is the direction in ${\mathbb{R}}^6$ where the rows of A are most coherent, meaning that

$$\begin{array}{cc}\hfill w_1& =\underset{w,\parallel w\parallel =1}{arg\; max}\left|Aw\right|,\hfill \end{array}$$ (50)

which has the interpretation that $w_1$ is the direction that is most separated from the null-space of A. The information about j that is contained in A is directly linked to the separation between the null-space and the column space of A. The intuition behind this can be seen by comparing the system of linear equations in (27) to the definition of $w_1$ in (50), where it appears most unlikely that j should be parallel with $w_1$. In fact, the least-squares estimator for j given by

$$\begin{array}{cc}\hfill \widehat{j}& =\underset{j}{arg\; min}{\parallel Aj\parallel }^2,\hfill \end{array}$$ (51)

has solutions on the line in ${\mathbb{R}}^6$, which is spanned by $w_6$, the right-singular vector associated with the smallest singular value. If we add the constraints $\parallel j_1\parallel =\parallel j_2\parallel =1$ the two solutions with correct sign pairing, corresponding to $(j_1,j_2)$ and $(-j_1,-j_2)$ can be obtained through normalization. A problem arises when multiple singular values are close to zero, in which case the value of ${\parallel Aj\parallel }^2$ will be small in more than one direction, and the uncertainty in the estimate increases. If A is only allowed to have $N_{max}$ rows, we should therefore only remove measurements whose rows in A are most coherent with $w_1$, the direction with most information. This way, we make sure that space is always allocated for measurements with rows that do not align with $w_1$, which over time should increase the discrepancy between the two smallest singular values and increase the certainty of the least-squares estimator.

The coherence between a row in A and the right-singular vector $w_1$ is computed as the vector $c\in {\mathbb{R}}^M$, with the elements

$$\begin{array}{cc}\hfill c_k& =\frac{|A_k{w}_1|}{\parallel A_k\parallel \parallel w_1\parallel },\hfill \end{array}$$ (52)

$$\begin{array}{cc}\hfill A_k& =\left[\begin{array}{cc}{y}_{a,1}^{\top }(t_k)& -y_{a,2}^{\top }(t_k)\end{array}\right],\hfill \end{array}$$ (53)

where $A_k$ is the $k^{\mathrm{th}}$ row vector in A, and $c_k$ has a value of 1 if $A_k$ is parallel to $w_1$ and 0 if they are orthogonal. A $c_k>0.5$ means that $A_k$ has most of its magnitude in the direction of $w_1$. Therefore, we choose to remove measurements with the largest $s_a(k)$ where $c_k>0.5$. This ensures that we also keep good measurements in the $w_1$ direction, while allocating space for measurements with new information about j. The algorithm for selecting accelerometer samples is described in Algorithm 3.

Algorithm 3 Accelerometer sample selection

Require: Data ${\mathcal{D}}^N=\{y_{\omega }^N,y_a^N\}$, number of allowed measurements $N_{max}$, window size n, threshold $E_{\mathrm{th}}$. 1:if $N>N_{max}$ then 2:    Compute $s_a(k),\forall k$ according to (46) using window size n. 3:    Remove measurements where $s_a(k)>E_{\mathrm{th}}$ from a. 4:    $N\leftarrow |y_a|$. 5:    while $N>N_{max}$ do 6:        Compute the SVD $A=U\mathsf{\Sigma }W$, with A given by (27). 7:        Compute the coherence c according to (52). 8:        Remove the measurement with largest $s_a(k)$ where $c_k>0.5$ from a. 9:        $N\leftarrow |y_a|$.      ▹ A changes in subsequent iterations. 10:    end while 11:end if 12:return $y_a^{N_{max}}$.

5.3. Online Implementation

The two proposed sample selection algorithms can be implemented for an online application. For Algorithm 2, simply save the scores $s_{\omega }$ and re-use them when a new batch of data is available, new $s_{\omega }$ only needs to be computed for the previously unseen measurements. The same principle holds for Algorithm 3 and $s_a$.

6. Uncertainty Quantification

When identifying an unknown quantity, it is useful for the user of the method to know if they can expect their estimate to be accurate given the data that is available, or if more informative data needs to be collected. Here we propose a method for quantifying both local and global uncertainty of an estimate $\widehat{x}$.

The local uncertainty is obtained through estimating the covariance matrix of the estimation errors using the Jacobian of the cost function. Global uncertainty is obtained through solving multiple parallel or sequential optimization problems with different random initializations, then comparing the resulting estimates to see if they correspond to the same joint axis.

The local and global uncertainty metrics are combined into an algorithm that can be used to determine if a current estimate is of acceptable accuracy or if more informative data needs to be collected.

6.1. Local Uncertainty

We approximate the cost function $V(x)$ (23) as a quadratic function near the estimate $\widehat{x}$

$$\begin{array}{c}\hfill \widehat{V}(x)=V(\widehat{x})+\frac{1}{2}{(x-\widehat{x})}^{\top }H(\widehat{x})(x-\widehat{x}),\end{array}$$ (54)

where $H(\widehat{x})\in {\mathbb{R}}^{4\times 4}$ is the approximate Hessian of $V(x)$ evaluated at $\widehat{x}$ according to (34).

We make the assumption that the uncertainty can be captured by a Gaussian distribution. Given the estimate $\widehat{x}$ and the covariance matrix $P_x$, the probability that x is the true parameter vector is given by the probability density function (PDF)

$$\begin{array}{cc}\hfill p(x|\widehat{x},P_x)& =\mathcal{N}(\widehat{x},P_x)=\frac{1}{\sqrt{{(2\pi )}^4\left|P_x\right|}}exp\left(-\frac{1}{2}{(x-\widehat{x})}^{\top }{P}_x^{-1}(x-\widehat{x})\right).\hfill \end{array}$$ (55)

This is the same as assuming the estimation errors $x-\widehat{x}$ to be zero-mean Gaussian with covariance $P_x$. We are interested in finding $P_x$ to quantify the uncertainty of estimates. We now consider the negative log-likelihood of this PDF

$$\begin{array}{cc}\hfill -logp(x|\widehat{x},P_x)& =log\left(\sqrt{{(2\pi )}^4\left|P_x\right|}\right)+\frac{1}{2}{(x-\widehat{x})}^{\top }{P}_x^{-1}(x-\widehat{x}).\hfill \end{array}$$ (56)

Note the similarities to $\widehat{V}(x)$ in (54). If (54) is a good local approximation of the cost function and our estimator is unbiased, the distribution of the estimation errors $x-\widehat{x}$ will be asymptotically ($N\to \infty$) zero-mean Gaussian with covariance matrix [37]

$$\begin{array}{cc}\hfill P_x& \approx {\left(J_s{(\widehat{x})}^{\top }{J}_s(\widehat{x})\right)}^{-1},\hfill \end{array}$$ (57)

where $J_s(\widehat{x})$ is Jacobian from (31) where the partial derivatives of the gyroscope and acceleration residuals have been scaled by $1/\mathrm{std}(e_{\omega }(k,\widehat{x}))$ and $1/\mathrm{std}(e_a(k,\widehat{x}))$, respectively. Here, $\mathrm{std}(e(k,x))$ denotes the sample standard deviation of the residuals

$$\begin{array}{cc}\hfill \mathrm{std}(e(k,x))& =\sqrt{\frac{1}{N-1}\sum _{k=1}^N{\left(e(k,x)-\frac{1}{N}\sum _{k=1}^{N}e(k,x)\right)}^2}.\hfill \end{array}$$ (58)

We want to measure the uncertainty in terms of angular deviation

$$\begin{array}{c}\hfill \mathrm{AD}(v_1,v_2)={cos}^{-1}\left(\frac{v_1^{\top }{v}_2}{\parallel v_1\parallel \parallel v_2\parallel }\right),\end{array}$$ (59)

where $v_1$ and $v_2$ are vectors of the same dimension, $\mathrm{AD}(v_1,v_2)$ returns the positive angle between the two vectors. Let

$$\begin{array}{cc}\hfill z& =h(x)=\left[\begin{array}{c}\mathrm{AD}(j_1(x),j_1(\widehat{x}))\\ \mathrm{AD}(j_2(x),j_2(\widehat{x}))\end{array}\right],\hfill \end{array}$$ (60)

$$\begin{array}{cc}\hfill x_i& ={\left[\begin{array}{cc}{\theta }_i& {\varphi }_i\end{array}\right]}^{\top },\hfill \end{array}$$ (61)

then we want to find the probability distribution of $p(z)$ or its first two moments (mean ${\mu }_z$ and covariance matrix $P_z$).

We use a Monte Carlo method to estimate the mean ${\mu }_z$ and covariance $P_z$ [38]    

$$\begin{array}{cc}\hfill x_l& \sim \mathcal{N}({\mu }_x,P_x),l=1,\dots ,L\hfill \end{array}$$ (62)

$$\begin{array}{cc}\hfill z_l& =h(x_l)\hfill \end{array}$$ (63)

$$\begin{array}{cc}\hfill {\mu }_z& =\frac{1}{L}\sum _{l=1}^L{z}_l\hfill \end{array}$$ (64)

$$\begin{array}{cc}\hfill P_z& =\frac{1}{L-1}\sum _{l=1}^L(z_l-{\mu }_z){(z_l-{\mu }_z)}^{\top },\hfill \end{array}$$ (65)

where we let ${\mu }_x=\widehat{x}$, $P_x$ is obtained as in (57) and $h(x_l)$ is given by (60). The covariance matrix $P_z$ is estimated by the unbiased sample covariance estimator, hence the division by $L-1$.

The metric we will be using to determine local uncertainty is the mean plus two standard deviations, ${\mu }_z+2{\sigma }_z$, where ${\sigma }_z=\sqrt{\mathrm{diag}(P_z)}$.

6.2. Global Uncertainty

The cost function $V(x)$ may have multiple local minima. In the case that the local minima correspond to either the correct or the wrong sign pairing of $j_1$ and $j_2$, we can find the correct one by comparing minima located near the opposite sign of either $j_1$ or $j_2$. If these minima are not distinctly different in terms of the values of $V(x)$, we expect the estimates to have the correct sign half of the times our method finds a solution given that the initial estimates are uniformly spread over the parameter space. Furthermore, in the case where our data has little information about j, there may be other local minima that corresponds to wrong solutions. Wrong local minima can still have low local uncertainty, meaning that if our estimates are initialized near them, it is likely that wrong solutions are found. Therefore, to be confident that the method has found the global minimum, we need to solve the optimization problem multiple times with different initial estimates and compare the angular deviations of the sequential estimates.

We compute estimates ${\widehat{j}}_i^{(t)}$ for $t=1,2,\dots ,T$ as

$$\begin{array}{cc}\hfill {\widehat{j}}^{(t)}& =\left\{\begin{array}{cc}\widehat{j},\hfill & t=1\hfill \\ {arg\; min}_{j\in \{+\widehat{j},-\widehat{j}\}}{min}_{i\in \{1,2\}}AD(j_i,{\widehat{j}}_i^{(t-1)}),\hfill & t>1\hfill \end{array}\right.,\hfill \end{array}$$ (66)

where ${\widehat{j}}_i^{(t)}$ is chosen as either $\pm \widehat{j}$, such that one of the two estimated joint axes ${\widehat{j}}_i$ always has the sign that is most consistent with its previous estimate. Note that this only forces either ${\widehat{j}}_1$ or ${\widehat{j}}_2$ to be consistent with the previous estimate, whereas the other one may still be inconsistent. We then consider the maximum sequential angular deviation as our metric for whether the estimate at time t corresponds to the same minimum as the estimate at time $t-1$

$$\begin{array}{c}\hfill \mathrm{SEQAD}(t)=\left\{\begin{array}{cc}{180}^{\circ },\hfill & t\le 1\hfill \\ \underset{i\in \{1,2\}}{max}\mathrm{AD}({\widehat{j}}_i^{(t)},{\widehat{j}}_i^{(t-1)}),\hfill & t>1\hfill \end{array}\right..\end{array}$$ (67)

The $\mathrm{SEQAD}(t)$ metric corresponds to the angular deviation of the joint axis estimate that is most inconsistent with its previous estimate. Consecutive estimates will differ when there is no clear and consistent global minimum. Therefore, if we observe that $\mathrm{SEQAD}(t)\to 0$ as t increases, we can be more certain that the local minimum found by our solver corresponds to a global minimum.

6.3. Identifying Estimates with Acceptable Uncertainty

Suppose that we receive data sequentially, i.e., we obtain ${\mathcal{D}}^{N(t)}=\{y_{\omega }^{N(t)},y_a^{N(t)}\},\forall t\in \{0,\dots ,T\}$, the sets of $N(t)$ gyroscope and $N(t)$ accelerometer measurements that have been recorded from time $t=0$ to t. If we use sample selection according to Algorithms 2–3, then $N(t)\le N_{max},\forall t$. For each ${\mathcal{D}}^{N(t)}$ we obtain an estimate $\widehat{j}=j(\widehat{x})$ by solving the optimization problem (22). Furthermore, we will select the estimate associated with time t to be ${\widehat{j}}^{(t)}$ as in (66), such that either ${\widehat{j}}_1$ or ${\widehat{j}}_2$ is consistent with the sign of the previous estimate.

We now want to assess if ${\widehat{j}}^{(t)}$ has acceptable uncertainty. Let $E_{max}$ denote the maximum uncertainty that we accept. We use the following two criteria to determine if the local and global uncertainty is sufficiently small

• We require that ${\mu }_z+2{\sigma }_z<E_{max}$, where ${\mu }_z$ and ${\sigma }_z$ are obtained from (64) and (65) through the procedure described in Section 6.1.

• We require that the sequential angular deviations given by (67) satisfy $\mathrm{SEQAD}(t)<E_{max}$ for a minimum of $n_{min}$ consecutive estimates, that were randomly initialized uniformly over the parameter space. This is equivalent to

  $$\begin{array}{c}\hfill \underset{t^{\prime }\in [t-n_{min}+1,t]}{max}\left(\mathrm{SEQAD}(t^{\prime })\right)<E_{max}.\end{array}$$ (68)

We summarize the method for selecting an estimate ${\widehat{j}}^{(t)}$ of acceptable uncertainty in Algorithm 4.

Algorithm 4 Identifying an estimate of acceptable uncertainty

Require: Data ${\mathcal{D}}^{N(t)}=\{y_{\omega }^{N(t)},y_a^{N(t)}\},\forall t\in \{1,\dots ,T\}$, number of Monte Carlo samples L, maximum acceptable uncertainty $E_{max}$, threshold for minimum number of sequential estimates with acceptable deviation $n_{min}$. 1:$n\leftarrow 0$ 2:for $t\in \{1,\dots ,T\}$ do 3:    Obtain an estimate $\widehat{j}=j(\widehat{x})$ by solving the optimization problem (22) using the data ${\mathcal{D}}^{N(t)}$ and Algorithm 1. 4:    Obtain ${\widehat{j}}^{(t)}$ from (66). 5:    Compute the covariance matrix $P_x$ according to (57). 6:    Compute ${\mu }_z$ and $P_z$ according to the Monte Carlo method (62)–(65). 7:    Compute $\mathrm{SEQAD}(t^{\prime }),\forall t^{\prime }\in (t-n_{min}+1,t)$ as in (67). 8:    if ${\mu }_z+2\sigma <E_{max}$ AND $max\{\mathrm{SEQAD}(t^{\prime })\}<E_{max}$ then 9:        return ${\widehat{j}}^{(t)}$. 10:    end if 11:end for

7. Experiment

7.1. Data Acquisition

Data were collected from a 3D printed hinge joint system [39] with one wireless IMU (Xsens MTw) attached to each segment; see Figure 3. The sampling rate was set to 50Hz for both IMUs. The operating ranges of the IMUs were $\pm 160$ m/s² for the accelerometers and $\pm 21$ rad/s for the gyroscopes. The IMUs were attached in sockets such that the joint axis was parallel with the positive y-axes of the sensors and both pointing in the same direction (same sign), that is

$$\begin{array}{cc}\hfill j_1& =j_2={\left[\begin{array}{ccc}0& 1& 0\end{array}\right]}^{\top }.\hfill \end{array}$$ (69)

Figure 3.

The 3D printed hinge joint system, design by Dustin Lehmann, with the two IMUs (orange boxes, $34\times 58$ mm) attached.

The data consist of 14 recorded motions, listed in order below

1. Stationary system;

2. Free rotation, stiff joint, free joint axis;

3. Sequential rotation, horizontal joint axis;

4. Sequential rotation, tilting joint axis;

5. Simultaneous planar rotation, horizontal joint axis;

6. Simultaneous planar rotation, tilting joint axis;

7. Simultaneous free rotation, free joint axis;

8. [8–14] Same motions as 1–7, respectively, but with faster rotations.

The recorded angular velocity magnitudes for these motions are shown in Figure 4. For the sequential rotation, Segment 1 was always rotated first while Segment 2 was stationary, which was followed by the converse motion. Horizontal joint axis means that the joint axis was aligned to be approximately orthogonal to the gravitational acceleration vector. For the tilting joint axis case, the angle between the joint axis and the gravitation acceleration vector was maintained at $\approx {45}^{\circ }$ for the duration of the motion. For the free joint axis motions, the joint axis was not constrained to any particular orientation, but rotated freely in space. The hinge joint system was equipped with a screw, which when tightened prevented independent rotation of the two segments. The screw was tightened when the system was stationary and during the stiff joint rotations. Measurements of the transitions from one motion to another were removed from the recorded data, such that only the specified motions of interest could be isolated. The first set of stationary data was used to estimate the gyroscope bias $b_{\omega }$ in (3), which was then subtracted from all subsequent measurements.

Figure 4.

The angular velocity magnitudes for the 14 different motions that were recorded. The vertical lines and numbered sections indicate when the different motions begin and end.

Because the optimization problem (22) is formulated based on the kinematics at each sample time, and does not contain dynamics, we are allowed to shuffle around the measurements in our data set. Using the 14 different motions, four scenarios were designed where the motions appeared in different sequences. The sequences of motion for the different scenarios were

1. $1,2,3,4,5,6,7,8,9,10,11,12,13,14$;

2. $1,3^{*},{10}^{*},8,2,9,3,10,4,11,5,12,6,13,7,14$;

   ^* only 500 samples (10 seconds) from motions 3 and 10, which contain motion in only Segment 1.

3. $6^{*},1,8,2,9$;

   ^* only 1000 samples from motion 6.

4. $1,8,2^{†},9^{†},6^{*},2^{†},9^{†}$.

   ^* only 1000 samples (20 seconds) from motion 6.

   ^† samples divided in half.

Scenario 1 is the original sequence in which the motions were recorded. Scenario 2 starts with the sensors being stationary, then there is motion in only Segment 1, after which the system alternates between the slower and faster motions, starting with non-informative stiff-joint motions. For this scenario we expect to have good estimates of $j_1$ before we have any excitation in Segment 2. Scenario 3 has early excitation of both segments followed by measurements from a stationary system and non-informative stiff joint motion and Scenario 4 has the converse case where the excitation comes late in the sequence. Scenarios 3 and 4 are also designed to contain more non-informative motions, as the only informative motion is contained in the 1000 samples (20 seconds) from motion 6.

7.2. Evaluating Robustness of the Residual Weighting

To experimentally evaluate the robustness of the proposed method for different weights $w_{\omega }$ and $w_a$, we estimated the joint axis using data from motions 3 to 7 and 10 to 14. Data from a stationary system and rotations with a stiff joint were not used in these evaluations since the joint axis is not identifiable for these motions. The weights were chosen as

$$\begin{array}{cc}\hfill w_{\omega }& =\sqrt{w_0},w_a=\frac{1}{\sqrt{w_0}},\hfill \end{array}$$ (70)

where we let $w_0=\frac{w_{\omega }}{w_a}$ determine the relative weighting of the residuals $e_{\omega }$ and $e_a$. We estimated the joint axis for 100 different values of $w_0$, which had a logarithmic distribution on the interval $({10}^{-3},{10}^{10})$. For all different motions and for each value of $w_0$ the initial estimates of x were selected deterministically such that all possible sign pairings $(\pm j_1,\pm j_2)$ and $(\pm j_1,\mp j_2)$ were selected equally many times. The initial estimates for $j_1$ and $j_2$ were selected from a grid on the unit-sphere of ${\mathbb{R}}^3$ with 6 grid points the positive and negative axes. With all possible combinations for $j_1$ and $j_2$ this resulted in $M=36$ different initial conditions for the optimization algorithm. Here we use the root-mean-square angular error (RMSAE) for both joint axes as the metric to evaluate performance

$$\begin{array}{cc}\hfill \mathrm{RMSAE}& =\sqrt{\frac{1}{2M}\sum _{k=1}^M\mathrm{AD}{(j_1,j({\widehat{x}}_1^{(k)}))}^2+\mathrm{AD}{(j_2,j({\widehat{x}}_2^{(k)}))}^2},\hfill \end{array}$$ (71)

where AD is the angular deviation metric given by (59), and here we let the superscript k denote the estimates obtained from different initializations. Since we consider $(\pm j_1,\pm j_2)$ to be correct sign pairings of the joint axis, we select the sign of ${\widehat{j}}_1$ which has the lowest $AD$. If, as a result of this, ${\widehat{j}}_1$ changes sign, the sign of ${\widehat{j}}_2$ is also changed. This way $AD$ for ${\widehat{j}}_1$ will always be $\le {90}^{\circ }$ whereas the $AD$ for ${\widehat{j}}_2$ can be up to ${180}^{\circ }$, which corresponds to an error in the sign pairing.

7.3. Evaluating Sample Selection

To evaluate the proposed sample selection in Algorithms 2–3, we use the data according to the four scenarios specified in Section 7.1. Starting with the first second of recorded motion and incrementally adding subsequent data in small batches of one second at a time. That is, we receive sequential batches of data ${\mathcal{D}}^{N(t)},\forall t\in \{1,\dots ,T\}$ where T is the duration of the scenario.

We compute one estimate ${\widehat{j}}_i^{(t)}$ for each ${\mathcal{D}}^{N(t)}$. For each new batch, the joint axis is estimated again starting from a new initial estimate (i.e., no warm-start of the optimization method), which is randomly selected from a uniform distribution. The reason for this is that we also want to evaluate if the estimates are consistent over time, regardless of initialization of the optimization method. The relative weighting of the residuals (70) was set to $w_0=50$.

We compare the method when the proposed sample selection is used to the case where all available data is used ($N_{max}=N(t)$) for estimation for all four scenarios. When using the sample selection in Algorithms 2–3, the maximum sample sizes of $N_{max}\in \{1000,500,250,125\}$ were compared.

Other than N, the other user chosen parameters for the sample selection is related to the angular rate energy penalty (45). The window size of $n=21$ samples was used, which means the average energy is computed for $0.42$s of motion for our sensors. The threshold used to determine if the accelerometer is stationary in Algorithm 3, was set to $E_{\mathrm{th}}=1$rad²s⁻². This is around 10 times higher than the threshold used for the angular rate energy detector suggested for zero-velocity detection in human gait [40]. Measurements that we discard in line 2 of Algorithm 3 are therefore likely to be of significant motion.

7.4. Evaluating Uncertainty Quantification

To evaluate the efficacy of the proposed uncertainty quantification, we will use the same sequential batches of data for all four scenarios ${\mathcal{D}}^{N(t)},\forall t\in \{1,\dots ,T\}$ as in Section 7.3, but with a fixed maximum number of samples $N=1000$ chosen by Algorithms 2–3. Similarly to the procedure used to evaluate the sample selection method one estimate ${\widehat{j}}_i^{(t)}$ is obtained for each new batch, and initial estimates are independently randomized from a uniform distribution over the parameter space at each t. The relative weighting of the residuals (70) was set to $w_0=50$.

Here we use Algorithm 4, which returns an estimate $\widehat{j}$, when the local and global criteria indicate that the uncertainty is acceptable. This requires the user to choose the threshold for acceptable uncertainty, $E_{max}$, and the minimum number of sequential estimates that should have angular deviations below this threshold, $n_{min}$. For our evaluation we choose to set $E_{max}=3^{\circ }$, and $n_{min}\in \{3,10\}$. Algorithm 4 is then deemed to be successful if $\mathrm{AD}(j_i,{\widehat{j}}_i)\le E_{max}$. This procedure is repeated 100 times for each scenario, with different randomized initial estimates each time.

7.5. Evaluating Robustness to Sensor Bias

We evaluated the robustness of the complete method, which includes Algorithms 1–4, to measurement bias. The measurement bias refers to $b_a$ and $b_{\omega }$ in the measurement models (1)–(3). In the other evaluations presented here, we have compensated for gyroscope bias by estimating $b_{\omega }$ from the initial stationary data (Motion 1) and subtracting this bias from the subsequent measurements. We have not compensated for any accelerometer bias since it cannot be estimated from only one stationary position of the sensor. In this section, we will study the effect of sensor biases by adding artificially generated biases to both the previously bias-compensated gyroscope measurements and to the accelerometer measurements. These artificial biases have fixed magnitudes $\parallel b_a\parallel =1$ m/s² and $\parallel b_{\omega }\parallel =1^{\circ }/$s, but their directions are randomized by generating random unit vectors. To evaluate the effect of the added artificial bias, $M=100$ estimation runs are performed for all four scenarios, with and without the added artificial bias. The artificial biases are first added to the measurements, then the proposed method is applied as described in Section 7.4. Here, we set $N_{max}=500$, $E_{max}=1^{\circ }$ and $n_{min}=10$, other parameters are the same as in previous sections. We will use the RMSAE metric (71) and the maximum angular error (MAXAE)

$$\begin{array}{cc}\hfill \mathrm{MAXAE}& =\underset{1\le k\le M}{max}\{\mathrm{AD}(j_1,j({\widehat{x}}_1^{(k)})),\mathrm{AD}(j_2,j({\widehat{x}}_2^{(k)}))\},\hfill \end{array}$$ (72)

to evaluate the performance of the method across all $M=100$ estimation rounds for all scenarios.

8. Results

8.1. Robustness

The RMSAE for the different motions and weights $w_0$ are shown in Figure 5. Here, estimation is done separately and using all samples for each motion, i.e., without sample selection. The optimal choice for $w_0$ appears to be in the interval of $({10}^1,{10}^5)$, where the errors are small for all motions.

Figure 5.

Root-mean-square angular error (RMSAE) (71) for different motions and weights $w_0$. Normal speed motions are shown in the top plots and faster motions are shown in the bottom plots. The plots on the right show the same results as the plots to their respective lefts, but zoomed in.

8.2. Sample Selection

The angular errors for ${\widehat{j}}_1$ and ${\widehat{j}}_2$ obtained from testing the proposed sample selection as described in Section 7.3, are shown in Figure 6. From these results, we can compare the use of Algorithms 2–3 for different sample sizes $N_{max}$. This includes the case of $N_{max}=N(t)$, where $N(t)$ corresponds to making use of all samples that have been observed up to an integer t number of seconds.

Figure 6.

Angular errors over time for the four scenarios, see (a–d). Comparing the case $N_{max}=N(t)$, where all samples up to time t are used for estimation to $N_{max}\in \{1000,500,250,125\}$ samples being chosen by Algorithms 2–3 at each integer t seconds. Colored lines define these different cases as given by the legend in the top right. Vertical dashed lines and the numbers 1–14 are used to indicate from which motions (see Section 7.1) the data comes from.

Figure 7 shows which samples were selected for $N_{max}=1000$, for Scenarios 1 and 2 at the times given by the vertical axes.

Figure 7.

The figure shows which samples from Scenario 1 (a) and Scenario 2 (b) that were selected by Algorithms 2–3 with $N_{max}=1000$, at the times given by the vertical axes. Black/white indicates that a sample were selected/not selected respectively. As time increases and more samples become available, we see some previously selected samples being deselected in favor of new samples that are deemed superior by the algorithms. Vertical dashed lines and the numbers 1–14 indicate from which motions (see Section 7.1) the data comes from.

8.3. Uncertainty Quantification

With the parameter $n_{min}=10$, the final errors were below $E_{max}=3^{\circ }$ for all 100 estimation rounds for all four scenarios, meaning the estimates obtained from Algorithm 4 were acceptable $100\%$ of the time. With $n_{min}=3$ and the same $E_{max}$, the estimates were acceptable $8\%$ of the time for Scenario 1, $81\%$ of the time for Scenario 2, $100\%$ of the time for Scenario 3 and $0\%$ of the time for Scenario 4. Figure 8 compares the local and global uncertainty metrics to the angular errors of a single estimation round for each scenario and shows when Algorithm 4 accepted an estimate $\widehat{j}$ for $n_{min}=3$ (leftmost vertical dashed lines) and for $n_{min}=10$ (rightmost vertical dashed lines).

Figure 8.

The plots shows the local and global uncertainty metrics compared to the angular errors (red) for the four scenarios, see (a–d). Local uncertainty is quantified by ${\mu }_z+2{\sigma }_z$, where ${\mu }_z$ is the estimated mean AD (64) and ${\sigma }_z$ is the standard deviation, computed from the estimated covariance matrix (65). Local uncertainty (blue) is shown for both ${\widehat{j}}_1$ and ${\widehat{j}}_2$ for each scenario. Global uncertainty is quantified by (68). The global uncertainty (green) with $n_{min}=10$ is shown for each scenario. Horizontal dashed lines show the accuracy threshold $E_{max}=3^{\circ }$. Vertical dashed lines show when estimates $\widehat{j}$ were accepted by Algorithm 4. For each scenario, the leftmost vertical lines show the case of $n_{min}=3$ and the rightmost vertical lines show the case of $n_{min}=10$, where Algorithm 4 terminates when the estimates have reached the desired accuracy w.r.t. ground truth.

8.4. Robustness to Sensor Bias

The resulting RMSAE and MAXAE for the $M=100$ estimation rounds with and without added artificial bias are shown for all four scenarios in Table 1. Without the added artificial biases, the errors were at most ${2.16}^{\circ }$, and with the added artificial biases of magnitudes $\parallel b_a\parallel =1$ m/s² and $\parallel b_{\omega }\parallel =1^{\circ }/$s the errors were at most ${4.84}^{\circ }$.

Table 1.

Shows the RMSAE (71) and MAXAE (72) after $M=100$ runs with and without artificial bias for the four scenarios.

Scenario	$\parallel {\mathit{b}}_{\mathit{a}}\parallel$ [m/s2]	$\parallel {\mathit{b}}_{\mathit{\omega }}\parallel$ [°/s]	RMSAE [°]	MAXAE [°]
1	0	0	$1.55$	$1.67$
1	1	1	$1.73$	$4.41$
2	0	0	$1.58$	$2.16$
2	1	1	$1.97$	$4.84$
3	0	0	$1.50$	$2.07$
3	1	1	$1.58$	$3.09$
4	0	0	$1.47$	$1.98$
4	1	1	$1.30$	$2.32$

9. Discussion

9.1. The Method Is Not Sensitive to the Relative Weighting $w_0$

The parameter $w_0$, which is defined from (70), controls the relative weighting of the residuals $e_{\omega }$ and $e_a$. As $w_0$ increases, the relative weighting of the gyroscope residual is increased. As we see in Figure 5, the optimal choice of $w_0$ for most motions in terms of RMSAE (71), is somewhere in the large range between 10 and ${10}^5$. The errors are also small ($<3^{\circ }$) for $w_0<10$ for the slower planar motions (3–6), which shows that the acceleration information can be reliable for these motions. However, some larger errors can be observed for small $w_0$ for the faster planar motion 12 and the errors are also significantly larger for the free axis rotations (motions 7 and 14), which can be explained by the fact that these motions violate the acceleration constraint, meaning that the r.h.s. of (17) is nonzero.

Since we can select any $w_0$ from within such a large interval and still obtain similar performance, our method is not sensitive to the relative weighting of the residuals. It makes sense that $w_0>10$, since the angular velocities, measured in rad/s, have smaller magnitudes than the accelerations, that typically fluctuate around $9.82$ m/s² due to the gravitational acceleration. Furthermore, the angular velocity constraint always holds for a rigid hinge joint system. Hence, we expect the angular velocity information to be more reliable. The method is robust for larger $w_0$ up to ${10}^5$ where the RMSAE become large for motions 3 and 10. This large increase in RMSAE occurs when the acceleration residual becomes numerically indistinguishable to the tolerance of the optimization algorithm, and it becomes more likely that the method selects an estimate which corresponds to the wrong sign pairing. Therefore, as $w_0$ increases we see the RMSAE approach ${90}^{\circ }$ as the AD for ${\widehat{j}}_1$ is still small but the probability of selecting $\pm {\widehat{j}}_2$ is approaching $0.5$, meaning that approximately half of the estimates will have the wrong sign pairing. This can also depend on the numerical tolerance and stopping criteria of the optimization method, since a global minimum corresponding to the correct sign pairing might not be significantly different from other local minima that correspond to the wrong sign pairing.

9.2. Sample Selection Offers Substantial Benefits

From the results shown in Figure 6 we see that we can achieve similar, and in some cases even better performance by selecting relatively few measurements to use for estimation out of all $N(t)$ measurements that have been observed up to time t. For Scenario 1, $N_{max}\in \{1000,500,250\}$ have angular errors within ${0.5}^{\circ }$ and $N=125$ have errors within $1^{\circ }$ from the case with $N_{max}=N(t)$.

In Scenario 2, the errors for ${\widehat{j}}_2$ drop below $2^{\circ }$ at $t=85$s for the methods using sample selection, but it takes until $t=135$s for the method where $N_{max}=N(t)$ to stay consistently below $2^{\circ }$. However, the method with $N=125$ again shows a slightly larger deviation from the others, with some momentary spikes in error around $t=200$s and $t=300$s. Note that Scenario 2 is designed to have no independent rotation of Segment 2 until $t=255$s. So the only information about $j_2$ until then has to come from the accelerometer. This shows that carefully selecting accelerometer samples according to Algorithm 3 is beneficial, especially if angular velocity information is missing. Comparing the results from Scenarios 1 and 2, the final errors are very similar, indicating that the methods are not sensitive to the sequence of motions.

Scenarios 3 and 4 represent challenging cases where only a small minority of samples contain motion with independent rotation (only 20s of motion 6). In Scenario 3, we note that the final error for $N_{max}=N(t)$ is significantly larger than the cases with $N_{max}\in \{1000,500,250\}$, and for $N_{max}=125$ the final error for ${\widehat{j}}_2$ is at the same level as $N_{max}=N(t)$. Scenario 4 has a similar performance in terms of final errors. However, Scenario 4 does not have any motions with independent rotations of the segments until around $t=180$s. The only information about the joint axis until that point comes from the accelerometer, in Motions 1, 8, 2 and 9. The errors start to decrease around $t=150$s when data from Motion 9 comes in, but do not settle until after Motion 6. The large fluctuations in errors we see for $N_{max}=1000$ during Motion 9 indicate that there are still at least two local minima corresponding to the wrong joint axis at this point. Errors for $N_{max}<1000$ are smaller during motion 9, but still vary between $5^{\circ }$ and ${20}^{\circ }$.

Using Algorithms 2–3 is therefore beneficial, not only for reducing the computational complexity of the optimization problem, but it can even improve the performance in situations where gyroscope information is limited. However, judging by Scenario 4 in particular, $N_{max}\ge 1000$ appears to be the best choice in terms of overall performance. Even then, $N_{max}=1000$ is only a small fraction of the total number of measurements. With a sample period of $0.02$s, we have that $N(t=700)=35000$ and $N(t=250)=12500$.

Figure 7 shows the samples that were selected over time from Scenarios 1 and 2 with $N_{max}=1000$ and which motions these samples come from. For both scenarios, we see that gyroscope samples from non-informative motions 1,2,8,9 are all deselected by the end. Samples from these motions are only kept until enough samples from informative motions have been parsed by the algorithm. For the accelerometer, we see that samples from stationary sensors are preferred since many samples of motions 1 and 8 are kept, which is in line with the penalty we give samples based on the angular rate energy. It is also important that samples from other motions are selected since the criterion for identifiability requires a strong separation between the nullspace and the column space of the matrix A given by (27). Had the selection criterion of the accelerometer only been based on the angular rate energy, we would risk ending up in the situation where all samples are selected from the same stationary position, in which case all rows of A are linearly dependent. Lines 5–10 in Algorithm 3 prevent this by removing the worst samples that are coherent with the right-singular vector of the largest singular value. This can be thought of as allocating space in the A matrix for novel information by removing redundant information.

9.3. Reliability of the Proposed Uncertainty Quantification

We obtained reliable estimates with errors below that of the maximum acceptable error $E_{max}=3^{\circ }$, $100\%$ of the time when the parameter $n_{min}=10$. However, estimates were not reliable for $n_{min}=3$, where the results were particularly bad for Scenario 1, with $8\%$ of estimates of acceptable error and for Scenario 4 with $0\%$ of estimates of acceptable error. Both of these scenarios contained no informative motions in the beginning, and we found that the estimates that were returned often had not used any batch of informative data for estimation because the criteria for local and global uncertainty were satisfied prematurely by Algorithm 4.

In Figure 8 it can be seen that local uncertainty metric ${\mu }_z+2{\sigma }_z$ can be below $E_{max}$ (horizontal dashed lines) while the actual angular error fluctuates between values below and above $E_{max}$. This occurs when there exist multiple other local minima than those corresponding to the true joint axis. Furthermore, Figure 8 shows how the SEQAD remains large as the angular errors fluctuate in the same way as the angular errors. Interestingly, Scenario 2 appears to fluctuate between one correct and one wrong local minimum between $t=66$s and $t=82$s. If we assume that the probability of finding the correct local minimum is $0.5$, having $n_{min}=3$ that means that the probability of ending up in the wrong local minimum $n_{min}$ times in a row is ${0.5}^{n_{min}}=12.5\%$. This matches well with the results obtained for Scenario 2, where $88\%$ of the estimates were acceptable for $n_{min}=3$.

For Scenarios 1 and 4, where the results were significantly worse for $n_{min}=3$, it appears that wrong local minima were dominating. These two scenarios have sequences of stiff-joint motion before any informative motions are observed, which can explain why wrong local minima were found more frequently. Scenario 3, which had informative motion in the beginning did not have this issue, and hence $100\%$ of the estimates were acceptable even for $n_{min}=3$.

We can conclude that setting the parameter $n_{min}$ sufficiently large is important for fully capturing the global uncertainty. Sequential data dominated by non-informative motions in the beginning are more sensitive to the choice of $n_{min}$. The results showed that Algorithm 4 successfully identified all of the estimates that satisfied the accuracy criteria $E_{max}=3^{\circ }$ when $n_{min}=10$. Here, this corresponds to 10 consecutive estimates (computed once per second), that differed by $3^{\circ }$ at most.

9.4. The Method Is Robust to Realistic and Uncompensated Sensor Bias

As shown in Table 1, even with added artificial biases of relatively large magnitudes $\parallel b_a\parallel =1$m/s² and $\parallel b_{\omega }\parallel =1^{\circ }/$s, the errors were at most ${4.84}^{\circ }$ across all $M=100$ estimation runs for all four scenarios. The average errors in terms of RMSAE were less than $2^{\circ }$ even with the added artificial biases. As a comparison, the IMUs used in our experiments had bias magnitudes in the order of $\parallel b_a\parallel =0.1$ m/s² and $\parallel b_{\omega }\parallel ={0.5}^{\circ }/$s, so the artificial biases were significantly larger. This shows that the method is robust to sensor biases of at least these magnitudes. However, we had to lower the threshold $E_{max}$ from $3^{\circ }$ to $1^{\circ }$ to achieve this. This means that Algorithm 4 will be more conservative in selecting an estimate. With added artificial bias and $E_{max}=3^{\circ }$, the method would sometimes terminate prematurely, when no informative motion had been observed because a global minimum that satisfied this threshold value was found. Therefore, lowering $E_{max}$ was required to achieve robustness to the added artificial biases. It is therefore still highly recommend that pre-calibration of the biases is performed when possible. If bias drift is significant enough to exceed the magnitudes tested here across the duration of the experiment, it is advised to use a method that allows for online compensation of biases alongside the proposed method. Lowering $E_{max}$ is only an optional measure one would take in the unusual case where late bias occurs and is not compensated for.

10. Conclusions

We have proposed a method which facilitates plug-and-play sensor-to-segment calibration for two IMUs attached to the segments of a hinge joint system. The method identifies the direction of the joint axis j in the intrinsic reference frames of each sensor, thus providing the user with information about the sensors’ orientation with respect to the joint. Accurate sensor-to-segment calibration is crucial for tracking the motion of the segments.

The method was experimentally validated on data collected from a mechanical joint, which performed a wide range of motions with different identifiability properties. As soon as sufficiently informative data was available, the method achieved a sensor-to-segment calibration accuracy in the order of $2^{\circ }$, assessed as the angular deviation from the ground truth of the joint axis.

The proposed method includes the following features that were evaluated separately using the experimental data:

• Gyroscope and accelerometer information are weighted and combined, which makes the joint axis identifiable for a wider range of different motions. Experimental evaluation showed that the method is not sensitive to the weighting parameters, and that it performs comparably well for a wide range of different motions across a large interval of weights.

• A method to select a smaller subset of samples to use from a long sequence of recorded motion is proposed. Samples are selected from motions that yield identifiability, and measurements of non-informative motions are automatically discarded. The experimental evaluation showed that using between 125 and 1000 samples can achieve similar and in some cases even better performance than using all available samples collected from a long sequence of motions. Sample selection was shown to be particularly beneficial when data consisted of more non-informative than informative motions. Furthermore, using less samples for estimation reduces the computational complexity of the estimation.

• A method to quantify local and global uncertainty properties of sequential estimates, which provides the user with an estimate when criteria for acceptable uncertainty are met. The method successfully identified estimates that satisfied the uncertainty criteria ($E_{max}=3^{\circ }$).

The proposed method is the first truly plug-and-play calibration method that directly enables plug-and-play motion tracking in hinge joints. For the first time, the user can simply start using the sensors instead of performing precise or sufficiently informative motion in a predefined initial time window, and the proposed method provides reliable calibration parameters as soon as possible, which immediately enable calculation of accurate motion parameters from the incoming raw data as well as from already recorded data. Regardless of the performed motion, it provides only parameters that are actually accurate, which is not guaranteed by any state of the art method. This enables the kind of truly non-restrictive and reliable motion tracking that is needed in a range of application domains including ubiquitous motion assessment to wearable biofeedback systems.

In future work, the method could be extended to different joint types and be applied to motion tracking in mechatronic and biomechanical systems. For the latter case in particular, it would be of great interest to study the reliability of the method in non-rigid systems, such as human limbs, where motion of soft tissue is significant.

Author Contributions

Conceptualization, F.O., M.K., T.S. and K.H.; Data curation, F.O. and T.S.; Formal analysis, F.O.; Funding acquisition, K.H.; Investigation, F.O. and T.S.; Methodology, F.O., M.K., T.S. and K.H.; Software, F.O.; Supervision, M.K., T.S. and K.H.; Validation, F.O.; Visualization; F.O.; Writing—original draft, F.O. and T.S.; Writing—review and editing, F.O., M.K., T.S. and K.H. All authors have read and agreed to the published version of the manuscript.

Funding

This work was supported by the project “Mobile assessment of human balance” (Contract number: 2015-05054), funded by the Swedish Research Council.

Conflicts of Interest

The authors declare no conflict of interest.