**2.4 Theses**

**3.1.1 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}

=σ_{µ}^{2}δkl,
σ_{µ}^{2}

= deg

s 2 1

Hz, (3.2)

where σ^{2}_{µ} 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σ_{η}^{2} noise variance, i.e.,
E[η_{k}] = 0, E

η_{k}η_{l}^{T}

=σ^{2}_{η}δ_{kl},
σ_{η}^{2}

= 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} +ω_{k}Ts,
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_{0}, 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,k}andA_{z,k}are 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_{k}v^{T}_{l}

=σ_{v}^{2}δ_{kl},
σ^{2}_{v}

= deg^{2} 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σ^{2}_{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})^{T}i

→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_{k}wk^{T}
and
R = E

v_{k}vk^{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^{−1}

, (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_{k}H)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)^{T}i

. 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.