How it works: states prediction
================================

.. toctree::
   :maxdepth: 3
   :caption: Contents:

.. raw:: html

     <style> .fonttitle {color:#4baae9; font-weight:bold; font-size:18px} </style>

.. role:: fonttitle

Prediction models
-------------------

In the following we will note :math:`\mathbf{q}` the quaternion discribing body orientation relative to local frame.

Attitude
~~~~~~~~

Ideal operation
""""""""""""""""""""

The attitude prediction is done by integrating gyrometer data at the highest frequency possible on the platform. Actually,
the frequency is limited to 500Hz to fit into low power embedded systems and means integration numerical errors.

quaternion derivative is computed as follow:

.. math::

    \dot{\mathbf{q}} = \frac{1}{2} \Omega(\omega)\mathbf{q}

    \\

    \Omega(\omega) = \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix}

- :math:`\mathbf{\omega}` is the gyrometer rotation speed vector in body frame.

The first order integration used is:

.. math::

    \mathbf{q}_{k+1} = \mathbf{q}_{k} +  \dot{\mathbf{q}}\Delta t


Quaternion is the normalized at each step to avoid numerical errors.

Bias impact
""""""""""""""""""""

As we are dealing with gyro biases and they are part of our state vector, we need to know the coupling between attitude errors
and our Angle-axis rotation error vector in body frame (noted :math:`\theta_{\mathbf{err}}`):

The relation between real quaternion from ground truth quaternion and estimated quaternion is:

.. math::

    \mathbf{q}_{\mathbf{real}} = \mathbf{q}_{\mathbf{est}} \otimes \mathbf{q}_{\mathbf{err}}

    \\

    \mathbf{q}_{\mathbf{err}} \approx \begin{bmatrix} 1 & 0.5 \cdot \theta_{\mathbf{err}} \end{bmatrix}

Predicting real quaternion with gyrometer data:

.. math::

    \Delta\mathbf{q}_{\mathbf{gyro}_{unbiased}} \approx \begin{bmatrix} 1 & 0.5 \cdot (\mathbf{\Omega} - \mathbf{b}) \cdot \Delta t \end{bmatrix}

    \\

    \mathbf{q}_{\mathbf{real}_{k+1}} = \mathbf{q}_{\mathbf{real}_{k}} \otimes \Delta\mathbf{q}_{\mathbf{gyro}_{unbiased}}


- :math:`\mathbf{\Omega}` is the gyrometer rotation speed vector in body frame.
- :math:`\mathbf{b}` is the gyrometer bias vector in body frame.

Our angle-axis rotation error vector can be obtained with:

.. math::

    \mathbf{q}_{\mathbf{err}} = \overline{\mathbf{q}}_{\mathbf{est}} \otimes \mathbf{q}_{\mathbf{real}}

    \\

    \mathbf{\theta}_{\mathbf{err}_{k+1}} \approx 2 \cdot \begin{bmatrix} \mathbf{q}_{\mathbf{err} (1:3)} \end{bmatrix}

After simplification and the fact that we are dealing with unit quaternion, we obtain:

.. admonition:: result

    .. math::

        \left(\begin{array}{c} \mathbf{\theta x}_{\mathbf{err}_{k+1}}\\ \mathbf{\theta y}_{\mathbf{err}_{k+1}}\\ \mathbf{\theta z}_{\mathbf{err}_{k+1}} \end{array}\right) = \left(\begin{array}{c} \mathrm{\mathbf{\theta x}_{\mathbf{err}_{k}}}-\Delta t\,b_{\mathbf{x}_{k}}+\Delta t\,w_x-\frac{\mathrm{\mathbf{\theta y}_{\mathbf{err}_{k}}}\,\Delta t\,b_{\mathbf{z}_{k}}}{2}+\frac{\mathrm{\mathbf{\theta z}_{\mathbf{err}_{k}}}\,\Delta t\,b_{\mathbf{y}_{k}}}{2}+\frac{\mathrm{\mathbf{\theta y}_{\mathbf{err}_{k}}}\,\Delta t\,w_z}{2}-\frac{\mathrm{\mathbf{\theta z}_{\mathbf{err}_{k}}}\,\Delta t\,w_y}{2}\\ \mathrm{\mathbf{\theta y}_{\mathbf{err}_{k}}}-\Delta t\,b_{\mathbf{y}_{k}}+\Delta t\,w_y+\frac{\mathrm{\mathbf{\theta x}_{\mathbf{err}_{k}}}\,\Delta t\,b_{\mathbf{z}_{k}}}{2}-\frac{\mathrm{\mathbf{\theta z}_{\mathbf{err}_{k}}}\,\Delta t\,b_{\mathbf{x}_{k}}}{2}-\frac{\mathrm{\mathbf{\theta x}_{\mathbf{err}_{k}}}\,\Delta t\,w_z}{2}+\frac{\mathrm{\mathbf{\theta z}_{\mathbf{err}_{k}}}\,\Delta t\,w_x}{2}\\ \mathrm{\mathbf{\theta z}_{\mathbf{err}_{k}}}-\Delta t\,b_{\mathbf{z}_{k}}+\Delta t\,w_z-\frac{\mathrm{\mathbf{\theta x}_{\mathbf{err}_{k}}}\,\Delta t\,b_{\mathbf{y}_{k}}}{2}+\frac{\mathrm{\mathbf{\theta y}_{\mathbf{err}_{k}}}\,\Delta t\,b_{\mathbf{x}_{k}}}{2}+\frac{\mathrm{\mathbf{\theta x}_{\mathbf{err}_{k}}}\,\Delta t\,w_y}{2}-\frac{\mathrm{\mathbf{\theta y}_{\mathbf{err}_{k}}}\,\Delta t\,w_x}{2} \end{array}\right)


Which is then linearized to obtain the jacobian matrix relative to the state vector:

.. math::

    X_k = \left(\begin{array}{c} \mathbf{\theta x}_{\mathbf{err}_{k}}\\ \mathbf{\theta y}_{\mathbf{err}_{k}}\\ \mathbf{\theta z}_{\mathbf{err}_{k}}\\ b_{\mathbf{x}_{k}}\\ b_{\mathbf{y}_{k}}\\ b_{\mathbf{z}_{k}} \end{array}\right)

We obtain:

.. admonition:: jacobian

    .. math::

        X(0:2)_{k+1} = \left(\begin{array}{cccccc} 1 & \frac{\Delta t\,w_z}{2}-\frac{\Delta t\,b_{\mathbf{z}_{k}}}{2} & \frac{\Delta t\,b_{\mathbf{y}_{k}}}{2}-\frac{\Delta t\,w_y}{2} & -\Delta t & \frac{\mathrm{TZ}\,\Delta t}{2} & -\frac{\mathbf{\theta y}_{\mathbf{err}_{k}}\,\Delta t}{2}\\ \frac{\Delta t\,b_{\mathbf{z}_{k}}}{2}-\frac{\Delta t\,w_z}{2} & 1 & \frac{\Delta t\,w_x}{2}-\frac{\Delta t\,b_{\mathbf{x}_{k}}}{2} & -\frac{\mathrm{TZ}\,\Delta t}{2} & -\Delta t & \frac{\mathbf{\theta x}_{\mathbf{err}_{k}}\,\Delta t}{2}\\ \frac{\Delta t\,w_y}{2}-\frac{\Delta t\,b_{\mathbf{y}_{k}}}{2} & \frac{\Delta t\,b_{\mathbf{x}_{k}}}{2}-\frac{\Delta t\,w_x}{2} & 1 & \frac{\mathbf{\theta y}_{\mathbf{err}_{k}}\,\Delta t}{2} & -\frac{\mathbf{\theta x}_{\mathbf{err}_{k}}\,\Delta t}{2} & -\Delta t \end{array}\right) \cdot X_{k}


    Bias prediction is just consider it does not change over time:

    .. math::

        X(3:5)_{k+1} = X(3:5)_{k}

Speed and altitude
~~~~~~~~~~~~~~~~~~

We use the corrected quaternion to project body acceleration to NED frame and substract gravity to obtain vertical NED acceleration.
This acceleration can be integrated into speed, and integrated again to obtain altitude. This implies a coupling between NED Z speed
and attitude quaternion errors.

If we reuse the real quaternion from ground truth which is estimated quaternion corrected:

.. math::

    \mathbf{q}_{\mathbf{real}} = \mathbf{q}_{\mathbf{est}} \otimes \mathbf{q}_{\mathbf{err}}

Accelerometer measures projected in NED frame is:

.. math::

    \begin{bmatrix} 1 & a_{\mathbf{NED}} \end{bmatrix} = \mathbf{q}_{\mathbf{real}} \otimes \begin{bmatrix} 0 & a_x & a_y & a_z \end{bmatrix} \otimes \overline{\mathbf{q}}_{\mathbf{real}}

    \\

    V_{\mathbf{NED}_{k+1}} = V_{\mathbf{NED}_{k}} + \Delta t \cdot a_{NED} + \Delta t \cdot \mathbf{g}

    \\

    \mathbf{d}_{\mathbf{z}_{k+1}} = \mathbf{d}_{\mathbf{z}_{k}} + \Delta t \cdot V_{\mathbf{NED}_{k}}


These expressions are then used to obtain the jacobian matrix relative to the state vector

.. math::

    X_k = \left(\begin{array}{c} \mathbf{\theta x}_{\mathbf{err}_{k}}\\ \mathbf{\theta y}_{\mathbf{err}_{k}}\\ \mathbf{\theta z}_{\mathbf{err}_{k}}\\ b_{\mathbf{x}_{k}}\\ b_{\mathbf{y}_{k}}\\ b_{\mathbf{z}_{k}}\\ V_{\mathbf{NED}_{k}}\\ D_{\mathbf{NED}_{k}} \end{array}\right)

The coupling between NED Z speed and attitude errors means that vertical sensors corrections will impact attitude estimation.