.. DO NOT EDIT.
.. THIS FILE WAS AUTOMATICALLY GENERATED BY SPHINX-GALLERY.
.. TO MAKE CHANGES, EDIT THE SOURCE PYTHON FILE:
.. "auto_examples/trajectory_reconstruction/mahony_orientation.py"
.. LINE NUMBERS ARE GIVEN BELOW.

.. only:: html

    .. note::
        :class: sphx-glr-download-link-note

        Click :ref:`here <sphx_glr_download_auto_examples_trajectory_reconstruction_mahony_orientation.py>`
        to download the full example code

.. rst-class:: sphx-glr-example-title

.. _sphx_glr_auto_examples_trajectory_reconstruction_mahony_orientation.py:


.. _trajectory_mahony:

Mahony and Madgwick Orientation Estimation
==========================================

This example compares :class:`~gaitmap.trajectory_reconstruction.MahonyAHRS` and
:class:`~gaitmap.trajectory_reconstruction.MadgwickAHRS` on the same single-sensor recording.
Both algorithms estimate orientation directly from IMU data, but they use different feedback mechanisms:
Mahony applies proportional-integral feedback, while Madgwick uses a gradient-descent correction step.

.. GENERATED FROM PYTHON SOURCE LINES 15-20

Getting input data
------------------

We use a short section of the left foot IMU example data.
The data is already aligned to the gaitmap sensor frame and contains accelerometer and gyroscope signals.

.. GENERATED FROM PYTHON SOURCE LINES 20-29

.. code-block:: default

    import matplotlib.pyplot as plt
    import pandas as pd

    from gaitmap.example_data import get_healthy_example_imu_data
    from gaitmap.trajectory_reconstruction import MadgwickAHRS, MahonyAHRS

    imu_data = get_healthy_example_imu_data()["left_sensor"].iloc[500:900]
    sampling_frequency_hz = 204.8








.. GENERATED FROM PYTHON SOURCE LINES 30-36

Configuring and running both algorithms
---------------------------------------

`kp` controls how strongly the instantaneous accelerometer based correction is applied in Mahony.
`ki` enables an accumulated bias correction term.
Madgwick uses `beta` to control the correction strength.

.. GENERATED FROM PYTHON SOURCE LINES 36-54

.. code-block:: default

    mahony = MahonyAHRS(kp=0.8, ki=0.02)
    madgwick = MadgwickAHRS(beta=0.2)

    mahony = mahony.estimate(imu_data, sampling_rate_hz=sampling_frequency_hz)
    madgwick = madgwick.estimate(imu_data, sampling_rate_hz=sampling_frequency_hz)

    mahony_orientation = mahony.orientation_
    madgwick_orientation = madgwick.orientation_
    mahony_rotated_data = mahony.rotated_data_
    madgwick_rotated_data = madgwick.rotated_data_

    orientation_comparison = pd.concat(
        [mahony_orientation.add_prefix("mahony_"), madgwick_orientation.add_prefix("madgwick_")], axis=1
    )
    rotated_data_comparison = pd.concat(
        [mahony_rotated_data.add_prefix("mahony_"), madgwick_rotated_data.add_prefix("madgwick_")], axis=1
    )








.. GENERATED FROM PYTHON SOURCE LINES 55-57

Inspecting the results
----------------------

.. GENERATED FROM PYTHON SOURCE LINES 57-59

.. code-block:: default

    orientation_comparison.tail()






.. raw:: html

    <div class="output_subarea output_html rendered_html output_result">
    <div>
    <style scoped>
        .dataframe tbody tr th:only-of-type {
            vertical-align: middle;
        }

        .dataframe tbody tr th {
            vertical-align: top;
        }

        .dataframe thead th {
            text-align: right;
        }
    </style>
    <table border="1" class="dataframe">
      <thead>
        <tr style="text-align: right;">
          <th></th>
          <th>mahony_q_x</th>
          <th>mahony_q_y</th>
          <th>mahony_q_z</th>
          <th>mahony_q_w</th>
          <th>madgwick_q_x</th>
          <th>madgwick_q_y</th>
          <th>madgwick_q_z</th>
          <th>madgwick_q_w</th>
        </tr>
        <tr>
          <th>sample</th>
          <th></th>
          <th></th>
          <th></th>
          <th></th>
          <th></th>
          <th></th>
          <th></th>
          <th></th>
        </tr>
      </thead>
      <tbody>
        <tr>
          <th>396</th>
          <td>0.034201</td>
          <td>-0.123389</td>
          <td>0.009840</td>
          <td>0.991720</td>
          <td>0.093027</td>
          <td>-0.106233</td>
          <td>0.008683</td>
          <td>0.989942</td>
        </tr>
        <tr>
          <th>397</th>
          <td>0.036344</td>
          <td>-0.117614</td>
          <td>0.008073</td>
          <td>0.992361</td>
          <td>0.094949</td>
          <td>-0.100070</td>
          <td>0.007148</td>
          <td>0.990414</td>
        </tr>
        <tr>
          <th>398</th>
          <td>0.038364</td>
          <td>-0.112578</td>
          <td>0.006401</td>
          <td>0.992881</td>
          <td>0.096649</td>
          <td>-0.094321</td>
          <td>0.005678</td>
          <td>0.990823</td>
        </tr>
        <tr>
          <th>399</th>
          <td>0.040413</td>
          <td>-0.107899</td>
          <td>0.004685</td>
          <td>0.993329</td>
          <td>0.098965</td>
          <td>-0.088883</td>
          <td>0.004106</td>
          <td>0.991105</td>
        </tr>
        <tr>
          <th>400</th>
          <td>0.042394</td>
          <td>-0.103143</td>
          <td>0.003031</td>
          <td>0.993758</td>
          <td>0.100951</td>
          <td>-0.083484</td>
          <td>0.002615</td>
          <td>0.991379</td>
        </tr>
      </tbody>
    </table>
    </div>
    </div>
    <br />
    <br />

.. GENERATED FROM PYTHON SOURCE LINES 60-61

Comparing the quaternion course can help to spot how strongly both filters react to the same movement.

.. GENERATED FROM PYTHON SOURCE LINES 61-68

.. code-block:: default

    orientation_comparison[["mahony_q_w", "madgwick_q_w"]].plot(figsize=(10, 4))
    plt.xlabel("sample")
    plt.ylabel("q_w [a.u.]")
    plt.title("Mahony vs. Madgwick quaternion estimate")
    plt.tight_layout()
    plt.show()




.. image-sg:: /auto_examples/trajectory_reconstruction/images/sphx_glr_mahony_orientation_001.png
   :alt: Mahony vs. Madgwick quaternion estimate
   :srcset: /auto_examples/trajectory_reconstruction/images/sphx_glr_mahony_orientation_001.png
   :class: sphx-glr-single-img





.. GENERATED FROM PYTHON SOURCE LINES 69-71

The rotated accelerometer data is often easier to interpret, because the sensor axes are aligned with the estimated
global frame.

.. GENERATED FROM PYTHON SOURCE LINES 71-77

.. code-block:: default

    rotated_data_comparison[["mahony_acc_z", "madgwick_acc_z"]].plot(figsize=(10, 4))
    plt.xlabel("time [s]")
    plt.ylabel("acceleration [m/s^2]")
    plt.title("Rotated vertical acceleration after orientation estimation")
    plt.tight_layout()
    plt.show()



.. image-sg:: /auto_examples/trajectory_reconstruction/images/sphx_glr_mahony_orientation_002.png
   :alt: Rotated vertical acceleration after orientation estimation
   :srcset: /auto_examples/trajectory_reconstruction/images/sphx_glr_mahony_orientation_002.png
   :class: sphx-glr-single-img






.. rst-class:: sphx-glr-timing

   **Total running time of the script:** ( 0 minutes  4.052 seconds)

**Estimated memory usage:**  19 MB


.. _sphx_glr_download_auto_examples_trajectory_reconstruction_mahony_orientation.py:

.. only:: html

  .. container:: sphx-glr-footer sphx-glr-footer-example


    .. container:: sphx-glr-download sphx-glr-download-python

      :download:`Download Python source code: mahony_orientation.py <mahony_orientation.py>`

    .. container:: sphx-glr-download sphx-glr-download-jupyter

      :download:`Download Jupyter notebook: mahony_orientation.ipynb <mahony_orientation.ipynb>`


.. only:: html

 .. rst-class:: sphx-glr-signature

    `Gallery generated by Sphinx-Gallery <https://sphinx-gallery.github.io>`_