From 653e9cebd7623972fd8bd4c69696e467585e4365 Mon Sep 17 00:00:00 2001
From: Ramasamy The sensor data obtained from the driving system (gyroscope) given by (Gx, Gy, Gz)
+represents the calibrated angular rotation rate of the device in the 3-axes. Since
+the calibrated gyroscope data provides the rate of change of angle in each axis,
+the integration of the data acquired in each axis provides the orientation measure
+of the device. During integration of the gyroscope data the noise present in each
+instance of the measured data gets added up resulting in measured orientation
+affected by drift. First the measured gyroscope data is converted to a quaternion
+equivalent Qdriv [14, 15]. The initial value for the Qdriv is assigned based on the
+computed Qaid initial value (15). The quaternion differential which denotes the
+increment in rotation is computed by quaternion based multiplication [7, 9] of Qdriv
+with gyroscope sensor data as shown in (16). The symbol ⊙ denotes quaternion
+based multiplication in the following equations. Equation (17) represents the integration of the quaternion difference value with
+the driving system quaternion to yield the measured quaternion value for that time
+instant [1]. The value 'dt' represents the sampling interval for the gyroscope sensor
+and 'Π' denotes the mathematical constant and denotes the ratio of a circle's
+circumference to its diameter and is approximately equal to 3.14159. The driving
+system quaternion is then normalized as shown in equation (18).3.3. Orientation Computation Based on Driving System
+
The orientation computed using the driving system sensor data is affected due to +the drift (from the integration of angular rates), the orientation computed using +the aiding system sensor data is not accurate (measurements are affected due to gravity). +To compensate for the deficiencies in the measurements of each system, the quaternion +orientation computations are combined using sensor fusion to obtain corrected/more +accurate device orientation data. The Quaternion error Qerr is computed in (19), as +described in [1].
+ +We introduce the equations (20) and (21) to compute the Euler error from the +quaternion error and then using this Euler error to compensate for drift on the driving +system orientation quaternion. First, the quaternion error Qerr is converted to the Euler +angle representation Eerr based on (20). The quaternion representation of orientation +error is converted to Euler angles representation using the function quat2euler() for +which information can be found in [9].
+ +The driving system quaternion is then compensated for the Euler angle error Eerr and +normalized as per (21) and (22) to correct for drift observed in the driving system sensor +data.
+ +The quaternion Qdriv that is derived in (22) is now compensated for drift and corrected +for dynamic bias as shown in (6). The dynamic bias correction used in (6) is determined +using Kalman filtering as explained below. Based on [1], linear Kalman filtering is used +based on the error state equation, given in (23). The first three elements of the error +state vector are the Eerr values on the 3-axes (Φerr, Θerr, Ψerr) and the +next three elements are the bias compensation (Bx, By, Bz) that is mentioned in (6).
+ +The variance in the gyroscope angular rate measurements denoted by Qwn over a windowed +period is computed in (24). The random driving process noise variance Qwb is computed in +(25), where the noise standard deviation Ïw and time constant Ïw are obtained from the +gyroscope sensor datasheet. Based on [1], the overall process noise covariance Q is +derived from the computed Qwn and Qwb as shown in (26).
+ +The variance in the aiding system orientation (roll, pitch and yaw) measurements are +used to compute the measurement noise covariance R [1] which is shown in (27). The aiding +system orientation in terms of Euler angles Eaid can be obtained by converting the +quaternion obtained in (14) to its equivalent Euler angles.
+ +The equations (28), (29) and (30) are used to determine the transformation matrix F, +apriori state vector error Δx- and prediction covariance P- based in [1].
+ -The accelerometer and magnetometer data are normalized based on equations (3) +
The accelerometer and magnetometer data are normalized based on equations (3) and (4) to obtain the calibrated accelerometer data (Ax, Ay, Az) and magnetometer -data (Mx, My, Mz).
+data (Mx, My, Mz).