IMU/UWB Fusion Method Using a Complementary Filter and a Kalman Filter for Hybrid Upper Limb Motion Estimation

Abstract

Motion capture systems have enormously benefited the research into human–computer interaction in the aerospace field. Given the high cost and susceptibility to lighting conditions of optical motion capture systems, as well as considering the drift in IMU sensors, this paper utilizes a fusion approach with low-cost wearable sensors for hybrid upper limb motion tracking. We propose a novel algorithm that combines the fourth-order Runge–Kutta (RK4) Madgwick complementary orientation filter and the Kalman filter for motion estimation through the data fusion of an inertial measurement unit (IMU) and an ultrawideband (UWB). The Madgwick RK4 orientation filter is used to compensate gyroscope drift through the optimal fusion of a magnetic, angular rate, and gravity (MARG) system, without requiring knowledge of noise distribution for implementation. Then, considering the error distribution provided by the UWB system, we employ a Kalman filter to estimate and fuse the UWB measurements to further reduce the drift error. Adopting the cube distribution of four anchors, the drift-free position obtained by the UWB localization Kalman filter is used to fuse the position calculated by IMU. The proposed algorithm has been tested by various movements and has demonstrated an average decrease in the RMSE of 1.2 cm from the IMU method to IMU/UWB fusion method. The experimental results represent the high feasibility and stability of our proposed algorithm for accurately tracking the movements of human upper limbs.

1. Introduction

Motion capture (MoCap) systems provide technical support for the operation of space robots, which has enormously benefited the research into human–computer Interaction in the aerospace field. By capturing human upper limb movement and mapping it to space robot motion, space operators can remotely control space robot arms and allow them to perform complex and elaborate actions, such as grasping and handling. Thus, we need to accurately estimate the movement of human upper limbs [1,2,3]. Optical Mocap systems employ multiple cameras to capture the positions of reflective markers and have been widely used in human motion tracking. Although these systems share precise real-time tracking and strong anti-interference abilities, they are expensive and suffer from light and occlusion problems [4,5]. In order to flexibly track human movements, it is necessary to employ ubiquitous wearable sensors.

With the intensive study and rapid advancement of micro-electro-mechanical systems, wearable inertial measurement units (IMUs) have been extensively adopted for 3-D human motion tracking due to their low cost, portability, and high independence [6]. In many of the proposed IMU Mocap methods, accelerometers and gyroscopes are fused to estimate the body segment orientation [7,8], including the knee joint angle [9,10], arm adduction angle [11], and shoulder and elbow joint angles [12,13]. However, it has been illustrated that there is obvious drift error of the attitude angle calculated by the gyroscope, which contributes to the poor estimation accuracy and stability. Considering that the accelerometers can only compensate for the tilt angles relative to the direction of gravity, it is necessary to introduce magnetometers to provide compensation for the yaw angles, which is known as nine-axis IMU or the magnetic, angular rate, and gravity (MARG) system [14]. Data fusion of three inertial sensors can be achieved by adopting either a complementary filter [14,15,16,17] or a classic Kalman filter [18,19]. The Kalman filter has been widely used as an orientation filter for the majority of the proposed methods. Wu et al. [20,21,22,23] employed a particle filter and an unscented Kalman filter for information fusion and developed a micro-sensor Mocap system to achieve real-time tracking. Roetenberg et al. [24] designed a Kalman filter and improved the orientation estimation through the compensation of magnetic disturbances. Considering that the Kalman filter requires knowledge of noise distribution and is prone to computational load and parameter tuning issues, many researchers tend to use the complementary filter. Fourati et al. [25] proposed a complementary observer based on the quaternion method for motion tracking. However, only adopting inertial sensors fusion for motion estimation will still cause drift, so it is expected that a stable and high-precision technology will be introduced to fuse with IMU.

Considering the flexibility and high sampling rate of IMU, ultrawideband (UWB) technology is attractive for data fusion due to its low cost, portability, and low energy consumption [26,27], but it requires a clear line-of-sight (LOS) channel [28]. Compared with traditional UWB positioning methods consisting of maximum likelihood estimation (MLE), linearized least square estimation (LLSE), and weighted centroid estimation (WCE), the extended Kalman filter (EKF) performs with less computational time and less complexity [29]. Applying UWB and IMU fusion for human motion tracking can not only compensate for the low sampling rate of the UWB system, but reduce the drift of IMU as well. Depending on whether the fusion is based on raw time or location, data fusion methods are divided into tightly coupled [30] and loosely coupled [31,32] categories. Kok et al. [33] provided tightly coupled IMU and UWB fusion using an optimization method that showed good performance in terms of handling outliers, but suffered from clock skew challenges. Therefore, in order to facilitate the calculation, the loosely coupled method is often employed. Zihajehzadeh et al. [32,34,35] proposed a Kalman-filter-based IMU and UWB fusion method without a magnetometer and accurately captured lower body motion under magnetic disturbances. However, when extending the rotation matrices to the upper limb, their algorithm may suffer from the singularity problem. Zhang et al. [36] adopted a Mahony filter and quaternion for foot attitude estimation via IMU and UWB fusion, but the use of acceleration double integration to obtain the position would lead to huge cumulative errors.

In this paper, the human upper limb is taken as our research subject, and a novel IMU/UWB data fusion method is proposed for 3-D motion estimation by applying the Runge–Kutta Madgwick filter, the UWB localization Kalman filter, and the IMU/UWB Kalman filter. On the basis of the established kinematics model of the upper limbs, the quaternion method was employed to calculate the attitude angle to avoid the gimbal lock problem. Our proposed algorithm comprises the following two novel aspects: (1) we combined the Madgwick RK4 complementary orientation filter and Kalman filter for motion tracking, the Madgwick RK4 filter was employed to reduce gyroscope drift without leading to noise distribution, and the Kalman filter was implemented for UWB localization and fusion with known error distribution from the UWB system; and (2) the drift-free position obtained by the UWB localization system was used to fuse the position calculated by IMU for upper limb motion estimation, which enormously reduced the drift caused by the double integration of acceleration. The good experimental results represent the high feasibility and stability of our proposed algorithm.

The rest of the paper is structured as follows. In Section 2, the theoretical fusion method for 3-D upper limb tracking is described. The experimental setup and protocol are demonstrated in Section 3. Then, the experimental results are shown and analyzed in Section 4. Finally, conclusions are provided in Section 5.

2. Theoretical Method

In this section, the proposed information fusion algorithm for human upper limb motion estimation is described. As shown in Figure 1, we first simplified the human upper limb as a kinematics model with three joints (shoulder, elbow, and wrist) and two segments (upper arm and forearm) [21]. The shoulder joint was set as a fixed point with three degrees of freedom (DoFs) and two DoFs for the elbow and wrist [37]. Taking the right arm as an example, two IMU sensors were arranged on the lateral side above the wrist and the elbow, respectively, as well as two UWB tags. The navigation coordinate frame (n) was set as the reference system, which was consistent with the coordinate system of UWB and MoCap. The body frame (b) was attached to each body segment where the sensors were located. It aligned with the n-frame initially. Neglecting the installation errors, the sensor frame (s) was attached to each IMU and aligned with the b-frame to track the movements of human upper limbs.

The overall framework of the proposed algorithm consists of three parts, illustrated in Figure 2, including (1) Quaternion RK4 Based Madgwick Orientation Complementary Filter, (2) UWB localization Kalman filter, and (3) IMU/UWB Fusion Kalman filter. For the purpose of estimating the 3-D spatial trajectory of the movements of human upper limbs, we first employed the quaternion method to calculate the attitude angle using gyroscope measurements. However, the integration of gyroscope measurement errors contributed to an accumulating error in the quaternion algorithm [14]. Therefore, we considered adopting the Madgwick orientation filter to improve the motion estimation accuracy through the optimal fusion of the accelerometer, gyroscope, and magnetometer of the MARG system. Then, in order to further reduce the drift error, we proposed that the UWB localization system be combined with the IMU sensors. An extended Kalman filter was utilized to fuse IMU and UWB in order to perform stable and high-precision motion estimation of the human upper limbs.

2.1. Quaternion RK4 Based Madgwick Orientation Complementary Filter

2.1.1. Fourth-Order Runge–Kutta-Based Quaternion Update Algorithm

Rotation transformation can be described as a vector rotating around a specified rotation axis with a certain angle in a coordinate system. The unit quaternion is defined as

$$\mathit{q}=q_1+\mathit{i}{q}_2+\mathit{j}{q}_3+\mathit{k}{q}_4$$

(1)

For the same vector ${}^n\mathit{r}$ and ${}^b\mathit{r}$, defined in the n-frame and b-frame, respectively, ${}^n{\mathit{r}}^{\prime }$ and ${}^b{\mathit{r}}^{\prime }$ are their extended forms, containing a 0 inserted as the real part [14]. ${}_b{}^n\mathit{q}$ describes the rotation of the n-frame relative to the b-frame, so ${}^n{\mathit{r}}^{\prime }$ can be expressed as

$$\begin{array}{c}{}^n\mathit{r}^{\prime }={}_b{}^n\mathit{q}\otimes {}^b\mathit{r}^{\prime }\otimes {}_b{}^n\mathit{q}^{\ast }={}_b{}^n\mathit{q}\otimes \left[M^{\prime }\left({}_b{}^n\mathit{q}^{\ast }\right){}^b\mathit{r}^{\prime }\right]=M\left({}_b{}^n\mathit{q}\right)M^{\prime }\left({}_b{}^n\mathit{q}{}^{\ast }\right){}^b\mathit{r}^{\prime }\\ =\left[\begin{array}{cccc}{q}_1& -q_2& -q_3& -q_4\\ q_2& q_1& -q_4& q_3\\ q_3& q_4& q_1& -q_2\\ q_4& -q_3& q_2& q_1\end{array}\right]\left[\begin{array}{cccc}{q}_1& q_2& q_3& q_4\\ -q_2& q_1& -q_4& q_3\\ -q_3& q_4& q_1& -q_2\\ -q_4& -q_3& q_2& q_1\end{array}\right]\left[\begin{array}{c}0\\ x\\ y\\ z\end{array}\right]\end{array}$$

(2)

$${}_b{}^n\mathit{q}\otimes {}^b{\mathit{r}}^{\prime }=\left[\begin{array}{cccc}{q}_1& -q_2& -q_3& -q_4\\ q_2& q_1& -q_4& q_3\\ q_3& q_4& q_1& -q_2\\ q_4& -q_3& q_2& q_1\end{array}\right]\left[\begin{array}{c}0\\ x\\ y\\ z\end{array}\right]$$

(3)

where ${\mathit{q}}^{\ast }$ is the conjugate of $\mathit{q}$; the symbol $\otimes$ denotes the quaternion product [21], as defined in (3); $M\left({}_b{}^n\mathit{q}\right)$ is the left multiplication matrix of the quaternion ${}_b{}^n\mathit{q}$; and $M^{\prime }\left({}_b{}^n\mathit{q}{}^{\ast }\right)$ is the right multiplication matrix of the conjugate quaternion ${}_b{}^n\mathit{q}{}^{\ast }$. Furthermore, the rotation can be represented by a rotation matrix.

$${}^n\mathit{r}={}_b{}^n\mathit{C}{}^b\mathit{r}$$

(4)

where ${}_b{}^n\mathit{C}$ represents the rotation of the n-frame relative to the b-frame.

$${}_b{}^n\mathit{C}=\left[\begin{array}{ccc}(q_1{}^2+q_2{}^2-q_3{}^2-q_4{}^2)& 2(q_2{q}_3-q_1{q}_4)& 2(q_2{q}_4+q_1{q}_3)\\ 2(q_2{q}_3+q_1{q}_4)& (q_1{}^2-q_2{}^2+q_3{}^2-q_4{}^2)& 2(q_3{q}_4-q_1{q}_2)\\ 2(q_2{q}_4-q_1{q}_3)& 2(q_3{q}_4+q_1{q}_2)& (q_1{}^2-q_2{}^2-q_3{}^2+q_4{}^2)\end{array}\right]$$

(5)

The quaternion update algorithm uses the angular velocity increment in the sample period measured by the IMU sensors to calculate the quaternion at each time in order to update the human motion data. The quaternion derivative is given by

$$\begin{array}{l}{}_b{}^n\dot{\mathit{q}}=\frac{1}{2}{}_b{}^n\mathit{q}\otimes {}^b\mathit{\omega }=\frac{1}{2}M\left({}_b{}^n\mathit{q}\right){}^b\mathit{\omega }=\frac{1}{2}{M}^{\prime }\left({}^b\mathit{\omega }\right){}_b{}^n\mathit{q}\\ =\frac{1}{2}\left[\begin{array}{cccc}{q}_1& -q_2& -q_3& -q_4\\ q_2& q_1& -q_4& q_3\\ q_3& q_4& q_1& -q_2\\ q_4& -q_3& q_2& q_1\end{array}\right]\left[\begin{array}{c}0\\ {}_b\omega {}_x^{}\\ {}_b\omega {}_y^{}\\ {}_b\omega {}_z^{}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{}_b\omega {}_x^{}& -{}_b\omega {}_y^{}& -{}_b\omega {}_z^{}\\ {}_b\omega {}_x^{}& 0& {}_b\omega {}_z^{}& -{}_b\omega {}_y^{}\\ {}_b\omega {}_y^{}& -{}_b\omega {}_z^{}& 0& {}_b\omega {}_x^{}\\ {}_b\omega {}_z^{}& {}_b\omega {}_y^{}& -{}_b\omega {}_x^{}& 0\end{array}\right]\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\\ q_4\end{array}\right]\end{array}$$

(6)

where ${}^b\mathit{\omega }={\left[\begin{array}{cccc}0& {}^b\omega {}_x& {}_b\omega {}_y& {}_b\omega {}_z\end{array}\right]}^T$ is the angular velocity measured by the gyroscope.

Given the initial value and the rotational angular velocity, the orientation at each time ${}_b{}^n\mathit{q}{}_{\omega ,t}$ can be obtained by numerically integrating the quaternion derivative ${}_b{}^n\dot{\mathit{q}}{}_{\omega ,t}$.

$${}_b{}^n\mathit{q}{}_{\omega ,t}={}_b{}^n\widehat{\mathit{q}}{}_{est,t-1}+{}_b{}^n\dot{\mathit{q}}{}_{\omega ,t}\Delta t={}_b{}^n\widehat{\mathit{q}}{}_{est,t-1}+\frac{1}{2}\Delta t{}_b{}^n\widehat{\mathit{q}}{}_{est,t-1}\otimes {}^b\mathit{\omega }{}_t$$

(7)

where $\Delta t$ is the sampling period, ${}_b{}^n\widehat{\mathit{q}}{}_{est,t-1}$ is the estimation of the quaternion at the previous time, and ${}_b\mathit{\omega }{}_t$ is the angular velocity at time t.

Although increasing the order of the above algorithm can improve the computational accuracy, the complexity is also increasing. The Runge–Kutta method is a high-precision single-step algorithm; the most classic and widely used is the fourth-order Runge–Kutta algorithm. It can perform iterative operations on definite solution problems with known initial values and equations without solving differential equations, which enormously reduces the computational complexity. In this paper, the calculation of human upper limb motion can be regarded as the initial value problem of a differential equation. The calculation formulas are as follows.

$$\begin{array}{l}\Delta {}_b{}^n\dot{\mathit{q}}{}_1=\frac{1}{2}{}_b{}^n\widehat{\mathit{q}}{}_{est,t-1}\otimes {}^b\mathit{\omega }{}_{t-1}\\ \Delta {}_b{}^n\dot{\mathit{q}}{}_2=\frac{1}{2}\left({}_b{}^n\widehat{\mathit{q}}{}_{est,t-1}+\frac{\Delta {}_b{}^n\dot{\mathit{q}}{}_1}{2}\right)\otimes {}^b\mathit{\omega }{}_{t-1/2}\\ \Delta {}_b{}^n\dot{\mathit{q}}{}_3=\frac{1}{2}\left({}_b{}^n\widehat{\mathit{q}}{}_{est,t-1}+\frac{\Delta {}_b{}^n\dot{\mathit{q}}{}_2}{2}\right)\otimes {}^b\mathit{\omega }{}_{t-1/2}\\ \Delta {}_b{}^n\dot{\mathit{q}}{}_4=\frac{1}{2}\left({}_b{}^n\widehat{\mathit{q}}{}_{est,t-1}+\Delta {}_b{}^n\dot{\mathit{q}}{}_3\right)\otimes {}^b\mathit{\omega }{}_t\\ {}_b{}^n\mathit{q}{}_{\omega ,t}={}_b{}^n\widehat{\mathit{q}}{}_{est,t-1}+\frac{1}{6}\Delta t\left(\Delta {}_b{}^n\dot{\mathit{q}}{}_1+2\Delta {}_b{}^n\dot{\mathit{q}}{}_2+2\Delta {}_b{}^n\dot{\mathit{q}}{}_3+\Delta {}_b{}^n\dot{\mathit{q}}{}_4\right)\end{array}$$

(8)

where ${}^b\mathit{\omega }{}_{t-1/2}$ is the angular velocity at the intermediate time between t − 1 and t.

The quaternion fourth-order Runge–Kutta algorithm is designed to interpolate in the integration interval, and the slope is iteratively optimized at each step of the calculation to obtain an updated value.

2.1.2. Madgwick RK4 Orientation Complementary Filter for MARG

By formulating an objective function, this filter fuses the data of the tri-axis accelerometer, gyroscope, and magnetometer of the MARG system, then iteratively optimizes it to calculate the orientation ${}_b{}^n\widehat{\mathit{q}}$.

Let the predefined reference vector in the n-frame be ${}^n\widehat{\mathit{d}}$ and the sensor measurement in the b-frame be ${}^b\widehat{\mathit{s}}$. The objective function is defined as

$$\begin{array}{c}\underset{{}_b{}^n\widehat{\mathit{q}}\in {ℝ}^4}{{\displaystyle \min }} \mathit{f}\left({}_b{}^n\widehat{\mathit{q}},{}^n\widehat{\mathit{d}},{}^b\widehat{\mathit{s}}\right)\\ \mathit{f}\left({}_b{}^n\widehat{\mathit{q}},{}^n\widehat{\mathit{d}},{}^b\widehat{\mathit{s}}\right)={}_b{}^n\widehat{\mathit{q}}{}^{\ast }\otimes {}^n\widehat{\mathit{d}}\otimes {}_b{}^n\widehat{\mathit{q}}-{}^b\widehat{\mathit{s}}\\ {}^n\widehat{\mathit{d}}=\left[\begin{array}{cccc}0& d_x& d_y& d_z\end{array}\right]\\ {}^b\widehat{\mathit{s}}=\left[\begin{array}{cccc}0& s_x& s_y& s_z\end{array}\right]\end{array}$$

(9)

By recording the initial quaternion ${}_b{}^n\widehat{\mathit{q}}{}_0$ and step size $\mu$, the estimation of ${}_b{}^n\widehat{\mathit{q}}{}_{n+1}$ is obtained for $n$ iterations, adopting the general gradient descent algorithm illustrated in (10). The symbol $\nabla$ represents the gradient of the objective function, which can be computed by the objective function and its Jacobian, as shown in (11).

$${}_b{}^n\widehat{\mathit{q}}{}_{k+1}={}_b{}^n\widehat{\mathit{q}}{}_k-\mu \frac{\nabla \mathit{f}\left({}_b{}^n\widehat{\mathit{q}}{}_k,{}^n\widehat{\mathit{d}},{}^b\widehat{\mathit{s}}\right)}{‖\nabla \mathit{f}\left({}_b{}^n\widehat{\mathit{q}}{}_k,{}^n\widehat{\mathit{d}},{}^b\widehat{\mathit{s}}\right)‖}, k=0,1,2\cdots n$$

(10)

$$\nabla \mathit{f}\left({}_b{}^n\widehat{\mathit{q}}{}_k,{}^n\widehat{\mathit{d}},{}^b\widehat{\mathit{s}}\right)={\mathit{J}}^T\left({}_b{}^n\widehat{\mathit{q}}{}_k,{}^n\widehat{\mathit{d}}\right)\mathit{f}\left({}_b{}^n\widehat{\mathit{q}}{}_k,{}^n\widehat{\mathit{d}},{}^b\widehat{\mathit{s}}\right)$$

(11)

The specific algorithm expression of the accelerometer is described in (12). Normalized gravity ${}^n\widehat{\mathit{g}}$ and normalized accelerometer measurements ${}^b\widehat{\mathit{a}}$ substituted ${}^n\widehat{\mathit{d}}$ and ${}^b\widehat{\mathit{s}}$, respectively.

$$\begin{array}{cc}\hfill {}^n\widehat{\mathit{g}}& =\left[\begin{array}{cccc}0& 0& 0& 1\end{array}\right]\hfill \\ \hfill {}^b\widehat{\mathit{a}}& =\left[\begin{array}{cccc}0& a_x& a_y& a_z\end{array}\right]\hfill \\ \hfill {\mathit{f}}_g\left({}_b{}^n\widehat{\mathit{q}},{}^b\widehat{\mathit{a}}\right)& =\left[\begin{array}{c}2\left(q_2{q}_4-q_1{q}_3\right)-a_x\\ 2\left(q_1{q}_2+q_3{q}_4\right)-a_y\\ 2\left(0.5-q_2^2-q_3^2\right)-a_z\end{array}\right]\hfill \\ \hfill {\mathit{J}}_g\left({}_b{}^n\widehat{\mathit{q}}\right)& =\left[\begin{array}{cccc}-2q_3& 2q_4& -2q_1& 2q_2\\ 2q_2& 2q_1& 2q_4& 2q_3\\ 0& -4q_2& -4q_3& 0\end{array}\right]\hfill \end{array}$$

(12)

The accelerometer compensated the tilt angle of motion estimation obtained by the gyroscope measurement, but the heading angle was not compensated, as this requires the introduction of a magnetometer. The magnetometer can be used to measure the strength and direction of the magnetic field and to determine the orientation of a device.

As for the magnetometer, the geomagnetic field ${}^n\widehat{\mathit{b}}$, as the substitution of ${}^n\widehat{\mathit{d}}$, can be decomposed into a horizontal axis and a vertical axis theoretically. Additionally, the normalized magnetometer measurement ${}^b\widehat{\mathit{m}}$ is the substitution of ${}^b\widehat{\mathit{s}}$. The specific form can be written as

$$\begin{array}{cc}\hfill {}^n\widehat{\mathit{b}}& =\left[\begin{array}{cccc}0& 0& b_y& b_z\end{array}\right]\hfill \\ \hfill {}^b\widehat{\mathit{m}}& =\left[\begin{array}{cccc}0& m_x& m_y& m_z\end{array}\right]\hfill \\ \hfill {\mathit{f}}_b\left({}_b{}^n\widehat{\mathit{q}},{}^n\widehat{\mathit{b}},{}^b\widehat{\mathit{m}}\right)& =\left[\begin{array}{c}2b_y\left(q_1{q}_4+q_2{q}_3\right)+2b_z\left(q_2{q}_4-q_1{q}_3\right)-m_x\\ 2b_y\left(0.5-q_2^2-q_4^2\right)+2b_z\left(q_1{q}_2+q_3{q}_4\right)-m_y\\ 2b_y\left(q_3{q}_4-q_1{q}_2\right)+2b_z\left(0.5-q_2^2-q_3^2\right)-m_z\end{array}\right]\hfill \\ \hfill {\mathit{J}}_b\left({}_b{}^n\widehat{\mathit{q}},{}^n\widehat{\mathit{b}}\right)& =\left[\begin{array}{cccc}2b_y{q}_4-2b_z{q}_3& 2b_y{q}_3+2b_z{q}_4& 2b_y{q}_2-2b_z{q}_1& 2b_y{q}_1+2b_z{q}_2\\ 2b_z{q}_2& -4b_y{q}_2+2b_z{q}_1& 2b_z{q}_4& -4b_y{q}_4+2b_z{q}_3\\ -2b_y{q}_2& -2b_y{q}_1-4b_z{q}_2& 2b_y{q}_4-4b_z{q}_3& 2b_y{q}_3\end{array}\right]\hfill \end{array}$$

(13)

However, the magnetometer may be disturbed by the bias of hard iron and soft iron, causing errors in the measurement direction of the earth’s magnetic field, so magnetic distortion compensation needs to be employed.

$${}^n\widehat{\mathit{h}}{}_t=\left[\begin{array}{cccc}0& h_x& h_y& h_z\end{array}\right]={}_b{}^n\widehat{\mathit{q}}{}_{est,t-1}\otimes {}^b\widehat{\mathit{m}}{}_t\otimes {}_b{}^n\widehat{\mathit{q}}{}_{est,t-1}^{\ast }$$

(14)

$${}^n\widehat{\mathit{b}}{}_t=\left[\begin{array}{cccc}0& 0& \sqrt{h_x^2+h_y^2}& h_z\end{array}\right]$$

(15)

where ${}^n\widehat{\mathit{h}}{}_t$ is the normalized magnetometer measurement in the n-frame and ${}^n\widehat{\mathit{b}}{}_t$ is the compensated geomagnetic field at time $t$.

Combining the specific algorithms of the accelerometer and magnetometer, the formulas were developed as follows.

$$\begin{array}{c}{\mathit{f}}_{g,b}\left({}_b{}^n\widehat{\mathit{q}},{}^b\widehat{\mathit{a}},{}^n\widehat{\mathit{b}},{}^b\widehat{\mathit{m}}\right)=\left[\begin{array}{c}{\mathit{f}}_g\left({}_b{}^n\widehat{\mathit{q}},{}^b\widehat{\mathit{a}}\right)\\ {\mathit{f}}_b\left({}_b{}^n\widehat{\mathit{q}},{}^n\widehat{\mathit{b}},{}^b\widehat{\mathit{m}}\right)\end{array}\right]\\ {\mathit{J}}_{g,b}\left({}_b{}^n\widehat{\mathit{q}},{}^n\widehat{\mathit{b}}\right)=\left[\begin{array}{c}{\mathit{J}}_g^T\left({}_b{}^n\widehat{\mathit{q}}\right)\\ {\mathit{J}}_b^T\left({}_b{}^n\widehat{\mathit{q}},{}^n\widehat{\mathit{b}}\right)\end{array}\right]\end{array}$$

(16)

By substituting (16) into (11), the gradient of the combined objective function can be written as

$$\nabla \mathit{f}={\mathit{J}}_{g,b}^T\left({}_b{}^n\widehat{\mathit{q}}{}_{est,t-1},{}^n\widehat{\mathit{b}}\right){\mathit{f}}_{g,b}\left({}_b{}^n\widehat{\mathit{q}}{}_{est,t-1},{}^b\widehat{\mathit{a}},{}^n\widehat{\mathit{b}},{}^b\widehat{\mathit{m}}\right)$$

(17)

And the estimated quaternion ${}_b{}^n\mathit{q}{}_{\nabla ,t}$ calculated at time $t$ can be given by

$${}_b{}^n\mathit{q}{}_{\nabla ,t}={}_b{}^n\widehat{\mathit{q}}{}_{est,t-1}-{\mu }_t\frac{\nabla \mathit{f}}{‖\nabla \mathit{f}‖}$$

(18)

Employing the Madgwick orientation filter, an estimated rotation ${}_b{}^n\mathit{q}{}_{est,t}$ was obtained by fusing ${}_b{}^n\mathit{q}{}_{\omega ,t}$ and ${}_b{}^n\mathit{q}{}_{\nabla ,t}$.

$${}_b{}^n\mathit{q}{}_{est,t}={\gamma }_t{}_b{}^n\mathit{q}{}_{\nabla ,t}+\left(1-{\gamma }_t\right){}_b{}^n\mathit{q}{}_{\omega ,t}, 0\le {\gamma }_t\le 1$$

(19)

where ${\gamma }_t$ represents weight, ensuring that the weighted divergence of ${}_b{}^n\mathit{q}{}_{\omega ,t}$ equals the weighted convergence of ${}_b{}^n\mathit{q}{}_{\nabla ,t}$.

According to [14], it is known that when noise augmentation of ${\mu }_t$ is assumed to be extremely high, (19) can be rewritten as

$${}_b{}^n\mathit{q}{}_{est,t}={}_b{}^n\widehat{\mathit{q}}{}_{est,t-1}+{}_b{}^n\dot{\mathit{q}}{}_{est,t}\Delta t={}_b{}^n\widehat{\mathit{q}}{}_{est,t-1}+\Delta t\left({}_b{}^n\dot{\mathit{q}}{}_{\omega ,t}-\beta \frac{\nabla \mathit{f}}{‖\nabla \mathit{f}‖}\right)$$

(20)

where $\beta$ is the divergence rate of ${}_b{}^n\mathit{q}{}_{\omega ,t}$.

By substituting (8) into (20), it can be written in the fourth-order Runge–Kuta form as

$${}_b{}^n\mathit{q}{}_{est,t}={}_b{}^n\widehat{\mathit{q}}{}_{est,t-1}+\Delta t\left(\frac{1}{6}\left(\Delta {}_b{}^n\dot{\mathit{q}}{}_1+2\Delta {}_b{}^n\dot{\mathit{q}}{}_2+2\Delta {}_b{}^n\dot{\mathit{q}}{}_3+\Delta {}_b{}^n\dot{\mathit{q}}{}_4\right)-\beta \frac{\nabla \mathit{f}}{‖\nabla \mathit{f}‖}\right)$$

(21)

The process of the quaternion RK4-based Madgwick orientation filter algorithm is summarized as follows in Algorithm 1.

Algorithm 1: Process of quaternion RK4-based Madgwick orientation complementary filter.

Initialization:${}_b{}^n\mathit{q}{}_0$

Input: ${}^b\mathit{\omega }{}_t,\Delta t,{}^b\widehat{\mathit{a}}{}_t,{}^b\widehat{\mathit{m}}{}_t,\beta$

Output: ${}_b{}^n\mathit{q}{}_{est,t}$

1: ${}_b{}^n\dot{\mathit{q}}{}_{\omega ,t}\leftarrow 0.5 {}_b{}^n\widehat{\mathit{q}}{}_{est,t-1}\otimes {}^b\mathit{\omega }{}_t$

2: ${}_b{}^n\mathit{q}{}_{\omega ,t}\leftarrow {}_b{}^n\widehat{\mathit{q}}{}_{est,t-1}+{}_b{}^n\dot{\mathit{q}}{}_{\omega ,t}\Delta t$

3: $\Delta {}_b{}^n\dot{\mathit{q}}{}_1,\Delta {}_b{}^n\dot{\mathit{q}}{}_2,\Delta {}_b{}^n\dot{\mathit{q}}{}_3,\Delta {}_b{}^n\dot{\mathit{q}}{}_4\leftarrow {}_b{}^n\widehat{\mathit{q}}{}_{est,t-1},{}^b\mathit{\omega }{}_{t-1},{}^b\mathit{\omega }{}_{t-1/2},{}^b\mathit{\omega }{}_t$

4: ${}_b{}^n\mathit{q}{}_{\omega ,t}\leftarrow {}_b{}^n\widehat{\mathit{q}}{}_{est,t-1}+\Delta t\left(\Delta {}_b{}^n\dot{\mathit{q}}{}_1+2\Delta {}_b{}^n\dot{\mathit{q}}{}_2+2\Delta {}_b{}^n\dot{\mathit{q}}{}_3+\Delta {}_b{}^n\dot{\mathit{q}}{}_4\right)/6$

5: ${}^n\widehat{\mathit{h}}{}_t\leftarrow {}_b{}^n\widehat{\mathit{q}}{}_{est,t-1}\otimes {}^b\widehat{\mathit{m}}{}_t\otimes {}_b{}^n\widehat{\mathit{q}}{}_{est,t-1}^{\ast }$; ${}^n\widehat{\mathit{b}}{}_t\leftarrow {}^n\widehat{\mathit{h}}{}_t$

6: ${\mathit{f}}_{g,b}\left({}_b{}^n\widehat{\mathit{q}},{}^b\widehat{\mathit{a}},{}^n\widehat{\mathit{b}},{}^b\widehat{\mathit{m}}\right)\leftarrow {\left[\begin{array}{cc}{\mathit{f}}_g\left({}_b{}^n\widehat{\mathit{q}},{}^b\widehat{\mathit{a}}\right)& {\mathit{f}}_b\left({}_b{}^n\widehat{\mathit{q}},{}^n\widehat{\mathit{b}},{}^b\widehat{\mathit{m}}\right)\end{array}\right]}^T$

7: ${\mathit{J}}_{g,b}\left({}_b{}^n\widehat{\mathit{q}},{}^n\widehat{\mathit{b}}\right)\leftarrow {\left[\begin{array}{cc}{\mathit{J}}_g^T\left({}_b{}^n\widehat{\mathit{q}}\right)& {\mathit{J}}_b^T\left({}_b{}^n\widehat{\mathit{q}},{}^n\widehat{\mathit{b}}\right)\end{array}\right]}^T$

8: $\nabla \mathit{f}\leftarrow {\mathit{J}}_{g,b}^T\left({}_b{}^n\widehat{\mathit{q}}{}_{est,t-1},{}^n\widehat{\mathit{b}}\right){\mathit{f}}_{g,b}\left({}_b{}^n\widehat{\mathit{q}}{}_{est,t-1},{}^b\widehat{\mathit{a}},{}^n\widehat{\mathit{b}},{}^b\widehat{\mathit{m}}\right)$

9: ${}_b{}^n\mathit{q}{}_{est,t}\leftarrow {}_b{}^n\mathit{q}{}_{\omega ,t}-\Delta t\beta \frac{\nabla \mathit{f}}{‖\nabla \mathit{f}‖}$

Return: ${}_b{}^n\mathit{q}{}_{est,t}$

2.2. UWB Localization Kalman Filter

The UWB positioning system contained a tag-anchor wireless communication channel, as shown in Figure 3. The calculation of the distances between each tag and anchor exploiting double-sided two-way ranging (DS-TWR) method and position estimation employed distances [28,29]. In addition to classical UWB positioning algorithms such as MLE, LLSE, and WCE, we used EKF for position estimation due to its lower complexity and shorter computational time [29].

It was defined that the UWB localization system was aligned with the n-frame. The system model adopted for UWB EKF algorithm can be expressed as

$$\begin{array}{c}{\mathit{x}}_{u,t}^{}={\mathit{x}}_{u,t-1}^{}+{\mathit{w}}_{u,t-1}^{}\\ {\mathit{y}}_{u,t}^{}=h_{u,t}\left({\mathit{x}}_{u,t}^{}\right)+{\mathit{v}}_{u,t}^{}\end{array}$$

(22)

where the subscript $u$ represents the UWB localization Kalman filter, and the state vector ${\mathit{x}}_{u,t}^{}={\mathit{P}}_{UWB,t}={\left[\begin{array}{ccc}{x}_t& y_t& z_t\end{array}\right]}^T$ is a $3\times 1$ vector where ${\mathit{P}}_{UWB,t}$ represents the 3-D position of UWB tags in the n-frame. The measurement vector ${\mathit{y}}_{u,t}^{}={\left[\begin{array}{cccc}{d}_{1,t}& d_{2,t}& d_{3,t}& d_{4,t}\end{array}\right]}^T$ denotes the distances between each anchor and tag. The expansion form of $h_{u,t}\left({\mathit{x}}_{u,t}^{}\right)$ is expressed in (23). The process noise ${\mathit{w}}_{u,t-1}^{}$ and measurement noise ${\mathit{v}}_{u,t}^{}$ are zero mean additional Gaussian white noise with covariance matrices of ${\mathit{Q}}_{u,t-1}$ and ${\mathit{R}}_{u,t}$ respectively. These matrices were obtained from the UWB system, and distinguished the ranging accuracy of different anchors.

$$h_{u,t}\left({\mathit{x}}_{u,t}^{}\right)=\left[\begin{array}{c}\sqrt{{\left(x_t-x_1\right)}^2+{\left(y_t-y_1\right)}^2+{\left(z_t-z_1\right)}^2}\\ \sqrt{{\left(x_t-x_2\right)}^2+{\left(y_t-y_2\right)}^2+{\left(z_t-z_2\right)}^2}\\ \sqrt{{\left(x_t-x_3\right)}^2+{\left(y_t-y_3\right)}^2+{\left(z_t-z_3\right)}^2}\\ \sqrt{{\left(x_t-x_4\right)}^2+{\left(y_t-y_4\right)}^2+{\left(z_t-z_4\right)}^2}\end{array}\right]$$

(23)

where $x_i,y_i,z_i,i=1,2,3,4$ represent the position of the i-th anchor.

Thus, the Jacobian matrix ${\mathit{H}}_{u,t}$ could be calculated as shown in (24). And for the sake of brevity, $D_{i,t}, i=1,2,3,4$ is the corresponding row of $h_{u,t}\left({\mathit{x}}_{u,t}^{}\right)$.

$${\mathit{H}}_{u,t}={\left.\frac{\partial h_{u,t}\left({\mathit{x}}_{u,t}^{}\right)}{\partial {\mathit{x}}_{u,t}^{}}\right|}_{x_{u,t}^{}}=\left[\begin{array}{ccc}\frac{\left(x_t-x_1\right)}{D_{1,t}}& \frac{\left(y_t-y_1\right)}{D_{1,t}}& \frac{\left(z_t-z_1\right)}{D_{1,t}}\\ \frac{\left(x_t-x_2\right)}{D_{2,t}}& \frac{\left(y_t-y_2\right)}{D_{2,t}}& \frac{\left(z_t-z_2\right)}{D_{2,t}}\\ \frac{\left(x_t-x_3\right)}{D_{3,t}}& \frac{\left(y_t-y_3\right)}{D_{3,t}}& \frac{\left(z_t-z_3\right)}{D_{3,t}}\\ \frac{\left(x_t-x_4\right)}{D_{4,t}}& \frac{\left(y_t-y_4\right)}{D_{4,t}}& \frac{\left(z_t-z_4\right)}{D_{4,t}}\end{array}\right]$$

(24)

Assuming that ${\mathit{P}}_{u,0}$ is the initial state estimation covariance and is known as the prior, as well as the initial, state vector ${\mathit{x}}_{u,0}$, the posterior estimations ${\mathit{P}}_{u,t}$ and ${\mathit{x}}_{u,t}$ were recursively obtained by employing the prediction and update functions. It was defined that ${\mathit{P}}_{u,t|t-1}$ and ${\mathit{x}}_{u,t|t-1}$ were the prediction forms illustrated in (25) and could be calculated using the posterior estimations ${\mathit{P}}_{u,t-1}$ and ${\mathit{x}}_{u,t-1}$ at time $t-1$.

$$\begin{array}{l}{\mathit{P}}_{u,t|t-1}={\mathit{P}}_{u,t-1}+{\mathit{Q}}_{u,t-1}\\ {\mathit{x}}_{u,t|t-1}={\mathit{x}}_{u,t-1}\end{array}$$

(25)

Then, the prediction and the measurement vector ${\mathit{y}}_{u,t}^{}$ were used to update the prior estimations, and the posterior estimations at time $t$ could be expressed as

$$\begin{array}{l}{\mathit{k}}_{u,t}={\mathit{P}}_{u,t|t-1}{\mathit{H}}_{u,t}^T{\left[{\mathit{H}}_{u,t}{\mathit{P}}_{u,t|t-1}{\mathit{H}}_{u,t}^T+{\mathit{R}}_{u,t}\right]}^{-1}\\ {\mathit{x}}_{u,t}={\mathit{x}}_{u,t|t-1}+{\mathit{k}}_{u,t}\left[{\mathit{y}}_{u,t}^{}-h_{u,t}\left({\mathit{x}}_{u,t|t-1}\right)\right]\\ {\mathit{P}}_{u,t}=\left({\mathit{I}}_{3\times 3}-{\mathit{k}}_{u,t}{\mathit{H}}_{u,t}\right){\mathit{P}}_{u,t|t-1}\end{array}$$

(26)

where ${\mathit{x}}_{u,t}$ is the 3-D position of the UWB tag, and will be used as the measurement vector in the IMU/UWB Kalman filter.

2.3. IMU/UWB Fusion Kalman Filter

The innovative aspect of this IMU/UWB Kalman filter is that it uses the drift-free position calculated by the UWB system to compensate for the orientation and position estimated by the IMU system. The information obtained by the IMU system was employed as the state vector, and the position calculated by the UWB system was applied as the measurement vector. The system model can be expressed as

$$\begin{array}{l}{\mathit{x}}_{iu,t}^{}=f_{iu,t-1}\left({\mathit{x}}_{iu,t-1}^{},{\mathit{u}}_{iu,t-1}^{},{\mathit{w}}_{iu,t-1}^{}\right)\\ {\mathit{y}}_{iu,t}^{}=h_{iu,t}\left({\mathit{x}}_{iu,t}^{},{\mathit{v}}_{iu,t}^{}\right)\end{array}$$

(27)

where the subscript $iu$ represents the IMU/UWB Kalman filter. ${\mathit{x}}_{iu,t}$ is the state vector, ${\mathit{u}}_{iu,t-1}^{}$ is the input vector, ${\mathit{y}}_{iu,t}$ is the measurement vector, and ${\mathit{w}}_{iu,t-1}^{}$ and ${\mathit{v}}_{iu,t}^{}$ are the process noise and measurement noise, respectively.

The state vector ${\mathit{x}}_{iu,t}^{}={\left[\begin{array}{cc}{\mathit{P}}_{IMU,t}& {}_b{}^n\mathit{q}{}_{est,t}\end{array}\right]}^T$ is a $7\times 1$ vector where ${\mathit{P}}_{IMU,t}$ represents the 3-D position of human upper limb and ${}_b{}^n\mathit{q}{}_{est,t}$ is the orientation obtained from the Madgwick complementary filter. The state model consists of two parts, and the part ${}_b{}^n\mathit{q}{}_{est,t}$ can be expressed as

$$\begin{array}{ll}{}_b{}^n\mathit{q}{}_{est,t}& ={}_b{}^n\mathit{q}{}_{est,t-1}+\Delta t{}_b{}^n\dot{\mathit{q}}{}_{est,t}\\ & ={}_b{}^n\mathit{q}{}_{est,t-1}+\Delta t\left({}_b{}^n\dot{\mathit{q}}{}_{\omega ,t}-\beta \frac{\nabla \mathit{f}}{‖\nabla \mathit{f}‖}\right)\\ & ={}_b{}^n\mathit{q}{}_{est,t-1}+\Delta t\left[\frac{1}{2}{}_b{}^n\mathit{q}{}_{est,t-1}\otimes \left({}^b\omega {}_t+{}^b\omega {}_{b,t}\right)-\beta \frac{\nabla \mathit{f}}{‖\nabla \mathit{f}‖}\right]\end{array}$$

(28)

where ${}^b\omega {}_{b,t}$ is the bias of the gyroscope and $\frac{\nabla \mathit{f}}{‖\nabla \mathit{f}‖}$ is the input vector.

Supposing that ${\mathit{P}}_{IMU,0}$ is the initial position of the IMU sensors, then ${\mathit{P}}_{IMU,t}$ can be recursively calculated from a geometric perspective using the rotation quaternion ${}_b{}^n\mathit{q}{}_{est,t}$ or the rotation matrix ${}_b{}^n\mathit{C}{}_t$ in (5), updated by ${}_b{}^n\mathit{q}{}_{est,t}$. The position part of the state model is given by (29) and (30).

$$\begin{array}{l}{\mathit{P}}_{IMU,1}^u={\left[{}_b{}^n\mathit{C}{}_{u,1}{\left({\mathit{P}}_{IMU,0}^u\right)}^T\right]}^T\\ {\mathit{P}}_{IMU,2}^u={\left[{}_b{}^n\mathit{C}{}_{u,2}{\left({\mathit{P}}_{IMU,0}^u\right)}^T\right]}^T={\left[{}_b{}^n\mathit{C}{}_{u,2}{}_b{}^n\mathit{C}{}_{u,1}^T{\left({\mathit{P}}_{IMU,1}^u\right)}^T\right]}^T\\ {\mathit{P}}_{IMU,3}^u={\left[{}_b{}^n\mathit{C}{}_{u,3}{\left({\mathit{P}}_{IMU,0}^u\right)}^T\right]}^T={\left[{}_b{}^n\mathit{C}{}_{u,3}{}_b{}^n\mathit{C}{}_{u,2}^T{\left({\mathit{P}}_{IMU,2}^u\right)}^T\right]}^T\\ \cdots \\ {\mathit{P}}_{IMU,t}^u={\left[{}_b{}^n\mathit{C}{}_{u,t}{}_b{}^n\mathit{C}{}_{u,t-1}^T{\left({\mathit{P}}_{IMU,t-1}^u\right)}^T\right]}^T\end{array}$$

(29)

where ${\mathit{P}}_{IMU,t}^u$ and ${}_b{}^n\mathit{C}{}_{u,t}$ represent the position and rotation matrix of the upper arm. The position of the forearm relies on the upper arm, and is given by

$${\mathit{P}}_{IMU,t}^f={\mathit{P}}_{IMU,t}^u+{\left[{}_b{}^n\mathit{C}{}_{f,t}{}_b{}^n\mathit{C}{}_{f,t-1}^T{\left({\mathit{P}}_{IMU,t-1}^f-{\mathit{P}}_{IMU,t-1}^u\right)}^T\right]}^T$$

(30)

where ${\mathit{P}}_{IMU,t}^f$ and ${}_b{}^n\mathit{C}{}_{f,t}$ represent the position and rotation matrix of the forearm. When using the Kalman filter for the forearm, the input vector should contain the upper arm position.

The Jacobian matrices ${\mathit{F}}_{iu,t-1}$ and ${\mathit{L}}_{iu,t-1}$ were obtained.

$${\mathit{F}}_{iu,t-1}={\left.\frac{\partial f_{iu,t-1}}{\partial {\mathit{x}}_{iu,t-1}^{}}\right|}_{x_{iu,t-1}^{}}=\left[\begin{array}{cc}{}_b{}^n\mathit{C}{}_t{}_b{}^n\mathit{C}{}_{t-1}^T& 0_{3\times 4}\\ 0_{4\times 3}& {\mathit{I}}_{4\times 4}+\frac{1}{2}\Delta t{\mathit{M}}^{\prime }\left({}^b\omega {}_t+{}^b\omega {}_{b,t}\right)\end{array}\right]$$

(31)

$${\mathit{L}}_{iu,t-1}={\left.\frac{\partial f_{iu,t-1}}{\partial {\mathit{w}}_{iu,t-1}^{}}\right|}_{x_{iu,t-1}^{}}=\left[\begin{array}{c}{0}_{3\times 4}\\ \frac{1}{2}\Delta t\mathit{M}\left({}_b{}^n\mathit{q}{}_{est,t-1}\right)\end{array}\right]$$

(32)

The measurement vector ${\mathit{y}}_{iu,t}^{}={\left[{\mathit{P}}_{UWB,t}\right]}^T$ is a $3\times 1$ vector obtained from the UWB localization Kalman filter, representing the 3-D position of UWB tags in the n-frame. The measurement equation can be expressed as

$${\mathit{y}}_{iu,t}^{}={\mathit{H}}_{iu,t}{\mathit{x}}_{iu,t}^{}+{\mathit{v}}_{iu,t}^{}$$

(33)

where ${\mathit{H}}_{iu,t}$ is the Jacobian matrix, expressed as

$${\mathit{H}}_{iu,t}=\left[\begin{array}{cc}{\mathit{I}}_{3\times 3}& 0_{3\times 4}\end{array}\right]$$

(34)

The covariance matrices ${\mathit{Q}}_{iu,t-1}$ and ${\mathit{R}}_{iu,t}$ of the process noise ${\mathit{w}}_{iu,t-1}^{}$ and the measurement noise ${\mathit{v}}_{iu,t}^{}$ were calculated as

$$\begin{array}{cc}\hfill {\mathit{Q}}_{iu,t-1}& =E\left[{\mathit{w}}_{iu,t-1}^{}{\mathit{w}}_{iu,t-1}^T\right]={\sum }_G={\sigma }_G^2{\mathit{I}}_{4\times 4}\hfill \\ \hfill {\mathit{R}}_{iu,t}& =E\left[{\mathit{v}}_{iu,t}^{}{\mathit{v}}_{iu,t}^T\right]={\sum }_{P,UWB}={\sigma }_{P,UWB}^2{\mathit{I}}_{3\times 3}\hfill \end{array}$$

(35)

It was assumed that ${\mathit{P}}_{iu,0}$ and ${\mathit{x}}_{iu,0}$ were the initial state estimation covariance and state vector, respectively, and the prediction forms ${\mathit{P}}_{iu,t|t-1}$ and ${\mathit{x}}_{iu,t|t-1}$ could be recursively calculated using the following equations.

$$\begin{array}{l}{\mathit{P}}_{iu,t|t-1}={\mathit{F}}_{iu,t-1}{\mathit{P}}_{iu,t-1}{\mathit{F}}_{iu,t-1}^T+{\mathit{L}}_{iu,t-1}{\mathit{Q}}_{iu,t-1}{\mathit{L}}_{iu,t-1}^T\\ {\mathit{x}}_{iu,t|t-1}=f_{iu,t-1}\left({\mathit{x}}_{iu,t-1}^{},{\mathit{u}}_{iu,t-1}^{},0\right)\end{array}$$

(36)

Then, the prior estimations were updated, and the posterior estimations ${\mathit{P}}_{iu,t}$ and ${\mathit{x}}_{iu,t}$ were written as

$$\begin{array}{l}{\mathit{k}}_{iu,t}={\mathit{P}}_{iu,t|t-1}{\mathit{H}}_{iu,t}^T{\left[{\mathit{H}}_{iu,t}{\mathit{P}}_{iu,t|t-1}{\mathit{H}}_{iu,t}^T+{\mathit{R}}_{iu,t}\right]}^{-1}\\ {\mathit{x}}_{iu,t}={\mathit{x}}_{iu,t|t-1}+{\mathit{k}}_{iu,t}\left[{\mathit{y}}_{iu,t}^{}-h_{iu,t}\left({\mathit{x}}_{iu,t|t-1},0\right)\right]\\ {\mathit{P}}_{iu,t}=\left({\mathit{I}}_{7\times 7}-{\mathit{k}}_{iu,t}{\mathit{H}}_{iu,t}\right){\mathit{P}}_{iu,t|t-1}\end{array}$$

(37)

where ${\mathit{x}}_{iu,t}$ is the fused 3-D position in the IMU/UWB Kalman filter, which was used for 3-D motion reconstruction of human upper limbs. A detailed flowchart of the UWB localization and the IMU/UWB Kalman filters is shown in Figure 4. The parameters were set as follows. The initial state estimation covariance ${\mathit{P}}_{u,0}$ was set to $0.1\times {\mathit{I}}_{3\times 3}$. All elements of ${\mathit{P}}_{iu,0}$ were set to 0.1. ${\mathit{x}}_{u,0}$ and ${\mathit{x}}_{iu,0}$ were obtained from the UWB system and the optical MoCap system for each trial of movement, respectively.

3. Experimental Demonstration

3.1. Experimental Setup

To estimate the movement of human upper limbs, wearable 9-axis MPU9250 inertial sensor units (each sensor unit included a tri-axis accelerometer, a tri-axis gyroscope, and a tri-axis magnetometer) were adopted in our experiments. To avoid the extrusive effect of muscles of the human upper limbs, these two IMU sensors were placed on the lateral side, above the wrist and the elbow, respectively [11,21]. Each IMU sensor was connected to the computer via a serial port module for data transmission. The sample rate of the IMU sensors was set to 100 Hz, and the installation direction is shown in Figure 1.

Additionally, a DW1000 UWB real-time localization system manufactured by Haoru Technology, Dalian, China, was employed to track the 3-D position with an update rate of about 10 Hz. The UWB communication technology was implemented between the anchors and tags through the DS-TWR method based on time-domain transmission of radio signals, and the anchors were also connected to the computer via a serial port module. It provided an accuracy of 10 cm for the X/Y axis and 30 cm for the Z axis. Two UWB tags were fixed on the bracelets with the IMU sensors to avoid relative movement, as shown in Figure 5. To ensure the positioning stability of UWB, four anchors were arranged in our laboratory in a cube distribution.

The Nokov optical MoCap system captures human motion through sixteen cameras evenly arranged in the laboratory, as illustrated in Figure 5, and is used as a reference system for algorithm verification and further comparison [34]. For each segment of the human upper limb, at least four reflective markers were attached to the skin surface and sensors, constituting an envelope rather than a cluster [9,11,38,39]. The optical MoCap system achieved sub-millimeter positioning accuracy and higher reliability.

3.2. Experimental Protocol

Our experiments aimed to capture several common movements of the human upper limbs, including simple whole-arm movements and combined upper-arm and forearm movements. These common movements comprised motion 1: shoulder flexion/extension and abduction/adduction (the whole arm moving along a circular trajectory); motion 2: shoulder abduction/adduction, internal/external rotation, and elbow flexion/extension; and motion 3: shoulder flexion/extension, internal/external rotation, and elbow flexion/extension, as shown in Figure 6. In each trial, one type of movement was performed periodically, with a duration of about one minute.

As for the UWB system, four anchors arranged in conventional cube distribution were employed, as shown in Figure 7. The above movements were performed by one subject under each distribution throughout the experiment. The subject was a 24-year-old healthy female 165 cm in height and 40 kg in weight. It is important to mention that the optical MoCap system tracked the movement of the human upper limbs for each trial, which could be used to initialize the IMU sensors and obtain the positions of UWB anchors. Before each trial, the IMU sensors were calibrated, and a few seconds of stillness then enhanced the stability of the proposed algorithm.

4. Experimental Results and Discussion

4.1. Performance of Madgwick RK4 Orientation Filter

The performance of the Madgwick orientation filter in terms of attitude angle and 3-D position estimation is shown in this section. Figure 8 shows the Euler angles of the wrist for motion 1, the whole arm moving along a circular trajectory, which is beneficial for us in verifying the tri-axis rotation (roll, pitch, and yaw) of the upper limbs. On the basis of the attitude angle obtained by the quaternion fourth-order Runge–Kutta algorithm and the original second-order update algorithm illustrated in (7), the Madgwick orientation filter was combined, as shown in (21) and (20), respectively, and the tri-axis positions of sensor unit 1 during the movement were calculated as shown in Figure 9. The solid lines represent the positions calculated using various methods from the IMU system, whereas the black dotted lines are the reference positions obtained by the optical Mocap system. As can be seen, the proposed methods were able to track the movements of the upper limbs in spite of the errors at the inflection points.

The position accuracies of the three kinds of movements are summarized in

Table 1. The tri-axis position accuracy of forearm motion tracking by the Madgwick RK4 method and the original second-order update algorithm compared with the optical Mocap system.

	Position RMSE—Original Second -Order (cm)			Position RMSE—MadgwickRK4 (cm)
	X	Y	Z	X	Y	Z
Motion 1	9.8361	4.9402	10.1491	8.6124	6.1745	9.3199
Motion 2	7.4554	7.5898	10.7254	6.6717	7.6876	10.5322
Motion 3	13.2050	11.1597	11.2611	12.7563	10.3026	11.9849

. It shows the 3-D position accuracy of forearm motion tracking by the fourth-order Runge–Kutta Madgwick method and the original second-order update algorithm compared with the optical Mocap system. According to Table 1, the root mean square error (RMSE) values of tri-axis motion tracking of Madgwick RK4 method varied from 6.17 cm to 12.76 cm, which shows the feasibility of precise motion tracking of the human upper limbs. In order to further reduce the error, it is necessary to utilize the UWB measurements.

4.2. Performance of UWB Localization Kalman Filter

As for the UWB localization Kalman filter, we utilized multiple sets of movements of the human upper limb to verify the accuracy of the UWB positioning system. It is important to mention that the installation direction of the IMU was always the same as that in Figure 1 during the experiment. And the subject stood at the edge of the UWB area to ensure that there was no occlusion between the tags and anchors during the movement of the upper limbs. Before the experiment, we first kept the tag still and verified the static positioning accuracy of UWB.

Table 2. Static positioning variance of the four-anchor UWB system.

Static Positioning Variance (cm2)
	X	Y	Z
1	0.2970	0.4106	0.4080
2	0.2803	0.2255	0.7632
3	0.1668	0.3004	0.2671
4	0.1990	0.1649	0.2265
5	0.3813	0.4273	0.9591
mean	0.2649	0.3057	0.5248

shows the static positioning variance of the four-anchor UWB system. The mean values of the tri-axis variances fell within 0.6 cm², so it shared the good positioning robustness of the UWB system.

Figure 10 shows a comparison of the position of upper limb motion tracking for three kinds of movements by the IMU, UWB, and optical Mocap systems. Motion 1, represented by Figure 10a, consisted of movement along a circular trajectory, which is consistent with of the motion in Figure 9. And motion 2 was a combined motion, represented by Figure 10b, comprising shoulder abduction/adduction, internal/external rotation, and elbow flexion/extension. Motion 3 was another combined motion, represented by Figure 10c and similar to motion 2, consisting of shoulder flexion/extension, internal/external rotation, and elbow flexion/extension. As can be seen in these figures, it is obvious that compared with the position calculated by the IMU sensors, the position calculated by the UWB system was closer to the reference value of the optical Mocap system at certain steps, especially the inflection points. Therefore, as for the measurement, the UWB system can be fused with IMU for upper limb motion estimation to reduce the error caused by the drift of the IMU sensors.

4.3. Performance of IMU/UWB Kalman Filter

The performance of the IMU/UWB Kalman filter in 3-D upper limb motion tracking is shown in this section. As mentioned previously, by adopting the cube distribution of four anchors, the measurement of UWB was fused with IMU for drift error reduction and better positioning accuracy in motion estimation. The position’s RMSE values are illustrated in

Table 3. Comparison of the 3-D position accuracy of various upper limb motion tracking methods, i.e., the IMU method, UWB method, and IMU/UWB fusion method with the optical Mocap system.

	Position RMSE—IMU (cm)			Position RMSE—UWB (cm)			Position RMSE—IMU/UWB (cm)
	X	Y	Z	X	Y	Z	X	Y	Z
Motion 1	9.1448	8.6423	8.5407	5.7678	4.3311	12.8657	5.4827	8.3291	6.3664
Motion 2	5.1256	8.5176	10.7520	11.5741	5.6851	15.9024	4.9606	7.8206	9.7357
Motion 3	12.7599	10.2743	11.9922	16.0117	12.5021	13.7803	12.2170	9.0845	11.1169

, where motion 1, motion 2, and motion 3 are represented in Figure 10a–c, respectively. This shows a comparison of the 3-D position accuracy of various upper limb motion tracking methods, i.e., the IMU method, UWB method, and IMU/UWB fusion method with the optical Mocap system. Although the position accuracy of the UWB system may have sometimes fallen lower than that of the IMU, the adopted Kalman filter adaptively adjusted the weights between them based on the covariances. It can be observed that the tri-axis position RMSE calculated by the IMU/UWB Kalman filter was consistently less than that without UWB fusion. In our experiment, the proposed method was been extensively tested by various movements. For simple movements like motion 1, consisting of a circular trajectory, the tri-axis position RMSE values were all less than 10 cm. Compared with the IMU method, the relative errors calculated by the IMU/UWB fusion method were reduced by 40%, 3.6%, and 25.5% in the X-axis, Y-axis, and Z-axis, respectively. Complex combined movements such as motion 2 and 3 always consisted of multiple simple movements, such as shoulder flexion/extension, abduction/adduction, internal/external rotation, and elbow flexion/extension. Despite the fact that the tri-axis errors with combined motions are usually more significant than those with simple movements, the IMU/UWB fusion method still demonstrated better position accuracy than the IMU method, with a maximum RMSE of 12.2 cm. And our proposed methodology achieved an average decrease in the RMSE of 1.2 cm from the IMU method to the IMU/UWB fusion method. In comparison, by transferring the angle RMSE into position RMSE, the position RMSE values illustrated in Table 3 fell within the accuracy range of forearm motion without the constraints represented in [21].

Figure 11 shows a comparison of the 3-D spatial trajectory of upper limb motion reconstructed by the proposed algorithm and the real movement for motion 3. As can be seen in this figure, the proposed method was able to accurately track the movement of the human upper limb, and showed high feasibility and stability.

4.4. Power Consumption and Cost

According to the datasheet of the MPU9250 which was adopted in our experiment, the power consumption of the sensor depends on the selected operating mode and sensor configuration. In general, the device consumes between 8.3 μA and 3.9 mA. When the voltage is 3.3 V, the power consumption is as follows: in low-power mode, it ranges from 27.4 μW to 66.7 μW; in normal mode, with both the accelerometer and the gyroscope enabled, it varies from 31.5 μW to 764.7 μW; and in normal mode with all three sensors (accelerometer, gyroscope, and magnetometer) enabled, the power consumption ranges from 37.3 μW to 926.7 μW. The average price for a single MPU9250 chip is between USD 1 and 5.

As for the UWB system, according to the DW1000 datasheet, the typical power consumption values for the device in different modes are as follows: in idle mode, 24.75 mW; in receive mode, 56.84 mW; in transmit mode (at 6.8 Mbps data rate), around 270.76 mW; and in sleep mode, 20 nW. Typically, the cost of a single DW1000 module ranges from around USD 10 to 25.

However, the power consumption of an optical MoCap system can range from a few hundred watts to several kilowatts. This is because the cameras employed in this system require significant processing power and strict light conditions. And the cost of a basic system with a few cameras and basic software is around USD 1000. Therefore, compared with the optical MoCap system, the IMU/UWB system which we adopted exhibits extremely lower power consumption and is more economical and applicable.

5. Conclusions

In this paper, we proposed a novel method for hybrid upper limb motion tracking and 3-D positioning by fusing the IMU and UWB systems. We first simplified the human upper limb as a kinematics model and employed the quaternion method to calculate the attitude angle of each segment. To compensate for the accumulated error of gyroscope measurement, the fourth-order Runge–Kutta Madgwick orientation filter was adopted to improve the accuracy of 3-D motion tracking through the optimal fusion of the accelerometer, gyroscope, and magnetometer of the MARG system. The RMSE values varied from 6.17 cm to 12.76 cm, which shows feasibility for the precise motion tracking of human upper limbs. The error was mainly caused by drift of the IMU sensors. And it was inevitable that there would be a slight misalignment of coordinate frames at the initial moment.

In order to further reduce the drift error, we combined the UWB localization system with the IMU sensors. The static positioning variance of the four-anchor UWB system was tested, and the mean values of the tri-axis variances were within 0.6 cm², so it shared good positioning robustness. By employing the UWB localization Kalman filter, the accuracy of UWB was verified by multiple sets of movements of the human upper limbs.

Adopting the four-anchor UWB system, we employed various movements to test the IMU/UWB Kalman filter. The experimental results represent that our proposed fusion algorithm achieved an average decrease in the RMSE of 1.2 cm from the IMU method to the IMU/UWB fusion method. With high feasibility and stability, our proposed algorithm was able to accurately track the movements of the human upper limbs.

In future work, we aim to study the effects of various distributions of multiple anchors on UWB localization accuracy, as well as to extend the proposed algorithm to the whole-body motion estimation.