Quasi-Real Time Estimation of Angular Kinematics Using Single-Axis Accelerometers

Abstract

In human movement modeling, the problem of multi-link kinematics estimation by means of inertial measurement units has been investigated by several authors through efficient sensor fusion algorithms. In this perspective a single inertial measurement unit per link is required. This set-up is not cost-effective compared with a solution in which a single-axis accelerometer per link is used. In this paper, a novel fast technique is presented for the estimation of the sway angle in a multi-link chain by using a single-axis accelerometer per segment and by setting the boundary conditions through an ad hoc algorithm. The technique, based on the windowing of the accelerometer output, was firstly tested on a mechanical arm equipped with a single-axis accelerometer and a reference encoder. The technique is then tested on a subject performing a squat task for the knee flexion-extension angle evaluation by using two single-axis accelerometers placed on the thigh and shank segments, respectively. A stereo-photogrammetric system was used for validation. RMSEs (mean ± std) are 0.40 ± 0.02° (mean peak-to-peak range of 147.2 ± 4.9°) for the mechanical inverted pendulum and 1.01 ± 0.11° (mean peak-to-peak range of 59.29 ± 2.02°) for the knee flexion-extension angle. Results obtained in terms of RMSE were successfully compared with an Extended Kalman Filter applied to an inertial measurement unit. These results suggest the usability of the proposed algorithm in several fields, from automatic control to biomechanics, and open new opportunities to increase the accuracy of the existing tools for orientation evaluation.

1. Introduction

The inverted pendulum (IP) is a simple system that finds application in many disciplines of science. Despite its simple nature, it represents a non-linear, unstable and non-minimum phase system, finding several applications in control theory and biomechanics. Achieving stability of an IP has become a common engineering challenge for researchers and the problem has been discussed theoretically by several authors [1–5] and experimentally demonstrated by others [4–7].

There are many examples of the IP model, both man made and found in the natural world. In control theory the challenge of control made the IP system a classic tool in control laboratories [8–12]. The balancing of an IP by moving a cart along a horizontal track is a classic problem in the area of control. Usually one of the state variables of the system which is also the controlled variable, the sway angle, is evaluated from expensive measurement system (e.g., encoder) placed at the pivot joint. The main limitation of this approach is that placing the encoder at the pivot is not always possible.

Arguably the most prevalent example of an IP is a human being. A standing human looks like an IP with the center of mass well above the ground [13–15]. The mechanism to keep balance during the standing posture has intrigued scientists from several fields for a long time [14] and has much significance in clinics. Several authors focused on the estimation of the sway angle since this measurement is related to oscillation of the center of mass of a subject and to the neural control of posture during perturbed and unperturbed stance. However, recent studies [16,17] focused on the contribution of hip and knee to balance control. Given the implications that non-rigidity at the knee and hip may have for the postural control during unperturbed stance, a comprehensive analysis of the ankle, knee and hip movements is required. The analysis revealed that the one-segment IP model is an oversimplification of reality. A multilink model which takes into account thigh, shank and arm-trunk-head segments should be adopted to have a more accurate description of standing balance. From this perspective, the IP model plays the role of the basic element of a multilink chain [18,19]. Kinematics analysis of a multi-link chain should be analyzed using the framework of multiple IPs [16].

In the last years, sensing hardware developments have made available on the market miniaturized inertial (accelerometers and/or gyroscopes) and magnetic sensors. These sensors have found applications in robotics and biomechanics because of their low cost, small size and weight, low power consumption, ease of use and portability. The problem of accurate tracking of orientation by means of these sensors has thus become important in several domains since these wearable sensors can be considered the most valuable opportunity to monitor kinematics and dynamics of human subjects outside specialized laboratories. The problem of solving orientation estimation by using efficient sensor fusion filtering algorithms has been investigated by several authors [20–24] in order to overcome some relevant limitations related to the use of inertial and magnetic sensors one by one:

• orientation estimated by time-integrating, from unknown initial conditions, the signals from a triad of mutually orthogonal uni-axial gyros is prone to errors that grow unbounded over time, due to low-frequency gyro bias drifts;

• it is difficult give a simple interpretation to the accelerometer signals, where the component due to the gravity field (vertical reference) coexists with the component related to the motion of the object;

• nearby ferromagnetic materials are critically disturbing sources when attempts are made to interpret the signals from a tri-axial magnetic sensor as the horizontal reference.

However, the set-up including one inertial measurement unit (IMU) per segment is not cost-effective compared with a solution in which a single-sensor is used. Recently, Bagalà et al.[19] showed that it is possible to separate the two different contributions (gravity and inertia) of the accelerometer signal through a bidirectional low-pass filter and a model-based approach. The accuracy in the joint angles estimation using a single-axis accelerometer (SAA) per segment was comparable with that obtained in several previously published studies where two or more sensors per segment were required [25–30].

The aim of this study is to provide a novel technique, faster than that proposed previously [19], based on the Thomas algorithm [31], for the quasi-real time estimation of the sway angle of an IP using (only) one SAA. The algorithm is then extended to a 2-link chain for the estimation of the knee flexion-extension angle of a subject performing a squat task. Furthermore, a comparison with an Extended Kalman Filter (EKF) applied to different sensor configurations was performed.

2. Experimental Section

2.1. Inverted Pendulum Kinematics

An IP model in 2D is analyzed. A SAA is placed at height h from the pivot point P with the sensitive axis orthogonal to the longitudinal axis of the IP (Figure 1(a)). The accelerometer output, a_x(t) can be expressed in the continuous-time domain as the sum of an inertial contribution depending on the angular acceleration component, α(t) (the second derivative of the sway angle, θ(t)) and a gravitational term depending on the sway angle, as follows:

$$a_x(t)=h\alpha (t)-gsin\theta (t)$$ (1)

where g is the gravitational acceleration. Equation (1) is a second order differential equation which has a clear similarity with the equation of motion of the IP: under the assumption of no friction or any other resistance to movement, if d is the distance between the center of mass of the pendulum and the pivot, m is the mass of the pendulum, M is the moment at the pivot and J is the moment of inertia, the equation of motion is M = Jα(t) – mgd·sinθ(t). Dividing by md the equation of motion $\frac{M}{md}=\frac{J}{md}\cdot \alpha (t)-g\cdot sin\theta (t)$ becomes similar to Equation (1) since $\frac{M}{md}$ has the dimension of an acceleration and $\frac{J}{md}$ the dimension of a length. Moreover, from a mechanical point of view, the accelerometer is described by a mass-spring system whose behavior is similar to that of a pendulum. The accelerated mass has the same direction of the gravity when the accelerometer is placed on a classical pendulum (stable behavior) and has opposite direction in the case here presented of the IP (unstable behavior).

Figure 1.

(a) IP model and (b) IP model with misalignment of the sensitive axis of the SAA.

Several authors [28–30] used an IP and a quasi-static model in which the inertial term in Equation (1) is neglected, so the sway angle can be easily evaluated from the gravitational term and the accelerometer output. This approximation introduces significant errors when the inertial term is not negligible and the frequency bandwidth content of the angular sway is wide.

Unlike the ideal condition of the mathematical model described by Equation (1), the placement of the sensor on a mechanical link potentially introduces some errors due to the non-orthogonality of the sensitive axis of the SAA to the segment. Under the assumption of small misalignment angle, Equation (1) is modified taking into account the projections of the centripetal, tangential and gravity accelerations on the sensitive axis:

$$a_x(t)=h\alpha (t)-gsin\theta (t)+\beta \left[h{\omega }^2(t)-gcos\theta (t)\right]$$ (2)

where ω(t) represents the angular velocity and β describes the SAA non-orthogonality, as shown in Figure 1(b).

In the discrete-time domain, Equation (2) can be rewritten as:

$$\begin{array}{l}{a}_{x,k}=h{\alpha }_k-gsin{\theta }_k+\beta \left[h{\omega }_k^2-gcos{\theta }_k\right]\hfill \\ k=1,\dots ,N\hfill \end{array}$$ (3)

where N is the number of samples.

By using the first and second derivative approximations for the angular velocity, ω(t), and the angular acceleration, α(t), the relationship between the accelerometer output and the angular sway becomes:

$$\begin{array}{l}{a}_{x,k}=\frac{h}{T^2}({\vartheta }_{k+1}-2{\theta }_k+{\theta }_{k-1})-g\frac{sin{\theta }_k}{{\theta }_k}{\theta }_k+\beta \left[h{\left(\frac{{\theta }_{k+1}-{\theta }_{k-1}}{2T}\right)}^2-gcos{\theta }_k\right]\hfill \\ k=2,\dots ,N-1\hfill \end{array}$$ (4)

where T is the sample time. Equation (4) can be rewritten as follows:

$$\begin{array}{l}{a}_{x,k}=\frac{h}{T^2}{\theta }_{k-1}\frac{h}{T^2}{\theta }_{k+1}+\left(-2\frac{h}{T^2}-g\frac{sin{\theta }_k}{{\theta }_k}\right){\vartheta }_k+\beta \left(\frac{h}{T^2}\frac{{\left({\theta }_{k+1}-{\theta }_{k-1}\right)}^2}{4}-gcos{\theta }_k\right)\hfill \\ k=2,\dots ,N-1\hfill \end{array}$$ (5)

For a given time window of W samples, Equation (5) is a system of W − 2 non linear 2nd order differential equations. After setting the two boundary conditions θ₁, θ_W, whose details will be provided in the next section, Equation (5) can be expressed in a matrix form as:

$$\underset{\mathbf{\text{A}}}{\underbrace{\left[\begin{array}{c}{a}_{x,2}\\ ⋮\\ a_{x,W-1}\end{array}\right]}}=\underset{\mathbf{\text{C}}}{\underbrace{\left[\begin{array}{ccccc}{C}_2& B& 0& \dots & 0\\ B& C_3& \ddots & \dots & 0\\ 0& \ddots & \ddots & \ddots & ⋮\\ ⋮& \ddots & \ddots & \ddots & B\\ 0& \dots & 0& B& C_{W-1}\end{array}\right]}}\underset{\mathbf{\theta }}{\underbrace{\left[\begin{array}{c}{\theta }_2\\ ⋮\\ {\theta }_{W-1}\end{array}\right]}}+\underset{\mathbf{\text{D}}}{\underbrace{\left\{B\left[\begin{array}{c}{\theta }_1\\ 0\\ ⋮\\ 0\\ {\theta }_W\end{array}\right]+\frac{\beta B}{4}\left[\begin{array}{c}{\left({\theta }_3-{\theta }_1\right)}^2\\ {\left({\theta }_4-{\theta }_2\right)}^2\\ ⋮\\ {\left({\theta }_{W-1}-{\theta }_{W-3}\right)}^2\\ {\left({\theta }_W-{\theta }_{W-2}\right)}^2\end{array}\right]-g\beta \left[\begin{array}{c}cos{\theta }_2\\ ⋮\\ cos{\theta }_{W-1}\end{array}\right]\right\}}}$$ (6)

where $B=\frac{h}{T^2}$, $C_k=-2B-g\cdot \frac{sin{\theta }_k}{{\theta }_k}(k=2,\dots ,W-1)$. Equation (6) can be rewritten in matrix form as A = Cθ + D and the sway angle θ can be obtained from the accelerometer output vector A, through the C-matrix inversion as:

$$\mathbf{\theta }={\mathbf{\text{C}}}^{-1}(\mathbf{\text{A}}-\mathbf{\text{D}})$$ (7)

The computational cost for the inversion of N-square tridiagonal matrix is $\frac{1}{6}{N}^3+O(N^2)$, which can be reduced to O(N) by using the tridiagonal matrix algorithm proposed by Thomas [31], which is a simplified form of Gaussian elimination, based on three steps (factorization, forward and backward substitution).

In order to apply the Thomas algorithm for the evaluation of the sway angle, Equation (7) was rewritten in the form of a tridiagonal equation system as:

$$\mathbf{\text{C}}\mathbf{\theta }=\underset{{\mathbf{\text{A}}}^{\prime }}{\underbrace{(\mathbf{\text{A}}-\mathbf{\text{D}})}}\Rightarrow \mathbf{\text{C}}\mathbf{\theta }={\mathbf{\text{A}}}^{\prime }$$ (8)

2.2. Algorithm Overview

In order to solve Equation (8), a novel algorithm (Figure 2) for the quasi-real time estimation of the sway angle θ from the accelerometer output vector A, was implemented in Matlab2008. The algorithm is based on the windowing of W sample sliding window to the accelerometer output vector A. As shown in the right member of Equation (6), the solution of Equation (8) requires setting the two boundary conditions, θ₁, θ_W of the sway angle vector θ. They are zeroed when the process starts and are updated, step by step, through the equations described in the following. If the boundary conditions are unknown, as usually occurs in experimental conditions, the angular sway estimate is negatively affected by a transient response at the boundaries. Nevertheless, if the window size is sufficiently large, the central term of the vector θ, obtained after solving Equation (8), is largely independent from the boundary conditions.

Figure 2.

Algorithm structure.

In the following, the vectors θ^(k) and A^(k) represent the sliding window of W samples at step k. Assuming known the misalignment angle, β, the sway angle can be obtained by the following steps (Figure 2):

1. Initialization: at the first step, Equation (8) is initialized by zeroing the windowed angle vector θ⁽¹⁾ = [θ₂, …, θ_W−1]^T and the two boundary conditions (θ₁ = 0, θ_W = 0). The terms on the diagonal of the matrix C are assumed to be constant (C_k = −2B – g, k = 2, …, W − 1) by approximating $\frac{sin{\theta }_k}{{\theta }_k}\approx 1$. The sliding window of the accelerometer output A⁽¹⁾ = [a_x,2 …, a_x,W−1]^T is considered as input.

2. Thomas algorithm: the sway-angle vector θ^(k) is estimated from Equation (8) with I iterations by using the Thomas algorithm [31]. A single iteration is enough to have accurate estimates, except for the first cycle, which requires more iterations (I = 3) to converge because of the initialization process. Increasing the number of iterations I does not significantly improve the estimation accuracy but increases the computational cost.

3. Get center sample: as discussed before, at step k, the center sample ${\theta }_{W+1}^{(k)}/2$ of the estimated vector θ^(k) is considered as correct estimate, under the assumption of adequately large time windows.

4. Boundary conditions update: according to the result provided by the Thomas algorithm at the previous step, at step k > 1 the two boundary conditions are set as the first element of the vector θ^(k) (left boundary) and as the linear interpolation of the last two elements of the vector θ^(k) (right boundary) as follows:

   $$\{\begin{array}{ll}\begin{array}{l}{\theta }_1^{(k+1)}={\theta }_2^{(k)}\hfill \\ {\theta }_W^{(k+1)}=2{\theta }_{W-1}^{(k)}-{\theta }_{W-2}^{(k)}\hfill \end{array}\hfill & k=2,\dots ,N-1\hfill \end{array}$$

   The terms of the matrix C are updated with the angle vector elements.

5. Window Slide: the input vector A^(k) is updated by adding the new sample at the end of the vector and deleting the first one.

Steps (2–5) are iterated for the total length of the accelerometer output.

The algorithm's computational cost, C_T can be expressed as:

$$C_T=f_s(N-W)\overline{I}{C}_i$$ (9)

where C_i is the computational cost to process W samples by the Thomas algorithm, Ī the average number of iterations per window and N the total number of samples of the signal to be processed.

The computational cost is therefore reduced with respect to the offline method previously published [19], based on the bidirectional filtering of the accelerometer output. In a linear case (small sway angles), the similarity between the quasi-real time algorithm and the previously published method is demonstrated in Appendix A.

2.3. Mechanical Inverted Pendulum

To test the method, the same experimental setup used in [19] was adopted. An aluminum rectangular link was used as an IP driven by hand to sway with a fixed pivot point. The frequency content of the angular sway is [0–2] Hz. Five trials were performed. The mechanical arm was equipped with an absolute encoder (Gurley, mod. 7700, resolution 19 bit, Gurley Precision Instruments, Troy, NY, USA) and an IMU (MTx, Xsens Technologies, Eschede, The Netherlands), consisting of a tri-axial accelerometer, a tri-axial gyroscope and a tri-axial magnetometer, placed at height h = 0.20 m from the pivot point P (Figure 3).

Figure 3.

Mechanical Inverted Pendulum.

The IMU's outputs were acquired at f_s = 50 Hz sampling frequency. To test the algorithm presented in this paper, only the accelerometer output related to the axis orthogonal to the mechanical arm, a_x was considered as input. A preliminary calibration, in which the encoder output was used as reference, was performed to estimate the two geometric parameters h and β. The two parameters were estimated by minimizing the RMSE between the encoder output and the estimated angle.

In order to test the robustness of the method proposed in this paper, results in terms of RMSE were compared with:

• those obtained by using the offline method proposed in [19], where a bidirectional low-pass filtering of the accelerometer output, with a cut-off frequency depending on the position of the sensor, was implemented;

• those obtained by using an EKF applied to the outputs of the IMU (Figure 3), in particular to the accelerometers with sensitive axis orthogonal and along the mechanical arm, a_x and a_y, and to the output of the gyroscope with sensitive axis orthogonal to the plane of the movement, g_z as discussed in the next section.

2.4. Extended Kalman Filter

The outputs of the accelerometers along the x and y directions and the output of the gyroscope with the sensitive axis orthogonal to the plane of the movement (z) are expressed as follows:

$$\begin{array}{l}{a}_{x,k}=h{\alpha }_k-gsin{\theta }_k+\beta \left[h{\omega }_k^2-gcos{\theta }_k\right]\hfill \\ a_{y,k}=-h{\omega }_k^2+gcos{\theta }_k+\beta \left[h{\alpha }_k-gsin{\theta }_k\right]\hfill \\ g_{z,k}={\omega }_k\hfill \\ k=1,\dots ,N\hfill \end{array}$$ (10)

The main equations of the EKF are shown in Equation (11), where w_k and v_k represent the process and measurement noise, assumed to be independent, white and with normal probability distribution with a process noise covariance Q and a measurement noise covariance R. More details about the algorithm are presented elsewhere [32,33]. The state-space model of the EKF is described by the process and measurement model equations:

$$\begin{array}{l}{\mathbf{\text{x}}}_{k+1}=\mathbf{\text{A}}{\mathbf{\text{x}}}_k+{\mathbf{\text{w}}}_k\hfill \\ {\mathbf{\text{z}}}_k=h({\mathbf{\text{x}}}_k)+{\mathbf{\text{v}}}_k\hfill \end{array}$$ (11)

The state vector of the EKF, x(k)_[3×1], at every sampled instant of time k, was defined as $\mathbf{\text{x}}(k)={\left[\theta (k)\omega (k)\alpha (k)\right]}^T$.

In the discrete-time domain the predicted state at the instant k + 1 was obtained as:

$${\mathbf{\text{x}}}_{k+1}=\underset{{\mathbf{\text{A}}}_{[3\times 3]}}{\underbrace{\left[\begin{array}{ccc}1& \text{T}& \stackrel{T^2}{}/\underset{2}{}\\ 0& 1& \text{T}\\ 0& 0& 1\end{array}\right]}}{\mathbf{\text{x}}}_k$$ (12)

The vector of the measurements z(k)_[3×1] was defined considering the outputs of the sensors as $\mathbf{\text{z}}(k)={\left[a_x(k)a_y(k)g_z(k)\right]}^T$.

According to (10), the output vector is related to the state vector through the non-linear relationship z(k) = h(x_k). The output matrix H(k)_[3×3] which relates the measurements z(k)_[3×1] to the state x(k) was thus obtained evaluating the Jacobian matrix of h(x_k) with respect to the state vector.

The process covariance matrix Q_[3×3] was defined under the assumption that process noise affects the jerk only and there are no correlations between the jerk noise sequences. Q_[3×3] has therefore only one non-zero element Q(3,3) set to 10⁻³. The measurement noise covariance matrix R_[3×3] was defined considering the noise which affects the gyroscope and accelerometer outputs. Since correlations between noise of the sensors were assumed to be zero, the covariance matrix was put in the form 10⁻⁸·I_[3×3]. All these tuning parameters were defined after an optimization procedure in which the encoder was assumed as validation standard. In order to start the filtering procedure, initial estimate of the state vector was zeroed whereas the initial estimate of the error covariance matrix P_[3×3] was set equal to the identity matrix.

In order to compare the performance of the window-based algorithm proposed in this study with the EKF, seven different configurations of the EKF were implemented by considering the following measurement vectors:

• $\mathbf{\text{z}}(k)={\left[a_x(k)a_y(k)g_z(k)\right]}^T$;

• $\mathbf{\text{z}}(k)={\left[a_x(k)g_z(k)\right]}^T$;

• $\mathbf{\text{z}}(k)={\left[a_y(k)g_z(k)\right]}^T$;

• $\mathbf{\text{z}}(k)={\left[a_x(k)a_y(k)\right]}^T$;

• z(k) = a_x(k);

• z(k) = a_y(k);

• z(k) = g_z(k);

For each case, the output matrix H(k) and the measurement noise covariance matrix R were adjusted to the dimension of the measurement vector. The tuning parameters, Q and R, were optimized for each case to have the best performance in terms of RMSE of the angle estimation.

2.5. 2-Link Chain: Knee Flexion-Extension Angle Evaluation

In order to show an application of the algorithm proposed in this paper and its extension to a multi-link model for human movement kinematics estimation, the knee flexion-extension angle of a subject performing a squat task was evaluated.

The method is tested on one subject (male, 29 years old, weight 74 kg, height 176 cm) who participated after giving his informed consent. In order to estimate the thigh and shank sway in the sagittal plane during squat tasks [34], a 2-link biomechanical model is introduced for the two segments (Figure 4). The feet are supposed rigidly connected to the ground; the ankle and the knee joints are represented as two hinge joints and the shank (segment 1, length l₁ = 0.41 m and the thigh (segment 2) are modeled as two rigid segments. The HAT kinematics has not been investigated in the present study. The subject was asked to perform a repetition of squat exercises for one minute with arms folded, keeping his movement in the sagittal plane. Five trials were performed. In order to estimate the knee flexion-extension angle, two IMU's (MTx, Xsens Technologies) were placed at measured heights h₁ = 0.27 m, h₁ = 0.19 m with respect to the ankle and knee joint, respectively. Each of the two sensors was placed on a rhomboid rigid plate and mounted on the skin at the lateral side of the thigh and the shank by using three hook-and-loop fastener belts.

Figure 4.

Two-links model for the knee flexion-extension angle estimation.

Four reflective markers were placed on the vertices of each plate, and a stereo-photogrammetric system (SMART eMOTION, BTS) was used for calibration of the geometric parameters (h₁, h₂, l₁, β₁, β₂). The two reference angles with respect to the vertical were evaluated through the 2D Singular Value Decomposition (SVD) method [35,36]. Stereo-photogrammetric and inertial data were sampled at 100 Hz.

The outputs of the sensor placed on the thigh can be evaluated by modifying Equation (10) by adding the projection of the accelerations a_t and a_c on the measurement axis of the sensor.

These two contributions can be evaluated considering the second derivative of the position of the knee joint with respect to the ankle joint:

$$\{\begin{array}{l}{a_{t,x}=l_1\frac{d^2[sin{\theta }_1(t)]}{d{t}^2}|}_{t=kT}\approx l_1\frac{sin{\theta }_{1,k+1}-2sin{\theta }_{1,k}+sin{\theta }_{1,k-1}}{T^2}\hfill \\ {a_{c,x}=l_1\frac{d^2[cos{\theta }_1(t)]}{d{t}^2}|}_{t=kT}\approx l_1\frac{cos{\theta }_{1,k+1}-2cos{\theta }_{1,k}+cos{\theta }_{1,k-1}}{T^2}\hfill \end{array}$$ (13)

The accelerometer and gyroscope outputs placed on the thigh can thus be expressed as follows:

$$\begin{array}{l}{a}_{x,k}^{(2)}=h_2{\alpha }_{2.k}-gsin{\theta }_{2.k}+{\beta }_2\left[h_2{\omega }_{2,k}^2-gcos{\theta }_{2.k}\right]+a_{t,k}cos\left({\theta }_{2,k}+{\beta }_2\right)-a_{c,k}sin\left({\theta }_{2,k}+{\beta }_2\right)\hfill \\ a_{y,k}^{(2)}=-h_2{\omega }_{2,k}^2+gcos{\theta }_{2.k}+{\beta }_2\left[h_2{\alpha }_{2.k}-gsin{\theta }_{2.k}\right]+a_{t,k}sin\left({\theta }_{2,k}+{\beta }_2\right)+a_{c,k}cos\left({\theta }_{2,k}+{\beta }_2\right)\hfill \\ g_{z,k}^{(2)}={\omega }_{2,k}\hfill \\ k=1,\dots ,N\hfill \end{array}$$ (14)

where the Equation (A2) indicates that Equation (14) are referred to the thigh segment.

The accelerometer and gyroscope outputs placed on the shank segment are expressed as Equation (10) as follows:

$$\begin{array}{l}{a}_{x,k}^{(1)}=h_1{\alpha }_{1,k}-gsin{\theta }_{1,k}+{\beta }_1\left[h_1{\omega }_{1,k}^2-gcos{\theta }_{1,k}\right]\hfill \\ a_{y,k}^{(1)}=-h_1{\omega }_{1,k}^2+gcos{\theta }_k+{\beta }_1\left[h_1{\alpha }_{1,k}-gsin{\theta }_{1,k}\right]\hfill \\ g_{z,k}^{(1)}={\omega }_{1,k}\hfill \\ k=1,\dots ,N\hfill \end{array}$$ (15)

The accelerometers outputs $a_{x,k}^{(1)}$, $a_{x,k}^{(2)}$ are used as inputs to the Thomas algorithm. In particular, for the second link, the sway angle can be obtained from (7) as θ = C⁻¹(A−D′) where D′ is:

$${\mathbf{\text{D}}}^{\prime }=\mathbf{\text{D}}+\left[\begin{array}{c}{a}_{t,k}cos({\theta }_{2,2}+{\beta }_2)-a_{c,2}sin({\theta }_{2,2}+{\beta }_2)\\ ⋮\\ a_{t,W-1}cos({\theta }_{2,W-1}+{\beta }_2)-a_{c,W-1}sin({\theta }_{2,W-1}+{\beta }_2)\end{array}\right]$$ (16)

In addition, the IMU's outputs Equations (14) and (15) were considered as the measurement vector for the EKF and, as described in the previous section, seven different configurations were considered:

• $\mathbf{\text{z}}(k)={\left[a_x^{(1)}(k)a_y^{(1)}(k)g_z^{(1)}(k)a_x^{(2)}(k)a_y^{(2)}(k)g_z^{(2)}(k)\right]}^T$;

• $\mathbf{\text{z}}(k)={\left[a_x^{(1)}{g}_z^{(1)}{a}_x^{(2)}{g}_z^{(2)}\right]}^T$;

• $\mathbf{\text{z}}(k)={\left[a_y^{(1)}{g}_z^{(1)}{a}_y^{(2)}{g}_z^{(2)}\right]}^T$;

• $\mathbf{\text{z}}(k)={\left[a_x^{(1)}{a}_y^{(1)}{a}_x^{(2)}{a}_y^{(2)}\right]}^T$;

• $\mathbf{\text{z}}(k)={\left[a_x^{(1)}(k)a_x^{(2)}(k)\right]}^T$;

• $\mathbf{\text{z}}(k)={\left[a_y^{(1)}(k)a_y^{(2)}(k)\right]}^T$;

• $\mathbf{\text{z}}(k)={\left[g_z^{(1)}(k)g_z^{(2)}(k)\right]}^T$;

The knee flexion-extension angle was evaluated from the shank and thigh angles, θ_shank and θ_thigh respectively, as:

$${\mathbf{\theta }}_{\mathit{\text{knee}}}=\pi -({\mathbf{\theta }}_{\mathit{\text{shank}}}-{\mathbf{\theta }}_{\mathit{\text{thigh}}})$$ (17)

RMSEs between the flex-extension angles estimated by (1) the algorithm proposed in this paper, (2) the EKF in the seven different configurations described above, were evaluated with reference to the stereo-photogrammetric measurements.

3. Results

3.1. Mechanical Inverted Pendulum

Five trials were performed. For each of them, the calibration parameters were estimated (mean ± std): the distance h = 0.20 ± 0.00 m of the origin of the sensor reference system to the pivot point (the measured distance equals 0.20 m), and the angle β = −1.24 ± 0.07°, related to the non-orthogonality of the measurement axis of the SAA to the link. These values were then used along the sensor data to predict the sway angle which was compared with the encoder angle. RMSEs, averaged over the five trials, were computed and compared with those obtained in [19] and those obtained by using the EKF in seven different cases. Peak-to-peak range (mean ± std), averaged over the five trials, is 147.2 ± 4.9°. Results are reported in Table 1.

Table 1.

Averaged RMSEs for the mechanical IP.

Tested algorithms	RMSE [°] mean ± std
Quasi-real time algorithm	0.40 ± 0.02
Bagalàet al. algorithm [19]	0.39 ± 0.05
EKF: $\mathbf{\text{z}}(k)={\left[a_x(k)a_y(k)g_z(k)\right]}^T$	0.45 ± 0.05
EKF: $\mathbf{\text{z}}(k)={\left[a_x(k)g_z(k)\right]}^T$	0.46 ± 0.05
EKF: $\mathbf{\text{z}}(k)={\left[a_y(k)g_z(k)\right]}^T$	2.12 ± 0.15
EKF: $\mathbf{\text{z}}(k)={\left[a_x(k)a_y(k)\right]}^T$	20.64 ± 12.48
EKF: z(k) = ax(k)	26.71 ± 0.70
EKF: z(k) = ay(k)	41.83 ± 7.50
EKF: z(k) = gz(k)	8.87 ± 6.92

In Figure 5 the residual error between the estimated sway angle with the algorithm proposed in this paper and the encoder angle. The RMSE obtained by the quasi-real time algorithm is slightly greater than the RMSE obtained with the offline algorithm proposed by Bagalà et al.[19], but with lower standard deviation. Moreover, the proposed algorithm improves the previous method [19] by significantly reducing the computational cost (from 10–15 iterations to a single iteration). RMSEs obtained with the EKF are greater than the RMSE obtained with the algorithm proposed in this paper in all the seven configurations analyzed. In particular, RMSEs are comparable if at least two sensors are used. The best performance is obtained by fusing the outputs of the two accelerometers and the gyroscope but the RMSE dramatically increases when a single sensor is used, since integration errors are not compensated by fusing measurements from different sensors.

Figure 5.

Residual error between the encoder output and sway angle estimated from the accelerometer output in the mechanical inverted pendulum.

The time required by the algorithm was about 170 ms to process 50 s of acquisition with a window of 100 samples, by using a DELL Studio 1,535 computer (processor Intel Core 2 Duo T9300, 2.50 GHz, 4 GB Memory, OS: Windows Vista 32 bit).

A good trade-off between the algorithm's speed and accuracy may be obtained by using Figure 6 which shows the effect of the window size W on the time required to process 50 s of the signal in Figure 6(a), and on the percentage ratio between RMSE and peak-to-peak range in Figure 6(b).

Figure 6.

(a) Window size vs. algorithm speed. (b) Window size vs. RMSE [%].

It is trivial to note that high values of W increase the computational cost because the size of the matrix C to be solved by the Thomas algorithm increases.

The RMSE shows an exponential dependence on the window size with a significant reduction for a window duration defined by the time constant of the system in Equation (1). For a bidirectional 1st order filter, the transient effect is negligible if the window size is at least 9.2τ where $\tau =\sqrt{h/g^{\prime }}$ and g′ = g·sinθ/θ. Assuming h = 0.20 m, a maximum value of the sway equal to π/3 and a mean value of the term sinθ/θ ≈ 0.8 the window size should be W = 9.2τfs ≈ 73 samples. This is confirmed by Figure 6(b) where the RMSE significantly decreases when the window size exceeds 100 samples. Furthermore, as shown in Figure 6(a), the algorithm's computational cost linearly increases with the window size as one would expect: according to (9), if N ≫ W, C_T ∞ C_i and the computational cost C_i is O(W) for tridiagonal matrix inversion using the Thomas algorithm. Increasing the window size over 100 samples does not provide significant variation in terms of RMSE but produces an increase of the computational cost.

3.2. Knee Flex-Extension Angle

The mean values and the standard deviations of the calibration parameters for the five trials, obtained by minimizing sway angle errors between stereo-photogrammetric and inertial sensor data, are (mean ± std): h₁ = 0.20 ± 0.01 m, h₂ = 0.22 ± 0.00 m, l₁ = 0.40 ± 0.06 m, and β₁ = −8.98 ± 0.24°, β₂ = −2.25 ± 0.71°. These parameters are then used to predict angular sway by using the two accelerometer outputs, $a_{x,k}^{(1)}$, $a_{x,k}^{(2)}$, for the quasi-real time algorithm, and by using the IMUs outputs for the EKF, as described in the Section 2.4. RMSEs (mean ± std) are reported in Table 2. Peak-to-peak range (mean ± std), averaged over the five trials, is 59.29 ± 2.02°.

Table 2.

Averaged RMSEs for the knee flex-extension angle.

Tested algorithms	RMSE [°] mean ± std
Quasi-real time algorithm	1.01 ± 0.11
Bagalà et al. algorithm [19]	0.95 ± 0.48
EKF: $\mathbf{\text{z}}(k)={\left[a_x(k)a_y(k)g_z(k)\right]}^T$	2.43 ± 0.76
EKF: $\mathbf{\text{z}}(k)={\left[a_x(k)g_z(k)\right]}^T$	2.46 ± 0.62
EKF: $\mathbf{\text{z}}(k)={[a_y(k)g_z(k)]}^T$	2.83 ± 0.24
EKF: $\mathbf{\text{z}}(k)={\left[a_x(k)a_y(k)\right]}^T$	37.13 ± 18.28
EKF: z(k) = ax(k)	19.31 ± 0.64
EKF: z(k) = ay(k)	43.27 ± 5.05
EKF: z(k) = gz(k)	3.95 ± 2.59

RMSEs reported in Table 2 confirm the results obtained in the mechanical inverted pendulum. The quasi-real time algorithm provides an RMSE comparable with previous published method [19] and lower than that obtained using an EKF in the best configuration where accelerometers and gyroscope outputs are fused. Figure 7 shows residual error of the knee flexion-extension angle during a squat cycle evaluated using the quasi-real time algorithm and the EKF in the best case. Stereo-photogrammetric angle was used as reference.

Figure 7.

Residual error of the knee flexion-extension angle estimated by the quasi-real time algorithm and the EKF fusing accelerometers and gyroscope outputs. The angle estimated by stereo-photogrammetry was used as reference.

4. Discussion and Conclusions

This paper suggests a novel method for the quasi-real time estimation of the sway angle in an IP model by using one SAA per segment. The relationship between the sway angle and the SAA output is described by a second order differential equation which is solved by applying the Thomas algorithm [31] to the sliding window of the accelerometer output and by setting the boundary conditions of the sway angle through an ad hoc algorithm. The method allows the 2D orientation estimation of the IP overcoming the limitations of the double-integration of the accelerometer output, recently discussed in [20]. The method is successfully tested on a mechanical IP providing accurate estimation of the sway angle compared with the encoder output. In addition, the method is extended to a 2-link chain for an application in human movement analysis. The knee flexion-extension angle of a subject performing a squat task was evaluated by using the outputs of two SAAs, placed on the thigh and shank segments, and successfully compared with a stereo-photogrammetric system. The extension to a multi-link chain is straightforward. Our method is further compared with an EKF applied to the IMU's outputs both on the mechanical IP and the human subject. The proposed technique, based on the Thomas algorithm, is more accurate than the EKF as confirmed by the results reported in Tables 1 and 2. According with these considerations the quasi-real time algorithm applied to the output of a SAA represents a cost-effective solution when multi-link kinematics estimation is required: using one IMU per segment is much expensive than using a SAA per segment.

It is worth noting that the geometric parameters (position of the sensor with respect the pivot point, misalignment angle, segment lengths) are estimated by using encoder outputs or stereo-photogrammetric system as validation instruments only in a preliminary calibration phase. After this calibration the parameters are used to predict the sway angle with the algorithm proposed in the paper using inertial sensor only. As shown in the Results section, the estimated segment lengths and the position of the sensor with respect to the pivot are quite similar to those measured by hand. The misalignment angles are lower than 9° and neglecting the misalignment does not significantly increase the RMSE.

The method presented in this paper improves the previous method [19] by significantly reducing the computational cost (from 10–15 iterations to a single iteration). Furthermore, the algorithm is quasi-real time since it provides the estimated angles with a delay of W/2f_s s, while the previous proposed algorithm [19] is offline since it requires the whole accelerometer signal to be processed.

The performance of the estimation method depends on the window size. Figure 6(a,b) provides useful information about the window size, the algorithm speed and the RMSE according to the specifications required. If the speed of the algorithm and the real time performance are the most important requirements, choosing a short time window reduces the computational cost and the angular sway latency. Nevertheless, a short time window reduces the accuracy of the estimation because of the time required for the exhausting the transient effect. For these reasons, if the accuracy is the main specification, increasing the window size provides the best results in terms of RMSE. The sway angle is always estimated with a delay equal to half of the time window.

The results suggest possible applications in various fields, from automatic control to bioengineering. Mainly in biomechanics, the algorithm proposed in this paper may speed up the procedure for the kinematics evaluation of human movement with a cost-effective set-up. An IMU integrating accelerometers and gyroscopes is more expensive and requires the use of a more complex HW/SW architecture for efficient signal processing. These costs increase even more with a multilink chain where one sensor per segment is required. One of the limitations of this study is the quasi-real time implementation due to the intrinsic delay imposed by the window size. Future studies will be addressed to this issue.

Appendix A

For the sake of clarity, Equation (A1) is linearizable under the assumption of small angles and the sway angle θ(t) can be rewritten in the Laplace domain as sum of two first-order low-pass filters (F_F, forward filter; B_F, backward filter) applied to the accelerometer output:

$$\begin{array}{cc}\theta (s)=-\frac{a(s)}{2g}\left(\underset{F_F}{\underbrace{\frac{1}{1+\tau s}}}+\underset{B_F}{\underbrace{\frac{1}{1-\tau s}}}\right)& \tau =\sqrt{\frac{h}{g}}\end{array}$$ (A1)

In the discrete-time domain, assuming θ⃗k the signal obtained after the forward filtering and θ⃖k the signal after the backward filtering of the accelerometer output, Equation (A1) can be split in the two following equations:

$$\{\begin{array}{c}{\overrightarrow{\theta }}_k=-\frac{a_k}{2g}+\tau \frac{{\overrightarrow{\theta }}_{k+\frac{1}{2}}-{\overrightarrow{\theta }}_{k-\frac{1}{2}}}{T}\\ {\overleftarrow{\theta }}_k=-\frac{a_k}{2g}-\tau \frac{{\overleftarrow{\theta }}_{k+\frac{1}{2}}-{\overleftarrow{\theta }}_{k-\frac{1}{2}}}{T}\end{array}$$ (A2)

By summing up and subtracting the two filtered signals of Equation (A2) we got:

$$\{\begin{array}{l}{\overrightarrow{\theta }}_k+{\overleftarrow{\theta }}_k=-\frac{a_k}{g}+\frac{\tau }{T}\left({\overrightarrow{\theta }}_{k+\frac{1}{2}}-{\overrightarrow{\theta }}_{k-\frac{1}{2}}-{\overleftarrow{\theta }}_{k+\frac{1}{2}}+{\overleftarrow{\theta }}_{k-\frac{1}{2}}\right)={\theta }_k\hfill \\ {\overrightarrow{\theta }}_k-{\overleftarrow{\theta }}_k=-\frac{\tau }{T}\left({\overrightarrow{\theta }}_{k+\frac{1}{2}}-{\overrightarrow{\theta }}_{k-\frac{1}{2}}+{\overleftarrow{\theta }}_{k+\frac{1}{2}}-{\overleftarrow{\theta }}_{k-\frac{1}{2}}\right)=-\frac{\tau }{T}\left({\theta }_{k+\frac{1}{2}}-{\theta }_{k-\frac{1}{2}}\right)\hfill \end{array}$$ (A3)

By manipulating Equation (A3), the forward and backward filtered angles can be expressed as function of the sway angle as follows:

$$\{\begin{array}{l}{\overrightarrow{\theta }}_k=\frac{1}{2}\left[{\theta }_k+\frac{\tau }{T}\left({\theta }_{k-\frac{1}{2}}-{\theta }_{k+\frac{1}{2}}\right)\right]\hfill \\ {\overleftarrow{\theta }}_k=\frac{1}{2}\left[{\theta }_k-\frac{\tau }{T}\left({\theta }_{k-\frac{1}{2}}-{\theta }_{k+\frac{1}{2}}\right)\right]\hfill \end{array}$$ (A4)

By substituting the two terms of Equation (A4) in the first of Equation (A3), it is trivial to obtain:

$$\frac{h}{T^2}{\theta }_{k-1}-2(g+\frac{h}{T^2}){\theta }_k+\frac{h}{T^2}{\theta }_{k+1}=a_k$$ (A5)

which has the same form of the tridiagonal system a_ix_i−1 + b_ix_i + c_ix_i+1 = d_i (see Equation (8)) which is usually solved by applying the Thomas algorithm. It is worth noting that the coefficients of the tridiagonal system described by Equation (A5) are constant only for the linear case.