An Extended Complementary Filter for Full-Body MARG Orientation Estimation

Inertial sensing suites now permeate all forms of smart automation, yet a plateau exists in the real-world derivation of global orientation. Magnetic field fluctuations and inefficient sensor fusion still inhibit deployment. In this article, we introduce a new algorithm, an extended complementary filter (ECF), to derive 3-D rigid body orientation from inertial sensing suites addressing these challenges. The ECF combines computational efficiency of classic complementary filters with improved accuracy compared to popular optimization filters. We present a complete formulation of the algorithm, including an extension to address the challenge of orientation accuracy in the presence of fluctuating magnetic fields. Performance is tested under a variety of conditions and benchmarked against the commonly used gradient decent inertial sensor fusion algorithm. Results demonstrate improved efficiency, with the ECF achieving convergence 30% faster than standard alternatives. We further demonstrate an improved robustness to sources of magnetic interference in pitch and roll and to fast changes of orientation in the yaw direction. The ECF has been implemented at the core of a wearable rehabilitation system tracking movement of stroke patients for home telehealth. The ECF and accompanying magnetic disturbance rejection algorithm enables previously unachievable real-time patient movement feedback in the form of a full virtual human (avatar), even in the presence of magnetic disturbance. Algorithm efficiency and accuracy have also spawned an entire commercial product line released by the company x-io. We believe the ECF and accompanying magnetic disturbance routines are key enablers for future widespread use of wearable systems with the capacity for global orientation tracking.


I. INTRODUCTION
ECHATRONIC systems tracking movement are ubiquitous today; almost any smart machine, interface (e.g.phone/wearable/control device), automation, or navigation system now relies on some form of motion sensing.Inertial Measurement Unit (IMU) sensing packages are revolutionizing mature industries, 'turning agriculture into Smart agriculture, infrastructure into Smart infrastructure, and cities into Smart cities' [1], and creating entirely new fields of study in unmanned vehicles, tele-medicine, and human performance.They remain central to any discussion on smart automation and the 'Internet of Things' (IoT) [2].
Motion sensing packages, or Attitude Heading Reference Systems (AHRS), were first pioneered in aviation to provide sensory feedback for flight control.The development of Microelectromechanical Systems (MEMS) for inertial sensing radically reduced required dimension and cost, leading to widespread use of IMU/MARG (Magnetic, Angular Rate, and Gravity) sensor packages.Field applications are proliferating in robotics [3], virtual reality [4], aerospace [5], and a myriad of other industries.Moreover, inertial systems embedded in small wearable devices has seen rampant growth in human performance (e.g.sports, telemedicine) [6][7][8][9] and cybernetics (e.g.robot interface) [10][11][12][13], where small form factor and low power are vital.Despite this success, nearly all commercial wearables today are limited to simple output based on acceleration features for motion recognition [14].Derivation of global position and orientation in real-world applications is very rare given challenges in accurate integration of acceleration and dependency on measured geomagnetic gradient, which is often disrupted due to metal or electrical fields in indoor environs.There is very strong demand for robust orientation and kinematic metric data (e.g.limb joint angle) from wearables, which remains unmet.
AHRS calculation of global orientation demands fusion of multiple sensing modalities.Noise and bias in a single sensor leads to errors in orientation which propagate over time [15].The most common AHRS systems are IMUs consisting of a triaxis accelerometer and tri-axis gyroscope.MARG sensor packages contain both these sensors, as well as a tri-axis magnetometer.Combination of all three sensing modalities can provide orientation under ideal circumstances, but in practical application, a system must be able to compensate for noise in the data to maintain accuracy.Gyroscopic integration through finite sum methods are commonly used to track high frequency rotations, but this relies on a known initial position and suffers from drift due to noise and biases [16].Accelerometers and magnetometers can measure gravity and geomagnetic field, which give reference vectors to correct gyroscopic integration.However, they are sensitive to other sources of acceleration and magnetic field disruption from metal or electronics.Accelerometer arrays have been proposed in place of gyroscopes [17], bypassing errors common to gyroscope integration.While promising, this technique lacks maturity and remains unproven in real-world applications.Transforming MARG data to full rigid body orientation relies on fusing complementary aspects of gyroscope, accelerometer, and magnetometer sensor modalities.Gyroscopes tend to be sensitive to high frequency movements, and as a result form the basis of many algorithms.However, they induce low frequency sources of error.Accelerometers and magnetometers do not depend on a prior state, thus can be used to perform small corrections to the estimation given by the gyroscopes (e.g.accelerometers with a gravity vector, magnetometers with magnetic north).The most prominent algorithms for performing this fusion are Complementary Filters, Kalman Filters and Optimization filters [18].

A. Overview of IMU orientation estimation algorithms
Complementary filters use a combination of gyroscopic integration and accelerometer output to estimate orientation [19].This solution is computationally lean and easy to implement, however it is not robust to noisy/biased data, and is susceptible to gyroscopic drift around the gravitational vector [20].As such, complementary filters are not reliable or robust for regular use over time.Kalman Filters provide highly accurate orientation [21][22][23], and have been used in a number of commercial IMUs, including micro-strain [24], VectorNav [25] and xsens [26].When implemented correctly, they can maintain an accurate estimation for orientation in the presence of large sources of noise, such as persistent acceleration [27].However, Kalman Filters are computationally expensive, which may increase hardware cost and latency.They are also complex to implement which, combined with computational expense, can make the algorithm impractical for many applications [18].
Optimization filters derive orientation by estimating a vector representing the sensor output at the current orientation and minimizing the difference between predicted and observed output [12].These techniques have shown acceptable accuracy with lower computational expense than Kalman Filters.Popular manifestations have executed error minimization with PI controllers [28] and gradient descent algorithms [18].
Optimization filters are well-recognized for combining accuracy with computational expense and ease of implementation, but can suffer from unpredictable convergence [29].Our team has introduced an optimization filter dubbed the Gradient Descent orientation estimation Algorithm (GDA), commonly known as 'Madgwick's Algorithm' [30].This algorithm has become very popular as a computationally lean alternative to Kalman Filters.The GDA monitors changes in orientation using a gyroscope based finite sums approach.Corrections are made to this orientation by modelling expected sensor output at an orientation and attempting to minimize the error between predicted and measured output by updating the orientation.This algorithm is cited as having had a disruptive impact on AHRS research and practice [18,29], and has been used prolifically.Nevertheless, the GDA has recognized inefficiencies.Firstly, the method requires two sequential minimization steps, one based on the magnetometer and one based on the accelerometer data, which can lead to slow convergence.Secondly, the calculation of roll is not decoupled from the calculation of yaw, which can lead to unpredictable orientation errors [29].Nevertheless, the GDA remains very popular and is commonly used as a benchmark when evaluating other inertial sensor fusion algorithms [3,31,32].

B. Contributions of work
In this investigation, we present a new orientation estimation algorithm that builds upon the principles of a Complementary Filter, dubbed the Extended Complementary Filter (ECF).The ECF offers improved robustness to noise while maintaining low computational power and predictable convergence compared to other alternatives.It uses accelerometer and magnetometer data to generate a quaternion, which can be compared to the systems internal orientation updated using the gyroscopic data to define a predicted orientation error.This error is then corrected through the ECF.The ECF uses computationally inexpensive functions to produce its error term and can correct orientation with any error, thus it is ideal for implementation on low-power systems such as human wearables or microrobots.
We present the mathematical formation of the ECF and results from a series of experiments where it is benchmarked against the popular GDA, demonstrating improvements in convergence, computational efficiency, and robustness over available alternatives today.We also introduce a new form of orientation error correction for known movements in the presence of fluctuating magnetic fields, which is implemented as a part of the ECF architecture.The enhanced capabilities of the ECF are then demonstrated in a case study as a human motion monitoring wearable.In the study, the ECF is implemented to provide complete limb orientation feedback to stroke patients performing rehabilitation exercises, highlighting unique performance features in wearables for telemedicine.Novel aspects of the ECF have spawned a new commercial product line of inertial measurement products released by x-io Tech (www.x-io.co.uk).These systems are now very widely used in a breadth of commercial and academic applications.
In summary, new contributions of this work include: 1.The formulation of a new Extended Complementary Filter (ECF) for fusion of MARG data.2. A quantitative comparison of the ECF against the popular GDA alternative demonstrating veracity.3. A new formulation for magnetic disturbance compensation for known activities which enables capture of full orientation tracking despite inconsistencies in magnetic field.4. Complete implementation of the ECF in hardware in a wearable/human performance (rehabilitation) application.

Introduction of an entire commercial product line based upon
the new ECF algorithm.This is the first publication outlining the ECF formulation, benchmarking and application for full-body tracking.It is also the first publication highlighting its resulting product line.

C. Organization of paper
Section II describes the mathematical formulation of the ECF, proposes solutions to errors introduced by noisy data, and outlines a new magnetic correction scheme for known movements.Section III provides a quantitative benchmark evaluation of the ECF versus the popular GDA, with results extended to other inertial sensor fusion algorithms.Section IV describes the practical implementation of the algorithm as a part of a wearable for human performance assessment (telemedicine stroke rehabilitation).Section V summarizes the commercial product line and new research based on the ECF and Section VI draws conclusions from the investigation.

A. Algorithm derivation
The extended complementary filter introduced in this investigation is drawn from a quaternion vector (   ) representing the orientation of a global reference frame () relative to the IMU (I).This global reference frame is defined as one in which gravity points in the negative Z-direction, and the perpendicular component of the geomagnetic field points in the positive X-direction.
The value for    is obtained by integrating the rate of change the orientation (   ̇): An approximation to the rate of change can be made using gyroscopic data (), with the previous orientation (    ̂) as a reference point.Since, gyroscopic data is known to suffer from cumulative errors over time, a correction is applied using an error term (e), calculated using global reference data and scaled by the algorithms gain (K), such that: The error term () is calculated as follows (3-24): The reference vector for gravity ( () ) is defined as: (3) This reference vector is rotated into the local reference frame as described in [33] by multiplying by the current estimation for orientation (), to give (   () ).Given: () = . () . −1 (5) It has been shown [33] that: . () . −1 =   . () (6) Where: Therefore substituting (3) and ( 7) into (6) gives: N.B.For normalized quaternions: This can be proved using: (9) can therefore be used to convert between the forms in (7).
The correction to the orientation estimation () is performed through the calculation of two error terms, which are derived from the cross product of the measured and reference vectors for gravity and the geomagnetic field.All vectors ( ⃗) are normalized prior to use, according to: Given a measured vector ( () ), the first error term derived from the accelerometer   is defined by: To remove the effects of magnetic inclination: () = . () . −1 (16) Substituting ( 6), ( 7) and ( 15) into ( 16) gives: The measured magnetic vector  () must also be converted to magnetic east (orthogonal to gravity) to remove inclination, therefore the second error term is given by: Under normal conditions, the error is therefore given by: Due to required normalization, a singularity will occur if either  () = 0 or  () = 0, therefore, the error can be described: This error can then be combined with the gyroscopic integration to provide an accurate correction of gyroscopic drift.The value  describes the rotation needed to perform orientation estimation corrections.Since the vectors are normalized before use, the step size of the correction is dependent on the angle between the measured vectors and the transformed reference vectors.This can be further adjusted by including a gain value (), resulting in the final correction term ().

B. Universal convergence (all orientations)
Convergence can be proven by demonstrating that convergence is possible for any given rotation.A complete step towards convergence will consist of two correcting rotations, as shown in (19).The first rotation is calculated by taking the cross product of the accelerometer reference and the measured accelerometer vector.As a result, the correcting step will always be perpendicular to these two vectors.The product of two rotations is another rotation, so we can convert this cross product into a frame (l) where the reference vector ( ̂() ) is parallel to the X-axis, and the cross product (  ) is parallel to the Z-axis.Given this frame, the rotation  between  ̂() and   () can be described as a rotation of  around the Z-axis where: As a result, the size of the correcting step will be equal to the magnitude of the   vector (‖  ‖).This can be calculated using: ‖  ‖ = ‖ ̂() ‖‖   () ‖ sin  (22) Since both vectors are normalized prior to the cross product being calculated, the magnitude of both will be one, and therefore ‖  ‖ = sin   0 <  < .
This shows that the correcting step can be calculated at all points for rotations where the equivalent rotation can be defined as 0 <  <  around the axis defined by the cross product.This is shown in Figure 1.The amplitude of the correcting rotation as defined here is always positive, and therefore the algorithm will always converge to the correct value, with the exception of the single point where  = .Since sin  ≈  for small values of , this provides very efficient convergence.It can also be noted that since this is the case, gain () must be smaller than 1 to stable convergence.
While each complete convergence step requires two rotations, each rotation can be expressed in this frame, thus this is a valid description of both steps.Finally, due to the definition outlined in (18), the two steps will always be orthogonal to one another, meaning that the two convergences will not hinder each other.

C. Magnetic disturbance rejection: 6-DoF-9-DoF switching
Consistency in measuring magnetic orientation is arguably the most significant factor limiting widespread use of full orientation in wearable IMUs [29].Sources of magnetic interference are relatively common in indoor environments and can distort orientation estimators, which rely on the geomagnetic field.Such disturbances negate magnetic correction, which must then be disregarded for orientation estimation.Geomagnetic disturbances are, however, very hard to identify reliably.The geomagnetic field varies from ≈0.2-0.65 gauss on a global scale, but locally the signal strength is more consistent.Sources of magnetic interference can change the overall amplitude of the measured magnetic field.As a result, cases where  () fall outside the given range are likely to indicate the presence of a source of magnetic disturbance.
In practice, this can be identified by replacing  () in equations ( 16) and ( 18) with ′ () , given by: and   represent the minimum and maximum strength of the geomagnetic field in the units of the magnetometer.Therefore, ( 20) and ( 21) can be combined to give: <    2:   < ‖ () ‖ <   (24) When implemented, this form of magnetic disturbance rejection uses the magnetometer and accelerometer to perform corrections as outlined in the Section II-A.When interference is detected in the magnetometer data, the algorithm switches to only performing corrections using the accelerometer data.This is known as 9-DoF to 6-DoF switching and is useful in situations with intermittent magnetic interference.

D. Magnetic Disturbance compensation
In some specific cases, 9-DoF to 6-DoF switching is not sufficient to maintain a robust orientation.The 6-DoF algorithm can only correct errors introduced through gyroscopic integration in the pitch and roll directions.Gyroscopic drift in the yaw direction is undetectable using the accelerometer, since the axis of rotation is parallel to the measured vector.While this is a low frequency source of error, prolonged exposure to magnetic interference may leave time for the algorithm to accumulate noticeable error.
An alternative is to actively compensate for the errors in magnetometer.This can be achieved in applications where the IMU is performing known movements by providing simulated magnetometer data to the algorithm in real-time.In the implementations described here, this was achieved by using keyframes.Keyframes are instances during a movement which define that movement, and from which the entire movement can be reconstructed.In the example of a smooth movement from one orientation to another, the only required keyframes would be the initial and final orientations, since all the orientations in between can be derived.More complex movements will require a greater number of keyframes.Keyframes are often used in animation to ensure than an animator does not have to manually animate every frame to achieve a smooth movement.
Keyframes can be used to perform magnetic disturbance compensation in systems where variations on a known motion are being observed.To generate keyframes, the movement must be recorded with minimal magnetic interference or gyroscopic drift.This can be achieved by calibrating the gyroscopes and then performing the range of motions once.The quaternion  is recorded at orientations which are representative of the entire movement.The accelerometer and magnetometer vectors which converge to  can be calculated by substituting (3) and ( 15) into (5) to produce values for  () and  () for each sensor: Each keyframe therefore consists of two 3D vectors representing the accelerometer data and the magnetometer data.In cases where the magnetometer is unavailable, accelerometer data ( ̂() ) can be evaluated against  () for each keyframe.The keyframe with the smallest difference between the two frames is taken as the systems estimation for the current phase of movement, and the value for  () is used in (18) as follows: () =  ̂() ×  ̂() ×   () (26) The final implementation of this magnetic disturbance compensation scheme can be expressed as: This provides an orientation estimation scheme that is robust to both magnetic interference and gyroscopic drift for known activities in uncontrolled conditions.

E. Gain
The value for gain () controls the response of the algorithm to changes in the accelerometer and magnetometer.In normal use  will be comparatively low, since the purpose of including the error term is to compensate for low frequency errors in the gyroscopic integration, and therefore it is expected that the difference between the original orientation rotated by the gyroscopic integration and the actual orientation will be low at each step.The value for  used for normal operation can be defined as   .The scenario where is not satisfactory is during the initial convergence.Initially, the orientation of the IMU is not known, and so the first value of    ̂ is effectively random (although pragmatically an identity quaternion is often used).The algorithm therefore relies on the error term to determine the orientation, instead of simply correcting it.
For this initial period, a larger value for  is desirable since a larger correction will minimize the time taken to converge to the correct current orientation.We can define the length of this initial period as   and the larger value for  as   , then a gain strategy can be designed such that: Initial gain function is required for this implementation, since the error magnitude of the error used for correction will be defined by a sine relationship.As a result, if the difference in orientation between the previous orientation and the true orientation represents a rotation which is larger than π/2, the magnitude of the correction will decrease in size, and will continue to decrease until the rotation reaches π, at which point the algorithm will fail to converge.Selecting an appropriate value for   will avoid this problem.
The value for   ,   and   are usually found by examining the behavior of the algorithm in the final application, and tuned for specific effects, however the values   = 0.5,   = 10 and   = 3 were found to provide suitable performance for many applications during both the initialization phase and after.This high value of   is designed to lead to fast convergence in situations where the angle offset is close to , however it is observed from Section II-B that this will initially lead to an unstable convergence which becomes stable as  approaches 1.The values for   and   describe the period of initial convergence where the algorithm is not providing accurate orientation estimation, and therefore must be defined according to the requirements of the system.The   value is equivalent to the weighting value in a normal complementary filter, and is selected according to the sample rate, the anticipated frequency of the observed motion and the anticipated frequency of the sensor noise.

III. ECF -GDA PERFORMANCE BENCHMARKING:
We have executed a series of tests comparing the ECF against the well-known Gradient Decent Algorithm (GDA) [30].The GDA is well-acknowledged as one of the most popular inertial sensor fusion algorithms today [18,29] with a wealth of literature comparing its performance to available alternatives.It is commonly used to benchmark inertial sensor fusion (e.g.[3,12,32,34]).It was selected as a benchmark for comparison to maintain consistency with testing procedures in existing literature.Furthermore, a recent survey [29] evaluated the GDA as a part of a comparison of all common state-of-theart methods (including Kalman filtering) and found comparable performance, leading us to conclude performance assessment of the ECF to the GDA establishes a basis for comparison to all alternative sensor fusion methods.
Initially, the new ECF algorithm was implemented on a virtual IMU.The orientation of this IMU could be set, and the corresponding sensor output signals were generated and passed to GDA and ECF algorithms.This had two major benefits compared to in-situ testing: 1) the exact orientation of the IMU was known, and the sensor output signals were correct for that orientation, so convergence could be quantified, and 2) the signal condition was known.Initially, signals contained no noise or bias, however virtual noise was subsequently added.
We programmed the same implementation of the GDA that was previously compared to Extended Kalman filters and found to provide comparable accuracy under static conditions (within ~0.1º), dynamic conditions (within ~0.1º), and more robust to sources of magnetic interference [29].It was therefore decided that this comparison would provide the most useful benchmark for demonstration of ECF performance.We tuned the GDA with its default gain value (0.5) and the time taken for both algorithms to converge was observed.For 1,000 convergences between two randomly generated points, the ratio of convergence time was 0.68:1 (ECF:GDA).A samping of the convergence paths are shown in Figure 3.

A. Decoupling of rotation error: ECF vs GDA
A shortcoming of the GDA is that the algorithm does not decouple yaw from pitch and roll [18,29].This is a significant problem for a number of situations, particularly balancing applications and drone control.The ECF ensures rotations in each axis are independent of in the other axes.This can be demonstrated by placing the virtual IMU at a given orientation, rotating it π around one axis instantaneously and observing the convergence, as shown in Figure 2 for the GDA and ECF.

B. Magnetic disturbance rejection: ECF vs GDA
Performance was tested in the presence of magnetic field disturbance for both the ECF and GDA.In these tests, the virtual IMU was set at an orientation, and the algorithms were allowed to converge.Upon convergence, a virtual source of magnetic interference was introduced.The field strength of the interference was 0.55 gauss and orthogonal to both magnetometer and the accelerometer vectors.
In the first set of experiments, the interference was introduced as a step signal, which may be observed if a source of magnetic interference was switched on.In this case, the interference caused the magnitude of the measured magnetic vector to exceed the maximum defined (0.65 Gauss), and the algorithm made corrections based on gravity alone.ECF vs GDA convergence is shown in Figure 4.
In the second experiment, the source of magnetic interference was introduced over the course of 0.5 seconds, as if the IMU had been moved into the presence of the interference.In this case, the ECF did deviate from the correct value, however it stopped converging far sooner than the GDA.ECF vs GDA convergence is demonstrated in Figure 5.
Both experiments were designed to mimic actual use conditions observed in full-body motion tracking experiments in telemedicine [35], where orientation estimation was impossible with existing IMU sensor fusion algorithms.Additionally, the influence of magnetic interference will be very difficult to observe.Calibrating the values for   and   to reflect local field strength will further improve performance and reduce the number of sources of interference which will affect the convergence of the algorithm.

IV. IMPLEMENTATION: WEARABLE HUMAN MOTION TRACKING REHABILITATION SYSTEM
Human-Motion Tracking is vitally important in an array of fields.For example, human-machine interface (HMIs) and health trackers both rely on activity recognition, however these applications often use instantaneous, context agnostic classification, and as a result can be limited in the level of recognition they provide [12].Applications such as gaming and augmented/virtual reality require more information to perform accurately yet commonly rely on cameras to perform either full or partial body pose estimation which requires external hardware and suffers from image occlusion.Human Pose Estimation (HPE) can provide information on pose actions which increases the bandwidth of the information recorded and facilitates higher quality of interaction.However HPE can require expensive camera equipment, reducing accessibility and limiting the field of operation.
Rehabilitation and medical diagnostics are areas where HPE has the potential to provide important contributions.Many diagnostic tests require a patient to perform a series of actions which can be observed by an assessor, who can rate the movement for range, speed and other criteria.One example which has received extensive attention in literature is stroke rehabilitation.Patients who have suffered a stroke resulting in inhibited movement often rely on regular contact with a trained health professional.Data from the Global Burden of Diseases, Injuries, and Risk Factors Study 2010 indicated that there were approximately 33 million stroke survivors, of which 70% suffered arm function impairment, which was persistent in 40% of cases [36].Intensive therapy is a primary predictive factor for improved function [37], however regular and ongoing contact is often infeasible due to cost and therapist availability.
One way to increase the therapy time without increasing the time spent with a therapist is to use telehealth (TH) [35].TH technologies are those which facilitate long-distance healthcare.In rehabilitation applications, this may include both guided exercise and the remote generation of diagnostically relevant data.In applications such as this, accurate HPE is of vital importance, since changes in motion may be subtle.
A major challenge in TH is the lack of patient feedback provided by virtual systems.In our past work, user-needs analysis involving patients and therapists indicated a virtual 'avatar' presenting real-time feedback on their own movement would be a vitally important feature in a rehabilitation system for home use.Existing systems which simply demonstrate the correct motion to complete an exercise were deemed unhelpful and demotivational, since they did not allow the patients to selfcorrect, or to see improvement.Based on this, the Mechanical Muscle Activity with Real-time Kinematics (M-MARK) system was developed (https://youtu.be/mv18iZ9G7-8).A complete description of the clinical aims and contributions of this work is outlined by J. Burridge [35], and the fusion with muscle sensors is outlined by R. Woodward [38].
Our team has implemented the ECF with new magnetic compensation in fluctuating fields as a means of virtual feedback for patient exercises in the M-MARK system.As a case study, we detail the specific orientation estimation implementation.M-MARK is a take-home rehabilitation system designed to provide feedback to stroke patients during guided exercises with a wearable upper body sensor suite providing online HPE.The suite is integrated into a longsleeved shirt with MARG and/or muscle sensors on the torso, upper arm, lower arm and hand.
The MARG sensors in this application were first introduced for multi-degree-of-freedom robot teleoperation [12].In this application, all four MARG sensors were connected to a computer providing the visual feedback to the user through Bluetooth.Each sensor recorded measurements for linear acceleration, rotational acceleration and the local magnetic field, and transmitted this data at 500Hz.Data were passed to the algorithms described here as they were collected through the Bluetooth receiver on the computer.This allowed quaternion rotations, and subsequently kinematic information, to be generated in functionally real-time.The MARG sensors can be seen in Figure 6.
Home use required an avatar that was representative of the patient's movement, regardless of any sources of magnetic interference.The algorithm outlined in this paper was used to generate an orientation for each MARG sensor, allowing partial body kinematics to be observed in real-time.This was used to create both an anatomically correct animation, known as an avatar, which provided visual feedback to the user, facilitating self-correction, and to derive exercise specific metrics which could be transmitted back to the therapist for their reference at follow-up sessions.The system was used in case studies involving 6 patients and 2 therapists, and feedback reflected clinical judgement.
During home use, subjects are instructed to perform a known exercise, and the accelerometer/magnetometer data for the keyframes were loaded.The algorithm described in Section II was used, meaning that changes in orientation were tracked using the gyroscopic data.At each point, the accelerometer data were measured from all four MARG sensors.These four vectors were compared to the vectors generated for each of the keyframes to determine which part of the exercise the patient was likely to be in.This is done by calculating the difference value () for each keyframe, where: The keyframe which produced the smallest  value is selected, and the measured accelerometer data ( ̂()  ), along with the pre-recorded magnetometer data (   () ) were passed to the algorithm to predict the most likely orientation for each MARG sensor.As a result, an avatar which moved in the same way as the patient, but which was not subject to gyroscopic drift was achieved, facilitating self-correction and enabling TH for home stroke rehabilitation.The experimental layout is shown in Figure 7 and Figure 8.An example of this working is given in Figure 9.
We are also extending this work in a similar vein for Parkinson's disease (PD) where a neurologist uses movement to assess patient state and prescribe medication dose [39,40] An HPE system could automate this process, however existing systems only use acceleration data due to challenges in fusing data for full orientation which our system overcomes.Serg Technologies (http://sergtechnologies.com/) is leading this work to commercialize a home TH PD system.

V. COMMERCIAL TRANSLATION: X-IO TECHNOLOGIES
The ECF algorithm enables a level of accuracy and computational efficiency which has spawned an entire product line of commercial IMUs.Its implementation permits lower sampling rates and power consumption resulting greater robustness and extended battery life.There is strong market demand for these features since many commercial solutions for orientation monitoring rely on a tuned Kalman filter, necessitating a higher processing capacity and higher component cost.Three products based on the ECF are currently on the market created by x-io Technologies: the x-IMU, the x-BIMU and the NGIMU, shown in Figure 10.These selfcontained units consist of a MARG suite and a microcontroller to perform the sensor fusion.Algorithm efficiency enables lowpower and low-cost components hosting all processing in embedded firmware.
The x-IMU was the first product released as an all-in-one data logging platform for inertial monitoring.It was capable of recording 18+ hours of data in one session and can continuously stream orientation over Bluetooth for real-time applications.The board was 33x42x10mm, and at 12g, one of the smallest complete solutions commercially available.It has been involved in number of projects, including HMI applications, robot localization and rehabilitation.
The x-BIMU was subsequently released for real-time applications that required extremely small size.Since it did not require on-board logging, it was significantly smaller at 25x30x8mm and 5g, with weight reduction providing a resulting battery life of 5 hours.Data can be streamed from this device using either Bluetooth or a WPAN.
The NGIMU was released after the x-BIMU as a direct replacement for the x-IMU.While the volume and weight were similar, the NGIMU made the change to Wi-Fi as the wireless profile, allowing for easier integration into larger sensing  systems.It also increased the accelerometer range from 8G to 16G, increasing the number of potential use cases.Figure 10 shows the current ECF product line.
The IMU products described here form the flagship line of xio Technologies, with the aim to make the pervasive orientation monitoring widely accessible.Through this product line, the ECF has been implemented in well over 200 publications worldwide.A small sampling of very recent areas of impact that rely on the ECF through the product line include: human motion tracking [41], sports performance [42], safety [43], telemedicine [44], mobile robotics [3], rehabilitation [45], assistive technology [46], veterinary science [47], virtual reality [4], and even entirely new applications such as smart agriculture [48] and music composition [49].In the spirit of transparent dissemination we have released open source code for the ECF which has been downloaded over 50,000 times.The IMU product line with the ECF embedded in hardware has circulated 10,000 units for research and commercial use.A list of published research making use of the ECF and its open source code are linked from the x-io homepage at: https://x-io.co.uk/.

VI. CONCLUSIONS
This paper outlines the formulation of a new orientation estimation algorithm, an Extended Complementary Filter (ECF) fusing MARG data to provide robust full-body orientation.The algorithm combines computational efficiency, and ease of implementation with high-level of accuracy compared to existing alternatives.The algorithm was benchmarked against the popular Gradient Descent sensor fusion Algorithm (GDA) and found to have several benefits: 1.On average, the proposed algorithm converges in 68% of the time than the GDA.This results in faster initialization and faster correction of large errors.2. The ECF decouples yaw rotations from pitch and roll.As a result, convergence from any given point is predictable and robust to disturbances in one axis (e.g.magnetic fields disrupting yaw calculation).3. The algorithm is robust to conditions where the vectors measured by the magnetometer or accelerometer has a length of zero.This allows implementations that allow the quality of the measured signal to be assessed for noise.In noisy conditions, the sensor can simply be ignored without A) affecting the existing orientation and B) without inhibiting the use of data from the other sensor.4. The algorithm, as implemented in this investigation, includes a novel method of overcoming the problem of magnetometer interference in task specific applications by simulating output from one sensor in the fusion.The efficient implementation allows orientation calculation on low-power microcontrollers, facilitating small form-factor, self-contained orientation sensors.Sensors using the new algorithm have been implemented in a wearable suite in a stroke rehabilitation program to create a tele-health stroke rehabilitation solution to help patients complete assigned therapeutic exercises.This resulted in an avatar which appeared to follow the subject's movement but was not affected by gyroscopic drift or magnetometer interference.It is also noteworthy that the stroke rehabilitation system captures inertial and muscle activity simultaneously, which enables the possibility of fusing features of both [50], providing a new dimension in rehabilitation therapy.
The ECF has been implemented in the flagship product line of x-io Technologies, a commercial entity which specializes in MARG sensing solutions.The company has released three very successful products designed to provide an easy way to measure orientation and inertial movement.The products are very widely used and have contributed to thousands of applications in a wide variety of areas.Another company, Serg Technologies, is translating aspects of the technology in telehealth (TH).
The ECF algorithm has significantly impacted the field of inertial measurement and, with the new magnetic compensation scheme, is a key enabling technology for widespread use.There is a very strong need today for small lower power sensor packages (e.g.wearables) with the capacity for full orientation tracking.Future applications are cross a very broad spectrum.
S. O. H. Madgwick is with x-io Technologies Ltd, Bristol, UK. S. Wilson and C Kapatos are with Serg Technologies Ltd, London, UK. S. Wilson and R. Vaidyanathan (*r.vaidyanathan@ic.ac.uk) are with Imperial College London, UK.R Vaidyanathan is also with the UK Dementia Research Institute Care-Research and Technology Centre (DRI-CRT), London, UK.R. Turk and J. Burridge are with University of Southampton, Southampton, UK Sebastian O.H. Madgwick, Samuel Wilson, Ruth Turk, Jane Burridge, Christos Kapatos and Ravi Vaidyanathan* An Extended Complementary Filter (ECF) for Full-Body MARG Orientation Estimation M TMECH-01-2020-9733

Figure 1 -
Figure 1 -Amplitude of correcting step at any rotation

Figure 8 -
Figure 8 -Demonstration of magnetic compensation technique -(leftfeedback when using magnetometer data in a magnetically polluted environment, Torso and upper-arm are severely affected by magnetic interference during real-time use; right -feedback using context specific magnetometer data from previous recordings during real-time use, Torso and upper-arm provide contextually relevant data and respond as if magnetic interference was not present).

Figure 9 -
Figure 9 -Response of the avatar when performing rehabilitation exercise in clean (A) and magnetically polluted (B) environment.Left (red) -ECF with no disturbance detection (green) with detection and magnetic compensation in place.Right -Exercise being performed with real-time feedback.Full video available at: https://www.youtube.com/watch?v=pQeA-LO4ukc