# Attitude Estimation Using Kalman Filtering: External Acceleration Compensation Considerations

Authors: Romy Budhi Widodo; Chikamune Wada
Journal: Journal of Sensors
 (2016)

---

## Abstract

Attitude estimation is often inaccurate during highly dynamic motion due to the external acceleration. This paper proposes extended Kalman filter-based attitude estimation using a new algorithm to overcome the external acceleration. This algorithm is based on an external acceleration compensation model to be used as a modifying parameter in adjusting the measurement noise covariance matrix of the extended Kalman filter. The experiment was conducted to verify the estimation accuracy, that is, one-axis and multiple axes sensor movement. Five approaches were used to test the estimation of the attitude: (1) the KF-based model without compensating for external acceleration, (2) the proposed KF-based model which employs the external acceleration compensation model, (3) the two-step KF using weighted-based switching approach, (4) the KF-based model which uses thethreshold-based approach, and (5) the KF-based model which uses the threshold-based approach combined with a softened part approach. The proposed algorithm showed high effectiveness during the one-axis test. When the testing conditions employed multiple axes, the estimation accuracy increased using the proposed approach and exhibited external acceleration rejection at the right timing. The proposed algorithm has fewer parameters that need to be set at the expense of the sharpness of signal edge transition.


---

## Body

## 1. Introduction

The inertial measurement unit (IMU) is typically used to determine attitude, that is, roll and pitch, by fusing accelerometer and gyro data. The most notable disturbance of attitude determination is external acceleration [1], which is caused by a change of velocity in magnitude or direction [2]. Recently, the IMU has been widely used in many applications, such as gait assessment systems [3–5]; classification of human movement and falls [6, 7]; and specific modeling on certain body movements—for example, sit-to-stand [8, 9]. Attitude estimation using IMU is an important theme in the literature, specifically on how to improve its accuracy and reliability. This leads us to the aim of this study, which is to present a new algorithm to overcome the existence of external acceleration in order to improve attitude estimation accuracy.The attitude solution provided by the gyro is prone to being unbounded, to bias, and to random-walk errors. In static or slow movement, the accelerometer measures roll and pitch by leveling to correct the gyro-unbounded error. This is due to the trustworthiness of the gravitational measurement. Therefore, a proper fusion of IMU data and the algorithm to compensate for external acceleration is needed to overcome the shortcomings of each sensor and the effect of external acceleration. The fusion technique evolved along two major paths: one approach incorporates the use of a Kalman filter [10–12] while the other algorithm consists of a complementary filter [13, 14].Suh et al. [1], Sabatini [11], and Harada et al. [12] proposed the adapted measurement noise covariance matrix to overcome some disturbances, including external acceleration. The adapted algorithm in [1] is based on the weighted-switching approach. In contrast, Sabatini in [11] used a threshold-based approach, while Harada et al. [12] proposed a threshold-based approach combined with softened part.Inspired by Suh et al. [1], Sabatini [11], and Harada et al. [12], this paper proposed the extended Kalman filter- (EKF-) based attitude estimation using a new algorithm to overcome the existence of external acceleration. We extracted external acceleration from the accelerometer signal with a model and used it to adjust the measurement noise covariance matrix. The aforementioned algorithm is verified by conducting one-axis and multiple axes movement experiments.The rest of the paper is organized as follows: Section2 describes attitude determination and the proposed algorithm. Section 3 discusses the experimental setup and results. Section 4 then presents a discussion of the algorithm approaches and a comparison regarding the experimental results. In Section 5, we present our conclusion.


## 2. Method

### 2.1. Sensor Model

The relative attitude (roll and pitch) of two frames of reference used a direction cosine matrix (rotation matrix) for coordinate transformation or rotation of object.φ (roll), θ (pitch), and ψ (yaw) are the rotation angles about the x-, y-, and z-axes on the sensor body, as shown in Figure 1. C
I
/
B indicates a rotation matrix that represents the relative attitude of frame I (inertial frame) with respect to frame B (body of sensor), thus allowing the relation as follows:(1)
x
I
=
C
I
/
B
x
B
,where x
B is the coordinate vector in frame B and x
I is the coordinate in frame I.Figure 1
Orientation of axes of sensitivity for IMU.The well-known rotation matrix, with the function ofZ
→
Y
→
X Euler angles, is as follows:(2)
C
I
/
B
φ
,
θ
,
ψ
=
C
X
φ
C
Y
θ
C
Z
ψ
=
c
θ
c
ψ
-
c
φ
s
ψ
+
s
φ
s
θ
c
ψ
s
φ
s
ψ
+
c
φ
s
θ
c
ψ
c
θ
s
ψ
c
φ
c
ψ
+
s
φ
s
θ
s
ψ
-
s
φ
c
ψ
+
c
φ
s
θ
s
ψ
-
s
θ
s
φ
c
θ
c
φ
c
θ
,where s and c are the short forms of sine and cosine, respectively.Measurements model for attitude determination using accelerometers is constructed by the third row ofC
I
/
B that contains φ and θ. It is independent of rotation along the z-axis (ψ).Sensor signal from the accelerometer is modeled as follows:(3)
f
A
=
a
+
g
-
s
θ
s
φ
c
θ
c
φ
c
θ
T
,where f
A is a vector of f
x, f
y, and f
z; the acceleration is measured by the three-axis accelerometer on the body frame. The unwanted external acceleration information (a) in the accelerometer will be estimated. This acceleration is caused by the change of velocity in magnitude and/or direction. The external acceleration is the main source of errors in attitude estimation.


### 2.2. External Acceleration Compensation Method

In this section, we introduce the method for compensating for the external acceleration. Our method is based on the view that external acceleration is a disturbance in the original signal and therefore should be removed from the original signal. The disturbance rejection model was derived from a common control ratio model [15]. From this point on, the term “compensation model” will be used to replace the term “disturbance rejection model.” The three-axis accelerometer data is input for the compensation model. Each axis will be treated in the same step; therefore, for the sake of convenience, only the x-axis will be discussed. In (4), ideally the compensated x-axis accelerometer signal (f
∗
x
k) was determined by subtracting the compensation model in the x-axis (a
k) from the original signal (f
x
k):(4)
f
∗
x
k
=
f
x
k
-
a
k
.The external acceleration (a
k) is extracted from f
x
k by the compensation model. Figure 2 shows the ideal model diagram of (4). However, due to the dynamism of the system and many uncertainties, there is no ideal model. Consequently, we will use the result of the compensation model as one parameter for adjusting the noise covariance matrix in sensor fusion in the next section. The design approach for a compensation model is based on the specification that the model should not be affected by the disturbance input d
(
t
). In other words, the steady-state output should be zero if possible or a small constant; y
t
ss
=
0, d
(
t
)
≠
0. A control ratio in the s domain [15] is commonly written as follows:(5)
Y
s
D
s
=
K
s
w
+
c
w
-
1
s
w
-
1
+
⋯
+
c
1
s
+
c
0
s
n
+
q
n
-
1
s
n
-
1
+
⋯
+
q
1
s
+
q
0
.Assuming a step disturbance signal D
(
s
)
=
D
0
/
s, the output will be(6)
Y
s
=
K
D
0
s
w
+
c
w
-
1
s
w
-
1
+
⋯
+
c
1
s
+
c
0
s
s
n
+
q
n
-
1
s
n
-
1
+
⋯
+
q
1
s
+
q
0
,where K is gain constant, K
>
0, c and q are constant coefficients, and D
0 is the final value of step disturbance. The condition y
t
ss
=
0 will be achieved if c
0
=
0 and requires the numerator of Y
(
s
)
/
D
(
s
) to have at least one zero at the origin of the s-plane. On the other side, the location of the poles on the s-plane determined its corresponding response. To produce the damped response, the poles should be on the left-half s-plane. There are two possibilities for poles locations on the left-half side, such as complex-conjugate poles and real poles, which have the output characteristics of exponentially damped sinusoid and damped exponential response, respectively. In our model, we chose damped exponential response because the disturbance should be suppressed without a sinusoidally oscillating component. Finally, we determined the transfer function of our model: there is one zero at the origin and one real pole on the left-half of the s-plane which is based on (5) and becomes as follows:(7)
Y
s
D
s
=
K
s
s
+
q
0
.Figure 2
The input-output diagram of external acceleration compensation.The compensating model in (7) attenuates low frequencies and as a result a high frequency signal is obtained. This is in line with the fact that the external acceleration occurs when the change of velocity in magnitude and/or direction of each axis exceeds a certain limit.The analysis of the stability of model (7) will be introduced in the time domain as follows. In the time domain, the stability will be achieved based on the principle that there is a limited response to the limited input. We discuss the time response of singularity input functions to this model, such as step, ramp, rectangular pulse, and parabolic functions. The time response model (7) for a unit step input, u
(
t
), is y
(
t
)
=
K
e
-
q
0
t. We get steady-state response for y
(
t
) and the steady-state error as in(8)
y
t
ss
=
lim
t
→
∞
⁡
K
e
-
q
0
t
=
0
,
(9)
err
⁡
t
ss
=
lim
t
→
∞
⁡
1
-
K
e
-
q
0
t
=
1
.The time response for ramp input (d
(
t
)
=
t) is y
(
t
)
=
K
/
q
0
1
-
e
-
q
0
t. The steady-state output and error are calculated as follows:(10)
y
t
ss
=
lim
t
→
∞
⁡
K
q
0
1
-
e
-
q
0
t
=
K
q
0
,
err
⁡
t
ss
=
lim
t
→
∞
⁡
K
q
0
e
-
q
0
t
-
1
+
t
=
∞
.A rectangular pulse of amplitude 1
/
a and of duration a, d
(
t
)
=
u
(
t
)
-
u
(
t
-
a
)
/
a, has azero steady-state output and also azero steady-state error.For a parabolic function input,d
(
t
)
=
0.5
t
2, the time response is calculated as follows, with an infinity steady-state output and infinity steady-state error:(11)
y
t
=
K
q
0
t
+
K
q
0
2
e
-
q
0
t
-
1
.These four kinds of singularity inputs and their combination represent the possibility of disturbance waveforms. All of them have the tendency to be rejected and damped by model (7). The response of the parabolic input has not shown constant steady-state output; however, it represented the exponentially damped factor. Figures 3(a), 3(b), 3(c), and 3(d) illustrate all time responses for each basic input signal, using model (7) within K
=
1. Each figure has two outputs, that is, y
1
(
t
) and y
2
(
t
), where q
0 in y
1
(
t
) is greater than q
0 in y
2
(
t
).Figure 3
Time response plot corresponding to (7) for some basic input signals: (a) step function, (b) ramp, (c) parabolic, and (d) pulse.
(a)
(b)
(c)
(d)


### 2.3. Attitude Determination through Sensor Fusion Method

Attitude determination from gyro had errors accumulating along the process of numerical integration, but the gyro was sensitive to the change of rotation. While the attitude determination by the accelerometer does not show accumulation errors or divergence, the error in the roll and pitch axis is too large because those two axes are mutually affected. When there is a movement in the roll axis, the pitch axis also moves up and down with respect to the horizontal plane, and vice versa. In the proposed method, both sensors are complementary to each other to achieve performance in the sensor fusion technique using the extended Kalman filter (EKF).In this paper, the work will be divided into five modes.Mode 1 is the standard EKF using the proposed system model but without compensating the external acceleration.Mode 2 is the proposed system model, also containing the proposed external acceleration compensation, with a preprocessing technique for accelerometer data using model (7) before the EKF process. The difference betweenMode 2 andMode 1 is the addition of the measurement error covariance based on the external acceleration compensation model. A detailed explanation ofMode 1 andMode 2 is in Section 2.4.Mode 3 is derived from [1]: thetwo-step EKF which adjusts the measurement noise covariance using theweighted-switching approach.Mode 4 is the quaternion-based EKF, which uses athreshold-based approach to set the measurement error covariance. This mode is derived from [11].Mode 5 is derived from [12], which uses an unscented Kalman filter (UKF). This method works as threshold-based approach combined with a softened part of measurement noise covariance matrix adjustment.


### 2.4. Algorithm Description ofMode 1 andMode 2

#### 2.4.1. Process and Measurement Models

We initially defined the states and the observation variables for the system model. We set attitude and gyro bias as state variables, since the bias errors are a highly complex function to the ambient temperature. The Euler angle was the angle representation. The state variablex
(
t
) and the measurement variable z
(
t
) are defined as follows:(12)
x
=
φ
θ
ψ
b
x
b
y
b
z
T
,
z
=
φ
a
θ
a
ψ
a
T
,where φ (roll) and θ (pitch) are the rotation angles about the x- and y-axes and ψ is yaw angle, but it is not of concern in this study. These come from the integration of the rate of change from gyros, while b
x, b
y, and b
z are biases from gyro in x-, y-, and z-axis, respectively. We use the measurement from the accelerometer in order to calculate φ
a, θ
a, and ψ
a as measurement variables. The system equation is given by(13)
x
˙
t
=
f
x
t
+
w
t
,
z
t
=
H
x
t
+
v
t
,where f
(
x
(
t
)
) is a nonlinear function representing the relation between gyros data and kinematic equation for the Euler angles (Z
→
Y
→
X); H is state to measurement matrix as shown in (14); and w
(
t
) and v
(
t
) are process noise and measurement noise, respectively, which are assumed to be uncorrelated Gaussian distributed white noise. Consider(14)
f
x
=
1
s
φ
t
θ
c
φ
t
θ
0
0
0
0
c
φ
-
s
φ
0
0
0
0
s
φ
s
c
θ
c
φ
s
c
θ
0
0
0
0
0
0
1
0
0
0
0
0
0
1
0
0
0
0
0
0
1
·
p
-
b
x
q
-
b
y
r
-
b
z
b
x
b
y
b
z
,
H
=
1
0
0
0
0
0
0
1
0
0
0
0
0
0
1
0
0
0
.In (14), p, q, and r are angular velocities measured from the gyro along the x-, y-, and z-axes, respectively. Symbols of s, c, t, and s
c are short forms of sine, cosine, tangent, and secant, respectively. This process model is nonlinear since it contains trigonometric functions. Therefore, we use Jacobian values of the nonlinear model of f
(
x
) to replace state transition matrix (A), where the values in that matrix are obtained by applying the previous estimated states in the EKF process. In contrast, matrix H is the linear expression.The measurement from the accelerometer which is used to calculateφ
a and θ
a as measurement variables is a common initialized leveling equation [2, 16]:(15)
φ
a
=
arctan
2
⁡
f
y
,
f
z
θ
a
=
arctan
⁡
-
f
x
f
y
s
φ
a
+
f
z
c
φ
aFigure4 shows the structure of the proposed algorithm combined with the extended Kalman filter algorithm. The notations and meanings of x
^
k, x
^
k
-, P
k, and P
k
- are estimates of state, prediction of state, estimate of the error covariance, and the prediction of the error covariance, respectively.Figure 4
Structure of the proposed algorithm.


#### 2.4.2. Error Covariance

In Figure4, the estimated error covariance is computed by P
k
=
P
k
-
-
K
k
H
P
k
-. Error covariance indicates the difference between state estimation (x
^
k) and the unknown true state value (x
k). It also can be defined as the following [17]:(16)
P
k
=
E
x
k
-
x
^
k
x
k
-
x
^
k
T
.The error covariance of attitude states was initialized by calculating the angle variance using (15). The rest of the error covariances of the bias states were approximated from gyro data. All the variances were calculated using (17) and (18). Equation (17) was used to calculate the estimated mean value and (18) was used to calculate its covariance as follows: (17)
E
n
→
k
=
1
N
∑
i
=
1
N
n
→
k
i
,
(18)
c
o
v
n
→
k
=
1
N
∑
i
=
1
N
n
k
i
-
E
n
→
k
n
k
i
-
E
n
→
k
T
,where N is a number of samples and n
→
k is a vector of accelerometer data on the x-, y-, and z-axes.All calculations were taken from IMU and placed on the table without movement for approximately one minute with sampling periodT = 0.05sec. Figure 5 shows the accelerometer and gyroscope signals in static conditions on all three axes. The experimentally determined variance values for attitude states are σ
φ
2
=
6.05
E
-
6 and σ
θ
2
=
4.98
E
-
6 in rad2. And for the gyroscope along the x-, y-, and z-axes, the values are σ
b
x
2
=
0.046, σ
b
y
2
=
0.018, and σ
b
z
2
=
0.005 in rad/s2. These values will be the diagonal elements of the initial P
k.Figure 5
Three-axis gyroscope and accelerometer in static condition.


#### 2.4.3. Initial Values and Covariance of the Noise

The initial value of the bias gyro was determined heuristically through static IMU data in rad/s unit. The initial value for attitude states was chosen as one degree for each state. The initial states value in EKF model is as follows:(19)
x
k
=
1
=
φ
θ
ψ
b
x
b
y
b
z
T
=
1
1
1
-
0.019
-
0.008
0.029
T
.Since the noise is assumed to be in normal distribution and independent on each axis, we use the variance of noisew
(
t
) and v
(
t
). Noise process matrix (Q) was selected heuristically as q
11, q
22, and q
33 are 1 and q
44, q
55, and q
66 are 0.3.The measurement noise covariance matrix (R
n
o
m) is defined by (20) and the measurement comes from the attitude in (15); however, the measurement noise covariance matrix is obtained by calculating the variance of accelerometer signal through (17) and (18). These signals have a direct relation to the attitude measurement: (20)
R
n
o
m
=
E
v
k
v
k
T
.To determine the diagonal element of R
n
o
m, the variances of f
x, f
y, and f
z are calculated: r
11 = 9.04 × 10−6, r
22 = 1.04 × 10−5, and r
33 = 1.80 × 10−5 g2. Since the accelerometer is sensitive to disturbances, for the sake of practicality we tuned the value of R
n
o
m based on the application. Related to the proposed method, a
k from the external acceleration model is uncorrelated to noise v
k, and the discrete form of R is now modified to be as follows:(21)
R
k
=
R
n
o
m
+
λ
a
k
2
,where λ is a constant coefficient that is heuristically chosen based on the application; inMode 1, λ is set to zero. Particularly inMode 2, R is tuned based on the external acceleration model and a
k
2 is the element of the diagonal matrix, which consists of a
y
2, a
x
2, and a
z
2. The diagonal matrix of a
k
2 in (21) plays a key role in compensating for the external acceleration by increasing the R
k value. During static conditions or slow movement (i.e., a
k
2
≈
0), the covariance matrix will be the same as R
n
o
m.The above process and measurement models construct the procedure of the EKF as follows:(1)
Set the initial values for states and error covariance(22)
x
^
0
,
P
0
.
(2)
Predict states and error covariance(23)
x
^
k
-
=
f
x
^
k
-
1
,
P
k
-
=
A
P
k
-
1
A
T
+
Q
.
(3)
Compute the Kalman gain(24)
K
k
=
P
k
-
H
T
H
P
k
-
H
T
+
R
-
1
.
(4)
Compute the states estimate(25)
x
^
k
=
x
^
k
-
+
K
k
z
k
-
H
x
^
k
-
.
(5)
Compute the error covariance(26)
P
k
=
P
k
-
-
K
k
H
P
k
-
.


### 2.5. System Model ofMode 3

The two-step EKF [1] used separate measurements between the gyro and accelerometer. The external acceleration compensation technique is based onweighted-switching in setting the noise covariance of those measurements and was proposed by Suh et al.The statex
(
t
) forMode 3 is x
(
t
)
=
θ
φ
p
q
r
T and the measurement variable is z
(
t
)
=
f
x
f
y
p
q
r
T. The system equation is given by(27)
x
˙
t
=
A
t
x
t
+
w
t
,
z
t
=
f
x
t
+
v
t
,where (28)
A
t
=
0
0
0
c
φ
-
s
φ
0
0
1
s
φ
t
θ
c
φ
t
θ
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
,
f
x
t
=
s
θ
s
φ
c
θ
p
q
r
.InMode 3, there will be a setting for the measurement error covariance matrix, in which r
1 and r
2 belong to the accelerometer and r
3 belongs to the gyroscope, as in (29)
R
=
r
1
0
0
0
0
0
r
2
0
0
0
0
0
r
3
0
0
0
0
0
r
3
0
0
0
0
0
r
3
.The two-step measurement updated Kalman filter was used to update the Kalman gain, state estimation, and covariance matrix.First, the measurement was updated only for variance of the error measurement in the gyroscope (r
3).Second, the measurement was updated for r
1 and r
2 using aweighted-switching rule as in (32), based on the threshold rule of (31) to detect external acceleration. The threshold rule is derived from a necessary condition for acceleration free movement, as in (30). Theweighted-switching rule works on the principle that when there is external acceleration, the gyroscope outputs should be trusted more. This can be done by making r
1 and r
2 larger. For a more detailed explanation, refer to [1]. Consider the following:(30)
f
x
2
+
f
y
2
+
f
z
2
=
1
,
(31)
f
x
2
+
f
y
2
+
f
z
2
-
1
>
δ
,
(32)
r
1
,
k
r
2
,
k
=
max
⁡
α
1
r
1
,
k
-
1
r
2
,
k
-
1
+
α
2
z
k
,
1
-
C
1
,
k
x
^
k
,
g
,
r
1
,
n
o
m
r
2
,
n
o
m
.


### 2.6. System Model ofMode 4

Mode 4 is proposed by Sabatini [11] using quaternion-based EKF. The discrete state forMode 4 is composed of the rotation quaternion, accelerometer, and magnetometer bias vectors x
→
k
+
1
=
q
→
k
+
1
b
→
a
k
+
1
b
→
m
k
+
1
T. The measurement model is constructed by accelerometer and magnetometer measurement vectors z
→
k
+
1
=
a
→
k
+
1
m
→
k
+
1
T. The mechanism of adaptation of the measurement noise covariance matrix of acceleration is implemented using athreshold-based approach:(33)
σ
R
a
2
=
σ
a
2
,
a
→
j
+
1
-
g
→
<
ε
t
h
∀
j
∈
k
-
k
a
,
k
∞
,
otherwise.The measured acceleration magnitude is tested in advance to recognize deviations from gravity. If there is deviation greater than the threshold, the observation variance σ
R
a
2 is set to a high value.


### 2.7. System Model ofMode 5

Mode 5 is proposed by Harada et al. [12] using quaternion-based UKF. The method is threshold-based combined with a softened part of the measurement covariance matrix adaptation. The output of the accelerometer is detected as reliable if it satisfies the following condition: a
→
k
-
g
→
<
ε
th, where a
→
k is the output of the accelerometer on the body frame. The mechanism of the adaptation of the measurement covariance matrix is as follows:(34)
σ
a
,
k
2
=
σ
a
0
2
+
α
a
a
→
b
k
-
g
→
b
k
2
,where σ
a
0 is the offset and α
a
a
→
b
k
-
g
→
b
k
2 is the softened part of the adaptation mechanism. g
→
b
k is the predicted acceleration that comes from R
q
·
g
→. If the orientation quaternion q
=
e
→
T
,
q
4, where e
→
=
q
1
,
q
2
,
q
3
T is the vector part and q
4 is the scalar part of the quaternion, then (35)
g
→
b
k
=
1
e
→
2
+
q
4
2
2
q
1
q
3
-
q
2
q
4
2
q
2
q
3
-
q
1
q
4
-
q
1
2
-
q
2
2
+
q
3
2
+
q
4
2
.


## 2.1. Sensor Model

The relative attitude (roll and pitch) of two frames of reference used a direction cosine matrix (rotation matrix) for coordinate transformation or rotation of object.φ (roll), θ (pitch), and ψ (yaw) are the rotation angles about the x-, y-, and z-axes on the sensor body, as shown in Figure 1. C
I
/
B indicates a rotation matrix that represents the relative attitude of frame I (inertial frame) with respect to frame B (body of sensor), thus allowing the relation as follows:(1)
x
I
=
C
I
/
B
x
B
,where x
B is the coordinate vector in frame B and x
I is the coordinate in frame I.Figure 1
Orientation of axes of sensitivity for IMU.The well-known rotation matrix, with the function ofZ
→
Y
→
X Euler angles, is as follows:(2)
C
I
/
B
φ
,
θ
,
ψ
=
C
X
φ
C
Y
θ
C
Z
ψ
=
c
θ
c
ψ
-
c
φ
s
ψ
+
s
φ
s
θ
c
ψ
s
φ
s
ψ
+
c
φ
s
θ
c
ψ
c
θ
s
ψ
c
φ
c
ψ
+
s
φ
s
θ
s
ψ
-
s
φ
c
ψ
+
c
φ
s
θ
s
ψ
-
s
θ
s
φ
c
θ
c
φ
c
θ
,where s and c are the short forms of sine and cosine, respectively.Measurements model for attitude determination using accelerometers is constructed by the third row ofC
I
/
B that contains φ and θ. It is independent of rotation along the z-axis (ψ).Sensor signal from the accelerometer is modeled as follows:(3)
f
A
=
a
+
g
-
s
θ
s
φ
c
θ
c
φ
c
θ
T
,where f
A is a vector of f
x, f
y, and f
z; the acceleration is measured by the three-axis accelerometer on the body frame. The unwanted external acceleration information (a) in the accelerometer will be estimated. This acceleration is caused by the change of velocity in magnitude and/or direction. The external acceleration is the main source of errors in attitude estimation.


## 2.2. External Acceleration Compensation Method

In this section, we introduce the method for compensating for the external acceleration. Our method is based on the view that external acceleration is a disturbance in the original signal and therefore should be removed from the original signal. The disturbance rejection model was derived from a common control ratio model [15]. From this point on, the term “compensation model” will be used to replace the term “disturbance rejection model.” The three-axis accelerometer data is input for the compensation model. Each axis will be treated in the same step; therefore, for the sake of convenience, only the x-axis will be discussed. In (4), ideally the compensated x-axis accelerometer signal (f
∗
x
k) was determined by subtracting the compensation model in the x-axis (a
k) from the original signal (f
x
k):(4)
f
∗
x
k
=
f
x
k
-
a
k
.The external acceleration (a
k) is extracted from f
x
k by the compensation model. Figure 2 shows the ideal model diagram of (4). However, due to the dynamism of the system and many uncertainties, there is no ideal model. Consequently, we will use the result of the compensation model as one parameter for adjusting the noise covariance matrix in sensor fusion in the next section. The design approach for a compensation model is based on the specification that the model should not be affected by the disturbance input d
(
t
). In other words, the steady-state output should be zero if possible or a small constant; y
t
ss
=
0, d
(
t
)
≠
0. A control ratio in the s domain [15] is commonly written as follows:(5)
Y
s
D
s
=
K
s
w
+
c
w
-
1
s
w
-
1
+
⋯
+
c
1
s
+
c
0
s
n
+
q
n
-
1
s
n
-
1
+
⋯
+
q
1
s
+
q
0
.Assuming a step disturbance signal D
(
s
)
=
D
0
/
s, the output will be(6)
Y
s
=
K
D
0
s
w
+
c
w
-
1
s
w
-
1
+
⋯
+
c
1
s
+
c
0
s
s
n
+
q
n
-
1
s
n
-
1
+
⋯
+
q
1
s
+
q
0
,where K is gain constant, K
>
0, c and q are constant coefficients, and D
0 is the final value of step disturbance. The condition y
t
ss
=
0 will be achieved if c
0
=
0 and requires the numerator of Y
(
s
)
/
D
(
s
) to have at least one zero at the origin of the s-plane. On the other side, the location of the poles on the s-plane determined its corresponding response. To produce the damped response, the poles should be on the left-half s-plane. There are two possibilities for poles locations on the left-half side, such as complex-conjugate poles and real poles, which have the output characteristics of exponentially damped sinusoid and damped exponential response, respectively. In our model, we chose damped exponential response because the disturbance should be suppressed without a sinusoidally oscillating component. Finally, we determined the transfer function of our model: there is one zero at the origin and one real pole on the left-half of the s-plane which is based on (5) and becomes as follows:(7)
Y
s
D
s
=
K
s
s
+
q
0
.Figure 2
The input-output diagram of external acceleration compensation.The compensating model in (7) attenuates low frequencies and as a result a high frequency signal is obtained. This is in line with the fact that the external acceleration occurs when the change of velocity in magnitude and/or direction of each axis exceeds a certain limit.The analysis of the stability of model (7) will be introduced in the time domain as follows. In the time domain, the stability will be achieved based on the principle that there is a limited response to the limited input. We discuss the time response of singularity input functions to this model, such as step, ramp, rectangular pulse, and parabolic functions. The time response model (7) for a unit step input, u
(
t
), is y
(
t
)
=
K
e
-
q
0
t. We get steady-state response for y
(
t
) and the steady-state error as in(8)
y
t
ss
=
lim
t
→
∞
⁡
K
e
-
q
0
t
=
0
,
(9)
err
⁡
t
ss
=
lim
t
→
∞
⁡
1
-
K
e
-
q
0
t
=
1
.The time response for ramp input (d
(
t
)
=
t) is y
(
t
)
=
K
/
q
0
1
-
e
-
q
0
t. The steady-state output and error are calculated as follows:(10)
y
t
ss
=
lim
t
→
∞
⁡
K
q
0
1
-
e
-
q
0
t
=
K
q
0
,
err
⁡
t
ss
=
lim
t
→
∞
⁡
K
q
0
e
-
q
0
t
-
1
+
t
=
∞
.A rectangular pulse of amplitude 1
/
a and of duration a, d
(
t
)
=
u
(
t
)
-
u
(
t
-
a
)
/
a, has azero steady-state output and also azero steady-state error.For a parabolic function input,d
(
t
)
=
0.5
t
2, the time response is calculated as follows, with an infinity steady-state output and infinity steady-state error:(11)
y
t
=
K
q
0
t
+
K
q
0
2
e
-
q
0
t
-
1
.These four kinds of singularity inputs and their combination represent the possibility of disturbance waveforms. All of them have the tendency to be rejected and damped by model (7). The response of the parabolic input has not shown constant steady-state output; however, it represented the exponentially damped factor. Figures 3(a), 3(b), 3(c), and 3(d) illustrate all time responses for each basic input signal, using model (7) within K
=
1. Each figure has two outputs, that is, y
1
(
t
) and y
2
(
t
), where q
0 in y
1
(
t
) is greater than q
0 in y
2
(
t
).Figure 3
Time response plot corresponding to (7) for some basic input signals: (a) step function, (b) ramp, (c) parabolic, and (d) pulse.
(a)
(b)
(c)
(d)


## 2.3. Attitude Determination through Sensor Fusion Method

Attitude determination from gyro had errors accumulating along the process of numerical integration, but the gyro was sensitive to the change of rotation. While the attitude determination by the accelerometer does not show accumulation errors or divergence, the error in the roll and pitch axis is too large because those two axes are mutually affected. When there is a movement in the roll axis, the pitch axis also moves up and down with respect to the horizontal plane, and vice versa. In the proposed method, both sensors are complementary to each other to achieve performance in the sensor fusion technique using the extended Kalman filter (EKF).In this paper, the work will be divided into five modes.Mode 1 is the standard EKF using the proposed system model but without compensating the external acceleration.Mode 2 is the proposed system model, also containing the proposed external acceleration compensation, with a preprocessing technique for accelerometer data using model (7) before the EKF process. The difference betweenMode 2 andMode 1 is the addition of the measurement error covariance based on the external acceleration compensation model. A detailed explanation ofMode 1 andMode 2 is in Section 2.4.Mode 3 is derived from [1]: thetwo-step EKF which adjusts the measurement noise covariance using theweighted-switching approach.Mode 4 is the quaternion-based EKF, which uses athreshold-based approach to set the measurement error covariance. This mode is derived from [11].Mode 5 is derived from [12], which uses an unscented Kalman filter (UKF). This method works as threshold-based approach combined with a softened part of measurement noise covariance matrix adjustment.


## 2.4. Algorithm Description ofMode 1 andMode 2

### 2.4.1. Process and Measurement Models

We initially defined the states and the observation variables for the system model. We set attitude and gyro bias as state variables, since the bias errors are a highly complex function to the ambient temperature. The Euler angle was the angle representation. The state variablex
(
t
) and the measurement variable z
(
t
) are defined as follows:(12)
x
=
φ
θ
ψ
b
x
b
y
b
z
T
,
z
=
φ
a
θ
a
ψ
a
T
,where φ (roll) and θ (pitch) are the rotation angles about the x- and y-axes and ψ is yaw angle, but it is not of concern in this study. These come from the integration of the rate of change from gyros, while b
x, b
y, and b
z are biases from gyro in x-, y-, and z-axis, respectively. We use the measurement from the accelerometer in order to calculate φ
a, θ
a, and ψ
a as measurement variables. The system equation is given by(13)
x
˙
t
=
f
x
t
+
w
t
,
z
t
=
H
x
t
+
v
t
,where f
(
x
(
t
)
) is a nonlinear function representing the relation between gyros data and kinematic equation for the Euler angles (Z
→
Y
→
X); H is state to measurement matrix as shown in (14); and w
(
t
) and v
(
t
) are process noise and measurement noise, respectively, which are assumed to be uncorrelated Gaussian distributed white noise. Consider(14)
f
x
=
1
s
φ
t
θ
c
φ
t
θ
0
0
0
0
c
φ
-
s
φ
0
0
0
0
s
φ
s
c
θ
c
φ
s
c
θ
0
0
0
0
0
0
1
0
0
0
0
0
0
1
0
0
0
0
0
0
1
·
p
-
b
x
q
-
b
y
r
-
b
z
b
x
b
y
b
z
,
H
=
1
0
0
0
0
0
0
1
0
0
0
0
0
0
1
0
0
0
.In (14), p, q, and r are angular velocities measured from the gyro along the x-, y-, and z-axes, respectively. Symbols of s, c, t, and s
c are short forms of sine, cosine, tangent, and secant, respectively. This process model is nonlinear since it contains trigonometric functions. Therefore, we use Jacobian values of the nonlinear model of f
(
x
) to replace state transition matrix (A), where the values in that matrix are obtained by applying the previous estimated states in the EKF process. In contrast, matrix H is the linear expression.The measurement from the accelerometer which is used to calculateφ
a and θ
a as measurement variables is a common initialized leveling equation [2, 16]:(15)
φ
a
=
arctan
2
⁡
f
y
,
f
z
θ
a
=
arctan
⁡
-
f
x
f
y
s
φ
a
+
f
z
c
φ
aFigure4 shows the structure of the proposed algorithm combined with the extended Kalman filter algorithm. The notations and meanings of x
^
k, x
^
k
-, P
k, and P
k
- are estimates of state, prediction of state, estimate of the error covariance, and the prediction of the error covariance, respectively.Figure 4
Structure of the proposed algorithm.


### 2.4.2. Error Covariance

In Figure4, the estimated error covariance is computed by P
k
=
P
k
-
-
K
k
H
P
k
-. Error covariance indicates the difference between state estimation (x
^
k) and the unknown true state value (x
k). It also can be defined as the following [17]:(16)
P
k
=
E
x
k
-
x
^
k
x
k
-
x
^
k
T
.The error covariance of attitude states was initialized by calculating the angle variance using (15). The rest of the error covariances of the bias states were approximated from gyro data. All the variances were calculated using (17) and (18). Equation (17) was used to calculate the estimated mean value and (18) was used to calculate its covariance as follows: (17)
E
n
→
k
=
1
N
∑
i
=
1
N
n
→
k
i
,
(18)
c
o
v
n
→
k
=
1
N
∑
i
=
1
N
n
k
i
-
E
n
→
k
n
k
i
-
E
n
→
k
T
,where N is a number of samples and n
→
k is a vector of accelerometer data on the x-, y-, and z-axes.All calculations were taken from IMU and placed on the table without movement for approximately one minute with sampling periodT = 0.05sec. Figure 5 shows the accelerometer and gyroscope signals in static conditions on all three axes. The experimentally determined variance values for attitude states are σ
φ
2
=
6.05
E
-
6 and σ
θ
2
=
4.98
E
-
6 in rad2. And for the gyroscope along the x-, y-, and z-axes, the values are σ
b
x
2
=
0.046, σ
b
y
2
=
0.018, and σ
b
z
2
=
0.005 in rad/s2. These values will be the diagonal elements of the initial P
k.Figure 5
Three-axis gyroscope and accelerometer in static condition.


### 2.4.3. Initial Values and Covariance of the Noise

The initial value of the bias gyro was determined heuristically through static IMU data in rad/s unit. The initial value for attitude states was chosen as one degree for each state. The initial states value in EKF model is as follows:(19)
x
k
=
1
=
φ
θ
ψ
b
x
b
y
b
z
T
=
1
1
1
-
0.019
-
0.008
0.029
T
.Since the noise is assumed to be in normal distribution and independent on each axis, we use the variance of noisew
(
t
) and v
(
t
). Noise process matrix (Q) was selected heuristically as q
11, q
22, and q
33 are 1 and q
44, q
55, and q
66 are 0.3.The measurement noise covariance matrix (R
n
o
m) is defined by (20) and the measurement comes from the attitude in (15); however, the measurement noise covariance matrix is obtained by calculating the variance of accelerometer signal through (17) and (18). These signals have a direct relation to the attitude measurement: (20)
R
n
o
m
=
E
v
k
v
k
T
.To determine the diagonal element of R
n
o
m, the variances of f
x, f
y, and f
z are calculated: r
11 = 9.04 × 10−6, r
22 = 1.04 × 10−5, and r
33 = 1.80 × 10−5 g2. Since the accelerometer is sensitive to disturbances, for the sake of practicality we tuned the value of R
n
o
m based on the application. Related to the proposed method, a
k from the external acceleration model is uncorrelated to noise v
k, and the discrete form of R is now modified to be as follows:(21)
R
k
=
R
n
o
m
+
λ
a
k
2
,where λ is a constant coefficient that is heuristically chosen based on the application; inMode 1, λ is set to zero. Particularly inMode 2, R is tuned based on the external acceleration model and a
k
2 is the element of the diagonal matrix, which consists of a
y
2, a
x
2, and a
z
2. The diagonal matrix of a
k
2 in (21) plays a key role in compensating for the external acceleration by increasing the R
k value. During static conditions or slow movement (i.e., a
k
2
≈
0), the covariance matrix will be the same as R
n
o
m.The above process and measurement models construct the procedure of the EKF as follows:(1)
Set the initial values for states and error covariance(22)
x
^
0
,
P
0
.
(2)
Predict states and error covariance(23)
x
^
k
-
=
f
x
^
k
-
1
,
P
k
-
=
A
P
k
-
1
A
T
+
Q
.
(3)
Compute the Kalman gain(24)
K
k
=
P
k
-
H
T
H
P
k
-
H
T
+
R
-
1
.
(4)
Compute the states estimate(25)
x
^
k
=
x
^
k
-
+
K
k
z
k
-
H
x
^
k
-
.
(5)
Compute the error covariance(26)
P
k
=
P
k
-
-
K
k
H
P
k
-
.


## 2.4.1. Process and Measurement Models

We initially defined the states and the observation variables for the system model. We set attitude and gyro bias as state variables, since the bias errors are a highly complex function to the ambient temperature. The Euler angle was the angle representation. The state variablex
(
t
) and the measurement variable z
(
t
) are defined as follows:(12)
x
=
φ
θ
ψ
b
x
b
y
b
z
T
,
z
=
φ
a
θ
a
ψ
a
T
,where φ (roll) and θ (pitch) are the rotation angles about the x- and y-axes and ψ is yaw angle, but it is not of concern in this study. These come from the integration of the rate of change from gyros, while b
x, b
y, and b
z are biases from gyro in x-, y-, and z-axis, respectively. We use the measurement from the accelerometer in order to calculate φ
a, θ
a, and ψ
a as measurement variables. The system equation is given by(13)
x
˙
t
=
f
x
t
+
w
t
,
z
t
=
H
x
t
+
v
t
,where f
(
x
(
t
)
) is a nonlinear function representing the relation between gyros data and kinematic equation for the Euler angles (Z
→
Y
→
X); H is state to measurement matrix as shown in (14); and w
(
t
) and v
(
t
) are process noise and measurement noise, respectively, which are assumed to be uncorrelated Gaussian distributed white noise. Consider(14)
f
x
=
1
s
φ
t
θ
c
φ
t
θ
0
0
0
0
c
φ
-
s
φ
0
0
0
0
s
φ
s
c
θ
c
φ
s
c
θ
0
0
0
0
0
0
1
0
0
0
0
0
0
1
0
0
0
0
0
0
1
·
p
-
b
x
q
-
b
y
r
-
b
z
b
x
b
y
b
z
,
H
=
1
0
0
0
0
0
0
1
0
0
0
0
0
0
1
0
0
0
.In (14), p, q, and r are angular velocities measured from the gyro along the x-, y-, and z-axes, respectively. Symbols of s, c, t, and s
c are short forms of sine, cosine, tangent, and secant, respectively. This process model is nonlinear since it contains trigonometric functions. Therefore, we use Jacobian values of the nonlinear model of f
(
x
) to replace state transition matrix (A), where the values in that matrix are obtained by applying the previous estimated states in the EKF process. In contrast, matrix H is the linear expression.The measurement from the accelerometer which is used to calculateφ
a and θ
a as measurement variables is a common initialized leveling equation [2, 16]:(15)
φ
a
=
arctan
2
⁡
f
y
,
f
z
θ
a
=
arctan
⁡
-
f
x
f
y
s
φ
a
+
f
z
c
φ
aFigure4 shows the structure of the proposed algorithm combined with the extended Kalman filter algorithm. The notations and meanings of x
^
k, x
^
k
-, P
k, and P
k
- are estimates of state, prediction of state, estimate of the error covariance, and the prediction of the error covariance, respectively.Figure 4
Structure of the proposed algorithm.


## 2.4.2. Error Covariance

In Figure4, the estimated error covariance is computed by P
k
=
P
k
-
-
K
k
H
P
k
-. Error covariance indicates the difference between state estimation (x
^
k) and the unknown true state value (x
k). It also can be defined as the following [17]:(16)
P
k
=
E
x
k
-
x
^
k
x
k
-
x
^
k
T
.The error covariance of attitude states was initialized by calculating the angle variance using (15). The rest of the error covariances of the bias states were approximated from gyro data. All the variances were calculated using (17) and (18). Equation (17) was used to calculate the estimated mean value and (18) was used to calculate its covariance as follows: (17)
E
n
→
k
=
1
N
∑
i
=
1
N
n
→
k
i
,
(18)
c
o
v
n
→
k
=
1
N
∑
i
=
1
N
n
k
i
-
E
n
→
k
n
k
i
-
E
n
→
k
T
,where N is a number of samples and n
→
k is a vector of accelerometer data on the x-, y-, and z-axes.All calculations were taken from IMU and placed on the table without movement for approximately one minute with sampling periodT = 0.05sec. Figure 5 shows the accelerometer and gyroscope signals in static conditions on all three axes. The experimentally determined variance values for attitude states are σ
φ
2
=
6.05
E
-
6 and σ
θ
2
=
4.98
E
-
6 in rad2. And for the gyroscope along the x-, y-, and z-axes, the values are σ
b
x
2
=
0.046, σ
b
y
2
=
0.018, and σ
b
z
2
=
0.005 in rad/s2. These values will be the diagonal elements of the initial P
k.Figure 5
Three-axis gyroscope and accelerometer in static condition.


## 2.4.3. Initial Values and Covariance of the Noise

The initial value of the bias gyro was determined heuristically through static IMU data in rad/s unit. The initial value for attitude states was chosen as one degree for each state. The initial states value in EKF model is as follows:(19)
x
k
=
1
=
φ
θ
ψ
b
x
b
y
b
z
T
=
1
1
1
-
0.019
-
0.008
0.029
T
.Since the noise is assumed to be in normal distribution and independent on each axis, we use the variance of noisew
(
t
) and v
(
t
). Noise process matrix (Q) was selected heuristically as q
11, q
22, and q
33 are 1 and q
44, q
55, and q
66 are 0.3.The measurement noise covariance matrix (R
n
o
m) is defined by (20) and the measurement comes from the attitude in (15); however, the measurement noise covariance matrix is obtained by calculating the variance of accelerometer signal through (17) and (18). These signals have a direct relation to the attitude measurement: (20)
R
n
o
m
=
E
v
k
v
k
T
.To determine the diagonal element of R
n
o
m, the variances of f
x, f
y, and f
z are calculated: r
11 = 9.04 × 10−6, r
22 = 1.04 × 10−5, and r
33 = 1.80 × 10−5 g2. Since the accelerometer is sensitive to disturbances, for the sake of practicality we tuned the value of R
n
o
m based on the application. Related to the proposed method, a
k from the external acceleration model is uncorrelated to noise v
k, and the discrete form of R is now modified to be as follows:(21)
R
k
=
R
n
o
m
+
λ
a
k
2
,where λ is a constant coefficient that is heuristically chosen based on the application; inMode 1, λ is set to zero. Particularly inMode 2, R is tuned based on the external acceleration model and a
k
2 is the element of the diagonal matrix, which consists of a
y
2, a
x
2, and a
z
2. The diagonal matrix of a
k
2 in (21) plays a key role in compensating for the external acceleration by increasing the R
k value. During static conditions or slow movement (i.e., a
k
2
≈
0), the covariance matrix will be the same as R
n
o
m.The above process and measurement models construct the procedure of the EKF as follows:(1)
Set the initial values for states and error covariance(22)
x
^
0
,
P
0
.
(2)
Predict states and error covariance(23)
x
^
k
-
=
f
x
^
k
-
1
,
P
k
-
=
A
P
k
-
1
A
T
+
Q
.
(3)
Compute the Kalman gain(24)
K
k
=
P
k
-
H
T
H
P
k
-
H
T
+
R
-
1
.
(4)
Compute the states estimate(25)
x
^
k
=
x
^
k
-
+
K
k
z
k
-
H
x
^
k
-
.
(5)
Compute the error covariance(26)
P
k
=
P
k
-
-
K
k
H
P
k
-
.


## 2.5. System Model ofMode 3

The two-step EKF [1] used separate measurements between the gyro and accelerometer. The external acceleration compensation technique is based onweighted-switching in setting the noise covariance of those measurements and was proposed by Suh et al.The statex
(
t
) forMode 3 is x
(
t
)
=
θ
φ
p
q
r
T and the measurement variable is z
(
t
)
=
f
x
f
y
p
q
r
T. The system equation is given by(27)
x
˙
t
=
A
t
x
t
+
w
t
,
z
t
=
f
x
t
+
v
t
,where (28)
A
t
=
0
0
0
c
φ
-
s
φ
0
0
1
s
φ
t
θ
c
φ
t
θ
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
,
f
x
t
=
s
θ
s
φ
c
θ
p
q
r
.InMode 3, there will be a setting for the measurement error covariance matrix, in which r
1 and r
2 belong to the accelerometer and r
3 belongs to the gyroscope, as in (29)
R
=
r
1
0
0
0
0
0
r
2
0
0
0
0
0
r
3
0
0
0
0
0
r
3
0
0
0
0
0
r
3
.The two-step measurement updated Kalman filter was used to update the Kalman gain, state estimation, and covariance matrix.First, the measurement was updated only for variance of the error measurement in the gyroscope (r
3).Second, the measurement was updated for r
1 and r
2 using aweighted-switching rule as in (32), based on the threshold rule of (31) to detect external acceleration. The threshold rule is derived from a necessary condition for acceleration free movement, as in (30). Theweighted-switching rule works on the principle that when there is external acceleration, the gyroscope outputs should be trusted more. This can be done by making r
1 and r
2 larger. For a more detailed explanation, refer to [1]. Consider the following:(30)
f
x
2
+
f
y
2
+
f
z
2
=
1
,
(31)
f
x
2
+
f
y
2
+
f
z
2
-
1
>
δ
,
(32)
r
1
,
k
r
2
,
k
=
max
⁡
α
1
r
1
,
k
-
1
r
2
,
k
-
1
+
α
2
z
k
,
1
-
C
1
,
k
x
^
k
,
g
,
r
1
,
n
o
m
r
2
,
n
o
m
.


## 2.6. System Model ofMode 4

Mode 4 is proposed by Sabatini [11] using quaternion-based EKF. The discrete state forMode 4 is composed of the rotation quaternion, accelerometer, and magnetometer bias vectors x
→
k
+
1
=
q
→
k
+
1
b
→
a
k
+
1
b
→
m
k
+
1
T. The measurement model is constructed by accelerometer and magnetometer measurement vectors z
→
k
+
1
=
a
→
k
+
1
m
→
k
+
1
T. The mechanism of adaptation of the measurement noise covariance matrix of acceleration is implemented using athreshold-based approach:(33)
σ
R
a
2
=
σ
a
2
,
a
→
j
+
1
-
g
→
<
ε
t
h
∀
j
∈
k
-
k
a
,
k
∞
,
otherwise.The measured acceleration magnitude is tested in advance to recognize deviations from gravity. If there is deviation greater than the threshold, the observation variance σ
R
a
2 is set to a high value.


## 2.7. System Model ofMode 5

Mode 5 is proposed by Harada et al. [12] using quaternion-based UKF. The method is threshold-based combined with a softened part of the measurement covariance matrix adaptation. The output of the accelerometer is detected as reliable if it satisfies the following condition: a
→
k
-
g
→
<
ε
th, where a
→
k is the output of the accelerometer on the body frame. The mechanism of the adaptation of the measurement covariance matrix is as follows:(34)
σ
a
,
k
2
=
σ
a
0
2
+
α
a
a
→
b
k
-
g
→
b
k
2
,where σ
a
0 is the offset and α
a
a
→
b
k
-
g
→
b
k
2 is the softened part of the adaptation mechanism. g
→
b
k is the predicted acceleration that comes from R
q
·
g
→. If the orientation quaternion q
=
e
→
T
,
q
4, where e
→
=
q
1
,
q
2
,
q
3
T is the vector part and q
4 is the scalar part of the quaternion, then (35)
g
→
b
k
=
1
e
→
2
+
q
4
2
2
q
1
q
3
-
q
2
q
4
2
q
2
q
3
-
q
1
q
4
-
q
1
2
-
q
2
2
+
q
3
2
+
q
4
2
.


## 3. Experimental Setup and Results

### 3.1. Various Test Conditions

We have five modes of attitude estimation, as stated in Section2.3. There will be three tests in each mode, aimed at studying the performance under various dynamic conditions.Test A was performed by placing the IMU sensor on the slider table of a MISUMI RSH3 single-axis robot. By moving the slider back and forth with the robot controller, external acceleration is applied on thex-axis of the IMU sensor. Test A intended to test proposed method on the lateral movement as is done in [1], which will affect the pitch angle. In Test A, we conducted the trial by using such conditions as follows: (1) four acceleration coefficients in ascending order from the MISUMI software settings, that is, 0.1, 0.75, 1.5, and 2.5 m/s2; (2) four q
0 parameter values, that is, 0.5, 0.1, 0.05, and 0.01; (3) six λ values, that is, 35, 80, 100, 150, 170, and 200.Test B involved movement using free hand in thex- and y-axes of the IMU as performed in our previous study [13] by mimicking the pendulum swing movement. The movement along the x
-axis was considered as roll and the movement along the y
-axis was considered as pitch angle. In contrast with Test A, Test B was performed using external acceleration on a combination of axes.Test C was executed based on the application of attitude estimation on a shoe-type measurement device. An IMU sensor was placed on the top of one shoe. The subject was asked to walk straight, 3-4 strides, at a normal speed.


### 3.2. Test Setup

To verify and validate the proposed method, all tests were conducted using an IMU sensor consisting of an accelerometer (±16 g), a gyro (±1500 deg/s), and a magnetometer (±0.9 Ga) (from Logical Product, Japan). Sensor data were transmitted to the PC wirelessly. A combination of NI LabVIEW® and Simulink® was used for data acquisition and the MATLAB® program was used to execute the proposed method. In Test A, the MISUMI RSH3 single-axis robot (from MISUMI Group Inc., Japan) had a max speed of 300 mm/s, had an effective stroke of 500 mm, and was controlled by the RS-Manager support software used as a testbed (see Figures6(a) and 6(b)). Note that the attitude is not changed because the IMU sensor was moving in a lateral direction without rotation; that is, in order to validate the proposed method, the reference values are θ = 0° and φ = 0°.Figure 6
Test A setup for validating attitude estimation. The RSH3 table slider moves along thex-axis.
(a)
(b)In Test B, the IMU sensor was placed in plastic jar surrounded by styrofoam to avoid magnetic interference between the IMU and the electromagnetic motion tracking system receiver (Fastrak from Polhemus Inc., USA). We used Fastrak as the reference by recording the roll and pitch angles of the receiver using C# data acquisition program. Fastrak attitude data were transmitted to the PC via a cable using an RS-232 protocol (see Figure7).Figure 7
Test B setup consisted of Fastrak® attitude reference system and IMU inside a plastic jar.In Test C, we mounted the IMU sensor on the shoe. For a reference measurement, the experiment setup contained six OptiTrack® cameras, and four reflective markers were placed at the forefoot and heel. Figures8(a) and 8(b) show the measurement setup for validating Test C. In Tests B and C, we calculated cross-correlation in order to compare references and IMU data from different data acquisition programs. The time when the correlation was maximal will be used to synchronize both measurements. To accomplish time normalization between the reference and the IMU sensor, cubic spline data interpolation was employed.Figure 8
Measurement setup for Test C. (a) Room setup for recording the attitude; (b) four reflective markers were placed on the forefoot and heel.
(a)
(b)The quantitative performance assessment for Test A, Test B, and Test C is using mean squared error (MSE orS) in degree2 and maximum error (M) in degree between predicted attitude and reference attitude as described in(36)
S
θ
=
1
N
∑
k
=
1
N
θ
^
k
T
-
θ
k
T
2
,
M
θ
=
max
k
⁡
θ
^
k
T
-
θ
k
T
.


### 3.3. Experiment Results

#### 3.3.1. Test A

Mode 1 is not related to the compensation model in (7), while the measurement error covariance in (21) was set to R
n
o
m = 1.5 and λ = 0. As withMode 1, neither the compensation model nor updating covariance were used inMode 3, but the constant parameters were heuristic choices based on trial and error; that is, δ = 0.1 g2, α
1 = 0.4, and α
2 = 80. The experimental results ofMode 1 andMode 3 are presented in Tables 1(a) and 1(b).Table 1
(a) Test A: MSE (S) of Mode 1, Mode 3, Mode 4, and Mode 5 in detail. (b) Test A: maximum error (M) of Mode 1, Mode 3, Mode 4, and Mode 5 in detail.
(a)
Acc (m/s2)
Mode 1
Mode 3
Mode 4
Mode 5
S
θ pitch (deg2)
S
φ roll (deg2)
S
θ pitch (deg2)
S
φ roll (deg2)
S
θ pitch (deg2)
S
φ roll (deg2)
S
θ pitch (deg2)
S
φ roll (deg2)
0.1
9.60
2.92
9.31
2.71
99.42
26.73
9.04
2.89
0.75
24.77
3.22
24.66
3.20
92.56
183.10
22.92
2.82
1.5
29.79
3.59
29.41
4.02
113.20
227.13
28.79
2.86
2.5
36.84
4.88
34.49
5.60
44.94
168.43
33.82
4.03
Mean (sd)
25.25 (10)
3.65 (0.8)
24.47 (9.4)
3.88 (1.1)
87.53 (25.7)
151.35 (75.1)
23.64 (9.3)
3.15 (0.5)
(b)
Acc (m/s2)
Mode 1
Mode 3
Mode 4
Mode 5
M
θ pitch (deg)
M
φ roll (deg)
M
θ pitch (deg)
M
φ roll (deg)
M
θ pitch (deg)
M
φ roll (deg)
M
θ pitch (deg)
M
φ roll (deg)
0.1
26.05
8.61
26.05
8.61
18.52
10.19
21.97
8.99
0.75
27.93
7.76
27.93
7.76
20.87
19.02
22.97
6.56
1.5
29.16
6.81
29.16
6.81
23.72
25.44
22.45
5.74
2.5
29.70
12.30
29.70
12.30
16.44
20.74
23.69
9.35
Mean (sd)
28.21 (1.4)
8.87 (2.1)
28.21 (1.4)
8.87 (2.1)
19.89 (2.7)
18.85 (5.5)
22.77 (0.6)
7.66 (1.5)To follow the adaptation of measurement noise covariance matrix (33) inMode 4, the threshold ε
th used is 0.02 and σ
a
2 = 1.5 as used inMode 1 andMode 2. To meet the mechanism of external acceleration (34) inMode 5, σ
a
0
2 is 1.5, which is the same value as we used inMode 1 andMode 2. α
a is determined to be 5, which comes from experiments using α
a = 2 to 10. The MSE will decrease in value when α
a becomes greater, although the MSE decrease is smooth; it is in line with the function of α
a as the softened part of adaptation mechanism. The results ofMode 4 andMode 5 are also presented in Tables 1(a) and 1(b). Overall, the results ofModes 1, 3, 4, and5 are shown in Tables 1(a) and 1(b). The mean MSE ofMode 5 is the lowest amongModes 1, 3, and4, that is, 23.63 and 2.82 deg2 for pitch and roll, respectively.For the purpose of looking at the effect of the reduction of external acceleration, theMode 2 experiment was performed using a different compensating parameter value. Table 2 shows the average MSE of the attitude estimation from four different accelerations, with some variation in the values of q
0 and λ. The variation value of q
0 is 0.5, 0.1, 0.05, and 0.01, all of them using K
=
1
. While the variation value of λ is 35, 80, 100, 150, 170, and 200, in ascending order, the value settings of the EKF parameters are R
n
o
m = 1.5. The relationship between λ and q
0 in one-axis movement test is as follows. The MSE trend decreases as the value of λ increases or q
0 decreases. The best combination for one-axis movement in this experiment is λ = 200 and q
0 = 0.01; the MSE is 8.57 and 1.48 deg2 for pitch and roll, respectively.Table 2
Test A: MSE ofMode 2 during the variability of q
0 and λ, presented as mean (standard deviation).
MSE
q
0
λ
=
35
λ
=
80
λ
=
100
λ
=
150
λ
=
170
λ
=
200
Pitch (deg2)
0.5
15.28 (6.95)
12.94 (5.86)
12.31 (5.56)
11.2 (5.02)
10.86 (4.86)
10.44 (4.65)
0.1
13.77 (6.11)
11.52 (5.08)
10.93 (4.80)
9.89 (4.30)
9.58 (4.15)
9.17 (3.94)
0.05
13.50 (6.04)
11.29 (5.02)
8.9 (5.88)
9.67 (4.23)
9.35 (4.07)
8.94 (3.86)
0.01
13.13 (6.12)
10.94 (5.06)
10.36 (4.76)
9.31 (4.21)
8.99 (4.03)
8.57 (3.81)
Roll (deg2)
0.5
2.62 (0.96)
2.32 (1.02)
2.25 (1.04)
2.13 (1.09)
2.10 (1.10)
2.07 (1.12)
0.1
2.39 (1.00)
2.11 (1.07)
2.04 (1.09)
1.94 (1.12)
1.91 (1.13)
1.87 (1.14)
0.05
2.27 (1.02)
1.99 (1.08)
1.81 (1.20)
1.82 (1.11)
1.79 (1.12)
1.75 (1.12)
0.01
1.91 (1.04)
1.67 (1.08)
1.62 (1.09)
1.53 (1.10)
1.51 (1.10)
1.48 (1.11)The influence of the presentedq
0 and λ forMode 2 in Table 2 was also displayed as a column bar in Figures 9 and 10 for the pitch and roll, respectively. In the meantime, the MSE difference t-test [18] betweenMode 2 andMode 1, Mode 2 andMode 3,andMode 2 andMode 4 as well as that betweenMode 2 andMode 5 was evolved in those figures. In Figures 9 and 10, “a,” “b,” “c,” and “d” denote the significant difference between MSE ofMode 2 andMode 1, Mode 3, Mode 4, andMode 5 (p
<
0.05), respectively. Figures 11(a), 11(b), 11(c), 11(d), 11(e), 11(f), 11(g), 11(h), 11(i), and 11(j) show an example of the attitude estimation. We choose the fastest movement of the MISUMI table slider (2.5 m/s2) in this test as a figure of attitude example. As can be seen,Mode 2 (as compared toModes 1,3, and5) has the smallest amplitude of MSE, that is, 13.47 deg2
.Figure 9
MSE pitch ofMode 2 in Test A: influence of the variability of q
0 and λ. “a, b, c, d” denote significant difference fromMode 1,Mode 3,Mode 4, andMode 5, respectively.Figure 10
MSE roll ofMode 2 in Test A: influence of the variability of q
0 and λ. “a, b, c, d” denote significant difference fromMode 1,Mode 3,Mode 4, andMode 5, respectively.Figure 11
The MSE (S) in deg2 and maximum error (M) in deg of pitch and roll estimation by five modes in Test A, acceleration = 2.5 m/s2. Specifically forMode 2, q
0 = 0.05, λ = 150.Mode 1 in (a) and (b),Mode 2 in (c) and (d),Mode 3 in (e) and (f),Mode 4 in (g) and (h), andMode 5 in (i) and (j).
(a)
(b)
(c)
(d)
(e)
(f)
(g)
(h)
(i)
(j)


#### 3.3.2. Test B

Test B was executed by rotating the IMU sensor in a random manner by hand along thex- and y-axis. In order to determine the optimal q
0 value forMode 2, we conduct the variability of the q
0 test to calculate the MSE and maximum error along the timeline of the whole experiment. Table 4 presents the MSE and maximum errors using different values of q
0. It was found that q
0 = 0.05 has an optimum performance among all the possible choices. Figures 12(a), 12(b), and 12(c) show the influence of q
0 variability to the square output of the proposed compensation model a
k, as in the proposed model ofMode 2 in (7) and (21). Figures 13(a), 13(b), 13(c), 13(d), 13(e), 13(f), 14(a), 14(b), 14(c), 14(d), 15(a), 15(b), 15(c), 15(d), 15(e), 15(f), 15(g), 15(h), 15(i), 15(j), 15(k), and 15(l) are produced by using this parameter value to present the analysis and the result of attitude estimation.Figure 12
Graphs of the square of compensation model output of various value ofq
0 along the x-axis: (a) q
0 = 0.01, (b) q
0 = 0.1, and (c) q
0 = 0.3.
(a)
(b)
(c)Figure 13
External acceleration compensation: original accelerometer signal is shown in (a), (b), and (c); compensation model output is shown in (d), (e), and (f) along thex-, y-, and z-axis, respectively. In (a), the dash-dot line indicatesPhases I andII, respectively, while in (b), the dash-dot line indicatesPhases III andIV, respectively.Figure 14
(a), (b), and (c): graphs of the square of compensation model output forx-, y-, and z-axis ofMode 2. As a comparison is shown in (d): necessary condition ofMode 3.
(a)
(b)
(c)
(d)Figure 15
Test B attitude estimation result: five modes in comparison to the references (a and g) for pitch (left column) and roll (right column).Figures13(a), 13(b), and 13(c) present the accelerometer signal and Figures 13(d), 13(e), and 13(f) present the compensation model signal along the x-, y-, and z-axis of the accelerometer, using model parameters K = 1 and q
0 = 0.05. When the sensor is not experiencing external acceleration, the compensating signal tends to be around zero. While the sensor is being moved quickly, it experiences two major external acceleration periods along the x- and y-axes as shown in Figures 13(a) and 13(b). In Figure 13(a), we call the periodsPhase I andPhase II (indicated by the dash-dot line);Phase I is for 2.4 s during 5.8–8.2 s and followed byPhase II, 2 s during 12.8–14.8 s. Observing accelerometer signal along the y-axis in Figure 13(b), we usePhase III andPhase IV (indicated by the dash-dot line) to refer to the external acceleration experienced for 2 s during 9.2–11.2 s and 1.9 s during 16.2–18.1 s, respectively. The external acceleration duringPhases I andII is closely related to the pitch angle whilePhases III andIV are related to the roll angle.Mode 2 works based on an automatically adjusted measurement noise covariance matrix, as (21). The square of compensation model output was needed to adjust the measurement error covariance matrix and the squared signal output shown in Figures 14(a), 14(b), and 14(c). The timing of the occurrence of the enlarged amplitude of the square of the signal is aligned with the timing of the external acceleration inPhases I toIV. This means that R
k in (21) will increase during the dynamic condition of this phase. However, Figure 14(d) presents the necessary condition ofMode 3 as a comparison.Parameter values are used in each mode in Test B.Mode 1 uses R
n
o
m = 1.5, whileMode 2 uses q
0 = 0.05, R
n
o
m = 1.5, and λ = 3.5.Mode 3 uses thresholds δ = 0.1 g2, α
1 = 0.4, α
2 = 0.3, and r
1 = 0.093, r
2 = 0.034, and r
3 = 1.5.Mode 4 uses the thresholds ε
th = 0.1 and σ
a
2 = 0.3. We did experiments with some value of σ
a
2, that is, 0.2, 0.3, and 0.4; σ
a
2 = 0.3 produced a minimum MSE.Mode 5 works using σ
a
0
2 = 1.5 and the factor of softened part α
a = 3. One of the results of Test B using five modes is presented in Figures 15(b), 15(c), 15(d), 15(e), and 15(f) for pitch and Figures 15(h), 15(i), 15(j), 15(k), and 15(l) for roll estimation.Attitude reference signal from Fastrak is shown in Figures15(a) and 15(g) for pitch and roll, respectively. The quantitative evaluation using MSE between each mode at the time the external acceleration occurred, that is, duringPhase I toPhase IV, is shown in Table 5. Figures 16(a) and 16(b) present the column bar of Table 5 for the MSE of pitch and roll, respectively. In Table 6 and Figure 17 we conclude the MSE result of Test B, the average MSE ofPhases I andII as pitch estimation error, and the average ofPhases III andIV as roll estimation error.Figure 16
Test B: (a) MSE pitch of five modes, (b) MSE roll of five modes during external acceleration, all in deg2.
(a)
(b)Figure 17
Test B: the final MSE pitch and roll of each mode during external acceleration in deg2.


#### 3.3.3. Test C

Test C was executed by walking straight forward three strides along thex-axis of the measurement room coordinate frame. Figures 18(a), 18(b), 18(c), 18(d), 18(e), 18(f), 18(g), 18(h), 18(i), 18(j), 18(k), and 18(l) show the result of attitude estimation using five modes and the MSE result is presented in Table 7.Figure 18
Test C attitude estimation result: five modes in comparison to the motion capture system as references (a and g).Some parameters values are used in Test C for each mode.Mode 1 uses R
nom
⁡ = 1.5, whileMode 2 uses q
0 = 20, R
nom
⁡ = 1.5, and λ = 3
e
4.Mode 3 uses thresholds δ = 0.2 g2, α
1 = 0.8, α
2 = 0.3, and r
1 = 0.093, r
2 = 0.034, and r
3 = 1.5.Mode 4 uses the thresholds ε
th = 0.8 and σ
a
2 = 60.Mode 5 works using σ
a
0
2 = 1.5 and the factor of softened part α
a = 50.


## 3.1. Various Test Conditions

We have five modes of attitude estimation, as stated in Section2.3. There will be three tests in each mode, aimed at studying the performance under various dynamic conditions.Test A was performed by placing the IMU sensor on the slider table of a MISUMI RSH3 single-axis robot. By moving the slider back and forth with the robot controller, external acceleration is applied on thex-axis of the IMU sensor. Test A intended to test proposed method on the lateral movement as is done in [1], which will affect the pitch angle. In Test A, we conducted the trial by using such conditions as follows: (1) four acceleration coefficients in ascending order from the MISUMI software settings, that is, 0.1, 0.75, 1.5, and 2.5 m/s2; (2) four q
0 parameter values, that is, 0.5, 0.1, 0.05, and 0.01; (3) six λ values, that is, 35, 80, 100, 150, 170, and 200.Test B involved movement using free hand in thex- and y-axes of the IMU as performed in our previous study [13] by mimicking the pendulum swing movement. The movement along the x
-axis was considered as roll and the movement along the y
-axis was considered as pitch angle. In contrast with Test A, Test B was performed using external acceleration on a combination of axes.Test C was executed based on the application of attitude estimation on a shoe-type measurement device. An IMU sensor was placed on the top of one shoe. The subject was asked to walk straight, 3-4 strides, at a normal speed.


## 3.2. Test Setup

To verify and validate the proposed method, all tests were conducted using an IMU sensor consisting of an accelerometer (±16 g), a gyro (±1500 deg/s), and a magnetometer (±0.9 Ga) (from Logical Product, Japan). Sensor data were transmitted to the PC wirelessly. A combination of NI LabVIEW® and Simulink® was used for data acquisition and the MATLAB® program was used to execute the proposed method. In Test A, the MISUMI RSH3 single-axis robot (from MISUMI Group Inc., Japan) had a max speed of 300 mm/s, had an effective stroke of 500 mm, and was controlled by the RS-Manager support software used as a testbed (see Figures6(a) and 6(b)). Note that the attitude is not changed because the IMU sensor was moving in a lateral direction without rotation; that is, in order to validate the proposed method, the reference values are θ = 0° and φ = 0°.Figure 6
Test A setup for validating attitude estimation. The RSH3 table slider moves along thex-axis.
(a)
(b)In Test B, the IMU sensor was placed in plastic jar surrounded by styrofoam to avoid magnetic interference between the IMU and the electromagnetic motion tracking system receiver (Fastrak from Polhemus Inc., USA). We used Fastrak as the reference by recording the roll and pitch angles of the receiver using C# data acquisition program. Fastrak attitude data were transmitted to the PC via a cable using an RS-232 protocol (see Figure7).Figure 7
Test B setup consisted of Fastrak® attitude reference system and IMU inside a plastic jar.In Test C, we mounted the IMU sensor on the shoe. For a reference measurement, the experiment setup contained six OptiTrack® cameras, and four reflective markers were placed at the forefoot and heel. Figures8(a) and 8(b) show the measurement setup for validating Test C. In Tests B and C, we calculated cross-correlation in order to compare references and IMU data from different data acquisition programs. The time when the correlation was maximal will be used to synchronize both measurements. To accomplish time normalization between the reference and the IMU sensor, cubic spline data interpolation was employed.Figure 8
Measurement setup for Test C. (a) Room setup for recording the attitude; (b) four reflective markers were placed on the forefoot and heel.
(a)
(b)The quantitative performance assessment for Test A, Test B, and Test C is using mean squared error (MSE orS) in degree2 and maximum error (M) in degree between predicted attitude and reference attitude as described in(36)
S
θ
=
1
N
∑
k
=
1
N
θ
^
k
T
-
θ
k
T
2
,
M
θ
=
max
k
⁡
θ
^
k
T
-
θ
k
T
.


## 3.3. Experiment Results

### 3.3.1. Test A

Mode 1 is not related to the compensation model in (7), while the measurement error covariance in (21) was set to R
n
o
m = 1.5 and λ = 0. As withMode 1, neither the compensation model nor updating covariance were used inMode 3, but the constant parameters were heuristic choices based on trial and error; that is, δ = 0.1 g2, α
1 = 0.4, and α
2 = 80. The experimental results ofMode 1 andMode 3 are presented in Tables 1(a) and 1(b).Table 1
(a) Test A: MSE (S) of Mode 1, Mode 3, Mode 4, and Mode 5 in detail. (b) Test A: maximum error (M) of Mode 1, Mode 3, Mode 4, and Mode 5 in detail.
(a)
Acc (m/s2)
Mode 1
Mode 3
Mode 4
Mode 5
S
θ pitch (deg2)
S
φ roll (deg2)
S
θ pitch (deg2)
S
φ roll (deg2)
S
θ pitch (deg2)
S
φ roll (deg2)
S
θ pitch (deg2)
S
φ roll (deg2)
0.1
9.60
2.92
9.31
2.71
99.42
26.73
9.04
2.89
0.75
24.77
3.22
24.66
3.20
92.56
183.10
22.92
2.82
1.5
29.79
3.59
29.41
4.02
113.20
227.13
28.79
2.86
2.5
36.84
4.88
34.49
5.60
44.94
168.43
33.82
4.03
Mean (sd)
25.25 (10)
3.65 (0.8)
24.47 (9.4)
3.88 (1.1)
87.53 (25.7)
151.35 (75.1)
23.64 (9.3)
3.15 (0.5)
(b)
Acc (m/s2)
Mode 1
Mode 3
Mode 4
Mode 5
M
θ pitch (deg)
M
φ roll (deg)
M
θ pitch (deg)
M
φ roll (deg)
M
θ pitch (deg)
M
φ roll (deg)
M
θ pitch (deg)
M
φ roll (deg)
0.1
26.05
8.61
26.05
8.61
18.52
10.19
21.97
8.99
0.75
27.93
7.76
27.93
7.76
20.87
19.02
22.97
6.56
1.5
29.16
6.81
29.16
6.81
23.72
25.44
22.45
5.74
2.5
29.70
12.30
29.70
12.30
16.44
20.74
23.69
9.35
Mean (sd)
28.21 (1.4)
8.87 (2.1)
28.21 (1.4)
8.87 (2.1)
19.89 (2.7)
18.85 (5.5)
22.77 (0.6)
7.66 (1.5)To follow the adaptation of measurement noise covariance matrix (33) inMode 4, the threshold ε
th used is 0.02 and σ
a
2 = 1.5 as used inMode 1 andMode 2. To meet the mechanism of external acceleration (34) inMode 5, σ
a
0
2 is 1.5, which is the same value as we used inMode 1 andMode 2. α
a is determined to be 5, which comes from experiments using α
a = 2 to 10. The MSE will decrease in value when α
a becomes greater, although the MSE decrease is smooth; it is in line with the function of α
a as the softened part of adaptation mechanism. The results ofMode 4 andMode 5 are also presented in Tables 1(a) and 1(b). Overall, the results ofModes 1, 3, 4, and5 are shown in Tables 1(a) and 1(b). The mean MSE ofMode 5 is the lowest amongModes 1, 3, and4, that is, 23.63 and 2.82 deg2 for pitch and roll, respectively.For the purpose of looking at the effect of the reduction of external acceleration, theMode 2 experiment was performed using a different compensating parameter value. Table 2 shows the average MSE of the attitude estimation from four different accelerations, with some variation in the values of q
0 and λ. The variation value of q
0 is 0.5, 0.1, 0.05, and 0.01, all of them using K
=
1
. While the variation value of λ is 35, 80, 100, 150, 170, and 200, in ascending order, the value settings of the EKF parameters are R
n
o
m = 1.5. The relationship between λ and q
0 in one-axis movement test is as follows. The MSE trend decreases as the value of λ increases or q
0 decreases. The best combination for one-axis movement in this experiment is λ = 200 and q
0 = 0.01; the MSE is 8.57 and 1.48 deg2 for pitch and roll, respectively.Table 2
Test A: MSE ofMode 2 during the variability of q
0 and λ, presented as mean (standard deviation).
MSE
q
0
λ
=
35
λ
=
80
λ
=
100
λ
=
150
λ
=
170
λ
=
200
Pitch (deg2)
0.5
15.28 (6.95)
12.94 (5.86)
12.31 (5.56)
11.2 (5.02)
10.86 (4.86)
10.44 (4.65)
0.1
13.77 (6.11)
11.52 (5.08)
10.93 (4.80)
9.89 (4.30)
9.58 (4.15)
9.17 (3.94)
0.05
13.50 (6.04)
11.29 (5.02)
8.9 (5.88)
9.67 (4.23)
9.35 (4.07)
8.94 (3.86)
0.01
13.13 (6.12)
10.94 (5.06)
10.36 (4.76)
9.31 (4.21)
8.99 (4.03)
8.57 (3.81)
Roll (deg2)
0.5
2.62 (0.96)
2.32 (1.02)
2.25 (1.04)
2.13 (1.09)
2.10 (1.10)
2.07 (1.12)
0.1
2.39 (1.00)
2.11 (1.07)
2.04 (1.09)
1.94 (1.12)
1.91 (1.13)
1.87 (1.14)
0.05
2.27 (1.02)
1.99 (1.08)
1.81 (1.20)
1.82 (1.11)
1.79 (1.12)
1.75 (1.12)
0.01
1.91 (1.04)
1.67 (1.08)
1.62 (1.09)
1.53 (1.10)
1.51 (1.10)
1.48 (1.11)The influence of the presentedq
0 and λ forMode 2 in Table 2 was also displayed as a column bar in Figures 9 and 10 for the pitch and roll, respectively. In the meantime, the MSE difference t-test [18] betweenMode 2 andMode 1, Mode 2 andMode 3,andMode 2 andMode 4 as well as that betweenMode 2 andMode 5 was evolved in those figures. In Figures 9 and 10, “a,” “b,” “c,” and “d” denote the significant difference between MSE ofMode 2 andMode 1, Mode 3, Mode 4, andMode 5 (p
<
0.05), respectively. Figures 11(a), 11(b), 11(c), 11(d), 11(e), 11(f), 11(g), 11(h), 11(i), and 11(j) show an example of the attitude estimation. We choose the fastest movement of the MISUMI table slider (2.5 m/s2) in this test as a figure of attitude example. As can be seen,Mode 2 (as compared toModes 1,3, and5) has the smallest amplitude of MSE, that is, 13.47 deg2
.Figure 9
MSE pitch ofMode 2 in Test A: influence of the variability of q
0 and λ. “a, b, c, d” denote significant difference fromMode 1,Mode 3,Mode 4, andMode 5, respectively.Figure 10
MSE roll ofMode 2 in Test A: influence of the variability of q
0 and λ. “a, b, c, d” denote significant difference fromMode 1,Mode 3,Mode 4, andMode 5, respectively.Figure 11
The MSE (S) in deg2 and maximum error (M) in deg of pitch and roll estimation by five modes in Test A, acceleration = 2.5 m/s2. Specifically forMode 2, q
0 = 0.05, λ = 150.Mode 1 in (a) and (b),Mode 2 in (c) and (d),Mode 3 in (e) and (f),Mode 4 in (g) and (h), andMode 5 in (i) and (j).
(a)
(b)
(c)
(d)
(e)
(f)
(g)
(h)
(i)
(j)


### 3.3.2. Test B

Test B was executed by rotating the IMU sensor in a random manner by hand along thex- and y-axis. In order to determine the optimal q
0 value forMode 2, we conduct the variability of the q
0 test to calculate the MSE and maximum error along the timeline of the whole experiment. Table 4 presents the MSE and maximum errors using different values of q
0. It was found that q
0 = 0.05 has an optimum performance among all the possible choices. Figures 12(a), 12(b), and 12(c) show the influence of q
0 variability to the square output of the proposed compensation model a
k, as in the proposed model ofMode 2 in (7) and (21). Figures 13(a), 13(b), 13(c), 13(d), 13(e), 13(f), 14(a), 14(b), 14(c), 14(d), 15(a), 15(b), 15(c), 15(d), 15(e), 15(f), 15(g), 15(h), 15(i), 15(j), 15(k), and 15(l) are produced by using this parameter value to present the analysis and the result of attitude estimation.Figure 12
Graphs of the square of compensation model output of various value ofq
0 along the x-axis: (a) q
0 = 0.01, (b) q
0 = 0.1, and (c) q
0 = 0.3.
(a)
(b)
(c)Figure 13
External acceleration compensation: original accelerometer signal is shown in (a), (b), and (c); compensation model output is shown in (d), (e), and (f) along thex-, y-, and z-axis, respectively. In (a), the dash-dot line indicatesPhases I andII, respectively, while in (b), the dash-dot line indicatesPhases III andIV, respectively.Figure 14
(a), (b), and (c): graphs of the square of compensation model output forx-, y-, and z-axis ofMode 2. As a comparison is shown in (d): necessary condition ofMode 3.
(a)
(b)
(c)
(d)Figure 15
Test B attitude estimation result: five modes in comparison to the references (a and g) for pitch (left column) and roll (right column).Figures13(a), 13(b), and 13(c) present the accelerometer signal and Figures 13(d), 13(e), and 13(f) present the compensation model signal along the x-, y-, and z-axis of the accelerometer, using model parameters K = 1 and q
0 = 0.05. When the sensor is not experiencing external acceleration, the compensating signal tends to be around zero. While the sensor is being moved quickly, it experiences two major external acceleration periods along the x- and y-axes as shown in Figures 13(a) and 13(b). In Figure 13(a), we call the periodsPhase I andPhase II (indicated by the dash-dot line);Phase I is for 2.4 s during 5.8–8.2 s and followed byPhase II, 2 s during 12.8–14.8 s. Observing accelerometer signal along the y-axis in Figure 13(b), we usePhase III andPhase IV (indicated by the dash-dot line) to refer to the external acceleration experienced for 2 s during 9.2–11.2 s and 1.9 s during 16.2–18.1 s, respectively. The external acceleration duringPhases I andII is closely related to the pitch angle whilePhases III andIV are related to the roll angle.Mode 2 works based on an automatically adjusted measurement noise covariance matrix, as (21). The square of compensation model output was needed to adjust the measurement error covariance matrix and the squared signal output shown in Figures 14(a), 14(b), and 14(c). The timing of the occurrence of the enlarged amplitude of the square of the signal is aligned with the timing of the external acceleration inPhases I toIV. This means that R
k in (21) will increase during the dynamic condition of this phase. However, Figure 14(d) presents the necessary condition ofMode 3 as a comparison.Parameter values are used in each mode in Test B.Mode 1 uses R
n
o
m = 1.5, whileMode 2 uses q
0 = 0.05, R
n
o
m = 1.5, and λ = 3.5.Mode 3 uses thresholds δ = 0.1 g2, α
1 = 0.4, α
2 = 0.3, and r
1 = 0.093, r
2 = 0.034, and r
3 = 1.5.Mode 4 uses the thresholds ε
th = 0.1 and σ
a
2 = 0.3. We did experiments with some value of σ
a
2, that is, 0.2, 0.3, and 0.4; σ
a
2 = 0.3 produced a minimum MSE.Mode 5 works using σ
a
0
2 = 1.5 and the factor of softened part α
a = 3. One of the results of Test B using five modes is presented in Figures 15(b), 15(c), 15(d), 15(e), and 15(f) for pitch and Figures 15(h), 15(i), 15(j), 15(k), and 15(l) for roll estimation.Attitude reference signal from Fastrak is shown in Figures15(a) and 15(g) for pitch and roll, respectively. The quantitative evaluation using MSE between each mode at the time the external acceleration occurred, that is, duringPhase I toPhase IV, is shown in Table 5. Figures 16(a) and 16(b) present the column bar of Table 5 for the MSE of pitch and roll, respectively. In Table 6 and Figure 17 we conclude the MSE result of Test B, the average MSE ofPhases I andII as pitch estimation error, and the average ofPhases III andIV as roll estimation error.Figure 16
Test B: (a) MSE pitch of five modes, (b) MSE roll of five modes during external acceleration, all in deg2.
(a)
(b)Figure 17
Test B: the final MSE pitch and roll of each mode during external acceleration in deg2.


### 3.3.3. Test C

Test C was executed by walking straight forward three strides along thex-axis of the measurement room coordinate frame. Figures 18(a), 18(b), 18(c), 18(d), 18(e), 18(f), 18(g), 18(h), 18(i), 18(j), 18(k), and 18(l) show the result of attitude estimation using five modes and the MSE result is presented in Table 7.Figure 18
Test C attitude estimation result: five modes in comparison to the motion capture system as references (a and g).Some parameters values are used in Test C for each mode.Mode 1 uses R
nom
⁡ = 1.5, whileMode 2 uses q
0 = 20, R
nom
⁡ = 1.5, and λ = 3
e
4.Mode 3 uses thresholds δ = 0.2 g2, α
1 = 0.8, α
2 = 0.3, and r
1 = 0.093, r
2 = 0.034, and r
3 = 1.5.Mode 4 uses the thresholds ε
th = 0.8 and σ
a
2 = 60.Mode 5 works using σ
a
0
2 = 1.5 and the factor of softened part α
a = 50.


## 3.3.1. Test A

Mode 1 is not related to the compensation model in (7), while the measurement error covariance in (21) was set to R
n
o
m = 1.5 and λ = 0. As withMode 1, neither the compensation model nor updating covariance were used inMode 3, but the constant parameters were heuristic choices based on trial and error; that is, δ = 0.1 g2, α
1 = 0.4, and α
2 = 80. The experimental results ofMode 1 andMode 3 are presented in Tables 1(a) and 1(b).Table 1
(a) Test A: MSE (S) of Mode 1, Mode 3, Mode 4, and Mode 5 in detail. (b) Test A: maximum error (M) of Mode 1, Mode 3, Mode 4, and Mode 5 in detail.
(a)
Acc (m/s2)
Mode 1
Mode 3
Mode 4
Mode 5
S
θ pitch (deg2)
S
φ roll (deg2)
S
θ pitch (deg2)
S
φ roll (deg2)
S
θ pitch (deg2)
S
φ roll (deg2)
S
θ pitch (deg2)
S
φ roll (deg2)
0.1
9.60
2.92
9.31
2.71
99.42
26.73
9.04
2.89
0.75
24.77
3.22
24.66
3.20
92.56
183.10
22.92
2.82
1.5
29.79
3.59
29.41
4.02
113.20
227.13
28.79
2.86
2.5
36.84
4.88
34.49
5.60
44.94
168.43
33.82
4.03
Mean (sd)
25.25 (10)
3.65 (0.8)
24.47 (9.4)
3.88 (1.1)
87.53 (25.7)
151.35 (75.1)
23.64 (9.3)
3.15 (0.5)
(b)
Acc (m/s2)
Mode 1
Mode 3
Mode 4
Mode 5
M
θ pitch (deg)
M
φ roll (deg)
M
θ pitch (deg)
M
φ roll (deg)
M
θ pitch (deg)
M
φ roll (deg)
M
θ pitch (deg)
M
φ roll (deg)
0.1
26.05
8.61
26.05
8.61
18.52
10.19
21.97
8.99
0.75
27.93
7.76
27.93
7.76
20.87
19.02
22.97
6.56
1.5
29.16
6.81
29.16
6.81
23.72
25.44
22.45
5.74
2.5
29.70
12.30
29.70
12.30
16.44
20.74
23.69
9.35
Mean (sd)
28.21 (1.4)
8.87 (2.1)
28.21 (1.4)
8.87 (2.1)
19.89 (2.7)
18.85 (5.5)
22.77 (0.6)
7.66 (1.5)To follow the adaptation of measurement noise covariance matrix (33) inMode 4, the threshold ε
th used is 0.02 and σ
a
2 = 1.5 as used inMode 1 andMode 2. To meet the mechanism of external acceleration (34) inMode 5, σ
a
0
2 is 1.5, which is the same value as we used inMode 1 andMode 2. α
a is determined to be 5, which comes from experiments using α
a = 2 to 10. The MSE will decrease in value when α
a becomes greater, although the MSE decrease is smooth; it is in line with the function of α
a as the softened part of adaptation mechanism. The results ofMode 4 andMode 5 are also presented in Tables 1(a) and 1(b). Overall, the results ofModes 1, 3, 4, and5 are shown in Tables 1(a) and 1(b). The mean MSE ofMode 5 is the lowest amongModes 1, 3, and4, that is, 23.63 and 2.82 deg2 for pitch and roll, respectively.For the purpose of looking at the effect of the reduction of external acceleration, theMode 2 experiment was performed using a different compensating parameter value. Table 2 shows the average MSE of the attitude estimation from four different accelerations, with some variation in the values of q
0 and λ. The variation value of q
0 is 0.5, 0.1, 0.05, and 0.01, all of them using K
=
1
. While the variation value of λ is 35, 80, 100, 150, 170, and 200, in ascending order, the value settings of the EKF parameters are R
n
o
m = 1.5. The relationship between λ and q
0 in one-axis movement test is as follows. The MSE trend decreases as the value of λ increases or q
0 decreases. The best combination for one-axis movement in this experiment is λ = 200 and q
0 = 0.01; the MSE is 8.57 and 1.48 deg2 for pitch and roll, respectively.Table 2
Test A: MSE ofMode 2 during the variability of q
0 and λ, presented as mean (standard deviation).
MSE
q
0
λ
=
35
λ
=
80
λ
=
100
λ
=
150
λ
=
170
λ
=
200
Pitch (deg2)
0.5
15.28 (6.95)
12.94 (5.86)
12.31 (5.56)
11.2 (5.02)
10.86 (4.86)
10.44 (4.65)
0.1
13.77 (6.11)
11.52 (5.08)
10.93 (4.80)
9.89 (4.30)
9.58 (4.15)
9.17 (3.94)
0.05
13.50 (6.04)
11.29 (5.02)
8.9 (5.88)
9.67 (4.23)
9.35 (4.07)
8.94 (3.86)
0.01
13.13 (6.12)
10.94 (5.06)
10.36 (4.76)
9.31 (4.21)
8.99 (4.03)
8.57 (3.81)
Roll (deg2)
0.5
2.62 (0.96)
2.32 (1.02)
2.25 (1.04)
2.13 (1.09)
2.10 (1.10)
2.07 (1.12)
0.1
2.39 (1.00)
2.11 (1.07)
2.04 (1.09)
1.94 (1.12)
1.91 (1.13)
1.87 (1.14)
0.05
2.27 (1.02)
1.99 (1.08)
1.81 (1.20)
1.82 (1.11)
1.79 (1.12)
1.75 (1.12)
0.01
1.91 (1.04)
1.67 (1.08)
1.62 (1.09)
1.53 (1.10)
1.51 (1.10)
1.48 (1.11)The influence of the presentedq
0 and λ forMode 2 in Table 2 was also displayed as a column bar in Figures 9 and 10 for the pitch and roll, respectively. In the meantime, the MSE difference t-test [18] betweenMode 2 andMode 1, Mode 2 andMode 3,andMode 2 andMode 4 as well as that betweenMode 2 andMode 5 was evolved in those figures. In Figures 9 and 10, “a,” “b,” “c,” and “d” denote the significant difference between MSE ofMode 2 andMode 1, Mode 3, Mode 4, andMode 5 (p
<
0.05), respectively. Figures 11(a), 11(b), 11(c), 11(d), 11(e), 11(f), 11(g), 11(h), 11(i), and 11(j) show an example of the attitude estimation. We choose the fastest movement of the MISUMI table slider (2.5 m/s2) in this test as a figure of attitude example. As can be seen,Mode 2 (as compared toModes 1,3, and5) has the smallest amplitude of MSE, that is, 13.47 deg2
.Figure 9
MSE pitch ofMode 2 in Test A: influence of the variability of q
0 and λ. “a, b, c, d” denote significant difference fromMode 1,Mode 3,Mode 4, andMode 5, respectively.Figure 10
MSE roll ofMode 2 in Test A: influence of the variability of q
0 and λ. “a, b, c, d” denote significant difference fromMode 1,Mode 3,Mode 4, andMode 5, respectively.Figure 11
The MSE (S) in deg2 and maximum error (M) in deg of pitch and roll estimation by five modes in Test A, acceleration = 2.5 m/s2. Specifically forMode 2, q
0 = 0.05, λ = 150.Mode 1 in (a) and (b),Mode 2 in (c) and (d),Mode 3 in (e) and (f),Mode 4 in (g) and (h), andMode 5 in (i) and (j).
(a)
(b)
(c)
(d)
(e)
(f)
(g)
(h)
(i)
(j)


## 3.3.2. Test B

Test B was executed by rotating the IMU sensor in a random manner by hand along thex- and y-axis. In order to determine the optimal q
0 value forMode 2, we conduct the variability of the q
0 test to calculate the MSE and maximum error along the timeline of the whole experiment. Table 4 presents the MSE and maximum errors using different values of q
0. It was found that q
0 = 0.05 has an optimum performance among all the possible choices. Figures 12(a), 12(b), and 12(c) show the influence of q
0 variability to the square output of the proposed compensation model a
k, as in the proposed model ofMode 2 in (7) and (21). Figures 13(a), 13(b), 13(c), 13(d), 13(e), 13(f), 14(a), 14(b), 14(c), 14(d), 15(a), 15(b), 15(c), 15(d), 15(e), 15(f), 15(g), 15(h), 15(i), 15(j), 15(k), and 15(l) are produced by using this parameter value to present the analysis and the result of attitude estimation.Figure 12
Graphs of the square of compensation model output of various value ofq
0 along the x-axis: (a) q
0 = 0.01, (b) q
0 = 0.1, and (c) q
0 = 0.3.
(a)
(b)
(c)Figure 13
External acceleration compensation: original accelerometer signal is shown in (a), (b), and (c); compensation model output is shown in (d), (e), and (f) along thex-, y-, and z-axis, respectively. In (a), the dash-dot line indicatesPhases I andII, respectively, while in (b), the dash-dot line indicatesPhases III andIV, respectively.Figure 14
(a), (b), and (c): graphs of the square of compensation model output forx-, y-, and z-axis ofMode 2. As a comparison is shown in (d): necessary condition ofMode 3.
(a)
(b)
(c)
(d)Figure 15
Test B attitude estimation result: five modes in comparison to the references (a and g) for pitch (left column) and roll (right column).Figures13(a), 13(b), and 13(c) present the accelerometer signal and Figures 13(d), 13(e), and 13(f) present the compensation model signal along the x-, y-, and z-axis of the accelerometer, using model parameters K = 1 and q
0 = 0.05. When the sensor is not experiencing external acceleration, the compensating signal tends to be around zero. While the sensor is being moved quickly, it experiences two major external acceleration periods along the x- and y-axes as shown in Figures 13(a) and 13(b). In Figure 13(a), we call the periodsPhase I andPhase II (indicated by the dash-dot line);Phase I is for 2.4 s during 5.8–8.2 s and followed byPhase II, 2 s during 12.8–14.8 s. Observing accelerometer signal along the y-axis in Figure 13(b), we usePhase III andPhase IV (indicated by the dash-dot line) to refer to the external acceleration experienced for 2 s during 9.2–11.2 s and 1.9 s during 16.2–18.1 s, respectively. The external acceleration duringPhases I andII is closely related to the pitch angle whilePhases III andIV are related to the roll angle.Mode 2 works based on an automatically adjusted measurement noise covariance matrix, as (21). The square of compensation model output was needed to adjust the measurement error covariance matrix and the squared signal output shown in Figures 14(a), 14(b), and 14(c). The timing of the occurrence of the enlarged amplitude of the square of the signal is aligned with the timing of the external acceleration inPhases I toIV. This means that R
k in (21) will increase during the dynamic condition of this phase. However, Figure 14(d) presents the necessary condition ofMode 3 as a comparison.Parameter values are used in each mode in Test B.Mode 1 uses R
n
o
m = 1.5, whileMode 2 uses q
0 = 0.05, R
n
o
m = 1.5, and λ = 3.5.Mode 3 uses thresholds δ = 0.1 g2, α
1 = 0.4, α
2 = 0.3, and r
1 = 0.093, r
2 = 0.034, and r
3 = 1.5.Mode 4 uses the thresholds ε
th = 0.1 and σ
a
2 = 0.3. We did experiments with some value of σ
a
2, that is, 0.2, 0.3, and 0.4; σ
a
2 = 0.3 produced a minimum MSE.Mode 5 works using σ
a
0
2 = 1.5 and the factor of softened part α
a = 3. One of the results of Test B using five modes is presented in Figures 15(b), 15(c), 15(d), 15(e), and 15(f) for pitch and Figures 15(h), 15(i), 15(j), 15(k), and 15(l) for roll estimation.Attitude reference signal from Fastrak is shown in Figures15(a) and 15(g) for pitch and roll, respectively. The quantitative evaluation using MSE between each mode at the time the external acceleration occurred, that is, duringPhase I toPhase IV, is shown in Table 5. Figures 16(a) and 16(b) present the column bar of Table 5 for the MSE of pitch and roll, respectively. In Table 6 and Figure 17 we conclude the MSE result of Test B, the average MSE ofPhases I andII as pitch estimation error, and the average ofPhases III andIV as roll estimation error.Figure 16
Test B: (a) MSE pitch of five modes, (b) MSE roll of five modes during external acceleration, all in deg2.
(a)
(b)Figure 17
Test B: the final MSE pitch and roll of each mode during external acceleration in deg2.


## 3.3.3. Test C

Test C was executed by walking straight forward three strides along thex-axis of the measurement room coordinate frame. Figures 18(a), 18(b), 18(c), 18(d), 18(e), 18(f), 18(g), 18(h), 18(i), 18(j), 18(k), and 18(l) show the result of attitude estimation using five modes and the MSE result is presented in Table 7.Figure 18
Test C attitude estimation result: five modes in comparison to the motion capture system as references (a and g).Some parameters values are used in Test C for each mode.Mode 1 uses R
nom
⁡ = 1.5, whileMode 2 uses q
0 = 20, R
nom
⁡ = 1.5, and λ = 3
e
4.Mode 3 uses thresholds δ = 0.2 g2, α
1 = 0.8, α
2 = 0.3, and r
1 = 0.093, r
2 = 0.034, and r
3 = 1.5.Mode 4 uses the thresholds ε
th = 0.8 and σ
a
2 = 60.Mode 5 works using σ
a
0
2 = 1.5 and the factor of softened part α
a = 50.


## 4. Discussion

### 4.1.Mode 1

External acceleration compensation does not play a role inMode 1.Mode 1 used R
nom
⁡ in the measurement noise covariance matrix, as in (20), and did not use the external acceleration compensation model. An experiment result ofMode 1 in Test A was presented in Tables 1(a) and 1(b), as well as in Figures 9 and 10. The comparison in Table 3 shows that the MSE of pitch and roll estimation is 25.25 and 3.65 deg2, respectively. The attitude estimation accuracy ofMode 1 is lower thanMode 2. This result confirms the findings presented in the literature [1], where the standard EKF has lower accuracy in the one-axis test type.Table 3
Test A: MSE of all modes, presented as mean (standard deviation).
MSE
Mode 1
Mode 2
Mode 3
Mode 4
Mode 5
Pitch (deg2)
25.25 (10)
8.57 (3.8)
24.47 (9.4)
87.53 (25.7)
23.64 (9.3)
Roll (deg2)
3.65 (0.8)
1.48 (1.1)
3.88 (1.1)
151.35 (75.1)
2.82 (0.1)Table 4
Test B: MSE (S) (in deg2) and maximum error (M) (in deg) of attitude estimation of Mode 2 at various values of q
0 during all periods of time.
q
0
Mode 2
S
θ pitch
S
φ roll
M
θ pitch
M
φ roll
0.01
13.92
11.54
11.91
13.92
0.03
13.21
11.48
11.44
14.19
0.05
13.14
11.58
11.42
14.42
0.1
13.70
11.96
12.63
14.92
0.15
14.48
12.23
13.93
15.28
0.2
15.14
12.45
14.81
15.54
0.25
15.68
12.62
15.40
15.73
0.3
16.11
12.76
15.86
15.90Table 5
Test B: MSE (S) (in deg2) and maximum error (M) (in deg) for all modes during major external acceleration periods (in detail).
External acceleration period
Mode 1
Mode 2
Mode 3
Mode 4
Mode 5
S
θ pitch (deg2)
M
θ pitch (deg)
S
θ pitch (deg2)
M
θ pitch (deg)
S
θ pitch (deg2)
M
θ pitch (deg)
S
θ pitch (deg2)
M
θ pitch (deg)
S
θ pitch (deg2)
M
θ pitch (deg)
Phase I
74.59
18.51
42.03
11.42
51.92
12.47
123.83
19.45
30.46
10.31
Phase II
70.59
21.70
26.69
10.45
33.07
12.39
66.92
13.48
27.98
9.69
S
φ roll (deg2)
M
φ roll (deg)
S
φ roll (deg2)
M
φ roll (deg)
S
φ roll (deg2)
M
φ roll (deg)
S
φ roll (deg2)
M
φ roll (deg)
S
φ roll (deg2)
M
φ roll (deg)
Phase III
50.34
17.29
37.85
14.42
15.12
8.57
48.16
10.08
25.46
8.13
Phase IV
47.63
16.95
22.63
9.51
24.35
11.48
42.07
11.78
18.96
9.18Table 6
Test B: MSE of all modes in degree2.
MSE
Mode 1
Mode 2
Mode 3
Mode 4
Mode 5
Pitch (deg2)
72.59
34.36
42.49
95.37
29.22
Roll (deg2)
48.98
30.24
19.73
45.11
22.21Table 7
Test C: MSE in degree2 of all modes.
MSE
Mode 1
Mode 2
Mode 3
Mode 4
Mode 5
Pitch (deg2)
395.35
175.97
147.92
1036.2
160.87
Roll (deg2)
367.57
245.80
55.24
141.35
217.94The result ofMode 1 in Test B, as shown in Figures 15(b) and 15(h), demonstrates that the effect of external acceleration duringPhases I to IV on the pitch and roll estimation is still dominant. The final result ofMode 1 in Test B as shown in Table 6 and Figure 17 indicates thatMode 1 estimation accuracy is also lower thanMode 2,Mode 3, andMode 5. Kalman filtering employs a compensation mechanism that surpassesMode 1’s accuracy, except inMode 4.Mode 1 has a MSE of pitch 36% greater than that ofMode 2, 26% greater than that ofMode 3, and 43% greater than that ofMode 5. The result ofMode 1 in Test C is also in line with the result in Test B, which is thatMode 1 estimation accuracy is lower than that inModes 2,3, and5.


### 4.2.Mode 2

The proposed algorithm inMode 2 involved the external acceleration compensation models (7) and covariance matrix updating process (21), which plays an important role in improving the attitude estimation accuracy during fast movement. If K
=
1 was imposed for all experiments, then the only setting parameters are q
0 and λ.The two parametersq
0 and λ are chosen heuristically. However, there is some consideration when choosing these parameters. As shown in (7), a smaller q
0 reduces the cutoff frequency. The value of q
0 that determines the cutoff frequency affects the value of the compensating model output, a
k, where the effect continues to the value of the measurement noise covariance matrix (R) in attitude estimation. The impact of the q
0 variability to the square output of a
k along the x
-axis was presented in Figures 12(a) and 12(b). It appears that the smaller value of q
0 has an increasing amplitude of a
k
2, which will simultaneously increase the value of the measurement error covariance matrix. In the meantime, the effect of the λ is as an amplified factor for the softened part (λ
a
k
2). The increasing value of λ simultaneously increases the value matrix R, which reduces the Kalman gain. Therefore, the contribution of the measurement to the estimation process also decreases; thus, the estimate is less affected by measurement from an accelerometer.The result ofMode 2 in Test A is shown in Figures 9 and 10 and Table 3 as a concluding table. As can be seen, theMode 2 MSE of pitch is 8.57 and 1.48 deg2, which is the lowest among all those tested. The significant difference withMode 1 is generally started at λ = 150 and with q
0 up to 0.1.For Test A, the variability ofq
0 influences the estimation error; however, the attitude reference is always zero degrees, and therefore we cannot observe the effect of the q
0 variability to the attenuation of some important signal by model (7). Therefore, we need a test like Test B and Test C to observe the variability effect of q
0 on the appearance of the signal.In Test B, quantitative assessment of MSE on pitch and roll is done inPhases I toIV, as shown in Table 6 and Figure 17.Mode 2 has a MSE value of 34.36 and 30.24 degree2 for pitch and roll estimation, respectively. The MSE of pitch is lower than that inModes 1,3, and4, andMode 5’s accuracy surpasses the accuracy ofMode 2. In roll estimation, the accuracy ofMode 2 outperformed the accuracy ofModes 1 and4. It was observed thatMode 2 is able to reduce the effect of external acceleration on the correct timing when it occurs, that is, duringPhases I,II,III, andIV. The results of real world application, such as in Test C, are in Table 7 and Figures 18(a), 18(b), 18(c), 18(d), 18(e), 18(f), 18(g), 18(h), 18(i), 18(j), 18(k), and 18(l); the MSE ofMode 2 is 175.97 deg2 and 245.8 deg2 in pitch and roll estimation, respectively. This means that the average estimation error in every point calculation is around 13.3 deg and 15.7 deg. In Test C,Mode 2accuracy outperformed the accuracy ofMode 1 andMode 4 in pitch estimation and outperformedMode 1 in roll estimation. Overall, for Test C,Mode 3 outperformed the accuracy of other modes on both pitch and roll estimation.


### 4.3.Mode 3

In our Test A and Test B experiment,Mode 3 has lower estimation accuracy in pitch estimation (a bigger MSE) as compared toMode 2 andMode 5, as shown in Figure 9 and Table 6. It is important that we do not make the criticism thatMode 2 is superior toMode 3 in Test B, since different settings on the combinations of δ, α
1, and α
2 as in (31) and (32) might result in a better performance.The result in Table7 for Test C indicates thatMode 3 outperformed all modes. The number of setting parameters inMode 3 is higher than in other modes (i.e., three setting parameters), which provides a more softened setting. However, this also requires more effort than in the other modes. Our proposed algorithm inMode 2 has fewer parameter settings, that is, q
0 and λ. Furthermore, the execution of the compensation algorithm is fully dependent on the existence and magnitude of the square of external acceleration model, as in (7) and (21), rather than threshold-based approach, as in (31). However, we observed that the measurement model inMode 3 (as in (28)) has an advantage overMode 2. This model incorporates the data from gyro as well as accelerometer data. Therefore, the setting mechanism of matrix R consists of two parts: accelerometer and gyro. When the level of trust in the accelerometer lowers then it is possible to set the level of trust higher in gyro.


### 4.4.Mode 4

Of all the tests, the estimation accuracy ofMode 4 is the lowest. We suspect that this is caused by the presence of magnetometer measurement vectors in the measurement model ofMode 4. The earth’s magnetic vector, m
b, has a magnitude that is always changing over a large range of time [19]. Furthermore, the experiment room we used is not guaranteed to be free of magnetic interference and soft iron distortion. In [19] it is suggested that, to overcome this problem, initialization must be done carefully to find out the exact magnitude and orientation of the magnetic field. This vector can be used during the experiment.


### 4.5.Mode 5

In our experiments, the accuracy ofMode 5 outperformedMode 2 in Test B by around 8.1% and 15.3% for pitch and roll, respectively (as shown in Table 6). This also happened in Test C. As shown in Table 7, the estimation accuracy ofMode 5 is 4.5% and 6% overMode 2’s in pitch and roll estimation, respectively. In Test A,Mode 2 outperformsMode 5 in all λ and q
0 combinations.The difference betweenMode 2 andMode 5 is in the softened part of the measurement error covariance matrix.Mode 5 in (34) uses the absolute difference between measurement and predicted acceleration. However, inMode 2 we use the model of external acceleration as the softened part.


### 4.6. Experimental Limitation and Future Work

In all tests, the proposedMode 2 outperformedMode 1. From these results, we were able to ensure that a mechanism of external acceleration compensation has the influence to improve estimation accuracy. Even though there is an advantage to improving the estimation accuracy, some of the major limitations to the experiments will be described.First is the limitation of measurement model.Modes 1,2, and3 used Euler representation. The model inMode 1 andMode 2 in (12) did not include the measurement from a gyroscope. With a slow motion sensor this measurement model might be not a problem, because it is not necessary to compensate the external acceleration. The setting of the measurement noise covariance matrix (R) in (21) primarily relies on the roll and pitch from accelerometer data. When the value of R becomes larger due to the presence of external acceleration, the estimation process in Kalman filtering is less affected by accelerometer, but at the same time we cannot increase the level of trust in the gyroscope. Furthermore, it is important to consider a modification of the measurement model in future work.The second limitation is that the proposed external acceleration compensation in (7) did not include automatic calculation of parameter q
0. The model in (7) is used as an additional part for the measurement noise covariance in (21). This model relies on the frequency of the application that the model will use. Before using the model, the determination of the q
0 value is important. One prospective improvement of this model is the additional step of calculating the optimum value of q
0 from IMU data before the Kalman filtering process.The third limitation is considering the application ofMode 2 whenever the pitch angle (θ) reached the π
/
2 radian, even though there is an advantage of Euler representation over the quaternion representation. The limitation of Euler representation in this experiment is that whenever pitch angle (θ) reaches π
/
2, the state inMode 2 will be singular. This limitation need not be considered as long as the application is still able to accept the range of angle.The fourth limitation is related to the linearization process in the process model ofMode 2, which leads to first-order approximation error.Mode 2 uses EKF-based filtering that employs linearizing the nonlinear model. The first-order linearization might be the cause of degraded accuracy in all modes that employed EKF, that is,Mode 1,Mode 2,Mode 3, andMode 4. However,Mode 5 employs an unscented Kalman filter (UKF) that is free from linearizing through Jacobian matrix. As a result,Mode 5 outperformed almost all modes in Test B and outperformedMode 2 in Test C. One consideration to avoid the first-order approximation error besides using UKF is using a direction cosine matrix (DCM) method.


## 4.1.Mode 1

External acceleration compensation does not play a role inMode 1.Mode 1 used R
nom
⁡ in the measurement noise covariance matrix, as in (20), and did not use the external acceleration compensation model. An experiment result ofMode 1 in Test A was presented in Tables 1(a) and 1(b), as well as in Figures 9 and 10. The comparison in Table 3 shows that the MSE of pitch and roll estimation is 25.25 and 3.65 deg2, respectively. The attitude estimation accuracy ofMode 1 is lower thanMode 2. This result confirms the findings presented in the literature [1], where the standard EKF has lower accuracy in the one-axis test type.Table 3
Test A: MSE of all modes, presented as mean (standard deviation).
MSE
Mode 1
Mode 2
Mode 3
Mode 4
Mode 5
Pitch (deg2)
25.25 (10)
8.57 (3.8)
24.47 (9.4)
87.53 (25.7)
23.64 (9.3)
Roll (deg2)
3.65 (0.8)
1.48 (1.1)
3.88 (1.1)
151.35 (75.1)
2.82 (0.1)Table 4
Test B: MSE (S) (in deg2) and maximum error (M) (in deg) of attitude estimation of Mode 2 at various values of q
0 during all periods of time.
q
0
Mode 2
S
θ pitch
S
φ roll
M
θ pitch
M
φ roll
0.01
13.92
11.54
11.91
13.92
0.03
13.21
11.48
11.44
14.19
0.05
13.14
11.58
11.42
14.42
0.1
13.70
11.96
12.63
14.92
0.15
14.48
12.23
13.93
15.28
0.2
15.14
12.45
14.81
15.54
0.25
15.68
12.62
15.40
15.73
0.3
16.11
12.76
15.86
15.90Table 5
Test B: MSE (S) (in deg2) and maximum error (M) (in deg) for all modes during major external acceleration periods (in detail).
External acceleration period
Mode 1
Mode 2
Mode 3
Mode 4
Mode 5
S
θ pitch (deg2)
M
θ pitch (deg)
S
θ pitch (deg2)
M
θ pitch (deg)
S
θ pitch (deg2)
M
θ pitch (deg)
S
θ pitch (deg2)
M
θ pitch (deg)
S
θ pitch (deg2)
M
θ pitch (deg)
Phase I
74.59
18.51
42.03
11.42
51.92
12.47
123.83
19.45
30.46
10.31
Phase II
70.59
21.70
26.69
10.45
33.07
12.39
66.92
13.48
27.98
9.69
S
φ roll (deg2)
M
φ roll (deg)
S
φ roll (deg2)
M
φ roll (deg)
S
φ roll (deg2)
M
φ roll (deg)
S
φ roll (deg2)
M
φ roll (deg)
S
φ roll (deg2)
M
φ roll (deg)
Phase III
50.34
17.29
37.85
14.42
15.12
8.57
48.16
10.08
25.46
8.13
Phase IV
47.63
16.95
22.63
9.51
24.35
11.48
42.07
11.78
18.96
9.18Table 6
Test B: MSE of all modes in degree2.
MSE
Mode 1
Mode 2
Mode 3
Mode 4
Mode 5
Pitch (deg2)
72.59
34.36
42.49
95.37
29.22
Roll (deg2)
48.98
30.24
19.73
45.11
22.21Table 7
Test C: MSE in degree2 of all modes.
MSE
Mode 1
Mode 2
Mode 3
Mode 4
Mode 5
Pitch (deg2)
395.35
175.97
147.92
1036.2
160.87
Roll (deg2)
367.57
245.80
55.24
141.35
217.94The result ofMode 1 in Test B, as shown in Figures 15(b) and 15(h), demonstrates that the effect of external acceleration duringPhases I to IV on the pitch and roll estimation is still dominant. The final result ofMode 1 in Test B as shown in Table 6 and Figure 17 indicates thatMode 1 estimation accuracy is also lower thanMode 2,Mode 3, andMode 5. Kalman filtering employs a compensation mechanism that surpassesMode 1’s accuracy, except inMode 4.Mode 1 has a MSE of pitch 36% greater than that ofMode 2, 26% greater than that ofMode 3, and 43% greater than that ofMode 5. The result ofMode 1 in Test C is also in line with the result in Test B, which is thatMode 1 estimation accuracy is lower than that inModes 2,3, and5.


## 4.2.Mode 2

The proposed algorithm inMode 2 involved the external acceleration compensation models (7) and covariance matrix updating process (21), which plays an important role in improving the attitude estimation accuracy during fast movement. If K
=
1 was imposed for all experiments, then the only setting parameters are q
0 and λ.The two parametersq
0 and λ are chosen heuristically. However, there is some consideration when choosing these parameters. As shown in (7), a smaller q
0 reduces the cutoff frequency. The value of q
0 that determines the cutoff frequency affects the value of the compensating model output, a
k, where the effect continues to the value of the measurement noise covariance matrix (R) in attitude estimation. The impact of the q
0 variability to the square output of a
k along the x
-axis was presented in Figures 12(a) and 12(b). It appears that the smaller value of q
0 has an increasing amplitude of a
k
2, which will simultaneously increase the value of the measurement error covariance matrix. In the meantime, the effect of the λ is as an amplified factor for the softened part (λ
a
k
2). The increasing value of λ simultaneously increases the value matrix R, which reduces the Kalman gain. Therefore, the contribution of the measurement to the estimation process also decreases; thus, the estimate is less affected by measurement from an accelerometer.The result ofMode 2 in Test A is shown in Figures 9 and 10 and Table 3 as a concluding table. As can be seen, theMode 2 MSE of pitch is 8.57 and 1.48 deg2, which is the lowest among all those tested. The significant difference withMode 1 is generally started at λ = 150 and with q
0 up to 0.1.For Test A, the variability ofq
0 influences the estimation error; however, the attitude reference is always zero degrees, and therefore we cannot observe the effect of the q
0 variability to the attenuation of some important signal by model (7). Therefore, we need a test like Test B and Test C to observe the variability effect of q
0 on the appearance of the signal.In Test B, quantitative assessment of MSE on pitch and roll is done inPhases I toIV, as shown in Table 6 and Figure 17.Mode 2 has a MSE value of 34.36 and 30.24 degree2 for pitch and roll estimation, respectively. The MSE of pitch is lower than that inModes 1,3, and4, andMode 5’s accuracy surpasses the accuracy ofMode 2. In roll estimation, the accuracy ofMode 2 outperformed the accuracy ofModes 1 and4. It was observed thatMode 2 is able to reduce the effect of external acceleration on the correct timing when it occurs, that is, duringPhases I,II,III, andIV. The results of real world application, such as in Test C, are in Table 7 and Figures 18(a), 18(b), 18(c), 18(d), 18(e), 18(f), 18(g), 18(h), 18(i), 18(j), 18(k), and 18(l); the MSE ofMode 2 is 175.97 deg2 and 245.8 deg2 in pitch and roll estimation, respectively. This means that the average estimation error in every point calculation is around 13.3 deg and 15.7 deg. In Test C,Mode 2accuracy outperformed the accuracy ofMode 1 andMode 4 in pitch estimation and outperformedMode 1 in roll estimation. Overall, for Test C,Mode 3 outperformed the accuracy of other modes on both pitch and roll estimation.


## 4.3.Mode 3

In our Test A and Test B experiment,Mode 3 has lower estimation accuracy in pitch estimation (a bigger MSE) as compared toMode 2 andMode 5, as shown in Figure 9 and Table 6. It is important that we do not make the criticism thatMode 2 is superior toMode 3 in Test B, since different settings on the combinations of δ, α
1, and α
2 as in (31) and (32) might result in a better performance.The result in Table7 for Test C indicates thatMode 3 outperformed all modes. The number of setting parameters inMode 3 is higher than in other modes (i.e., three setting parameters), which provides a more softened setting. However, this also requires more effort than in the other modes. Our proposed algorithm inMode 2 has fewer parameter settings, that is, q
0 and λ. Furthermore, the execution of the compensation algorithm is fully dependent on the existence and magnitude of the square of external acceleration model, as in (7) and (21), rather than threshold-based approach, as in (31). However, we observed that the measurement model inMode 3 (as in (28)) has an advantage overMode 2. This model incorporates the data from gyro as well as accelerometer data. Therefore, the setting mechanism of matrix R consists of two parts: accelerometer and gyro. When the level of trust in the accelerometer lowers then it is possible to set the level of trust higher in gyro.


## 4.4.Mode 4

Of all the tests, the estimation accuracy ofMode 4 is the lowest. We suspect that this is caused by the presence of magnetometer measurement vectors in the measurement model ofMode 4. The earth’s magnetic vector, m
b, has a magnitude that is always changing over a large range of time [19]. Furthermore, the experiment room we used is not guaranteed to be free of magnetic interference and soft iron distortion. In [19] it is suggested that, to overcome this problem, initialization must be done carefully to find out the exact magnitude and orientation of the magnetic field. This vector can be used during the experiment.


## 4.5.Mode 5

In our experiments, the accuracy ofMode 5 outperformedMode 2 in Test B by around 8.1% and 15.3% for pitch and roll, respectively (as shown in Table 6). This also happened in Test C. As shown in Table 7, the estimation accuracy ofMode 5 is 4.5% and 6% overMode 2’s in pitch and roll estimation, respectively. In Test A,Mode 2 outperformsMode 5 in all λ and q
0 combinations.The difference betweenMode 2 andMode 5 is in the softened part of the measurement error covariance matrix.Mode 5 in (34) uses the absolute difference between measurement and predicted acceleration. However, inMode 2 we use the model of external acceleration as the softened part.


## 4.6. Experimental Limitation and Future Work

In all tests, the proposedMode 2 outperformedMode 1. From these results, we were able to ensure that a mechanism of external acceleration compensation has the influence to improve estimation accuracy. Even though there is an advantage to improving the estimation accuracy, some of the major limitations to the experiments will be described.First is the limitation of measurement model.Modes 1,2, and3 used Euler representation. The model inMode 1 andMode 2 in (12) did not include the measurement from a gyroscope. With a slow motion sensor this measurement model might be not a problem, because it is not necessary to compensate the external acceleration. The setting of the measurement noise covariance matrix (R) in (21) primarily relies on the roll and pitch from accelerometer data. When the value of R becomes larger due to the presence of external acceleration, the estimation process in Kalman filtering is less affected by accelerometer, but at the same time we cannot increase the level of trust in the gyroscope. Furthermore, it is important to consider a modification of the measurement model in future work.The second limitation is that the proposed external acceleration compensation in (7) did not include automatic calculation of parameter q
0. The model in (7) is used as an additional part for the measurement noise covariance in (21). This model relies on the frequency of the application that the model will use. Before using the model, the determination of the q
0 value is important. One prospective improvement of this model is the additional step of calculating the optimum value of q
0 from IMU data before the Kalman filtering process.The third limitation is considering the application ofMode 2 whenever the pitch angle (θ) reached the π
/
2 radian, even though there is an advantage of Euler representation over the quaternion representation. The limitation of Euler representation in this experiment is that whenever pitch angle (θ) reaches π
/
2, the state inMode 2 will be singular. This limitation need not be considered as long as the application is still able to accept the range of angle.The fourth limitation is related to the linearization process in the process model ofMode 2, which leads to first-order approximation error.Mode 2 uses EKF-based filtering that employs linearizing the nonlinear model. The first-order linearization might be the cause of degraded accuracy in all modes that employed EKF, that is,Mode 1,Mode 2,Mode 3, andMode 4. However,Mode 5 employs an unscented Kalman filter (UKF) that is free from linearizing through Jacobian matrix. As a result,Mode 5 outperformed almost all modes in Test B and outperformedMode 2 in Test C. One consideration to avoid the first-order approximation error besides using UKF is using a direction cosine matrix (DCM) method.


## 5. Conclusion

In this paper, the main contribution is the algorithm for external acceleration compensation, which aims to improve the attitude estimation accuracy. A Kalman filter-based attitude estimation using a compensating algorithm has been discussed. The experiment was performed using three types of tests: movement on one axis, dynamic movement using free-hand movement, and walking. We employed five different approaches to deal with the dynamic test and the proposed method is placed onMode 2. The first approach is the standard KF model, without using external acceleration compensation (Mode 1). The second approach is the modified KF model, using the proposed compensating procedure (Mode 2); the third is aweighted-switching method (Mode 3); the fourth is a quaternion-based EKF using athreshold-based method (Mode 4); and the fifth (Mode 5) uses an unscented Kalman filter and isthreshold-based combined with a softened part.The experiment results showed that, by using the external acceleration compensation process, the estimation accuracy of the proposed algorithm is improved when compared with the standard EKF procedure inMode 1 in all tests.Mode 2 also outperformed all modes in Test A by using the optimal parameter setting. In dynamic Test B,Mode 5 outperformed other modes; we suspect that this is caused by using UKF inMode 5. The UKF was free from first-order approximation of a nonlinear system. The advantage ofMode 3 over other modes is presented in Test C.Mode 3 used a measurement model that included accelerometer and gyroscope data, while the measurement model inMode 2 was related to accelerometer data.There is a lack of efficiency comparison to some modes in the experiments. Compared to the other modes, the advantage ofMode 2 overMode 3 is the number of parameters set;Mode 2 has fewer parameters.Mode 3 takes a two-step EKF, which leads to additional computational overhead. The advantage ofMode 2 overMode 4 andMode 5 is the parameterized spatial rotation; quaternion as used inMode 4 andMode 5 is hardly used because it is a burden to update its four variables [19]. However, Euler needs to update two variables. Specifically, comparingMode 2 andMode 5, their effectiveness has been unexplored; the computational time of the extended Kalman filter is much lower than in the unscented Kalman filter [20].Nevertheless, as a future problem to be solved, in order to increase the estimation accuracy potential for other applications, it needs the addition of a step that can perform adaptive parameter settings (q
0 and λ) based on the present input from the IMU. Using UKF and DCM is also one consideration to improve accuracy in order to be free from linearization approximation error.



---

*Source: 6943040-2016-08-07.xml*