## Orientation Filters for Low-Power Motion Tracking Devices

The full text of this paper can be downloaded here

The orientation estimation filter proposed by Madgwick [1] for inertial and inertial/magnetic sensors have been successfully used as a core solution for more advanced orientation filters in a variety of commercial low-power motion tracking devices [2]. The core orientation filter by Madgwick has a very simple structure and it offers a high accuracy and reduces the computational cost compared to the state-of-the-art Kalman-based methods [3], which could be about 20 times slower compared to the Madgwick filter even on a powerful microcontroller, e.g., ARM Cortex M4 [3]. Several adaptive variations of the Madgwick filter also exist in the literature [4].

We have proposed a modification to the Gradient Descent correction step in Madgwick's core filter, which guarantees the improvement of accuracy in quaternion estimation without increasing the total number of required scalar arithmetic operations. The proposed solution, which is an Orientation Model for Inertial Devices (OMID), is presented in a research paper and has been accepted for publication at the peer-reviewed International Symposium on Circuits and Systems (ISCAS) conference in 2016 [5]. The paper presents the implementation of the OMID filter for both the 6-axis IMU case (accelerometers and gyros), and the 9-axis MARG sensor array, which includes magnetometers as well. It has been addressed in the paper that the OMID filter does not require any additional arithmetic operations compared to the Madgwick filter [1]. However, the proposed filter can majorly improve accuracy, since it utilizes a gradient descent correction step, which involves all sensor readings at time \(t\). Monte Carlo simulations have been presented in [5] using typical statistical characteristics for sensor errors. The results have shown an average 37.2 percent improvement in dynamic root-mean-square error of Euler angles. All variations applicable to the Madwgick filter in [1] are applicable to the OMID filter as well, which is simply a better alternative to the core Madgwick filter.

Due to the lack of space in the research article [5], it does not address the analytical comparison of accuracy between OMID and Madgwick filters. Here, we elaborate on this matter showing how the OMID filter improves the overall accuracy in terms of both mean-square and worst-case absolute error.

### OMID Filter (6-axis IMU)

Let us revisit [5] to summarize how the OMID filter algorithm works for the 6-axis IMU case. Assuming that the 3-axis accelerometer and gyroscope readings have initially passed the necessary calibration and filtering techniques, we denote the sensor readings as follows:

Accelerometer readings at time \(t\): \( S_{a,t} = (a_x,a_y,a_z) \)

Gyroscope readings at time \(t\) in quaternion format: \( S_{g,t} = [0,g_x,g_y,g_z] \)

The IMU version of the OMID filter follows these calculations:

Step 1: Find the gyros quaternion estimate at time \(t\):

\begin{equation}
q_{g,t} = q_{est,t-1} + (\Delta t/2)(q_{est,t-1}\otimes S_{g,t})

\end{equation}

where \(q_{est,t-1}\) is the previous quaternion estimate at time \(t-1\), \(\Delta t\) is the sampling period, and \(\otimes\) is the Hamilton product.

Step 2: Use a gradient descent correction to push \(q_{g,t}\) from Step 1 towards the accelerometer readings \(S_{a,t}\) i.e., compute:

\[\nabla F(q_{g,t},S_{a,t}) = J_{F_g}(q_{g,t})^T F_g(q_{g,t},S_{a,t})\]

where \(J_{F_g}(q_{g,t})\) is the Jacobian of \(F_g\), which is a mismatch function representing the error between the initial quaternion estimate \(q\) and the accelerometer readings \(S_{a,t}\). For the IMU filter, we can define \( F_g(q_{g,t},S_{a,t}) \) as follows:

\[ F_g(q_{g,t},S_{a,t}) = q_{g,t}^*\otimes g \otimes q_{g,t} - S_{a,t}\]
where \(g = [0,0,0,1]\) is a quaternion representing the reference gravity vector in the Earth frame.

If we replace the initial estimate \(q = q_{g,t}\) in this step with \(q_{est,t-1}\), the gradient descent calculations become identical to the IMU filter presented by Madgwick [1]. This minor change can majorly improve the overall accuracy of the filter. The reason is that by using \(q_{g,t}\) as the initial guess, the gradient descent solution will return a correction vector that directly represents the mismatch between the gyro's estimate at time \(t\) and the accelerometer readings at time \(t\). This correction vector when combined with the rest of calculations for orientation estimation will deliver a much higher overall accuracy, which will be studied in details at the end of this article.

Step 3: Apply integration with a gain \(\beta\) to find the new quaternion estimate at time \(t\):

\[ q_{est,t} = q_{g,t} - (\beta\Delta t)\nabla F \]
It is notable that in Madgwick's solution \(\nabla F\) should be normalized as well, i.e., \(\nabla F/||F||\), which is not required in our filter. Hence, the proposed filter uses even fewer scalar arithmetic operations.

Step 4: Finally normalize the output \(q_{est,t}\) to return a unit-length quaternion estimate at time \(t\), i.e., \(q_{est,t}/||q_{est,t}||\).

### OMID & Madgwick Filters Error Analysis

In this section we compare the accuracy of the two filters. Let us first calculate the error present in the gyros' quaternion estimate in Eq. (1), which is utilized by both filters.

We will denote the error present in the previous quaternion estimate at time \(t-1\), i.e., \(q_{est,t-1}\), as \(e_{est,t-1}\), which means that we have:

\[q_{r,t-1} = q_{est,t-1} - e_{est,t-1}\]
where \(q_{r,t-1}\) is the actual reference quaternion orientation at time \(t-1\). We also denote the error present in the gyro readings at time \(t\), i.e., \(S_{g,t}\), as \(S_{e,t}\), while the actual reference angular velocities at time \(t\) are denoted as \(S_{r,t}\). In other words we have:

\[ S_{g,t} = S_{r,t} + S_{e,t} \]

Next, we can re-write Eq. (1) as follows:

\[q_{g,t} = (q_{r,t-1} + e_{est,t-1}) + (\Delta t/2)((q_{r,t-1} + e_{est,t-1})\otimes(S_{r,t} + S_{e,t}))\]

Further calculation gives:

\[q_{g,t} = (q_{r,t-1} + (\Delta t/2)q_{r,t-1}\otimes S_{r,t}) + e_{est,t-1} + (\Delta t/2)q_{r,t-1}\otimes S_{e,t} + \]
\begin{equation}
(\Delta t/2)e_{est,t-1}\otimes( S_{r,t} + S_{e,t}) = q_{r,t} + e_{est,t-1} + e_{g,t}
\end{equation}

where \(e_{g,t}\) is the error term originating from the current gyro readings at time \(t\) and the previous quaternion estimate at time \(t-1\), and is given below:

\[e_{g,t} = (\Delta t/2)q_{r,t-1}\otimes S_{e,t} + (\Delta t/2)e_{est,t-1}\otimes( S_{r,t} + S_{e,t})\]

Let us now evaluate the gradient descent step. This method aims to push an initial estimate quaternion \(q_{init}\) towards another quaternion estimate \(q_{am,t}\), which will be the best fit for the accelerometer readings at time \(t\), i.e., \(S_{a,t}\), in the 6-axis IMU case, or the combined accelerometer/magnetometer readings at time \(t\), i.e., \(S_{am,t}\), in the 9-axis mode. We consider the 9-axis mode from this point on for generality.

We can represent the correction vector generated by the gradient descent as follows:

\[ q_{\nabla,t} \approx \alpha _t(q_{init}-q_{am,t}) \]
where \(0<\alpha _t<1\) is an arbitrary gain depending on how fast the gradient descent step is pushing its initial guess \(q_{init}\) towards \(q_{am,t}\). From the above equation we get:

\[ q_{\nabla,t} = \alpha _t(q_{init}-q_{am,t}) + e_{\nabla,t} \]
where \(e_{\nabla,t}\) is the residual error corresponding to the gradient descent approximation itself. We can also replace \(q_{am,t}\) with \(q_{r,t} + e_{am,t}\), where \(q_{r,t}\) is the reference (actual) quaternion at time \(t\), and \(e_{am,t}\) is the error term generated by the inaccuracy in accelerometer/magnetometer readings at time \(t\). Hence, we get:

\[ q_{\nabla,t} = \alpha _t(q_{init} - q_{r,t} - e_{am,t}) + e_{\nabla,t} \]
We denote the quaternion correction vector \(q_{\nabla,t}\) for Madgwick filter as \(q^M_{\nabla,t}\), while in the case of the OMID filter, we denote it as \(q^O_{\nabla,t}\). The difference between the two filters is that Madwgick uses the orientation estimate at time \(t-1\), i.e., \(q_{est,t-1}\), for the initial guess \(q_{init}\) in the gradient descent step, while we use the gyros' quaternion estimate at time \(t\), i.e., \(q_{g,t}\), for the initial guess \(q_{init}\). Therefore, we can re-write the quaternion correction vectors for the two filters separately, as follows:

\[ q^O_{\nabla,t} = \alpha _t(q_{g,t} - q_{r,t} - e_{am,t}) + e_{\nabla,t} \]
\[ q^M_{\nabla,t} = \alpha _t(q_{est,t-1} - q_{r,t} - e_{am,t}) + e_{\nabla,t} \]

The final integration and gain applied to the two filters delivers the following outputs for the OMID and Madgwick filters, respectively:

\begin{equation}
q^O_{est,t} = q_{g,t} - (\beta\Delta t) ( \alpha _t(q_{g,t} - q_{r,t} - e_{am,t}) + e_{\nabla,t} )

\end{equation}

\[ q^M_{est,t} = q_{g,t} - (\beta\Delta t) ( \alpha _t(q_{est,t-1} - q_{r,t} - e_{am,t}) + e_{\nabla,t} ) \] \begin{equation} = q_{g,t} - (\beta\Delta t) ( \alpha _t(q_{r,t-1} + e_{est,t-1} - q_{r,t} - e_{am,t}) + e_{\nabla,t} ) \end{equation}

Now let us re-write Eq. (3) and Eq. (4) based on Eq. (2) to get:

#### OMID Filter Output

\[ q^O_{est,t} = q_{r,t} + e_{est,t-1} + e_{g,t} - (\beta\Delta t)( \alpha _t(q_{r,t} + e_{est,t-1} + e_{g,t} - q_{r,t} - e_{am,t}) + e_{\nabla,t} ) \] \begin{equation} = q_{r,t} + (1-\beta\Delta t\alpha _t)e_{est,t-1} + (1-\beta\Delta t\alpha _t)e_{g,t} + \beta\Delta t(\alpha _t e_{am,t} - e_{\nabla,t}) \end{equation}

#### Madgwick Filter Output

\[ q^M_{est,t} = q_{r,t} + e_{est,t-1} + e_{g,t} - (\beta\Delta t)( \alpha _t(q_{r,t-1} + e_{est,t-1} - q_{r,t} - e_{am,t}) + e_{\nabla,t} ) \] \begin{equation} = q_{r,t} + (1-\beta\Delta t\alpha _t)e_{est,t-1} + e_{g,t} + \beta\Delta t\alpha _t(q_{r,t} - q_{r,t-1}) + \beta\Delta t(\alpha _t e_{am,t} - e_{\nabla,t}) \end{equation}

The above two equations elaborate on all the error terms present in both filters' outputs. The error by Madgwick is a function of the previous error \(e_{est,t-1}\) and the gyro readings in \(e_{g,t}\), the error generated by accelerometer/magnetometer readings and their quaternion approximation, i.e., \(e_{am,t}\), the residual error using a single-step gradient descent step, i.e., \(e_{\nabla,t}\), as well as the orientation change, i.e., \((q_{r,t}-q_{r,t-1})\). Choosing a higher filter gain \(\beta\) for Madgwick reduces the absolute error originating from the previous estimate, i.e., \(|e_{est,t-1}|\), but will increase the effect of the absolute error generated by \(|q_{r,t}-q_{r,t-1}|\), \(|e_{am,t}|\), and \(|e_{\nabla,t}|\) as shown by Eq. (6).

By comparing Eq. (5) and Eq. (6), we observe that the error function is simplified in Eq. (5), i.e., the proposed filter. First, the error term corresponding to orientation change, i.e., \((q_{r,t}-q_{r,t-1})\), does not exist in Eq. (5). Secondly, the error term \(e_{g,t}\), which originates from the inaccuracy of gyro readings and the previous estimate \(q_{est,t-1}\), is attenuated to \((1-\beta\Delta t\alpha _t)e_{g,t}\) in Eq. (5), unlike Eq. (6).

Consequently, the proposed filter reduces the terms present in the overall error and it improves the accuracy in terms of both mean-square-error and worst-case absolute error. The improvement is magnified when \(|e_{g,t}|\) is increased, e.g., when gyros are showing lower precision. Furthermore, as the rate of orientation change, i.e., \(|q_{r,t}-q_{r,t-1}|\), increases, Eq. (5) becomes capable of delivering higher precision. This means that the OMID filter becomes even more efficient under quick motions. Monte Carlo simulations presented in our research article [5] confirm the analysis here as well.

Note that the optimal gain \(\beta\) for our filter is typically higher than the optimal \(\beta\) for Madgwick's filter assuming identical sensor characteristics for both filters. The reason is that choosing a higher gain will reduce the error term \((1-\beta\Delta t\alpha _t)|e_{g,t}|\) in Eq. (5). This is not the case in Eq. (6). Furthermore, choosing a higher gain for Madgwick's filter will result in an increase in the error term \(\beta\Delta t\alpha _t|q_{r,t} - q_{r,t-1}|\) in Eq. (6), but its corresponding negative impact is not observed by our filter. The experiments in [5] to find optimal gain values for the two filters also confirm this analysis.

### Final Remarks

The Madgwick filter has been used as the core algorithm for more advanced orientation filters as well as many state-of-the-art low-power motion tracking devices. Consequently, the improved more accurate version of the filter, i.e., OMID, which does not require any additional scalar arithmetic operations, can be helpful in a variety of applications. All variations applicable to the Madgwick filter, e.g., adaptive filter gains, can simply be applied to the OMID filter as well to achieve higher accuracy. Our Neblina module benefits from an extended adaptive version of the OMID filter to compute stable orientation quaternion with high accuracy and low computational complexity.

### References

[1] S. O. H. Madgwick, A. J. L. Harrison, R. Vaidyanathan, “Estimation of IMU and MARG orientation using a gradient descent algorithm”, IEEE ICORR, June 2011, pp. 1-7.

[2] Maxim Integrated, "MAX21100, Low-Power, Ultra-Accurate 6+3 DoF IMU, the Industry's Smallest and Thinnest 6DoF IMU with Stable and Accurate Gyroscope and Reliable Accelerometer", 2014: http://www.maximintegrated.com/en/products/analog/sensors-and-sensor-interface/MAX21100.html.

[3] A. Cavallo, A. Cirillo, P. Cirillo, G. De Maria, P. Falco, C. Natale, S. Pirozzi, “Experimental Comparison of Sensor Fusion Algorithms for Attitude Estimation”, World Congress the International Federation of Automatic Control, Aug. 2014, pp. 7585-7591.

[4] Ya Tian; Hamel, W.R., J. Tan "Accurate human navigation using wearable monocular visual and inertial sensors", IEEE Trans. On Instrumentation and Measurement, 63-1, pp. 203-213, Jan. 2014.

[5] O. Sarbishei, "On the Accuracy Improvement of Low-Power Orientation Filters Using IMU and MARG Sensor Arrays", accepted in IEEE Symposium on Circuits and Systems (ISCAS), May 2016.

Motsai designs and builds innovative embedded systems, and is the leading Canadian developer of miniature low-power, wireless devices used in wearables and human motion analysis. Please feel free to send us an email at info@motsai.com or drop us a line at +1-888 -849-6956. Don't forget to signup for blog updates below.