Documenti di Didattica
Documenti di Professioni
Documenti di Cultura
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.
3
Applicable to IMU consisted of 3-axis Gyro and Accel;
Filter benefits:
Low computational load - requiring 109 (IMU) or 277 (MARG) scalar arithmetic operations
each filter update;
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.
6
The quaternion conjugate describes the orientation of
frame A relative to frame B :
𝐴 ∗
𝐵𝒒 = 𝐵𝐴𝒒 = 𝑞0 −𝑞1 −𝑞2 −𝑞3 𝑇
𝒂 × 𝒃 = 𝑎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:
𝐵
𝒓 = 𝐵𝐴𝒒 × 𝐴𝒓 × 𝐵𝐴𝒒∗
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
𝐴
φ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 𝜔𝑋
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.
𝐸 𝑇 𝑆 𝑠𝑌 𝑠𝑍 𝑇
𝒅 = 0 𝑑𝑋 𝑑𝑌 𝑑𝑍 𝒔 = 0 𝑠𝑋
𝑆 𝑆
∇𝒇 𝐸𝑆𝒒, 𝐸𝒅, 𝑆𝒔
𝐸 𝒒𝑘 = 𝐸 𝒒𝑘−1 −𝜇 𝑆 𝐸 𝑆 , 𝑘 = 0,1,2, … , 𝑛
∇𝒇 𝐸 𝒒, 𝒅, 𝒔
∇𝒇 𝐸𝑆𝒒, 𝐸𝒅, 𝑆𝒔 = 𝑱𝑇 𝑆 𝐸
𝐸 𝒒, 𝒅 𝒇 𝐸𝑆𝒒, 𝐸𝒅, 𝑆𝒔
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
Then the objective function and its Jacobian are simplified and will be equal to:
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 𝑚𝑋 𝑚𝑌 𝑚𝑍 𝑇
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
23
5.4
𝑆
𝐸 𝒒𝑒𝑠𝑡,𝑡 = 𝛾𝑡 ∙ 𝐸𝑆𝒒∇,𝑡 + 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 divergences rate of 𝐸𝑆𝒒ሶ ω,𝑡 expressed as the magnitude of a quaternion
derivative corresponding to the Gyro measurement error.
𝜇𝑡 𝛽
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:
𝑆
∇𝒇 𝛽∆𝑡
𝐸 𝒒∇,𝑡 ≈ −𝜇𝑡 , 𝛾𝑡 ≈
∇𝒇 𝜇𝑡
𝑆
𝛽∆𝑡 ∇𝒇 𝑆
𝐸 𝒒𝑒𝑠𝑡,𝑡 = ∙ −𝜇𝑡 + 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
33
Declination errors (those in the horizontal plane relative to the Earth's surface) can
be corrected with an additional reference of heading only.
𝐸
𝒉𝑡 = 0 ℎ𝑋 ℎ𝑌 ℎ𝑍 𝑇
= 𝐸𝑆𝒒𝑒𝑠𝑡,𝑡−1 × 𝐸𝒎𝑡 × 𝐸𝑆𝒒∗𝑒𝑠𝑡,𝑡−1
𝑇
𝐸
𝒃𝑡 = 0 ℎ𝑋2 + ℎ𝑌2 0 ℎ𝑍
34
Example plots where basic AHRS Fusion results vs. AHRS Fusion+Magnet correction
are represented. If the difference is visible.
35
5.6
𝑆
∇𝒇
𝐸 𝒒ሶ 𝑒𝑠𝑡,𝑡 = 𝐸𝑆𝒒ሶ 𝜔,𝑡 − 𝛽 = 𝐸𝑆𝒒ሶ 𝜔,𝑡 − 𝛽 𝐸𝑆𝒒ሶ 𝜖,𝑡
∇𝒇
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.
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