Method for joint flexion angle estimation using UWB ranging with clock model compensation

This paper presents a wearable system for measurement and monitoring human body joint angles based on UWB ranging. The DW1000 chip was used with standard deviation of distance measurement within 10 cm with range up to 70 m. We propose a method for enhancing range measurement accuracy based on an estimator which compensates clock imperfections and relative pairwise movement of nodes. Since the estimator is valid only for small slices of time, we propose continuous motion estimation algorithm based on segment-by-segment data processing and stitching results into a final solution. The pairwise distances are approximated with Taylor series of a given order L in short measurement windows while timestamps are compensated with clock parameters of a first-order clock model. The main contribution of the proposed method is the ability to implement joint angle estimation by using low-cost off-the-shelf UWB components, without high-precision clock sources or a need for wired or wireless time synchronization. In order to determine an optimum order L and time slice length, Sprague and Geers' metric was used. The method was experimentally evaluated in static and dynamic conditions. The results show that the accuracy of the proposed system is comparable to similar solutions based on laboratory equipment.


Introduction
Human motion tracking (HMT) has gained a lot of attention in the last decade. Studying kinematics, or more simply motion of human body, is of interest in various areas of research. It is particularly important in medicine, but also in other fields, such as sports, entertainment industry, etc. Taking into account accurate biomechanical models, data collected by human motion capture systems can be used for diagnosis, rehabilitation and even in sports injury prevention. Similarly, HMT systems can be combined with virtually reality (VR) and visualization technologies giving it rise in numerous applications such are film and gaming industries.
Motion capture, of specific body parts or human body as a whole, can be accomplished by various approaches and technologies, yielding different levels of accuracy and dynamic characteristics. Typical sensor types used for human motion tracking are inertial, optical, magnetic, mechanical, ultrasound and radio frequency based. Regarding the application on tracked human subject, there are two basic categories of HMT systems: computer vision and wearable sensor based [1].
Vision-based systems typically require reflective markers to be placed on a subject. Solutions without markers, which are less intrusive and easier to setup, such as Leap Motion, 1 do exist, but they are either limited in the field of view or in their estimation accuracy. Their robustness is inferior in comparison with conventional marker-based computer vision systems [2]. Commercially available systems such as Optotrak 2 and Vicon, 3 provide high accuracy when operated in controlled environments. Multiple cameras are used to track predetermined points on a body but in a limited volume. Main drawbacks of these pure-imagebased systems are their price, occlusion problem and requirement for fixed static infrastructure where shooting cameras orientations and positions of light sources are fixed.
On the other hand, wearable HMT systems do not suffer from some of the previously mentioned drawbacks but they have their own shortcomings. In general, they are not suitable for large-scale deployment and excessively dynamic applications. Considering that the most of this type of systems are based on inertial measurement units (IMU) they share common source of error due to accumulated drift errors caused by accelerometer and gyroscope imperfections. The problem can be solved with implementation of higher accuracy modules such as XSens 4 or Invensense 5 or by using methods such as multi-sensor data fusion or zero velocity update algorithm. Both approaches have their own drawbacks. Precision hardware is very expensive which limits the number of nodes that can be used. In the latter approach, although filtering processes, such as Kalman filters, can slow down error influences, they cannot eliminate them completely. In many cases magnetometers are used as stabilizing factor to IMU measurements.
However, the use of magnetometers for compensation of the inherent IMU drawbacks may lead to unsatisfactory results due to the presence of magnetic disturbances in the environment, since the weak Earth's magnetic field can easily be disturbed by the presence of various ferromagnetic material in the immediate surroundings. Although these effects can be removed by re-calibration, the method is valid only for a short periods of time and it is confined to a specific location where the calibration was done. On the other hand, by using the zero velocity update algorithm with the IMU sensor mounted on a shoe, a drift error can be eliminated [3]. Nevertheless, the accuracy of these measurements still degrade over time, especially in situations of rapid movements and unsteady ground contact [4]. The drift problem limits the usage of such systems in a prolonged usage time and larger space environments. There are alternative systems based on pure mechanical solutions that use potentiometers and pulleys attached to tracked limb so that joint angles can be determined [5]. Those kind of systems are accurate but suffer from alignment issues which cause lower dynamic response. Therefore, miniaturized systems such as IMUs are preferred.
Due to the recent advances in radio-frequency (RF) technologies, especially in the field of ultrawideband (UWB) technology, such approach is viable to be used for HMT systems in chip form for measurements in dynamic conditions [6]. UWB is used for means of absolute localization of transciever nodes. Nowadays, with UWB technology it is possible to determine distances in centimetre range and this method does not suffer from some drawbacks inherent both to computer vision and IMU-based methods. Due to absolute accuracy localization, UWB systems are immune to the most serious IMU sensors drawback such as accumulated drift error. They also do not suffer from occlusion problem since radio signals can penetrate human body and most of occlusions. Another advantage of UWB technology is high immunity on multipath reflection errors, what makes it a better candidate than some other competing RF localization technologies. Unfortunately, this approach has some specific drawbacks on its own such as slow rate of measurements (in range of 10 Hz [1,3,7]), positional errors due to reflections and NLOS (non-line of sight) outliers which can degrade the localization accuracy. Nevertheless, with help of UWB localization data magnetometer-free design was developed in [3]. The proposed algorithm used UWB data not only for position tracking but also in yaw estimation with a loosely coupled filter. Completely UWB-based human motion tracking is achievable with estimation techniques [8]. Without sensor fusion and UWB in [4] system for 3D trajectory tracking of the foot for a long duration during treadmill walks was developed. Similarly, in [9] UWB was used for measuring joint angles with sufficient accuracy.
In this paper we propose a system based on wearable UWB modules built with DW1000 chip to determine joint angles. Since DW1000 chip provides distance measurements with standard deviation in range of 10 cm different approach is necessary to enhance the accuracy. We propose usage of an estimator that takes clock imperfection and relative pairwise movement into account. This approach was chosen to analyse viability of the solution in static and dynamic scenarios. Additionally, since estimator solution is valid for small slices of time we propose to estimate motion segment by segment in other to obtain the full picture of the motion. To determine the best combination of the input variable we propose usage of Sprague and Geers' metric. Our solution was validated on JACO Robotic Arm in various scenarios.

Problem formulation
As stated in [9] it is possible to use UWB as a distance measuring technique for determination of human joint angles. Angles can be determined by measuring distance between two modules placed around pivot point as it shown in Figure 1. Similarly, angle of the knee can also be measured. By determining values d 1 and d 2 before hand, we can use cosine law to calculate joint angle from the d: Generally, when using UWB, Time Of Arrival (TOA) method is mostly used for distance estimation. With differences of timestamps on two different nodes, the method cancels out clock offsets and unknown processing time delays. This method is used to avoid the need  for wired or wireless high accuracy time synchronization between two independent nodes. During message exchange four timestamps from T 1 to T 4 are acquired, as it it shown in Figure 2. Distance estimation can then be expressed with: where τ ij is propagation time from one to another node and c is speed of the electromagnetic wave in the medium. Equation (2) is also known as single-sided TWR method. Additional variations of TWR exist in form of symmetric double-sided TWR (SDS-TWR), alternative double-sided TWR (AltDS-TWR), asymmetric double-sided TWR (ADS-TWR), etc. In all of these variations clock drifts are dominant error sources that are analysed and modelled [10]. Nodes are presumed to be static in time with unvarying oscillator parameters. This is valid only in ideal conditions, where oscillators from these two independent nodes would be stable and synchronized to the global reference time. In reality, due to various causes, such are changes in temperature, imperfections in design or manufacturing methods, this is not true as it is demonstrated symbolically in Figure 2(b). Inherently these influences are nonlinear in nature and are cause of systematic error in ranging. Nevertheless, TWR methods are used under the assumption that measurement are performed in a necessarily small time window (minimal reply times) where deviations of the respective clock sources would be negligible. This approach is adequate either in sensor fusion approaches where UWB measurements are used to complement other drift prone sensors such as IMUs [1,11,12] or in cases where laboratory equipment is used for generating and receiving UWB signals [8,9]. For mobile nodes with their independent clock sources, a different approach is needed. Two main sources of angle measurement errors are stated in [9], misalignment of attached antennas to human joints and ranging measurement error between nodes. Because of this, error of estimated angle increases exponentially. By placing the arm in fixed angle position, with help of Equation (1) we can calculate error of estimated angle by introducing measurement error into term d. For instance, if arm was placed at 90 • angle, with DW1000, which has 10 cm standard measurement range deviation, estimated angle error would be almost 60 • . Therefore, it is not possible to use only TWR schemes.
To provide necessary accuracy for determining joint angle, valid clock model for distance estimation is a must.
Starting point is to model local time t i at node i as: where t is true time. Clock skew is denoted as ω i ∈ R + and clock offset as φ i ∈ R. C i (t i ) relates local time t i to the true time t and represents affine clock model t C i (t i ). For two nodes with simple TWR, as it is shown with Equation (2), there are five unknown parameters, two clock skews and offsets of each separate node and pairwise distance. It is impossible to estimate all five parameters through any number of packet exchange without some additional constraints [13]. In general, this means that one of the node needs to be declared as reference one with constraints α i = 1 and β i = 0. Even in this situation there are different approaches for estimation of each parameter. As previously stated one solution is to entirely disregard clock changes, but as it is shown this is not applicable in our case due to the exponential influence of ranging error.
There are different approaches available in literature to estimate unknown parameters [8,14,15]. From using maximum likelihood (ML) estimator which jointly estimates the positions and their respective range measurement offsets to the weighted least-squares (LS) solution which estimate behaviour of clock model described with Equation (3)  (s ij , s ji ) ← slice (T ij , T ji ) to i equal lengths 6: for j ← 2, max(L) do 7: for 10: be even be taken into account in TWR by means of linear interpolation of timestamps with a promise of 1 cm accuracy [16]. All of previously mentioned methods estimate clock skew or clock offset separately or jointly but fail to incorporate movement of the nodes themself.
In this paper we propose joint estimation of pairwise range and clock parameters as it is proposed in [17]. Originally estimator was used for distributed wireless network synchronization in static and dynamic conditions with attention to pairwise delays. These pairwise delays or rather distance are approximated with Taylor series of given order in small measurement window while timestamps are compensated with clock parameters of first-order clock model as it was shown in Equation (3). By using proposed estimator we can substantially increase accuracy of measured distance in environment of unknown clock parameters. Additionally, we propose algorithmic extension of [17] estimator so that it can be applied in dynamic environments with continuous motion. For it be applicable in a real time we also propose offline comparison metric to reference measurements with the goal of estimator parameter optimization.

Algorithm
In this paper we use estimator explained in [17] which will be elaborated in this chapter in short. Estimator is based on linear clock model described with Equation (3)  Usually, the distance d ij between two nodes i and j is expressed as constant and it can be determined by measuring the propagation delay τ ij . Relation can be expressed as τ ij = c −1 d ij . In our case nodes are presumed to be in relative motion to each other. Since we do not have any information concerning the nature of this nonlinear motion we can approximate it with Taylor series in a small measurement window T = T ij,K − T ij,1 : where coefficients ij ] T ∈ R L×1 are translated range parameters in terms of time, which incorporate clock discrepancy of node i. The order of the Taylor series L can be determined experimentally or estimated on basis of the measurement data.
To estimate clock parameters and Taylor series parameters θ = [α, β, γ ] T , we have to employ TWR measurements where all timestamps can be expressed in true clock time as: T ij,k corresponds to kth time when node i is communicated with node j, while similarly T ji,k denotes time when node j sends message to node i in time k. The direction of message is indicated with E ij,k where positive value +1 corresponds to direction from node i to j and negative value of −1 direction from node j to i. Since all timestamps are measured in each of the nodes local time they need to be expressed with corresponding time translations to the true time. Therefore Equation (5) transforms into: where corresponding influences of local clock oscillators were taken into account. Additionally, we have to account for joint measurement noise η ij,k since the sum of the timestamps is zero only in ideal condition. Finally, we can establish generalized joint clock and (L − 1) th order range model for pair of nodes in matrix form as: where (·) denotes element-wise matrix exponent. In other words, t ij and t ji are timestamps from node i and j respectively, while e ji is a known vector that contains information of transmission direction. Noise vector is defined as: Since matrix A ij,1 is rank deficient by 2, a unique solution of Equation (7) can be obtained by defining one of the node as reference as previously described in 2. In this case Equation (7) transforms into: where To obtain solution of Equation (11) we can use LS method by minimizing l 2 norm: T ij ] T can be then obtained with relations given by Equation (3). Distance for all points of k is then given with solution:d Ideally, model described with Equation (11) could be applied on the whole recorded motion. This would be unrealistic because model works on a small measurement window T and to describe even moderately complex motion we should have large L order which would significantly increase computation and memory costs. Additionally, large order of Taylor series increases possibility for unstable behaviour of the estimator. Our proposed solution is to estimate the motion segment by segment in other to obtain full picture of it. Method was is inspired by segmented least-squares problem. Unfortunately, dynamic programming solution which is typically used for this sort of problem cannot be directly applied because for a given set of points P = {x 1 , y 1 }, {x 2 , y 2 } · · · {x n , y n } the requirement that x 1 ≤ x 2 ≤ · · · ≤ x n is not fulfilled in the whole data space. To solve this problem we propose that T ij and T ji are sliced in equal lengths, their values normalized corresponding to last distance measurement of previous slice and then estimated with Equation (13) separately. Afterwards, each estimation is stitched to form continuous solution. Algorithm 1 needs to be executed offline on already collected movement data since it has high computational cost and depends on reference data d ij to calculate error term of the stitched estimation.

Hardware platform
The developed hardware node used for measuring the distance is based on the DW1000 UWB chip capable of determining time of arrival (TOA) timestamps in resolution of 15.6 ps. Decawave DW1000 is a single chip wireless transceiver compliant with the IEEE802.15.4-2011 standard. Some of the main characteristics of the previously mentioned chip are high data rate communications (up to 6.8 Mbps), immunity to the multipath fading and low power consumption. Chip supports 6 frequency bands with central frequencies ranging from 3.5 GHz up to 6.5 GHz. In our measurement setup, only channel 7 was used since it was experimentally shown that communication is most reliable on it in the environment where measurements were performed. In order to make logging of measured data as fast as possible, special care to system architecture and program structure was given. To simplify read out of the collected logs, FatFS was implemented on the microcontroller side. Additionally, logging and communication were handled with the help of the FreeRTOS operating system. Two major operations, data acquisition and logging were decoupled by means of circular FIFO buffers. By using FIFO circular buffers, data can be acquired continuously and held until it is ready to be logged. The platform used for data acquisition is based on STM32F4 microcontroller with 64 kB of RAM which was pre-allocated and used for before mentioned buffers. Data is collected immediately after interrupt occurs. Time spent in interrupts was minimal since their only purpose was to notify corresponding data acquisition tasks that new data is available. Afterwards, collected data was transferred to FIFO buffers and logging task was notified. All collected data in one acquisition was prepended with a timestamp generated by pre-synchronized internal RTC. To additionally increase logging speed, data was written in binary form to pre-allocated files on the SD card. In this way, execution time that may be lost because of data formatting or sector allocations was minimized. After each measurement, logs were flushed, truncated and SD card was unmounted so that consistency of data was preserved. The size of the developed prototype hardware platform is 60 x 25 mm, what makes it suitable for on-body placement and measurements.

JACO robotic arm
As a reference system JACO Robotic Arm 6 was used. The KINOVA JACO Assitive robot is a light-weight robotic arm with six inter-linked segments. It can be controlled with a separated controller or with computer with provided API support. User has freedom to operate the robot in three-dimensional space and grasp or release objects with the gripper. Reference measurements were done with Actuator 3 which is based on K-75+ actuator from the same company. Actuator has absolute position precision at start-up of 1.5 • . 7 Programmatically, angular speed of the actuator can be set in range 0-60 • per second. Small PC software support was written with the help of the provided SDK. Program tasks were to setup the arm, program the trajectories and sample out reported actuators positions every 5 ms.
Each sample was prepended with local timestamp in Unix format with millisecond accuracy. This was also done on mobile nodes side so that measurements can be synchronized in time.

Measurement method
Developed mobile nodes where placed on the robotic arm as far as possible from Actuator 3 as it was recommended in [9] per their error analysis. Placement of the nodes and their battery packs are shown in Figure 3. Nodes were and securely mounted with masking tape. To test plausibility of the proposed method, arm was firstly placed to static positions where Actuator 3 assumed values of 30 • to 180 • with 30 • steps. These values were chosen for easier calibration and measurement of values d 1 and d 2 which were approximately 30 cm. UWB nodes were calibrated beforehand with regards to the manufacturer recommendations. The distance between nodes was determined with 20 Hz refresh rate by using TWR method. Arm was kept in these positions for approximately 30 seconds.
To test dynamic behaviour of our proposed solution, arm was programmed to execute simple movement from 30 • to 180 • and back. Movement was recorded in three separate speeds, 10 • /s, 30 • /s and 60 • /s. All measured values from JACO arm were converted into distance values with help of Equation (1) measurements so that effects of the proposed solution can be closely analysed. Since all measurements are synchronized in time with Unix epoch timestamp, they can be compared qualitatively with Sprague and Geers' metric. This metric was chosen because effects of phase and magnitude differences between two waves can be combined into one value and expressed as e ij term in Algorithm 1. With this metric of comparison Algorithm 1 can be used to determine best case scenario for combination of Taylor series order L and lengths of each segments used in estimation. Magnitude error can be calculated as: where m represents whole previously stitched sample space of estimated movement and p reference movement acquired by JACO Robotic Arm while N is the number of samples. Similarly, phase error can be calculated as: Finally, complete error term between two waveforms can be expressed as:

Results
We first show the effect of Equation (13) estimator on measured angles values in static condition. It can be noticed on Figure 4 that estimated values follow reference values more closely than those that were measured with TWR method. Additionally, we can notice larger error between estimation and reference for angles larger than 100 • . Probable cause of this is inaccurate measurements of d 1 and d 2 values or the fact that calibration was done with TWR method per manufacturer instructions. In further research we will test applicability of Equation (13) in the calibration process. In angle ranges between 30 • and 90 • , max error rate between estimate and reference is 6.67 • which is comparable to the results mentioned and achieved in [9]. TWR method in 100 measurement between nodes achieved standard deviation of 45.8 mm. The worst case standard deviation in case of estimated values was 8.7 mm. This means that with help of Equation (13)  . It should be noted here that slice points are always divisible by two since T ij and T ji are generated from TWR timestamps as it is shown in Figure 2. For instance, if we say that estimator used 264 slice point, that means that we need to use 132 TWR measurements. In all three figures spurious discontinuities can be noticed. Some of maximum values of estimate overshoot defined range of angle measurement. In other words, arccos function is undefined in this region. This will be addressed in future research with additional data processing. Nevertheless, error based on Sprague and Geers' metric has shown to be a good choice. It is interesting to notice that for faster movement of 60 • /s higher order of Taylor series was chosen while slice points remained the same as in 30 • /s. If we ignore measurements that generated undefined values for Figure 5(a) the mean error difference between reference signal and estimated one is 1.2 • , with  [9]. Bigger standard deviation in the last case could be solved with higher refresh rate of TWR measurements which should lower estimation order of Taylor series. This in turn would minimize spurious discontinuities that are evident in estimation. This approach brings its own limitations since higher refresh rate leads to higher temperatures in chip which can lead to systematic offsets in timestamp measurements. For this problem to be solved temperature compensation is needed. Another approach is to enhance procedure of stitching separated estimated segments.

Conclusion
In this paper a wearable measurement system for static and dynamic monitoring of human body joint angles based on UWB ranging approach was developed and tested. Sensor nodes are based on the DW1000 integrated circuit, which provides timestamps with 15.6 ps accuracy, what translates in approximately 10 cm of standard deviation of ranging error. Range measurement is used to estimate a joint angle, what can have numerous practical application, especially in the fields of rehabilitation medicine and athlete performance monitoring.
The UWB ranging approach is able to successfully address some shortcomings of IMU solutions traditionally used in HMT, especially regarding drift errors due to accelerometer and gyroscope imperfections. Although the UWB ranging technology is a promising solution for elimination of inherent IMUs shortcomings, it brings another set of challenges to be solved. The most important limitation of UWB ranging approach is related to quality and accuracy of clock source. Other existing UWB solutions for HMT are either used with IMUs only to compensate their drift error instead of magnetometers, or are used as a standalone UWB solution, but with high precision laboratory equipment. Clock imperfections in these cases can be entirely neglected or generalized as range measurement noise, since high-precision clock sources of laboratory equipment are used. However, in practical applications where battery powered mobile nodes are placed on body, it is necessary to provide a solution that can be applied for everyday use cases. Such solutions can have clock sources with much lower accuracy than laboratory equipment and moreover each clock source is independent and not synchronized with other nodes' clocks. These clock imperfections translate into unacceptably large ranging errors.
Therefore, we proposed the clock parameters and range estimation technique which is valid for unsynchronized mobile UWB nodes where their clock parameters and pairwise distance motions are un known. Our research was focused on extending TOA method to increase the ranging accuracy of low-cost UWB nodes to be applicable for HMT, specifically for the joint angle estimation use case, in static and dynamic conditions. The main contribution of the proposed method is the ability to achieve acceptable results without need for high-precision clock source or wired or wireless time synchronization among nodes.
The experimental evaluation in static and dynamic conditions was performed on JACO Robotic Arm. The robotic arm enables precise control of angles and can be used as a reference without a need for additional external measurement devices, such as goniometers. Moreover, unlike tests on human subjects, experimental conditions can have high repeatability and very precise control of test parameters in static and dynamic conditions. In static conditions, standard deviation of range measurements was decreased by a factor of 5 (from 45.8 mm for case of simple TWR to 8.7 mm for case of application of the proposed estimator). Our results showed very small discrepancies between reference and estimated angles, not more than 6.7 • of maximum error in the range from 30 • to 90 • for static conditions. Dynamic measurements are even more challenging because relative motion between nodes also must be taken into account, along with clock imperfections. The results for dynamic measurements exhibited mean error of 1.2 • with standard deviation 6.3 • for reference angle speed of 10 • /s, mean error of 0.8 • with standard deviation 9.1 • for reference angle speed of 30 • /s, and mean error of 7.0 • with standard deviation 15.7 • for reference angle speed of 60 • /s. Larger errors were observed for case of the highest angle speed of 60 • /s. Probable source of larger error is insufficient speed of TWR ranging measurement.
The future research will be focused on usage of estimator for improved calibration between UWB nodes, higher frequency refresh rate of TWR ranging measurements, and corresponding temperature compensation. There is also space for improvement of the proposed algorithm in the part of stitching separately estimated segments in reconstruction of the whole movement trajectory. This research will serve as a base for future development of algorithm implementation running in a real-time since currently all calculation and post processing was done offline.