Pose estimation by extended Kalman filter using noise covariance matrices based on sensor output

This paper presents an extended Kalman filter for pose estimation using noise covariance matrices based on sensor output. Compact and lightweight nine-axis motion sensors are used for motion analysis in widely various fields such as medical welfare and sports. A nine-axis motion sensor includes a three-axis gyroscope, a three-axis accelerometer, and a three-axis magnetometer. Information obtained from the three sensors is useful for estimating joint angles using the Kalman filter. The extended Kalman filter is used widely for state estimation because it can estimate the status with a small computational load. However, determining the process and observation noise covariance matrices in the extended Kalman filter is complicated. The noise covariance matrices in the extended Kalman filter were found for this study based on the sensor output. Postural change appears in the gyroscope output because the rotational motion of the joints produces human movement. Therefore, the process noise covariance matrix was determined based on the gyroscope output. An observation noise covariance matrix was determined based on the accelerometer and magnetometer output because the two sensors’ outputs were used as observation values. During a laboratory experiment, the lower limb joint angles of three participants were measured using an optical 3D motion analysis system and nine-axis motion sensors while participants were walking. The lower limb joint angles estimated using the extended Kalman filter with noise covariance matrices based on sensor output were generally consistent with results obtained from the optical 3D motion analysis system. Furthermore, the lower limb joint angles were measured using nine-axis motion sensors while participants were running in place for about 100 s. The experiment results demonstrated the effectiveness of the proposed method for human pose estimation.


Introduction
Compact and lightweight nine-axis motion sensors have been developed through advances in micro-electromechanical systems technology; they have come to be used for motion analysis in widely various fields [1][2][3][4][5][6][7][8].The nine-axis motion sensors are applicable both indoors and outdoors because of their portability.Several experiments have been conducted to measure the motion of a skier gliding down a slope and jumping off a hill using motion sensors [9,10].The nine-axis motion sensors include a three-axis gyroscope, a three-axis accelerometer, and a three-axis magnetometer.Using information obtained from the motion sensors, several sensor fusion algorithms have been proposed for pose estimation: as one example, a sensor fusion algorithm that can correct gyroscope drift using information obtained from the other two sensors has been used for human pose estimation during daily activities and exercise [11][12][13].Furthermore, a sensor fusion algorithm able to correct the magnetometer output using information obtained from a gyroscope has been used for pose estimation in a variable magnetic field [14,15].The Kalman filter [16][17][18][19][20] and the complementary filter [21][22][23][24][25] are some pose estimation methods using sensor fusion.
The Kalman filter estimates the system state with a small computational load.Nevertheless, determining the process and observation noise covariance matrices in the Kalman filter is complicated.For a case in which the process and observation noise covariance matrices are timeinvariant, the estimation accuracy might decrease if the sensor output noise increases.Moreover, the noise of the sensor output might vary because of long-term measurements.For that reason, adjusting the noise covariance matrices based on sensor output is important.
To estimate the lower limb joint angles for this study, a method was devised to determine the process and observation noise covariance matrices in the extended Kalman filter based on sensor output.The postural change appears in the gyroscope output because the rotational motion of the joints produces human movement.Therefore, the process noise covariance matrix was set based on the gyroscope output.When the accelerometer output increased, the observation noise covariance matrix was set to increase.The observation noise covariance matrix was also set to increase when the magnetometer output drastically changed.During a laboratory experiment, the lower limb joint angles of three participants were measured using an optical 3D motion analysis system and nine-axis motion sensors while the participants were walking.Several studies have demonstrated that an optical 3D motion analysis system measured human movement with high accuracy.Therefore, the system is used for verifying the pose estimation accuracy in widely diverse fields [26][27][28][29].We verified the accuracy of the proposed method by comparing its results to those of an optical 3D motion analysis system.Furthermore, the lower limb joint angles were measured using nine-axis motion sensors while the participants were running in place.Finally, the effectiveness of the proposed method was verified using experiment results.

Definition of roll-pitch-yaw
The 3D posture of the sensor is represented by the roll angle ( φ ) around the x-axis, the pitch angle ( θ ) around the y-axis, and the yaw angle ( ψ ) around the z-axis.The reference coordinate system is a right-handed system with a vertical z-axis.The counterclockwise rotation is defined as positive.The reference coordinate system and the definition of the joint angles are presented in Fig. 1.
The initial roll and pitch angles were calculated using the accelerometer output at rest [30,31].The relation between the acceleration sensor output and the gravitational acceleration in the reference coordinate system is expressed using Eq. ( 1) because the accelerometer measures only the.
gravitational acceleration while at rest: where.
Therein, i A denotes the accelerometer output, o A represents the acceleration in the reference coordinate system, and g stands for gravitational acceleration.For the experiment, sensors 1, 2, 3, and 4 were placed respectively on the waist, left thigh, left shank, and left foot.In addition, the rotational matrix from the sensor coordinate system to the reference system o R i is the following: (1 Then, the accelerometer output i A is represented by substituting Eq. (2) into Eq.( 1) as shown below: The initial roll and pitch angles using Eq. ( 3) are: where i A x , i A y , and i A z respectively denote the accelerometer output for the x, y, and z axes.Therein, i φ A and i θ A respectively denote the initial roll and pitch.
To correct the yaw angle, calculations require the roll i φ A , pitch i θ A , and magnetometer output as: where i m x , i m y , and i m z respectively denote the magnetometer outputs for the x, y, and z axes.Therein, c,i m x , c,i m y , and c,i m z respectively represent the corrected magnetic field data for the x, y, and z axes.The positive directions of c,i m x , c,i m y , and c,i m z coincide with those of each axis of the reference coordinate system in Fig. 1.The x-axis pointed in the direction of the azimuth at 112.5 degrees (east-southeast).The y-axis pointed in the direction at the azimuth of 202.5 degrees (south-southwest) in the reference coordinates.
The following equation is used for calculating yaw: where i ψ m denotes the azimuth on the x-y plane of the reference coordinates in Fig. 1. (2 The differential Euler angles in the reference coordinate system are the following: Therein, i φ , i θ , and i ψ respectively represent the dif- ferential roll, pitch, and yaw angles, i ω y and i ω z respec- tively stand for the gyroscope output for the x, y, and z axes.Then the roll, pitch, and yaw angles are calculated by substituting Eq. ( 8) into Eq.( 9):

State-space model
The roll, pitch, and yaw angles of each sensor placed on the lower limb are estimated by the sensor fusion using the extended Kalman filter.The nonlinear state equation was developed using Eq. ( 9).The nonlinear observation equation was developed using Eq. ( 7) and the acceleration sensor output.The nonlinear state and observation equations are shown respectively in Eqs. 10 and 11: where, (8) In those equations, t , i θ t , and i ψ t respectively denote Euler angles of the sensor placed on each lower limb segment, as estimated using the extended Kalman filter.Ts stands for the sampling time.In addition, i ω x,t , i ω v,t , and i ω z,t respectively denote the gyroscope outputs for the x, y, and z axes.Also, i A S x , i A S y , and i A S z respectively express the accelerometer output for the x, y, and z axes.Therein, i w t and i v t denote white noise.
Yaw angle i ψ m , which was calculated using the mag- netometer output, and the accelerometer output were used as the observation values in Eq. (11).Equation (1) represents the relation between the accelerometer output and gravitational acceleration.Consequently, the right side of Eq. ( 1) was used in i H i x t of the observa- tion equation.Although the proportion of the centrifugal acceleration and the tangential acceleration in the accelerometer output would have increased during exercise, these acceleration components were processed as observation noise.In addition, i ψ m = i ψ t is a simple equation representing the relation between the magnetometer output and the yaw angle of the state values.Therefore, the yaw angle i ψ t of the state value was used in i H i x t of the observation equation.
The partial derivatives of i F i x t , i ω t and i H i x t are shown below: Then, the prediction step [Eqs.( 14) and ( 15)] and the filtering step [Eqs.( 18), ( 19), (20)] are calculated using the nonlinear discrete-time system represented by Eqs.(10) and (11)] .Here, Eq. ( 16) and Eq. ( 17) are used for calculating the likelihood of the state-space model: In those equations, i P represents the error covariance matrix, i V denotes the prediction error matrix, i B stands for the prediction error variance matrix, and i K denotes the Kalman gain.Therein, i Q and i R respectively denote the covariance matrices of process noise i w t in the non- linear state equation and observation noise i v t in the non- linear observation equation.

Noise covariance matrices based on sensor output
The process and observation noise covariance matrices in the extended Kalman filter were determined based on the state-space model dynamics and the sensor noise.The postural change appears in the gyroscope output because the rotational motion of the joints produces human movement.Consequently, the process noise covariance matrix was determined based on the gyroscope output as presented below: where, In those expressions, i ω x,t , i ω y,t , and i ω z,t respectively stand for the gyroscope output for x, y, and z axes.Also a and b are adjusting parameters.For this study, a and b were determined to maximize the log-likelihood ( i LL ) shown in Eq. ( 22): In that equation, N stands for the number of timeseries data; j represents time-series.In addition, i B j expresses the prediction error variance; i V j signifies the prediction error.
The observation noise covariance matrices must be set at a high value when the sensor noise increases [32].Therefore, the observation noise covariance matrix was determined based on the accelerometer and magnetometer output because the two sensor outputs were used as (18) observation values [33].The observation noise covariance matrix is presented below: where, In the matrix, c,i m x,t , c,i m y,t , and c,i m z,t respectively denote corrected magnetic field data for the x, y, and z axes.In addition, m represents the average value of the magnetometer output over the entire measurement time.Furthermore, i A x,t , i A y,t , and i A z,t respectively express the accelerometer outputs for the x, y, and z axes.Therein, c, d, e, and f are adjusting parameters.In addition, c, d, e, and f were determined to maximize the log-likelihood (LL) shown in Eq. (22).Several studies have proposed process and observation noise covariance matrices based on sensor output [31,33].In those earlier studies, noise covariance matrices were produced after calculating the deviation of each sensor using many equations.The adjusting parameters a to f in the noise covariance matrices are determined simply using only log-likelihood calculations presented in Eq. (22).In addition, interaction between the process noise covariance and observation noise covariance is considered in log-likelihood calculations.
The roll, pitch, and yaw angles in the sensor i coordinate system obtained from the sensor fusion are converted into the rotational matrix of the reference coordinate system using Eq. ( 2).The lower limb joint angles are calculated by substituting Eq. ( 2) into Eq.( 24) as shown below: In that equation, i−1 R i denotes the rotational matrix from the sensor i coordinate system to the sensor i-1 system.The hip joint angle is estimated using the output of the two sensors placed on the waist and thigh.The knee joint angle is estimated using the output of the two sensors placed on the thigh and shank.The ankle joint angle is estimated using the output of the two sensors positioned on the shank and foot.

Verification experiment Participants and experiment conditions
Three healthy participants (A, B, and C) were examined during the experiment.Anthropometric data are shown in Table 1.After maintaining the upright posture for about 5 s, the first step that a participant took was with the left foot.They were instructed to walk using a natural stride in time with a metronome (70 bpm).Measurement started simultaneously when a participant started to maintain the upright posture.The measurements finished when the participant placed the right foot flat on the floor during the sixth step.Following an explanation of the purpose and requirements of the study, the participants gave their written informed consent to participate in the study.Study approval was obtained from the Research Ethics Board, Kogakuin University, and National Institute of Technology, Akita College.
During the experiment, kinematic data were collected using an optical 3D motion analysis system (Bonita 10; Vicon Motion Systems Ltd.), two force plates (9286; Kistler Japan Co. Ltd.), and four nine-axis motion sensors in synchronization.The heel strike and toe off were ascertained from force plate data.The sensors were placed on the waist, left thigh, left shank, and left foot using double-sided tape and elastic straps.The sensor positions are presented in Fig. 2. Definitions of the length of the thigh, shank, and foot were referred from reports of earlier research [34].Positions of reflective markers for the optical 3D motion analysis system were found by reference to the Vicon Plug-in Gait model.The sampling frequencies of the nine-axis motion sensors, the optical 3D motion analysis system, and the force plates were 100 Hz.

Results
Table 2 shows parameters a to f in the walking experiment, which were found to maximize the log-likelihood in Eq. ( 22).In the walking experiment, more or less equivalent parameters were obtained for all participants.
The joint angles of participant A are depicted in Fig. 3. Black solid curves represent results obtained from the optical 3D motion analysis system.Red solid curves represent results obtained from the extended Kalman filter using the noise covariance matrices based on sensor output, hereinafter designated as NBS.Orange solid curves represent results obtained from NBS, which used gyroscope output for the process noise covariance matrix and which used a constant value for the observation noise covariance matrix, hereinafter designated as NBS (Only process noise).Green solid curves represent results obtained from NBS, which used the constant value for the process noise covariance matrix and which used accelerometer and magnetometer output for the observation noise covariance matrix, hereinafter designated as NBS (Only observation noise).Blue solid curves present results obtained from the extended Kalman filter using the constant noise covariance, hereinafter designated as CNC.In addition, Ωω (= 0.0005), Ωm (= 1500), and Ωa (= 1500) of CNC were determined to maximize the log likelihood (LL) shown in Eq. ( 22).The horizontal axis shows the normalized time, where one gait cycle is 100%.One gait cycle in Fig. 3 extends from the beginning of the stance phase of the left leg (the third step) until the end of the swing phase.Table 3 shows root mean square errors (RMSE) between the estimated results and the results obtained from the 3D motion analysis system.
Joint angles obtained from the three NBSs and CNC are generally consistent with the results obtained using the optical 3D motion analysis system.Results show the same tendency for joint angle variation as that found in an earlier study [35].
The ankle joint angle obtained from NBS (Only process noise) is generally consistent with results obtained using the optical 3D motion analysis system at the dorsiflexion peak after toe-off.The ankle joint angle obtained  from NBS (Only observation noise) is much smaller than the result obtained from the optical 3D motion analysis system.The results indicate that the process noise covariance matrix based on the gyroscope output contributed to increased accuracy of the dorsiflexion peak during the swing phase.In the early stance phase and the end of the swing phase, the ankle joint angle obtained from NBS (Only process noise) is much smaller than the result obtained from the optical 3D motion analysis system, whereas the ankle joint angle obtained from NBS (Only observation noise) is generally consistent with the result obtained using the optical 3D motion analysis system.The results indicate that the observation noise covariance matrix based on the accelerometer and magnetometer output contributed to increased accuracy at the early stance phase and at the end of the swing   phase.Therefore, the process noise covariance matrix based on the gyroscope output and the observed noise covariance matrix based on the accelerometer and magnetometer output might have contributed to the increased accuracy at different phases.
For knee and hip joint angles, all results show the same tendency.However, NBS (red line) has the smallest RMSE in all results of all three joints.The results show that using both processes of noise covariance matrix based on the gyroscope output and the observed noise covariance matrix based on the accelerometer and magnetometer output might have contributed to increased accuracy.The two noise covariance matrices seem to have influenced one another.

Participants and experiment conditions
The nine-axis motion sensors measured lower limb joint angles of the same participants while they were running in place to verify the effectiveness of NBS when continuously capturing data of fast-moving participants.The nine-axis motion sensors were placed in the same positions as those used for the verification experiment.The measurement time was about 100 s.During the experiment, kinematic data were collected using an optical 3D motion analysis system with four nine-axis motion sensors in synchronization.Participants were instructed to run in place in time with a metronome (150 bpm) after maintaining the upright posture for about 5 s.The sampling frequencies of the nine-axis motion sensors and the optical 3D motion analysis system were 100 Hz.

Results
Table 4 shows parameters a to f for the running.
experiment, which were determined to maximize the log-likelihood in Eq. (22).From the running experiment, different parameters were obtained among the joints.In addition, parameters a, c, and e for running measurements tended to be larger than those in the walking measurement.The results indicate that the noise covariance matrices for the running experiment might have had larger values because the process and observation noise can increase if the motion velocity increases.
The estimated joint angles of participant A are presented in Figs. 4, 5, and 6.In each of Figs. 4, 5, and 6, panels (a) present results obtained over the entire measurement time.Panels (b) present results obtained between 33 s and 35.5 s from the start of measurements.In each of Figs. 4, 5, and 6, panels (b) are used for a detailed examination of the results.Black solid curves present results obtained from the optical 3D motion analysis system.Red solid curves present results obtained from NBS. Blue solid curves present results obtained from CNC.
The estimated ankle joint angle using NBS.  in Fig. 4(a) changes periodically between − 25° and 25° over the entire measurement time, which is generally consistent with results obtained using the optical 3D motion analysis system.The estimated ankle joint angle using CNC in Fig. 4  the waveform of the result obtained using CNC in Fig. 4(b) is similar to the result obtained using NBS, the result obtained using CNC is much smaller than that obtained using NBS.Additionally, the waveform of the result obtained using CNC has a larger dorsiflexion peak than that obtained using NBS at about 33.7, 34.4, and 35.2 s.The estimated knee joint angle obtained using NBS in Fig. 5(a) changes periodically between 20° and 110° over the entire measurement time, which are generally consistent with the results obtained using the optical 3D motion analysis system.Whereas the estimated knee joint angle using CNC in Fig. 5(b) changes periodically between − 60° and 0° over the entire measurement time.Although the waveform of the result obtained using CNC in Fig. 5(b) is similar to the result obtained using NBS, the result obtained using CNC is much smaller than that obtained using NBS.Additionally, the waveform of the result obtained using CNC has a smaller flexion peak than that obtained using NBS at about 33.6, 34.35, and 35.2 s.
The hip joint angle estimated using NBS in Fig. 6(a) changes periodically between 10° and 35° over the entire measurement time, which are generally consistent with results obtained using the optical 3D motion analysis system.The estimated knee joint angle using CNC in Fig. 6(a) changes periodically between − 35° and − 15° over the entire measurement time.Although the waveform of the result obtained using CNC in Fig. 6(b) is similar to the result obtained using NBS, the result obtained using CNC is much smaller than that obtained using NBS.All results obtained for the other two participants showed similar tendencies.The results demonstrated the effectiveness of the extended Kalman filter using NBS.

Conclusions
For this study, a method for ascertaining the process and observation noise covariance matrices in the extended Kalman filter based on sensor output was constructed to estimate the lower limb joint angles.The lower limb joint angles of the three healthy participants during walking and running were estimated using the method.Results yielded the following conclusions.
1.The joint angles obtained from the extended Kalman filter using the process and observation noise covariance matrices based on sensor output were generally consistent with results obtained using the optical 3D motion analysis system in the verification experiment.2. In the running motion analysis, the results obtained using noise covariance matrices based on sensor output indicated that the estimated joint angles changed periodically within an appropriate range.The results obtained using the constant noise matrices covariance indicated that the estimated joint angles changed abnormally.
Noise covariance matrices based on sensor output can be effective for accurate pose estimation because noise covariance matrices can be time-variable when continuously capturing human motion with long-term measurements.The proposed method is expected to be useful for estimating motion in sports and healthcare applications.

Fig. 1
Fig. 1 Definition of the lower limb joint angles and the reference coordinate system

cFig. 3
Fig.3 Left lower limb joint angles during walking obtained using optical 3D motion analysis system, the extended Kalman filter using NBS, NBS (Only observation noise), NBS (Only process noise), and the extended Kalman filter using CNC (participant A). a The ankle joint angle.b The knee joint angle.c The hip joint angle.

Fig. 5 Fig. 6
Fig. 5 Results obtained for knee joint angles (Subject A). a The results over the entire measurement time.b The results between 33 and 35.5 s.

Table 1 Anthropometric data
2 Sensor positions and sensor coordinate system

Table 4 Adjusting parameters of NBS in the running experiment
(a) changes periodically between − 70° and 0° over the entire measurement time.Although Results obtained for ankle joint angles (Subject A). a The results over the entire measurement time.b The results between 33 and 35.5 s.