Module 5 5

1. Sensor Types
2. MEMS Accelerometers
3. MEMS Gyroscopes
4. MEMS Magnetometers
5. Accelerometer-based Euler angles estimation
6. Gyroscope-based Euler angles estimation
7. Accelerometer-Magnetometer-based Euler angles
estimation
2
 An IMU alone can only measure an attitude relative to the direction of gravity which
is sufficient for many applications;
 MARG (Magnetic, Angular Rate, and Gravity) systems, also known as AHRS (Attitude
and Heading Reference Systems) are able to provide a complete measurement of
orientation relative to the direction of gravity and the earth’s magnetic etc.

 About Gyro for calculating angles disadvantages;

 About Kalman high computational load;

3
 Applicable to IMU consisted of 3-axis Gyro and Accel;

 Applicable to MARG sensor consisted of 3-axis Gyro, Accel and Magnet;

 MARG implementation incorporates Gyro bias drift compensation and magnetic

distortions compensation;
 Filter uses quaternion representation and optimized gradient-descent algorithm;

 Filter benefits:

 Low computational load - requiring 109 (IMU) or 277 (MARG) scalar arithmetic operations
each filter update;

 Ability to operate at low sampling rates, e.g. 10 Hz;

 Contains 1 (IMU) or 2 (MARG) adjustable parameters only.

4
5.1
 A quaternion is a 4D-complex number that can be used to
represent the orientation of a coordinate frame (or ridged
body) in 3D space.

 An arbitrary orientation of frame B relative to frame A can

𝐴
be achieved through a rotation of angle around an axis 𝑟Ԧ
defined in frame A.

 The quaternion describing this rotation is:

𝑇
𝐴 𝑞2 𝑞3 𝑇 𝜃 𝜃 𝜃 𝜃
𝐵𝒒 = 𝑞0 𝑞1 = 𝑐𝑜𝑠 𝑟𝑋 𝑠𝑖𝑛 𝑟𝑌 𝑠𝑖𝑛 𝑟𝑍 𝑠𝑖𝑛
2 2 2 2

6
 The quaternion conjugate describes the orientation of
frame A relative to frame B :
𝐴 ∗
𝐵𝒒 = 𝐵𝐴𝒒 = 𝑞0 −𝑞1 −𝑞2 −𝑞3 𝑇

 The quaternion product can be used to define compound

orientations :
𝐴
𝐶𝒒 = 𝐵𝐶𝒒 × 𝐵𝐴𝒒

𝒂 × 𝒃 = 𝑎0 𝑎1 𝑎2 𝑎3 𝑇
× 𝑏0 𝑏1 𝑏2 𝑏3 𝑇
=
𝑎0 𝑏0 − 𝑎1 𝑏1 − 𝑎2 𝑏2 − 𝑎3 𝑏3
𝑎0 𝑏1 + 𝑎1 𝑏0 + 𝑎2 𝑏3 − 𝑎3 𝑏2
=
𝑎0 𝑏2 − 𝑎1 𝑏3 + 𝑎2 𝑏0 + 𝑎3 𝑏1
𝑎0 𝑏3 + 𝑎1 𝑏2 − 𝑎2 𝑏1 + 𝑎3 𝑏0
7
 A 3D-vector can be rotated by a quaternion:

𝐵
𝒓 = 𝐵𝐴𝒒 × 𝐴𝒓 × 𝐵𝐴𝒒∗

 The Quaternion to Rotation Matrix transformation:

2 𝑞02 + 𝑞12 − 1 2 𝑞1 𝑞2 + 𝑞0 𝑞3 2 𝑞1 𝑞3 − 𝑞0 𝑞2
𝐴
𝐵𝑹 = 2 𝑞1 𝑞2 − 𝑞0 𝑞3 2 𝑞02 + 𝑞22 − 1 2 𝑞2 𝑞3 + 𝑞0 𝑞1
2 𝑞1 𝑞3 + 𝑞0 𝑞2 2 𝑞1 𝑞3 − 𝑞0 𝑞2 2 𝑞02 + 𝑞32 − 1

 The XYZ sequence Euler angles from quaternion:

𝐴
φ123 𝐵𝑹 𝑎𝑡𝑎𝑛2 2 𝑞1 𝑞2 + 𝑞0 𝑞3 , 2 𝑞02 + 𝑞12 − 1
𝐴
𝑢123 𝐵𝑹 = 𝜃123 𝐵𝐴𝑹 = −𝑎𝑠𝑖𝑛 2 𝑞1 𝑞3 − 𝑞0 𝑞2
ψ123 𝐵𝐴𝑹 𝑎𝑡𝑎𝑛2 2 𝑞2 𝑞3 + 𝑞0 𝑞1 , 2 𝑞02 + 𝑞32 − 1
8
5.2
 The quaternion derivative describing the rate of change of orientation of the Earth
frame relative to the Sensor frame:
1𝑆
𝑆
𝐸 𝒒ሶ = 𝐸 𝒒 × 𝑆𝝎
2
𝑆 𝜔𝑌 𝜔𝑍 𝑇
𝝎 = 0 𝜔𝑋

 Numerical integration of the quaternion derivative provided that initial conditions

are known (Δt is the sampling period):
1𝑆
𝑆
𝐸 𝒒ሶ 𝜔,𝑡 = 𝐸𝒒ෝ𝑒𝑠𝑡,𝑡−1 × 𝑆𝝎𝑡
2
𝑆
𝐸 𝒒𝜔,𝑡 = 𝐸𝑆𝒒
ෝ𝑒𝑠𝑡,𝑡−1 + 𝐸𝑆𝒒ሶ 𝜔,𝑡 ∙ ∆𝑡

10
 Example of estimated angles on one log

11
 It will initially be assumed that an Accel will measure only gravity, a Magnet will
measure only the Earth's magnetic field.

 If the direction of an Earth’s field is known in the earth frame, a measurement of

the field's direction within the sensor frame will allow an orientation of the sensor
frame relative to the earth frame to be calculated.
 Orientation quaternion man be found minimizing the objective function:

𝒇 𝐸𝑆𝒒, 𝐸𝒅, 𝑆𝒔 = 𝐸𝑆𝒒∗ × 𝐸𝒅 × 𝐸𝑆𝒒 − 𝑆𝒔, 𝑆

min 𝒇 𝑆
𝐸 𝒒, 𝐸
𝒅, 𝑆
𝒔
𝟒
𝐸𝒒∈ℜ

𝐸 𝑇 𝑆 𝑠𝑌 𝑠𝑍 𝑇
𝒅 = 0 𝑑𝑋 𝑑𝑌 𝑑𝑍 𝒔 = 0 𝑠𝑋

where 𝐸 𝒅 is a predefined reference direction of the field in the Earth frame;

𝑆
𝒔 is the measured direction of the field in the sensor frame;
12
 One of the simplest to both implement and compute optimization algorithms
(finding the solution of the minimization problem) is the gradient descent algorithm:

𝑆 𝑆
∇𝒇 𝐸𝑆𝒒, 𝐸𝒅, 𝑆𝒔
𝐸 𝒒𝑘 = 𝐸 𝒒𝑘−1 −𝜇 𝑆 𝐸 𝑆 , 𝑘 = 0,1,2, … , 𝑛
∇𝒇 𝐸 𝒒, 𝒅, 𝒔

∇𝒇 𝐸𝑆𝒒, 𝐸𝒅, 𝑆𝒔 = 𝑱𝑇 𝑆 𝐸
𝐸 𝒒, 𝒅 𝒇 𝐸𝑆𝒒, 𝐸𝒅, 𝑆𝒔

where ∇𝒇 … is the gradient of the solution surface;

𝑆 𝐸
𝑱𝑇 𝐸 𝒒, 𝒅 is the Jacobian of the objective function.

13
 𝒇 𝐸𝑆𝒒, 𝐸𝒅, 𝑆𝒔 = 𝐸𝑆𝒒∗ × 𝐸𝒅 × 𝐸𝑆𝒒 − 𝑆𝒔 =

0
𝑑𝑋 1 − 2 𝑞22+ 𝑞32 + 2𝑑𝑌 𝑞1 𝑞2 + 𝑞0 𝑞3 + 2𝑑𝑍 𝑞1 𝑞3 − 𝑞0 𝑞2 − 𝑠𝑋
=
2𝑑𝑋 𝑞1 𝑞2 − 𝑞0 𝑞3 + 𝑑𝑌 1 − 2 𝑞12 + 𝑞32 + 2𝑑𝑍 𝑞2 𝑞3 + 𝑞0 𝑞1 − 𝑠𝑌
2𝑑𝑋 𝑞1 𝑞3 + 𝑞0 𝑞2 + 2𝑑𝑌 𝑞2 𝑞3 − 𝑞0 𝑞1 + 𝑑𝑍 1 − 2 𝑞12 + 𝑞22 − 𝑠𝑍

14
 Jacobian is the matrix of all first-order partial derivatives of a vector-valued function.

𝜕𝑓 0 𝜕𝑓 0 𝜕𝑓 0 𝜕𝑓 0
𝜕𝑞0 𝜕𝑞1 𝜕𝑞2 𝜕𝑞3
𝜕𝑓 1 𝜕𝑓 1 𝜕𝑓 1 𝜕𝑓 1
𝑆 𝐸 𝜕𝒇 𝜕𝒇 𝜕𝑞0 𝜕𝑞1 𝜕𝑞2 𝜕𝑞3
𝑱 𝐸 𝒒, 𝒅 = ; …; =
𝜕𝑞0 𝜕𝑞3 𝜕𝑓 2 𝜕𝑓 2 𝜕𝑓 2 𝜕𝑓 2
𝜕𝑞0 𝜕𝑞1 𝜕𝑞2 𝜕𝑞3
𝜕𝑓 3 𝜕𝑓 3 𝜕𝑓 3 𝜕𝑓 3
𝜕𝑞0 𝜕𝑞1 𝜕𝑞2 𝜕𝑞3

15
  Jacobian is the matrix of all first-order partial derivatives of a vector-valued function.

𝑱 𝐸𝑆𝒒, 𝐸𝒅 =

0 0 0 0
2 𝑑𝑌 𝑞3 − 𝑑𝑍 𝑞2 2 𝑑𝑌 𝑞2 + 𝑑𝑍 𝑞3 2 −2𝑑𝑋 𝑞2 + 𝑑𝑌 𝑞1 − 𝑑𝑍 𝑞0 2 −2𝑑𝑋 𝑞3 + 𝑑𝑌 𝑞0 + 𝑑𝑍 𝑞1
=
2 −𝑑𝑋 𝑞3 + 𝑑𝑍 𝑞1 2 𝑑𝑋 𝑞2 − 2𝑑𝑌 𝑞1 + 𝑑𝑍 𝑞0 2 𝑑𝑋 𝑞1 + 𝑑𝑍 𝑞3 2 −𝑑𝑋 𝑞0 − 2𝑑𝑌 𝑞3 + 𝑑𝑍 𝑞2
2 𝑑𝑋 𝑞2 − 𝑑𝑌 𝑞1 2 𝑑𝑋 𝑞3 − 𝑑𝑌 𝑞0 − 2𝑑𝑍 𝑞1 2 𝑑𝑋 𝑞0 + 𝑑𝑌 𝑞3 − 2𝑑𝑍 𝑞2 2 𝑑𝑋 𝑞1 + 𝑑𝑌 𝑞2

16
5.3

Orientation quaternion from Accelerometer and Magnetometer readings

 Previous equations describe the general form of the algorithm applicable to a field
predefined in any direction.
 Assume that the direction of gravity defines the vertical, z axis only:
𝐸
𝒅 = 𝐸𝒈 = 0 0 0 1 𝑇 𝑆
𝒔 = 𝑆𝒂 = 0 𝑎𝑋 𝑎𝑌 𝑎𝑍 𝑇

 Then the objective function and its Jacobian are simplified and will be equal to:

𝒇 𝐸𝑆𝒒, 𝐸𝒈, 𝑆𝒂 = 𝐸𝑆𝒒∗ × 𝐸𝒈 × 𝐸𝑆𝒒 − 𝑆𝒂 =

0 0 0 0
0 −2𝑞2 2𝑞3 −2𝑞0 2𝑞1
𝑱 𝐸𝑆𝒒 =
2 𝑞1 𝑞3 − 𝑞0 𝑞2 − 𝑎𝑋 2𝑞1 2𝑞0 2𝑞3 2𝑞2
= 2 𝑞2 𝑞3 + 𝑞0 𝑞1 − 𝑎𝑌 0 −4𝑞1 −4𝑞2 0
𝑑𝑍 1 − 2 𝑞12 + 𝑞22 − 𝑎𝑍

18
 The Earth's magnetic field can be considered to have components in one horizontal
axis and the vertical axis (the vertical component due to the inclination of the field
which is between 2 and 2.05 deg to the horizontal in the Addis-Ababa):

𝐸
𝒅 = 𝐸𝒃 = 0 𝑏𝑋 0 𝑏𝑍 𝑇 𝑆
𝒔 = 𝑆𝒎 = 0 𝑚𝑋 𝑚𝑌 𝑚𝑍 𝑇

 Then the objective function is simplified and will be equal to:

0
𝑏𝑋 1 − 2 𝑞22
+ 𝑞32
+ 2𝑏𝑍 𝑞1 𝑞3 − 𝑞0 𝑞2 − 𝑚𝑋
𝒇 𝐸𝑆𝒒, 𝐸𝒃, 𝑆𝒎 = 𝐸𝑆𝒒∗ × 𝐸𝒃 × 𝐸𝑆𝒒 − 𝑆𝒎 = 2𝑏𝑋 𝑞1 𝑞2 − 𝑞0 𝑞3 + 2𝑏𝑍 𝑞2 𝑞3 + 𝑞0 𝑞1 − 𝑚𝑌
2𝑏𝑋 𝑞1 𝑞3 + 𝑞0 𝑞2 + 𝑏𝑍 1 − 2 𝑞12 + 𝑞22 − 𝑚𝑍

19
 The Jacobian of the objective function is equal to:

0 0 0 0
−2𝑏𝑍 𝑞2 2𝑏𝑍 𝑞3 2 −2𝑏𝑋 𝑞2 − 𝑏𝑍 𝑞0 2 −2𝑏𝑋 𝑞3 + 𝑏𝑍 𝑞1
𝑱 𝐸𝑆𝒒, 𝐸𝒃 =
2 −𝑏𝑋 𝑞3 + 𝑏𝑍 𝑞1 2 𝑏𝑋 𝑞2 + 𝑏𝑍 𝑞0 2 𝑏𝑋 𝑞1 + 𝑏𝑍 𝑞3 2 −𝑏𝑋 𝑞0 + 𝑏𝑍 𝑞2
2𝑏𝑋 𝑞2 2 𝑏𝑋 𝑞3 − 2𝑏𝑍 𝑞1 2 𝑏𝑋 𝑞0 − 2𝑏𝑍 𝑞2 2𝑏𝑋 𝑞1

20
 The measurement of gravity or the Earth's magnetic field alone will not provide a
unique orientation of the sensor.
 To do so, the measurements and reference directions of both fields may be combined,
provided that 𝑏𝑋 ≠ 0:

𝑆 𝑆 𝐸 𝑆 𝑆 𝐸 𝑆
𝒇𝑔,𝑏 𝐸𝒒 = 𝒇𝑔 𝐸 𝒒, 𝒈, 𝒂 𝒇𝑏 𝐸 𝒒, 𝒃, 𝒎

𝐸
𝑱𝑔,𝑏 = 𝑱𝑇𝑔 𝑆
𝐸𝒒 𝑱𝑇𝑏 𝑆
𝐸 𝒒, 𝒃

∇𝒇
𝑆
𝐸 𝒒∇,𝑡 = 𝐸𝑆𝒒𝑡−1 − 𝜇𝑡 , ∇𝒇 = 𝑱𝑇𝑔,𝑏 𝑆
𝐸 𝒒𝑘−1 ∙ 𝒇𝑔,𝑏 𝑆
𝐸 𝒒𝑘−1
∇𝒇

 Note: Now algorithm is not iterative! There are no ‘n’-iterations, just one calculation
for each sensor sample.
21
 ∇𝒇
𝑆
𝐸 𝒒∇,𝑡 = 𝐸𝑆𝒒𝑡−1 − 𝜇𝑡 , ∇𝒇 = 𝑱𝑇𝑔,𝑏 𝑆
𝐸 𝒒𝑘−1 ∙ 𝒇𝑔,𝑏 𝑆
𝐸 𝒒𝑘−1
∇𝒇

 It is acceptable to compute one iteration per time sample provided that the
convergence rate governed by 𝜇𝑡 is equal or greater than the physical rate of change
of orientation.

 An optimal value of 𝜇𝑡 can be defined as that which ensures the convergence rate of
𝑆
is limited to the physical orientation rate as this avoids overshooting due an
𝐸 𝒒∇,𝑡
unnecessarily large step size. Therefore 𝜇𝑡 can be calculated as:
𝑆
𝜇𝑡 = 𝛼 𝐸 𝒒ሶ ω,𝑡 ∆𝑡, 𝛼>1

 where α is an augmentation of 𝜇𝑡 to account for noise in Accel and Magnet

measurements, 𝐸𝑆𝒒ሶ ω,𝑡 is the physical orientation rate measured by Gyro.
22
 Example of estimated angles on one log

23
5.4

Mixing Gyro solution with Accel and Magnet solution

 Final estimated orientation of the Sensor frame relative to the Earth frame is:

𝑆
𝐸 𝒒𝑒𝑠𝑡,𝑡 = 𝛾𝑡 ∙ 𝐸𝑆𝒒∇,𝑡 + 1 − 𝛾𝑡 ∙ 𝐸𝑆𝒒𝜔,𝑡

where 𝐸𝑆𝒒∇,𝑡 is the orientation obtained from Accel and Magnet measurements,
𝑆
𝐸 𝒒𝜔,𝑡 is the orientation obtained from Gyro measurements,
𝛾𝑡 denotes a weight coefficient (0 ≤ 𝛾𝑡 ≤ 1).

25
 Optimal value of 𝛾𝑡 ensures the weighted divergence of 𝐸𝑆𝒒𝜔,𝑡 is equal to the weighted
convergence of 𝐸𝑆𝒒∇,𝑡 .

 𝜇𝑡 /∆𝑡 = 𝛼 𝐸𝑆𝒒ሶ ω,𝑡 is the convergences rate of 𝐸𝑆𝒒∇,𝑡 .

 𝛽 = 𝛼 𝐸𝑆𝒒ሶ ω,𝑡 is the divergences rate of 𝐸𝑆𝒒ሶ ω,𝑡 expressed as the magnitude of a quaternion
derivative corresponding to the Gyro measurement error.

 Then 𝛾𝑡 can be defined as:

𝜇𝑡 𝛽
1 − 𝛾𝑡 𝛽 = 𝛾𝑡 , 𝛾𝑡 = 𝜇
∆𝑡 𝛽 + ∆𝑡𝑡

26
 Convergence rate of 𝐸𝑆𝒒∇,𝑡 governed by 𝛼 is equal or greater than the physical rate of
change of orientation. Therefore 𝛼 has no upper bound. If 𝛼 is assumed to be very
large then 𝜇𝑡 also becomes very large and:

𝑆
∇𝒇 𝛽∆𝑡
𝐸 𝒒∇,𝑡 ≈ −𝜇𝑡 , 𝛾𝑡 ≈
∇𝒇 𝜇𝑡

 Then final fusion solution can be rewritten as:

𝑆
𝛽∆𝑡 ∇𝒇 𝑆
𝐸 𝒒𝑒𝑠𝑡,𝑡 = ∙ −𝜇𝑡 + 1−0 ∙ 𝐸 𝒒𝑒𝑠𝑡,𝑡−1 + 𝐸𝑆𝒒ሶ 𝜔,𝑡 ∙ ∆𝑡
𝜇𝑡 ∇𝒇

27
 𝑆
𝛽∆𝑡 ∇𝒇 𝑆
𝐸 𝒒𝑒𝑠𝑡,𝑡 = ∙ −𝜇𝑡 + 1−0 ∙ 𝐸 𝒒𝑒𝑠𝑡,𝑡−1 + 𝐸𝑆𝒒ሶ 𝜔,𝑡 ∙ ∆𝑡
𝜇𝑡 ∇𝒇

𝑆 𝑆 𝑆
∇𝒇
𝐸 𝒒𝑒𝑠𝑡,𝑡 = 𝐸 𝒒𝑒𝑠𝑡,𝑡−1 + 𝐸 𝒒ሶ 𝜔,𝑡 −𝛽 ∙ ∆𝑡
∇𝒇

𝑆
∇𝒇
𝐸 𝒒ሶ 𝑒𝑠𝑡,𝑡 = 𝐸𝑆𝒒ሶ 𝜔,𝑡 − 𝛽 = 𝐸𝑆𝒒ሶ 𝜔,𝑡 − 𝛽 𝐸𝑆𝒒ሶ 𝜖,𝑡
∇𝒇

𝑆
𝐸 𝒒𝑒𝑠𝑡,𝑡 = 𝐸𝑆𝒒𝑒𝑠𝑡,𝑡−1 + 𝐸𝑆𝒒ሶ 𝑒𝑠𝑡,𝑡 ∙ ∆𝑡

 The filter calculates the orientation 𝐸𝑆𝒒𝑒𝑠𝑡,𝑡 by numerically integrating the estimated
orientation rate 𝐸𝑆𝒒ሶ 𝑒𝑠𝑡,𝑡
28
 It is necessary to remove corrections

29
30
 Example plots where Acc, Gyro separately are represented along with basic IMU
Fusion results

 Example plots where Acc+Magnet, Gyro separately are represented along with basic
AHRS Fusion results

31
5.5

Magnetic distortion compensation

 Declination errors (those in the horizontal plane relative to the Earth's surface) can
be corrected with an additional reference of heading only.
 Inclination errors (those in the vertical plane relative to the Earth's surface) may be
compensated for by the Accel measurements (as we investigated before).

Correct Magnetometer Apply corrected

measurements using measurements to
previous attitude filter calculate the current
estimate filter output

33
 Declination errors (those in the horizontal plane relative to the Earth's surface) can
be corrected with an additional reference of heading only.

 Inclination error correction:

𝐸
𝒉𝑡 = 0 ℎ𝑋 ℎ𝑌 ℎ𝑍 𝑇
= 𝐸𝑆𝒒𝑒𝑠𝑡,𝑡−1 × 𝐸𝒎𝑡 × 𝐸𝑆𝒒∗𝑒𝑠𝑡,𝑡−1

𝑇
𝐸
𝒃𝑡 = 0 ℎ𝑋2 + ℎ𝑌2 0 ℎ𝑍

 Corrected magnetometer measurements further are applied in initial filter scheme.

34
 Example plots where basic AHRS Fusion results vs. AHRS Fusion+Magnet correction
are represented. If the difference is visible.

35
5.6

Gyro bias drift compensation

 The 1st step. Previously the following formula was obtained:

𝑆
∇𝒇
𝐸 𝒒ሶ 𝑒𝑠𝑡,𝑡 = 𝐸𝑆𝒒ሶ 𝜔,𝑡 − 𝛽 = 𝐸𝑆𝒒ሶ 𝜔,𝑡 − 𝛽 𝐸𝑆𝒒ሶ 𝜖,𝑡
∇𝒇

where 𝐸𝑆𝒒ሶ 𝜖,𝑡 is the estimated error in the rate of change of orientation, 𝐸𝑆𝒒ሶ ω,𝑡 ,
obtained using Gyro measurements.
1 𝑆
 The 2nd step. The inverse relationship to the formula 𝐸𝑆𝒒ሶ = 𝐸𝑆𝒒 × 𝝎:
2
𝑆
𝝎𝜖,𝑡 = 2 𝐸𝑆𝒒∗𝑒𝑠𝑡,𝑡−1 × 𝐸𝑆𝒒ሶ 𝜖,𝑡
𝑆
allows estimating the angular error in each gyroscope axis, 𝝎𝜖,𝑡 .

37
 𝑆
 The 3rd step. The gyroscope bias, 𝝎𝑏,𝑡 , is represented by the DC component of 𝑆𝝎𝜖,𝑡 ,
and can be calculated as the integral of 𝑆𝝎𝜖,𝑡 weighted by an appropriate gain ζ:

𝑆
𝝎𝑏,𝑡 = ζ ෍ 𝑆𝝎𝜖,𝑡 ∙ ∆𝑡
𝑡

𝑆
 Then the bias-compensated Gyro measurements, 𝝎𝑐,𝑡 , are calculated as:

𝑆
𝝎𝑐,𝑡 = 𝑆𝝎𝑡 − 𝑆𝝎𝑏,𝑡

𝑆
 The compensated Gyro measurements, 𝝎𝑐,𝑡 , may then be used in place of the of
the raw Gyro measurements, 𝑆𝝎𝑡 , for evaluating 𝐸𝑆𝒒𝜔,𝑡 .

38
39
 Example plots where basic AHRS Fusion results vs. AHRS Fusion+Gyro correction are
represented. If the difference is visible.

 Example plots where basic AHRS Fusion results vs. AHRS Fusion+Magnet+Gyro
correction are represented. If the difference is visible.

40
5.7
 The 1st parameter – the filter gain 𝛽 represents all zero-mean Gyro measurement
errors, expressed as the magnitude of a quaternion derivative.

 It is convenient to define 𝛽 using the angular quantity 𝜔

෥𝛽 represents the estimated
mean zero Gyro measurement error of each axis (𝒒 is any unit quaternion):

1 3
𝛽= 𝒒× 0 𝜔
෥𝛽 𝜔
෥𝛽 𝜔
෥𝛽 = 𝜔
෥
2 4 𝛽

 In experimental part of the paper it is proposed to set 𝛽 equal to 0.033 for IMU filter
version and 0.041 for MARG realization.
 However, an initial value of 2.5 was used for the first 10 sec. of any experiment to
ensure the convergence of algorithm states from initial conditions.

42
 The 2nd parameter – the filter gain ζ represents the rate of convergence to remove
Gyro measurement errors which are not mean zero, also expressed as the magnitude
of a quaternion derivative.

 It is convenient to define 𝛽 using the angular quantity 𝜔ሶ ζ represents the estimated rate
of gyroscope bias drift in each axis:

1 3
ζ= 𝒒× 0 𝜔ሶ ζ 𝜔ሶ ζ 𝜔ሶ ζ = 𝜔ሶ
2 4 ζ

 In experimental part of the paper the parameter was set to 0 as the calibrated
orientation sensor data was not subject to gyroscope bias drift.

43
5.8

Practical Tasks
1. Capture Gyroscope measurements which correspond to the following test cases:
device is in level plane, wait 3-5 sec., then rotate it to 900 around X, Y or Z axis,
hold in rotated position 3-5 sec., then return back to initial position and wait for 3-5
sec. (log 1); perform the same as in test case 1 but three-four times (log 2);

2. Show the dependencies of Gyroscope vector components on time; explain the results
and prove why the plots correspond to the test cases;

3. Calculate angle values using all three methods discussed; analyze the results and
find the most accurate one;

4. Show and explain the pros and cons of angles estimation using Gyroscope
measurements only.

45