2 <head><title>Sensor Fusion for Tizen Sensor Framework</title></head>
4 <h1><center>Sensor Fusion for Tizen Sensor Framework</center></h1>
6 <h2>1. Introduction</h2>
8 <p>Sensor Fusion is the process of combining the accelerometer,
9 gyroscope and geo-magnetic sensor in order to generate accurate virtual sensor
10 outputs such as Orientation, Gravity, Linear Acceleration, etc. Sensor Fusion
11 is used for extracting individual virtual sensor components from composite
12 sensor data and/or combining multiple sensor data to create new sensor component
13 data while compensating for individual sensor errors. Ideally the following
14 errors would have to be corrected during sensor fusion:-</p>
16 <p> - Bias: Any non-zero sensor
17 output when the input is zero</p>
19 <p> - Scale factor error: error
20 resulting from aging or manufacturing tolerances</p>
22 <p> - Nonlinearity: Present in
23 most sensors to some degree</p>
25 <p> - Scale factor sign asymmetry:
26 Often mismatched push-pull amplifiers</p>
28 <p> - Dead zone: usually due to
29 mechanical lock-in</p>
31 <p> - Quantization error: inherent
32 in all digitized systems</p>
34 <h2>2. Sensors Used for Sensor Fusion</h2>
36 <p>Accelerometer Sensor :- Accelerometer data is a combination of linear
37 acceleration and gravity components. Applications would be interested in using
38 linear acceleration and gravity sensor data separately. Sensor fusion could be
39 used to separate linear acceleration and gravity components. Additionally,
40 accelerometer is used for correcting the roll and pitch orientation measurements
41 generated from the gyroscope. Using the same, corrected tilt measurement (roll
42 and pitch) is generated. </p>
44 <p>Gyroscope Sensor :- It is ideal to determine the orientation of the device,
45 but has the problem of long term drift in the measured sensor values. Sensor
46 Fusion could be used to combine Accelerometer, Gyroscope and Geomagnetic (Compass)
47 sensor data to produce corrected orientation data without drift.</p>
49 <p>Geo-Magnetic Sensor :- Provides the direction the device is pointed in relation
50 to the earth's magnetic field. Could be used along with gyroscope angular rotation
51 along Z axis to produce correct yaw measurement. Geo-Magnetic sensor along with
52 GPS latitude-longitude measurements could be used to accurately estimate
53 heading of the device.</p>
55 <h2>3. Orientation Estimation</h2>
59 <img src="./diagram/device_orientation.png" width="30%" height="40%">
60 <FIGCAPTION>Fig. 1. Device Orientation in Euler Angles.</FIGCAPTION>
64 <p>The rotation of the device along the y-axis is considered as roll (Φ),
65 x-axis as pitch (Θ) and the z-axis as yaw (Ψ) as shown in Fig. 1. The
66 orientation of the device can be represented either in terms of Euler angles
67 (roll, pitch, yaw), or in the form of Rotation Matrix, or in the form of
68 Quaternions. These are different mathematical forms of representing the same
69 device orientation data. During orientation estimation all these representations
70 would be used based on requirements and for stability reasons. When the device is
71 lying flat on the x-y axis, the earth’s gravitational force is observed along the
72 device z-axis. The reference axis for the device considered in this paper is shown
73 in Fig. 1. The device reference x and y axis may change for each device, based on
74 the individual MEMS sensor chip reference axis (that can be obtained from the
75 datasheet) or the orientation of the sensor chip when it gets integrated into the
76 device. The equations related to the computation of orientation, gravity and
77 linear acceleration virtual sensors would have to be modified based on the change
78 in reference axes.</p>
82 <img src="./diagram/block_diagram_orientation_estimation.png" width="40%"
84 <FIGCAPTION>Fig. 2. Quaternion based Sensor Fusion Approach for Orientation
85 Estimation.</FIGCAPTION>
89 <p>The overall method for determining orientation of a device based on sensor
90 fusion is shown in Fig. 2. This paper proposes to improve on the existing approach
91 [1] to obtain more accurate orientation estimate. The Aiding System is used in sensor
92 fusion for computing an inaccurate value for device orientation based on the
93 accelerometer and the magnetometer sensor data. The accelerometer and magnetometer
94 sensor data is combined using the Triad algorithm explained in [3] to obtain an
95 inaccurate orientation measure. The problem with the orientation measured using the
96 aiding system is that the smaller orientation changes of the device are not detected
97 accurately. The orientation measured using the aiding system is not accurate due to
98 the effect of gravity on the sensor data. But the aiding system orientation measured
99 has the advantage that it is not affected by drift. The driving system is used for
100 computing the orientation of a device using the 3D angular rates measured by the
101 gyroscope sensor. The gyroscope sensor is accurate in detecting even small
102 orientation changes but the orientation derived from it are affected due to drift.
103 The drift in the measured gyroscope orientation is due to the integration of the
104 noise components present along with angular rates samples measured with the gyroscope.
107 <p>The Kalman filtering stage consists of two systems, the time update system where
108 the current instances of the state vector and prediction error covariance are
109 estimated based on the measurements obtained from the previous time instance. The
110 orientation data that is measured using the aiding system and the driving system are
111 fused during this stage. The second stage of the Kalman filtering process is the
112 measurement update system, where the estimated state and prediction error covariance
113 are corrected based on the Kalman gain factor that computed in this stage. The bias
114 that is estimated during this stage is used to correct the pre-processed gyroscope
115 sensor data that is given as input to the time update system.</p>
117 <h3>3.1. Preprocessing of Sensor Data</h3>
118 <p>The raw sensor data obtained from accelerometer (RAx, RAy, RAz), gyroscope
119 (RGx, RGy, RGz) and magnetometer (Magx, Magy, Magz) would have to pre-processed
120 before the sensor fusion process. The raw sensor data obtained from the
121 accelerometer and gyroscope sensors are affected by static bias, which are the
122 non-zero sensor data values observed when the device is void of any external forces.
123 These static bias components on the 3-axes (BAx, BAy, BAz) for accelerometer and
124 (BGx, BGy, BGz) for gyroscope are removed from the reported sensor values as shown in
125 (1) and (2). There is no static bias compensation for magnetometer data, as the
126 sensor measures the deviation of the x-axis relative to the earth's magnetic poles
127 and it is not possible to determine if the phone is deviating from exact north. The
128 'i' in the equations below specifies the current time instant, 'i-1' specifies the
129 previous time instant and a '0' specifies it as an initialization value.</p>
133 <img src="./equation/equation_1.png" width="35%" height="4%">
139 <img src="./equation/equation_2.png" width="35%" height="5%">
143 <p>The accelerometer and magnetometer data are normalized based on equations (3)
144 and (4) to obtain the calibrated accelerometer data (Ax, Ay, Az) and magnetometer
145 data (Mx, My, Mz).</p>
149 <img src="./equation/equation_3.png" width="35%" height="9%">
155 <img src="./equation/equation_4.png" width="35%" height="9%">
159 <p>The paper proposes that the gyroscope angular rates (on all 3 axes) before
160 being processed further are scaled to the range -1 to +1 (for accurate orientation
161 estimation) by dividing all data received by the maximum possible gyroscope
162 angular rate value apriori in each axis (5).</p>
166 <img src="./equation/equation_5.png" width="35%" height="8%">
170 <p>Based on [1], the dynamically variable Gyroscope Bias (Bx, By, Bz) is computed
171 using the Kalman filter and provided as feedback to the input and is subtracted
172 from scaled gyroscope data to obtain the corrected and pre-processed gyroscope
173 data (Gx, Gy, Gz) given in (6).</p>
177 <img src="./equation/equation_6.png" width="35%" height="4%">
182 <h3>3.2. Orientation Computation Based on Aiding System</h3>
184 <p>The device is placed on a flat surface as shown in the Fig. 1. The gravity
185 vector in the earth frame for the device (Ae) is given by (0, 0, ±1) since the
186 effect of gravity is observed on the z-axis reading of the Accelerometer. The sign
187 assigned is based on whether the z-axis of the accelerometer (when facing up) is
188 aligned towards gravity or against it. The magnetic field vector in the earth frame
189 for the device (Me) is given by (0, ±1, 0) since the magnetic north is detected on
190 the y-axis of the magnetometer. The sign assigned is based on whether the y-axis
191 of the magnetometer is aligned to earth’s magnetic field or against it. The gravity
192 vector in the device body frame (Ab) is given by (Ax, Ay, Az), which represents the
193 calibrated accelerometer sensor data. The magnetic field vector in the device body
194 frame (Mb) is given by (Mx, My, Mz) which represents the calibrated magnetometer
197 <p>The orientation of the device is computed based on the aiding system
198 (accelerometer + magnetometer) data and can be computed using the triad algorithm
199 [3]. The triad algorithm determines the orientation of the device based on the
200 gravity and magnetic field vectors obtained in the earth and device body frames.
201 The following equations represent the triad algorithm using which the orientation
202 of the device in the form of rotation matrix can be obtained. The symbol × denotes
203 the cross product of two vectors [8]. Combining the vectors obtained from (7) and
204 (9) along with the body frame gravity vector (Ab) the intermediate rotation matrix
205 is obtained for body frame MATb, as shown in (11). Combining the vectors obtained
206 from (8) and (10) along with the earth frame gravity vector (Ae) the intermediate
207 rotation matrix is obtained for earth frame MATe, as shown in (12).</p>
211 <img src="./equation/equation_7.png" width="35%" height="4%">
217 <img src="./equation/equation_8.png" width="35%" height="4%">
223 <img src="./equation/equation_9.png" width="35%" height="4%">
229 <img src="./equation/equation_10.png" width="35%" height="4%">
235 <img src="./equation/equation_11.png" width="35%" height="4%">
241 <img src="./equation/equation_12.png" width="35%" height="3.5%">
245 <p>Finally the rotation matrix of the device representing the orientation derived
246 from the aiding system RMaid is computed by multiplying the matrices MATb and MATe
247 as shown in (13).</p>
251 <img src="./equation/equation_13_updated.png" width="35%" height="5%">
255 <p>The device orientation computed in the form of aiding system rotation matrix is
256 then converted to quaternion representation Qaid (14) using the function
257 RotMat2Quat() which is explained in [6, 9]. Using quaternions to estimate
258 orientation overcomes the singularity issues faced when using Euler Angles and at
259 the same time they are computationally more efficient than using rotation matrix.</p>
263 <img src="./equation/equation_14.png" width="35%" height="4%">
267 <h3>3.3. Orientation Computation Based on Driving System</h3>
269 <p>The sensor data obtained from the driving system (gyroscope) given by (Gx, Gy, Gz)
270 represents the calibrated angular rotation rate of the device in the 3-axes. Since
271 the calibrated gyroscope data provides the rate of change of angle in each axis,
272 the integration of the data acquired in each axis provides the orientation measure
273 of the device. During integration of the gyroscope data the noise present in each
274 instance of the measured data gets added up resulting in measured orientation
275 affected by drift. First the measured gyroscope data is converted to a quaternion
276 equivalent Qdriv [14, 15]. The initial value for the Qdriv is assigned based on the
277 computed Qaid initial value (15). The quaternion differential which denotes the
278 increment in rotation is computed by quaternion based multiplication [7, 9] of Qdriv
279 with gyroscope sensor data as shown in (16). The symbol ⊙ denotes quaternion
280 based multiplication in the following equations.</p>
284 <img src="./equation/equation_15.png" width="35%" height="3.5%">
290 <img src="./equation/equation_16.png" width="35%" height="6%">
294 <p>Equation (17) represents the integration of the quaternion difference value with
295 the driving system quaternion to yield the measured quaternion value for that time
296 instant [1]. The value 'dt' represents the sampling interval for the gyroscope sensor
297 and 'Π' denotes the mathematical constant and denotes the ratio of a circle's
298 circumference to its diameter and is approximately equal to 3.14159. The driving
299 system quaternion is then normalized as shown in equation (18).</p>
303 <img src="./equation/equation_17.png" width="35%" height="4%">
309 <img src="./equation/equation_18.png" width="35%" height="6%">
313 <h3>3.4. Time Update System</h3>
315 <p>The orientation computed using the driving system sensor data is affected due to
316 the drift (from the integration of angular rates), the orientation computed using
317 the aiding system sensor data is not accurate (measurements are affected due to gravity).
318 To compensate for the deficiencies in the measurements of each system, the quaternion
319 orientation computations are combined using sensor fusion to obtain corrected/more
320 accurate device orientation data. The Quaternion error Qerr is computed in (19), as
321 described in [1].</p>
325 <img src="./equation/equation_19.png" width="35%" height="3.75%">
329 <p>We introduce the equations (20) and (21) to compute the Euler error from the
330 quaternion error and then using this Euler error to compensate for drift on the driving
331 system orientation quaternion. First, the quaternion error Qerr is converted to the Euler
332 angle representation Eerr based on (20). The quaternion representation of orientation
333 error is converted to Euler angles representation using the function quat2euler() for
334 which information can be found in [9].</p>
338 <img src="./equation/equation_20.png" width="35%" height="6%">
342 <p>The driving system quaternion is then compensated for the Euler angle error Eerr and
343 normalized as per (21) and (22) to correct for drift observed in the driving system sensor
348 <img src="./equation/equation_21.png" width="35%" height="3.75%">
354 <img src="./equation/equation_22.png" width="35%" height="6%">
358 <p>The quaternion Qdriv that is derived in (22) is now compensated for drift and corrected
359 for dynamic bias as shown in (6). The dynamic bias correction used in (6) is determined
360 using Kalman filtering as explained below. Based on [1], linear Kalman filtering is used
361 based on the error state equation, given in (23). The first three elements of the error
362 state vector are the Eerr values on the 3-axes (Φerr, Θerr, Ψerr) and the
363 next three elements are the bias compensation (Bx, By, Bz) that is mentioned in (6).</p>
367 <img src="./equation/equation_23.png" width="35%" height="3.75%">
371 <p>The variance in the gyroscope angular rate measurements denoted by Qwn over a windowed
372 period is computed in (24). The random driving process noise variance Qwb is computed in
373 (25), where the noise standard deviation σw and time constant τw are obtained from the
374 gyroscope sensor datasheet. Based on [1], the overall process noise covariance Q is
375 derived from the computed Qwn and Qwb as shown in (26).</p>
379 <img src="./equation/equation_24.png" width="35%" height="8%">
385 <img src="./equation/equation_25.png" width="35%" height="8%">
391 <img src="./equation/equation_26.png" width="35%" height="6%">
395 <p>The variance in the aiding system orientation (roll, pitch and yaw) measurements are
396 used to compute the measurement noise covariance R [1] which is shown in (27). The aiding
397 system orientation in terms of Euler angles Eaid can be obtained by converting the
398 quaternion obtained in (14) to its equivalent Euler angles.</p>
402 <img src="./equation/equation_27.png" width="35%" height="14%">
406 <p>The equations (28), (29) and (30) are used to determine the transformation matrix F,
407 apriori state vector error Δx- and prediction covariance P- based in [1].</p>
411 <img src="./equation/equation_28.png" width="35%" height="15%">
417 <img src="./equation/equation_29.png" width="35%" height="3.5%">
423 <img src="./equation/equation_30.png" width="35%" height="3.5%">
428 <h3>3.5. Measurement Update System</h3>
429 <p>The measurement update system is used to determine the bias value that would be
430 deducted from the Gyroscope values (6). The equations (31), (32) and (33) are used to
431 compute the Kalman gain K, aposteriori state error computation Δx+(i) and
432 aposteriori prediction covariance P+, as shown in [4]. In equation (33), I denotes the
433 identity matrix H denotes the measurement matrix (identity matrix is used here) and the
434 apriori prediction covariance estimate P- (33).</p>
438 <img src="./equation/equation_31.png" width="35%" height="4%">
444 <img src="./equation/equation_32.png" width="35%" height="4%">
450 <img src="./equation/equation_33.png" width="35%" height="4%">
454 <p>The bias compensation (Bx, By, Bz) obtained from Δx+(i) is used for removing
455 dynamic bias from the calibrated gyroscope values as shown in (6). The corrected
456 orientation that is determined using the above sensor fusion method, is obtained from
457 (22) by using the conversion function quat2euler [9] as shown in (34). This estimated
458 orientation is used in Section 3 to compute Gravity virtual sensor data.</p>
462 <img src="./equation/equation_34.png" width="35%" height="4%">
466 <h3>4. Determination of Gravity and Linear Acceleration</h3>
467 <p>When a device is subjected to motion in Euclidean space, the 3D accelerometer
468 data generated from the device is a combination of linear acceleration and gravity
469 components which are a measure of linear and rotational motion respectively.The
470 gravity measurements in 3D space are derived from the orientation (pitch, roll
471 and yaw) measurements that is output from the Kalman filter. The process to compute
472 gravity and linear acceleration is shown in figure below.
476 <img src="./diagram/block_diagram_gravity_and_linear_acceleration.png"
477 width="40%" height="40%">
478 <FIGCAPTION>Fig. 4. Computation of Gravity and Linear Acceleration.</FIGCAPTION>
484 <img src="./equation/equation_35.png" width="35%" height="4%">
490 <img src="./equation/equation_36.png" width="35%" height="4%">
496 <img src="./equation/equation_37.png" width="35%" height="4%">
500 <p>Gravity virtual sensor data provides the measure of the direction in which the
501 Earth's gravitational force is observed in the device frame of reference. The
502 orientation of the device decides the measure of the influence of Earth's gravitational
503 force on the 3-axes of the device. The following equations are used for projecting
504 the tilt(pitch, roll) of the device on the Earth's gravity axis to determine earth's
505 gravitational effect on the devices reference axis.</p>
509 <img src="./diagram/orientation_effect_on_gravity.png">
510 <FIGCAPTION>Fig. 5. Effect of Device Orientation on Gravity.</FIGCAPTION>
514 <p>When the device tilt values (pitch,roll) are changed from (0,0) to (0,Π/2),
515 phone is rotated around y-axis, the x-axis gets aligned to earth's gravitational field
516 after rotation instead of the z-axis. When this rotation is applied to the equations
517 given above, the values (GRx,GRy,GRz) are converted from (0,0,G) to (G,0,0) due to the
518 shift in the axis which experiences the gravitational field (G is measure of Earth's
521 <h2>Determination of Linear Acceleration</h2>
523 <p>Accurate linear acceleration data are calculated by subtracting gravity components
524 from the 3-axes calibrated accelerometer data as shown in (38), (39) and (40). As shown
525 in (38) the rotation of the device on y-axis (GRy) reflects on the accelerometer x-axis
526 sensor data (Ax). The linear acceleration measurement on x-axis (LAx) directly
527 corresponds to the accelerometer x-axis sensor data (Ax) meaning linear motion along
528 x-axis is directly measured on the accelerometer x-axis.</p>
533 <img src="./equation/equation_38.png" width="35%" height="4%">
539 <img src="./equation/equation_39.png" width="35%" height="4%">
545 <img src="./equation/equation_40.png" width="35%" height="4%">
551 <p>[1] Gebre-Egziabher, H., Rhayward, R. C. & Powell, J. D. Design of Multi-Sensor
552 Attitude Determination Systems. IEEE Transactions on AESS, 40(2), 627 - 649 (2004)</p>
554 <p>[2] Abyarjoo1, et al. Implementing a Sensor Fusion Algorithm for 3D Orientation
555 Detection with Inertial/Magnetic Sensors. The International Joint Conferences on CISSE
558 <p>[3] Marcard, T. V., Design and Implementation of an attitude estimation system to
559 control orthopedic components. Chalmers University. Master thesis published on the
560 university link http://publications.lib.chalmers.se/records/fulltext/125985.pdf(2010)</p>
562 <p>[4] Welch, G. & Bishop, G. An introduction to the Kalman Filter. SIGGRAPH 2001</p>
564 <p>[5] Grewal, M. S., et al. Global Positioning Systems, Inertial Navigation, and
565 Integration (John Wiley & Sons., 2001)</p>
567 <p>[6] Greenberg, M. J., Euclidean and non-Euclidean geometry: Development and History
570 <p>[7] Conway, J. H & Smith, D. A. On Quaternions and Octonions: Their Geometry,
571 Arithmetic, and Symmetry (Peters, 2003)</p>
573 <p>[8] Zill, D. G & Cullen, M. R. "Definition 7.4: Cross product of two vectors".
574 Advanced engineering mathematics (Jones & Bartlett Learning, 2006)</p>
576 <p>[9] NASA Mission Planning and Analysis Division. Euler Angles, Quaternions, and
577 Transformation Matrices. Document Link-
578 http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf. Last Accessed
581 <p>[10] Android Sensor Fusion Library. Source code link:
582 https://android.googlesource.com/platform/frameworks/native/+/b398927/services/sensorservice/.
583 Last Accessed - July 2014.</p>
585 <p>[11] GNU Octave High Level Interpreter. Software Link: http://www.gnu.org/software/octave/.
586 Last Accessed - July 2014.</p>
588 <p>[12] Tizen OS - https://www.tizen.org/. Last Accessed - July 2014.</p>
590 <p>[13] Marins, J. L., et al. An Extended Kalman Filter for Quaternion-Based Orientation
591 Estimation Using MARG Sensors. IEEE/RSJ International Conference on Intelligent Robots and
594 <p>[14] Favre, J., et al. Quaternion-based fusion of gyroscopes and accelerometers to improve
595 3D angle measurement. ELECTRONICS LETTERS, 25th May 2006, Vol. 42 No. 11</p>
597 <p>[15] Sabatini, A.M., Quaternion based attitude estimation algorithm applied to signals
598 from body-mounted gyroscopes. ELECTRONICS LETTERS, 13th May 2004, Vol. 40 No. 10</p>
projects / platform / core / system / sensord.git / blob