Algorithm

3.1.1.1 Gyroscope model

The raw gyroscope measurement Ω^B_k consists of three main components: the true angular velocity ω_k^B, the non-static bias term ω0,k, and the additive measurement noise µk. It is given by

Ω^B_k =ω_k^B+ω_0,k+µ_k, (3.1)

where the superscript B denotes vectors expressed in the body frame and k is a discrete-time variable. In equation (3.1), the additive rate noise is assumed a zero-mean, white, Gaussian variable with the following characteristics:

E[µk] = 0, E µkµ^T_l

=σ_µ²δkl, σ_µ²

= deg

s 2 1

Hz, (3.2)

where σ²_µ denotes the noise variance and δ_kl is the Kronecker delta. Moreover, the term ω_0,k in equation (3.1) is considered a slowly varying bias (as a result of temperature sensitivity) modeled by the random walk process

ω_0,k =ω0,k−1+η_k, (3.3)

whereηk denotes driving Gaussian noise with aσ_η² noise variance, i.e., E[η_k] = 0, E

η_kη_l^T

=σ²_ηδ_kl, σ_η²

= deg

s 2

Hz. (3.4)

After being reformulated from the gyroscope measurement (equation (3.1)), the true angular velocity, can be integrated numerically to compute the orientation using θ_k+1 = θ_k +ω_kTs, where Ts is the sampling time. However, due to the bias term ω0,k and the presence of the measurement noise µ_k, the gyro-based realization ofθ_k produces an unbounded accumulation error. Therefore, gyroscope measurements yield only short-term accuracy (i.e., high-frequency attitude realization can be achieved with rate gyro measurements).

3.1.1.2 Accelerometer model

The raw accelerometer output A^B_k consists of four main components: the external accelera-tion α^B_k, the contribution of gravitational acceleration g_k^B, the bias term a₀, and the additive measurement noise νk, and it is given by

A^B_k = α^B_k −g^B_k

+a0+νk. (3.5)

The bias term is usually compensated for during the calibration procedure H¨oflinger et al.

(2013), although the process for achieving this exceeds the scope of this article. If a robot (or another mobile mechatronic system) is in stationary state (i.e., no dynamic acceleration is occurring and α^B_k ≈ 0), then the gravity vector can be used to calculate the raw attitude realizationθ_A,k with Li and Wang (2013)

θ_A,k= arctan2 (A_y,k, A_z,k) =θ_k+v_k, (3.6) whereA_y,kandA_z,kare the instantaneous measurements of the accelerometer. This realization, which is described by equation (3.6), can also be considered the sum of the real attitudeθk and additive noise v_k that represents the effects of ν_k from equation (3.5) after the trigonometric function has been evaluated. In a manner similar to the gyroscope model, v_k is assumed to be Gaussian white noise with the following characteristics:

E[v_k] = 0, E v_kv^T_l

=σ_v²δ_kl, σ²_v

= deg² 1

Hz. (3.7)

Based on equations (3.5) and (3.6), we can deduce that if there is an external acceleration (α^B_k 6= 0), then the attitude realization given by equation (3.6) provides unreliable results and a drastically reduced accuracy because the ratio ofAy,k to Az,k does not provide relevant attitude information (i.e., the pure gravity vector cannot be applied). In this implementation, the external acceleration is not modeled explicitly. Instead, its effects are absorbed by v_k (i.e., a significantly largerσ²_v noise variance is expected when α^B_k 6= 0). Therefore, the accelerometer measurements yield adequate attitude realizations (so-called low-frequency attitude realizations) only when the system is in a non-accelerating mode.

3.1.1.3 Sensor fusion with Kalman filter

The previously described approaches can be synthesized to utilize the advantages of both types of sensors and to obtain attitude results with higher reliabilities and accuracies. In such a synthesis, the gyro-based attitude realization is extended by the attitude realization derived from the accelerometer data. Therefore, the unbounded integration error is compensated for and long-term stability is achieved. To fuse these sensors, a linear KF is implemented.

This KF is a recursive algorithm that provides an optimal estimation ˆx of the noisy state vectorx (whose dynamics are described by a state-space equation) such that:

E[x_k−xˆ_k] = 0, Eh

(x_k−xˆ_k) (x_k−xˆ_k)^Ti

→inf. (3.8)

When MEMS IMU data are considered, the state dynamics are constructed from both the discrete-time integrated true angular velocity (ω_k reformulated from equation (3.1)) and the random walk process of the bias term (equation (3.3)). This means that the state propagation

is described by the state vector x_k = (θ_k, ω_0,k)^T, the input variable u_k = Ω_k, and the process

where Φ is the state transition matrix, Γ denotes the input matrix, andTs= 1/fsis the sampling time. Based on equation (3.6), the measurement equation is formed (as an updated absolute orientation) with an output of zk =θA,k, measurement noise ofvk, and an output matrix of H as

According to the stochastic hypothesis, if the process and measurement noise vectors w_k and vk are uncorrelated (E

wkv_l^T

= 0) and modeled with zero-mean, white, Gaussian random variables (as was assumed in both the gyroscope and accelerometer models represented by equations (3.2), (3.4), and (3.7)), then the Kalman filter provides an optimal estimation with a minimum state-vector variance. The recursive algorithm uses the state-space equations (3.9) and (3.10) along with the covariance matrices of w_k and v_k given by Q = E

w_kwk^T and R = E

v_kvk^T

, respectively, to propagate the states, process the measurements, and update the covariance estimates in the time and measurement update equations Welch and Bishop (2001). Namely, the time update equations determine the a priori state estimate (ˆx⁻_k) and estimate error covariance (P_k⁻) as:

ˆ

x⁻_k = Φˆxk−1+ Γuk−1,

P_k⁻= ΦPk−1Φ^T +Q, (3.11)

where ˆxk−1 and Pk−1 denote the a posteriori (updated) state estimate and estimate error covariance at stepk−1. The measurement update equations are responsible for correcting the a priori estimates employing both the measurement zk and its noise covariance R. First, the Kalman gain is calculated:

G_k=P_k⁻H^T HP_k⁻H^T +R⁻¹

, (3.12)

then, both the state estimate and the estimate error covariance are updated (i.e., thea posteriori state vector and error covariance matrix are determined):

ˆ

xk= ˆx⁻_k +Gk zk−Hxˆ⁻_k ,

P_k= (I−G_kH)P_k⁻, (3.13)

whereI denotes the 2×2 identity matrix.

Because the matrices Φ, Γ, and H in equations (3.9) and (3.10) contain constants, the es-timation (or filter) performance is directly influenced, and thus determined, by the respective choices of the process and measurement noise covariancesQandRas well as the initial value of the estimation error covariance P0 =Eh

(x(0)−x0) (x(0)−x0)^Ti

. The following subsections propose first a solution based on numerical optimization for automatic tuning of these para-meters. Then, I develop an adaptive KF approach that varies the aforementioned covariance values based on the dynamical behavior of the system.