Attitude Estimation Using a Quaternion-Based Kalman Filter With Adaptive and Norm-Based Estimation of External Acceleration

This project aimed to estimate the attitude of a vehicle using measurements from onboard sensors. The sensors used were inertial sensors (gyroscope and accelerometer) and a magnetometer. The project utilized quaternion algebra and an indirect Kalman filter to estimate the vehicle's orientation. To estimate external acceleration, two methods were employed: norm-based estimation [1] and adaptive estimation [2]. The filter was tested using simulated and real data to validate its effectiveness.

Prerequisite • Use • Scripts • Functions Library • Presentation • Indirect Kalman Filter

References

1. Y. S. Suh, "Orientation Estimation Using a Quaternion-Based Indirect Kalman Filter With Adaptive Estimation of External Acceleration," in IEEE Transactions on Instrumentation and Measurement, vol. 59, no. 12, pp. 3296-3305, Dec. 2010, doi:10.1109/TIM.2010.2047157
2. A. Nez, L. Fradet, F. Marin, T. Monnet and P. Lacouture, "Identification of Noise Covariance Matrices to Improve Orientation Estimation by Kalman Filter." Sensors 2018, 18, 3490, doi:10.3390/s18103490
3. N. Trawny and S.I. Roumeliotis, "Indirect Kalman filter for 3D attitude estimation." In Technical Report; Department of Computer Science and Engineering, University of Minnesota: Minneapolis, MN, USA, 2005, Google Scholar
4. A. M. Sabatini, "Quaternion-based extended Kalman filter for determining orientation by inertial and magnetic sensing," in IEEE Transactions on Biomedical Engineering, vol. 53, no. 7, pp. 1346-1356, July 2006, doi:10.1109/TBME.2006.875664
5. R. Schneider and C. Georgakis, "How To NOT Make the Extended Kalman Filter Fail". Industrial & Engineering Chemistry Research 2013 52 (9), 3354-3362, doi:10.1021/ie300415d

How to Use

1. Add this repository to your MATLAB path (replace /path/to/this/repository with the actual path to the directory where the repository is located on your computer):

   addpath('/path/to/this/repository')

2. Add your flight data to the data folder
3. If desired, edit the scripts in the scripts folder according to your needs as detailed here
4. Run the main.m script
5. If the 'saving_flag' is turned on in the plot_results.m script, figures will be saved in the img folder

Folders

This repository contains four folders:

• data: Contains real data acquired from sensors and the magnetic field parameters used.
• functions: Contains a collection of low-level, useful MATLAB functions utilized by the scripts in the 'scripts' folder.
• img: Contains any figures saved if the 'saving_flag' is turned on.
• scripts: Contains MATLAB scripts that use the functions in the 'functions' folder.

Data

The data folder should contain flight data collected from real sensors (gyroscope, accelerometer, magnetometer, and inclinometer), as well as magnetic field parameters. The magnetic field parameters can be generated using the most recent World Magnetic Model (WMM) from 2019-2024, which can be accessed through the following link: https://www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml.

The images presented in the presentation were generated using magnetic field parameters calculated for a specific date (February 13th, 2020) and location (43°52'52"N, 10°14'6"E, Viareggio, IT) where real data was collected.

Images

The img folder contains the images generated and saved by the plot_results.m script.

Scripts

The scripts folder contains all the scripts used. Possible editing, such as switching between synthetic and real data, should be done by modifying the code itself. All scripts have extensive comments in the code.

Notation used:

• prev = k - 1 (or k, respectively)
• next = k (or k + 1, respectively)

e.g., q_next_prev denotes q(k|k-1) (or q(k+1|k), respectively)

1. generate_synthetic_data.m
   This script generates synthetic data.
   

   • Please select one of the following rate_mode:
     • null: null rate
     • const: constant rate
     • roll: zero(0-50 s) - phi=phi+90° - zero (150-200 s)
     • pitch: zero(0-50 s) - theta=theta+45° - zero (150-200 s)
     • yaw: zero(0-50 s) - psi=psi+90° - zero (150-200 s)
     • mult_const: miscellaneous (multiple) constant rates
     • mult_ramp: miscellaneous (multiple), fast, ramp-like rates
   • Choose either ON or OFF (case insensitive, by the way):
     • noise_gyro: If ON, it adds noise to gyroscope measurements
     • noise_acc: If ON, it adds noise to accelerometer measurements
     • noise_mag: If ON, it adds noise to magnetometer measurements
     • bias_gyro: If ON, it adds bias to gyroscope measurements
     • bias_acc: If ON, it adds bias to accelerometer measurements
     • ext_acc: If ON, it creates 3 external accelerations:
       • $[10 m/s², 5 m/s², 20 m/s²]^T$ from $80 s$ to $81 s$
       • $[0 m/s², -7 m/s², 0 m/s²]^T$ from $120 s$ to $122 s$
       • $[-4 m/s², -3 m/s², 8 m/s²]^T$ from $140 s$ to $140.5 s$

2. import_real_data.m
   This script loads real data from the data folder (the specific folder to be used is determined within the code.).
   

   • Please select one of the following interpolation modes:
     • 1: interpolates samples (using Matlab function interp1(nearest))
     • 2: linearly interpolates samples (using lin_interpolate, located in the functions folder); it is a time-consuming task
     • 3: loads previously interpolated data otherwise do nothing

   Make sure to adjust the paths according to your real data folder location.

3. kalmanCorrect_acc.m
   This script adjusts the projected estimate by an actual accelerometer measurement at that time.
   

   • Please select one of the following Q_hat_a_b__algorithm:
     • suh: uses Suh's algorithm [1]
     • sabatini: uses Sabatini's algorithm [2]
     • zeros: uses a (3 x 3) zeros matrix

4. kalmanCorrect_mag.m
   This script adjusts the projected estimate by an actual magnetometer measurement at that time.

5. kalmanPredict.m
   This script projects the current state estimate ahead in time by two actual gyroscope measurements: at that time and the previous one.

6. main.m
   This is the main script that should be executed on Matlab.
   

   • Please select one of the following sensor_data types:
     • synthetic: Computer-generated sequence of angular velocity, acceleration, and magnetic field. See generate_synthetic_data.m for further details.
     • real: Load sequence of angular velocity, acceleration, magnetic field, and attitude measured by real sensors (gyroscope, accelerometer, magnetometer, and inclinometer, respectively). See import_real_data.m for further details.

7. plot_results.m
   This script has the capability to generate and, if desired, save all figures in the desired format (.png, .pdf, etc.). Users can customize the settings by selecting from the following options:

   • saving_flag: set this to 1 to save the generated plots in the desired format (specified in image_extension), and set it to any other value to disable saving.
   • title_flag: set this to 1 to enable adding titles to the generated plots, and set it to any other value to disable adding titles.
   • plot_detected_ext_acc: set this to 1 to display detected external acceleration instant dots on the acceleration plot, and set it to any other value to disable displaying detected external acceleration instant dots.
   • image_extension: specify the format for saving the images. This can include file types such as .png, .pdf, and so on.
   • images_path: set this variable to the folder path where you want to save the generated images (e.g., '/Users/a_user_name/Documents/MATLAB/quaternion-kalman-filter/images'). Modify the path as needed.
   • main_path: set this variable to the folder path where the source code is located (e.g., '/Users/a_user_name/Documents/MATLAB/quaternion-kalman-filter'). Modify the path as needed.

Functions

The functions folder includes a set of MATLAB functions library used by the scripts in the scripts folder. A list of the functions contained in the folder and their descriptions can be found below.

1. computeAccMode

   function acceleration_mode = computeAccMode(lambda, mu)
   

   [Eq. pre-34 Suh]
   Computes Shu's acceleration mode, which is capable of detecting external acceleration (i.e., acceleration caused by forces other than gravity).

   • INPUT:
     • lambda       The 3 eigenvalues of the matrix U, at times k, k-1, k-2       (3 x (M_2+1)) matrix
     • mu       Defined after [Eq. 32 Suh]       (3 x (M_2+1)) matrix
   • OUTPUT:
     • acceleration_mode       Possible values:
       • '1' (i.e., Mode 1 meaning 'No external acceleration Mode')
       • '2' (i.e., Mode 2 meaning 'External acceleration Mode')

2. create_extAcc

   function a_b = create_extAcc(a_b_OLD, t_i, ext_acc, length)
   

   Generates constant external acceleration ext_acc from t_i to t_i + length, overwriting previous external acceleration a_b_OLD.

   • INPUT:
     • a_b_OLD       Previous external acceleration; it'll be overwritten       (3 x N_samples) vector       [m / s^2]
     • t_i       Ext. acc. starting (initial) time       scalar       [s]
     • ext_acc       Ext. acc. constant value       (3 x 1) vector       [m / s^2]
     • length       Ext. acc. duration       scalar       [s]
   • OUTPUT:
     • a_b       External acceleration a_b(t)       (3 x N_samples) vector [m / s^2]

3. estimateExtAccCov_Sab

   function Q_hat_a_b = estimateExtAccCov_Sab(y_a)
   

   [Eq. 37 Suh]
   Implements the accelerometer norm-based adaptive algorithm by A. M. Sabatini [1] for estimating external acceleration covariance matrix Q__a_b.

   • INPUT:
     • y_a       Measured acceleration       (3 x 1) vector       [m/s^2]
   • OUTPUT:
     • Q_hat_a_b       Estimated external acceleration covariance       (3 x 3) matrix       [-]

4. estimateExtAccCov_Suh

   function [Q_hat_a_b, lambda, mu] = estimateExtAccCov_Suh(r_a, lambda, mu, H_a, P__next_prev, R_a)
   

   [Eq. 34 - 35 Suh]
   Implements the adaptive estimation algorithm by Y. S. Suh [2] for estimating external acceleration covariance matrix Q__a_b.

   • INPUT:
     • r_a       Residual in the accelerometer measurement update       (3 x M_1) vector       [m / s^2]
     • lambda       Threshold between first and second condition       (3 x (M_2+1)) matrix
     • mu       Defined after [Eq. 32 Suh]       (3 x (M_2+1)) matrix
     • H_a       Accelerometer measurement matrix       (3 x 9) matrix
     • P__next_prev       State covariance matrix       (9 x 9) matrix
     • R_a       Covariance measurement matrix of the accelerometer       (3 x 3) matrix
   • OUTPUT:
     • Q_hat_a_b       Estimated external acceleration covariance       (3 x 3) matrix
     • lambda       (newly computed) lambda       (3 x (M_2+1)) matrix
     • mu       (newly computed) mu       (3 x (M_2+1)) matrix

5. euler2quat

   function q = euler2quat(angles)
   

   [https://marc-b-reynolds.github.io/math/2017/04/18/TaitEuler.html#mjx-eqn%3Aeq%3Atait, Eq. 2 - 3 - 4 - 5]
   Converts Euler angles to quaternion.

   • INPUT:
     • angles       Euler angles (angles = [roll; pitch; yaw])       (3 x 1) vector       [rad]
   • OUTPUT:
     • q       Quaternion (q = [q0; q1; q2; q3], where the first element, q0, is the scalar part aka real part)       (4 x 1) vector       [-]

6. euler2RotMat

   function C = euler2RotMat(angles)
   

   Converts Euler angles to rotation matrix (aka Direction Cosine Matrix, DCM). The Euler angles rotate the frame n (navigation) to the frame b (body) according to the 'zyx' sequence.

   • INPUT:
     • angles       Euler angles (angles = [roll; pitch; yaw])       (3 x 1) vector       [rad]
   • OUTPUT:
     • C       Coordinate transformation matrix from n to b (3 x 3) matrix       [-] N.B.: That is, v_b = C * v_n. (_b or _n are the frames in which the vector v can be expressed, representing the body and navigation frames, respectively.)

7. euler_angle_range_three_axis

   function a = euler_angle_range_three_axis(angles)
   

   Limits Euler angles range. For three-axis rotation, the angle ranges are [-pi, pi], [-pi/2, pi/2] and [-pi, pi]. For two-axis rotation, the angle ranges are [-pi, pi], [0, pi] and [-pi, pi].

   • INPUT:
     • angles       Euler angles       (3 x 1) vector       [rad]
   • OUTPUT:
     • a       Limited Euler angles       (3 x 1) vector       [rad]

8. lin_interpolate

   function values_interpolated = lin_interpolate(T, values, N_samples, N_samples__theoretical)
   

   [Eq. pre-34 Suh]
   Linearly interpolates values.

   • INPUT:
     • T       Time stamp array       (1 x N_samples) vector       [s]
     • values       Array with elements to be interpolated       (1 x N_samples) vector       [-]
     • N_samples       Number of samples of T and values       scalar       [-]
     • N_samples__theoretical       Theoretical number of samples       scalar       [-]
   • OUTPUT:
     • values_interpolated       N_samples__theoretical interpolated values       (1 x N_samples__theoretical) array       [-]
       N.B.: N_samples <= N_samples__theoretical

9. omega2quat

   function q = omega2quat(omega)
   

   Creates a pure imaginary quaternion (i.e., null scalar part, that is, q(1) = 0) from the angular velocity omega.
   Please note: the quaternion thus obtained is NOT a unitary quaternion.

   • INPUT:
     • omega       Angular velocity omega(k)       (3 x 1) vector       [rad / s]
   • OUTPUT:
     • q       Quaternion (q = [q0; q1; q2; q3], where the first element, q0, is the scalar part aka real part)       (4 x 1) vector       [-]

10. Omega

    function Omega_omega = Omega(omega)
    

    [Eq. A12 ("A Kalman Filter for Nonlinear Attitude Estimation Using Time Variable Matrices and Quaternions" - Alvaro Deibe)]
    Computes the Omega(omega) matrix.
    N.B.: Omega(omega) appears in the product of a vector and a quaternion, and is used for example in the quaternion derivative.
    (N.B.: [Eq. 64 Trawny] DOES NOT work properly anymore, probably since q(1) = q_0 is the scalar part)

    • INPUT:
      • omega_next       Angular velocity omega(k)       (3 x 1) vector       [rad / s]
    • OUTPUT:
      • Omega__omega       Omega matrix       (3 x 3) matrix

11. Omega_avg_omega

    function Omega__avg_omega = Omega_avg_omega(omega_next, omega_prev, dt)
    

    [Eq. 129 Trawny]
    Computes the Omega(omega_avg) matrix.

    • INPUT:
      • omega_next       Angular velocity omega(k+1)       (3 x 1) vector       [rad / s]
      • omega_prev       Angular velocity omega(k)       (3 x 1) vector       [rad / s]
      • dt       Time step (dt = t(k+1) - t(k))       scalar       [s]
    • OUTPUT:
      • Omega__avg_omega       Omega(omega_avg) matrix       (3 x 3) matrix       [rad / s]

12. Omega_dot_omega

    function Omega__dot_omega = Omega_dot_omega(omega_next, omega_prev, dt)
    

    [Eq. 126 Trawny]
    Defines the derivative of the turn rate (omega_dot) and the associated matrix (Omega(omega_dot)), which - in the linear case - is constant.

    • INPUT:
      • omega_next       Angular velocity omega(k+1)       (3 x 1) vector       [rad / s]
      • omega_prev       Angular velocity omega(k)       (3 x 1) vector       [rad / s]
      • dt       Time step (dt = t(k+1) - t(k))       scalar       [s]
    • OUTPUT:
      • Omega__dot_omega       Omega matrix       (3 x 3) matrix

13. Phi_matrix

    function Phi = Phi_matrix(y_g__next, Phi_mode)
    

    [Eq. post-15 Suh] ('precise') OR [Eq. 16a Suh] ('approximated') OR [Eq. 187 Trawny] ('Trawny')
    Computes the transition matrix Phi(t + dt, t).

    • INPUT:
      • y_g__next       Measured angular velocity (i.e., gyro output) y_g(k)       (3 x 1) vector       [rad / s]
      • Phi_mode       Possible values:
        • precise: [Eq. post-15 Suh]
        • approximated: [Eq. 16a Suh]
        • Trawny: [Eq. 187 Trawny]
    • OUTPUT:
      • Phi       Transition matrix Phi(t + dt, t)       (9 x 9) matrix

14. Q_d_matrix

    function Q_d = Q_d_matrix(y_g__next, Q_d_mode)
    

    [Eq. post-15 Suh] ('precise') OR [Eq. 16b Suh] ('approximated') OR [Eq. 208 Trawny] ('Trawny')
    Computes the Noise Covariance Matrix Q_d.

    • INPUT:
      • y_g__next       Measured angular velocity (i.e., gyro output) y_g(k)       (3 x 1) vector       [rad / s]
      • Q_d_mode       Possible values:
        • 'precise': [Eq. post-15 Suh]
        • 'approximated': [Eq. 16a Suh]
    • OUTPUT:
      • Q_d       Noise Covariance Matrix Q_d(k)       (9 x 9) matrix

15. quat2euler

    function r = quat2euler(q)
    

    https://marc-b-reynolds.github.io/math/2017/04/18/TaitEuler.html#mjx-eqn%3Aeq%3Atait
    Converts quaternion to Euler angles.

    • INPUT:
      • q       Quaternion (q = [q0; q1; q2; q3], where the first element, q0, is the scalar part aka real part)       (4 x 1) vector       [-]
    • OUTPUT:
      • r       Euler angles (r = [roll; pitch; yaw])       (3 x 1) vector       [rad]

16. quat2RotMat

    function C_from_q = quat2RotMat(q)
    

    [Eq. 90 Trawny] [Eq. 1 Suh]
    Computes the rotation matrix associated to the quaternion q.

    • INPUT:
      • q       Quaternion (q = [q0; q1; q2; q3], where the first element, q0, is the scalar part aka real part)       (4 x 1) vector       [-]
    • OUTPUT:
      • C_from_q       Coordinate transformation matrix associated to q       (3 x 3) matrix       [-]

17. quatConjugate

    function q_conjugate = quatConjugate(q)
    

    [Eq. 13 Trawny]
    Takes the complex conjugate (that is, the inverse for a unit quaternion) of a given quaternion.
    N.B.: The inverse rotation is described by the inverse or complex conjugate quaternion).

    • INPUT:
      • q       Quaternion q(k) (q = [q0; q1; q2; q3], where the first element, q0, is the scalar part aka real part)       (4 x 1) vector       [-]
    • OUTPUT:
      • q_conjugate       (not unitary) Quaternion, conjugate of q       (4 x 1) vector       [-]

18. quatDerivative

    function q_dot = quatDerivative(q, omega)
    

    [Eq. 106 Trawny] OR [Eq. 2 Suh] (and [Eq. 4 Suh])
    Computes the quaternion derivative.

    • INPUT:
      • q       Quaternion (q = [q0; q1; q2; q3], where the first element, q0, is the scalar part aka real part)       (4 x 1) vector       [-]
      • omega       Angular velocity omega(k)       (3 x 1) vector       [rad / s]
    • OUTPUT:
      • q_do,       Quaternion derivative of q       (4 x 1) vector       [1 / s]

19. quatFirstIntegration

    function q_next = quatFirstIntegration(q_prev, omega_next, omega_prev, integration_mode)
    

    [Eq. 18 Suh] ('Suh') OR [Eq. 131 Trawny] ('Trawny') OR [Eq. 8 Yuan] ('Yuan')
    Computes the first-order quaternion integration. It makes the assumption of a linear evolution of omega during the integration interval dt.
    N.B.: Use Suh for integration_mode, not Trawny.

    • INPUT:
      • q       Quaternion q(k) (q = [q0; q1; q2; q3], where the first element, q0, is the scalar part aka real part)       (4 x 1) vector       [-]
      • omega_next       Angular velocity omega(k+1)       (3 x 1) vector       [rad/s]
      • omega_prev       Angular velocity omega(k)       (3 x 1) vector       [rad/s]
      • integration_mode       Modality of integration:
        • Suh, [Eq. 18 Suh]
        • Trawny, [Eq. 131 Trawny]
        • Yuan, [Eq. 8 Yuan]
    • OUTPUT:
      • q_next       q(k+1); unit quaternion from integration of q       (4 x 1) vector       [-]

20. quatInverse

    function q_inverse = quatInverse(q)
    

    [Eq. 13 Trawny]
    Takes the inverse of a given quaternion.
    (N.B.: The inverse rotation is described by the inverse - or complex conjugate for unit quaternions quaternion)

    • INPUT:
      • q       Quaternion q(k) (q = [q0; q1; q2; q3], where the first element, q0, is the scalar part aka real part)       (4 x 1) vector       [-]
    • OUTPUT:
      • q_inverse       (not unitary) Quaternion, the inverse of q       (4 x 1) vector       [-]

21. quatMultiplication

    function q_times_p = quatMultiplication(q, p)
    

    https://en.wikipedia.org/wiki/Quaternion#Hamilton_product
    Multiplies the quaternions q and p, thus obtaining their Hamilton product.
    Please note: the quaternion product is NOT commutative!
    (N.B.: [Eq. post-5 Trawny] is not used)

    • INPUT:
      • q       Quaternion q(k) (q = [q0; q1; q2; q3], where the first element, q0, is the scalar part aka real part)       (4 x 1) vector       [-]
      • p       Quaternion p(k) (p = [p0; p1; p2; p3], p0 is the scalar)       (4 x 1) vector       [-]
    • OUTPUT:
      • q_times_p       (not unitary) Quaternion, the product of (in this order) q and p       (4 x 1) vector       [-]

22. rand_range

    function x = rand_range(xmin, xmax)
    

    https://it.mathworks.com/matlabcentral/answers/66763-generate-random-numbers-in-range-from-0-8-to-4
    Generates a value from the uniform distribution on the interval (xmin, xmax).

    • INPUT:
      • xmin       Minimum possible value       scalar       [-]
      • xmax       Maximum possible value       scalar       [-]
    • OUTPUT:
      • x       Pseudo-random number in (xmin, xmax)       scalar       [-]

23. RotMat2euler

    function r = RotMat2euler(C)
    

    https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2012/07/euler-angles1.pdf
    Converts direction cosine matrix to Euler angles ('zyx' sequence).
    N.B.: "We’ll follow the notational conventions of Shoemake’s “Euler Angle Conversion”, Graphics Gems IV, pp. 222-9".

    • INPUT:
      • C       Coordinate transformation matrix       (3 x 3) matrix       [-]
    • OUTPUT:
      • r       Euler angles (r = [roll; pitch; yaw])       (3 x 1) vector       [rad]

24. RotMat2quat

    function q = RotMat2quat(C)
    

    [Eq. 98a - 98b - 99a - 99b Trawny]
    Converts direction cosine matrix to quaternion.

    • INPUT:
      • C       Coordinate transformation matrix       (3 x 3) matrix       [-]
    • OUTPUT:
      • q       Quaternion (q = [q0; q1; q2; q3], where the first element, q0, is the scalar part aka real part)       (4 x 1) vector       [-]

25. rotX

    function C_x = rotX(alpha)
    

    Computes a basic rotation about the x-axis by an angle alpha.

    • INPUT:
      • alpha       angle       scalar       [rad]
    • OUTPUT:
      • C       Coordinate transformation matrix       (3 x 3) matrix       [-]

26. rotY

    function C_y = rotY(alpha)
    

    Computes a basic rotation about the y-axis by an angle alpha.

    • INPUT:
      • alpha       angle       scalar       [rad]
    • OUTPUT:
      • C       Coordinate transformation matrix       (3 x 3) matrix       [-]

27. rotZ

    function C_y = rotY(alpha)
    

    Computes a basic rotation about the z-axis by an angle alpha.

    • INPUT:
      • alpha       angle       scalar       [rad]
    • OUTPUT:
      • C       Coordinate transformation matrix       (3 x 3) matrix       [-]

28. skewSymmetric

    function skew_symmetric_matrix = skewSymmetric(p)
    

    [Eq. post-9 Suh]
    Computes the skew-symmetric matrix operator [px], formed from p.

    • INPUT:
      • p       Possible values:
        • angular velocity,       (3 x 1) vector       [rad / s]
        • quaternion (only the vector part (aka imaginary part) of p is considered),       (4 x 1) vector       [-]
    • OUTPUT:
      • skew_symmetric_matrix       Skew-symmetric matrix formed from p       (3 x 3) matrix       [-]

29. wrapTo90

    function alpha_wrapped = wrapTo90(alpha)
    

    https://it.mathworks.com/matlabcentral/answers/324032-how-to-wrap-angle-in-radians-to-pi-2-pi-2
    Wraps the given angle to $[-90\degree, 90\degree]$.

    • INPUT:
      • alpha       Angle       scalar       [deg]
    • OUTPUT:
      • alpha_wrapped       Wrapped angle       scalar       [deg]

30. wrapToPi2

    function alpha_wrapped = wrapToPi2(alpha)
    

    https://it.mathworks.com/matlabcentral/answers/324032-how-to-wrap-angle-in-radians-to-pi-2-pi-2
    Wraps the given angle to $[-\pi/2, \pi/2]$.

    • INPUT:
      • alpha       Angle       scalar       [rad]
    • OUTPUT:
      • alpha_wrapped       Wrapped angle       scalar       [rad]

Indirect Kalman Filter

The objective of the filter is to estimate the attitude quaternion $q$, i.e., $\hat{q}$, from the sensor outputs $y_g$ (gyroscope), $y_a$ (accelerometer), and $y_m$ (magnetometer). It consists of two phases, which alternate:

1. The propagation stage, where the filter produces a prediction of the attitude $\hat{q}$ based on the previous estimate of the state $𝑥 = [q_e, b_g, b_a]^T$, i.e., $\hat{𝑥} = [\hat{q}_e, \hat{b}_g, \hat{b}_a]^T$,and the most recent gyroscope output $y_g$.

2. The update stage, which corrects the values predicted in the propagation stage. It consists of a two-step measurement update:

   1. An accelerometer measurement update, where the most recent accelerometer output $y_a$ is used
   2. A magnetometer measurement update, where the most recent magnetometer output $y_m$ is used