• Nem Talált Eredményt


In document Obuda University ´ (Pldal 65-68)

2.4 Theses

3.1.1 Algorithm Gyroscope model

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

BkkB0,kk, (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µTl

µ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,k0,k−1k, (3.3)

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


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 = θkkTs, 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). Accelerometer model

The raw accelerometer output ABk consists of four main components: the external accelera-tion αBk, the contribution of gravitational acceleration gkB, the bias term a0, and the additive measurement noise νk, and it is given by

ABk = αBk −gBk

+a0k. (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 αBk ≈ 0), then the gravity vector can be used to calculate the raw attitude realizationθA,k with Li and Wang (2013)

θA,k= arctan2 (Ay,k, Az,k) =θk+vk, (3.6) whereAy,kandAz,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 vk 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, vk is assumed to be Gaussian white noise with the following characteristics:

E[vk] = 0, E vkvTl

v2δkl, σ2v

= deg2 1

Hz. (3.7)

Based on equations (3.5) and (3.6), we can deduce that if there is an external acceleration (αBk 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 vk (i.e., a significantly largerσ2v noise variance is expected when αBk 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. 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[xk−xˆk] = 0, Eh

(xk−xˆk) (xk−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 xk = (θk, ω0,k)T, the input variable uk = Ω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 zkA,k, measurement noise ofvk, and an output matrix of H as

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


= 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 wk and vk given by Q = E

wkwkT and R = E


, 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 (ˆxk) and estimate error covariance (Pk) as:


xk = Φˆxk−1+ Γuk−1,

Pk= Φ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:

Gk=PkHT HPkHT +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= ˆxk +Gk zk−Hxˆk ,

Pk= (I−GkH)Pk, (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.

In document Obuda University ´ (Pldal 65-68)