Module 4 4

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
 Prediction
based on the
𝑋𝑘−1 𝑋𝑘−
model

Correction
Filter OUTPUT based on new Filter INPUT
𝐶𝑢𝑟𝑟𝑒𝑛𝑡 𝑒𝑠𝑡𝑖𝑚𝑎𝑡𝑒𝑠 measurements 𝑀𝑒𝑎𝑠𝑢𝑟𝑒𝑚𝑒𝑛𝑡𝑠
𝑋𝑘 𝑍Ԧ𝑘

3
4
Equation 1 - State Extrapolation Equation
(or Predicted (a priori) state estimate)

Equation 1 follows the Kalman filter model

assumes the true state at time k is evolved
from the state at (k − 1) according to:

5
Equation 2 - Covariance Extrapolation Equation
(or Predicted (a priori) error covariance)

6
Equation 3 – Kalman Gain Equation

7
 Equation 4 – State Update Equation

Equation 4 can also be rewritten as:

8
 Equation 5 – Covariance Update Equation
(or Updated (a posteriori) estimate covariance)

9
10
11
4.1

Single angle estimation

 The filter estimates one measuring angle based on the measurements from the
accelerometer and gyroscope.

 The filter state vector X consists of one measuring angle: 𝑿𝑘 = 𝜃

 The measurement vector Z consists of Accel measurements: 𝒁𝑘 = 𝑎 𝑘

 The filter model is based on the following dependence: 𝜃𝑘 = 𝜃𝑘−1 + 𝜔𝑘 ∙ ∆𝑡

where 𝜔𝑘 is Gyro measurement.

 Beforehand we have to measure (or know) standard deviation (std) of Gyro and
Accel measurements: 𝜎𝐺 and 𝜎𝐴

13
 Then for the State Extrapolation Equation

𝑭𝑘 = 𝑭 = 1 𝑩𝑘 = 𝑩 = ∆𝑡 𝑼𝑘 = 𝜔𝑘𝑋 𝑿− 𝑋
𝑘 = 𝑿𝑘−1 + 𝜔𝑘 ∙ ∆𝑡

 For the Covariance Extrapolation Equation

𝑷𝑘−1 = 𝑷𝑖𝑛𝑖𝑡 = 100 𝑸𝑘 = 𝑸 = 𝜎𝐺2 ∙ ∆𝑡 𝑷−

𝑘 = 𝑷𝑘−1 + 𝑸

 For the Measurement Innovation Equation

𝒁𝑘 = 𝜃෨𝑘𝑋 𝑯𝑘 = 𝑯 = 1 𝒀𝑘 = 𝒁𝑘 − 𝑯 ∙ 𝑿𝑘− = 𝜃෨𝑘𝑋 − 𝜃𝑘−

where 𝜃෨𝑘𝑋 denotes the angle derived from corresponding Accel measurement.

14
 For the Kalman Gain Equation
𝑷−
𝑘
𝑹𝑘 = 𝑹 = 𝜎𝐴2 𝑺𝑘 = 𝑷−
𝑘 +𝑹
𝑲𝑘 = −
𝑷𝑘 + 𝑹

 For the State Update Equation

𝑿𝑘 = 𝑿− − − − ෨𝑋 −
𝑘 + 𝑲𝑘 ∙ 𝒀𝑘 = 𝑿𝑘 + 𝑲𝑘 𝒁𝑘 − 𝑯 ∙ 𝑿𝑘 = 𝑿𝑘 + 𝑲𝑘 𝜃𝑘 − 𝜃𝑘

 For the Covariance Update Equation

𝑷𝑘 = 𝑰1𝑥1 − 𝑲𝑘 ∙ 𝑯 𝑷− −
𝑘 = 1 − 𝑲𝑘 𝑷𝑘

15
 Depict initial filter parameters (Gyro std (from datasheet?) and Accel std (from meas?))

 Provide results of estimation with Ground Truth data

16
4.2

Single angle estimation with bias correction

 The filter estimates one measuring angle and gyro bias based on the measurements
from the accelerometer and gyroscope.
𝑇
 The filter state vector X consists of two items: 𝑿𝑘 = 𝜃𝑘 𝜃ሶ 𝑘𝑏

 The measurement vector Z consists of Accel measurements: 𝒁𝑘 = 𝑎 𝑘

 The filter model is based on the following dependence: 𝜃𝑘 = 𝜃𝑘−1 + 𝜔𝑘 − 𝜃ሶ 𝑘−1

𝑏
∙ ∆𝑡

where 𝜔𝑘 is Gyro measurement and 𝜃ሶ𝑘−1

𝑏
is an estimated Gyro bias.

 Beforehand we have to measure (or know) Gyro, its bias and Accel measurements
std values: 𝜎𝐺 , 𝜎𝑏 𝐺 and 𝜎𝐴

18
 Then for the State Extrapolation Equation

1 −∆𝑡 ∆𝑡
𝑭𝑘 = 𝑭 = 𝑩𝑘 = 𝑩 = 𝑼𝑘 = 𝜔𝑘𝑋 𝑿−
𝑘 = 𝑭 ∙ 𝑿𝑘−1 + 𝑩 ∙ 𝑼𝑘
0 1 0

 For the Covariance Extrapolation Equation

10 0 𝜎𝐺2 0
𝑷𝑘−1 = 𝑷𝑖𝑛𝑖𝑡 = 𝑸𝑘 = 𝑸 = 2 ∙ ∆𝑡 𝑷− 𝑻
𝑘 = 𝑭 ∙ 𝑷𝑘−1 ∙ 𝑭 + 𝑸
0 10 0 𝜎𝑏 𝐺

 For the Measurement Innovation Equation

𝒁𝑘 = 𝜃෨𝑘𝑋 𝑯𝑘 = 𝑯 = 1 0 𝒀𝑘 = 𝒁𝑘 − 𝑯 ∙ 𝑿𝑘− = 𝜃෨𝑘𝑋 − 𝜃𝑘−

where 𝜃෨𝑘𝑋 denotes the angle derived from corresponding Accel measurement.

19
 For the Kalman Gain Equation

𝑹𝑘 = 𝑹 = 𝜎𝐴2 𝑷−
𝑘 ∙𝑯
𝑻
𝑺𝑘 = 𝑯 ∙ 𝑷−
𝑘 ∙ 𝑯𝑻 + 𝑹 𝑲𝑘 =
𝑯 ∙ 𝑷− 𝑻
𝑘 ∙𝑯 +𝑹

 For the State Update Equation

𝑿𝑘 = 𝑿− − − − ෨𝑋 −
𝑘 + 𝑲𝑘 ∙ 𝒀𝑘 = 𝑿𝑘 + 𝑲𝑘 𝒁𝑘 − 𝑯 ∙ 𝑿𝑘 = 𝑿𝑘 + 𝑲𝑘 𝜃𝑘 − 𝜃𝑘

 For the Covariance Update Equation

𝑷𝑘 = 𝑰2𝑥2 − 𝑲𝑘 ∙ 𝑯 𝑷𝑘− = 𝑰2𝑥2 − 𝑲𝑘 ∙ 𝑯 𝑷−

𝑘

20
 Depict initial filter parameters (Gyro std (from datasheet?) and Accel std (from meas?))

 Provide results of estimation with Ground Truth data

21
3.4

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.

23