Mahony's AHRS update method

Description

Mahony's algorithm for AHRS update method.

Usage

1
2
MahonyAHRSupdate(gxi, gyi, gzi, axi, ayi, azi, mxi, myi, mzi, sampleFreqi, 
twoKpi, twoKii, integralFBxi, integralFByi, integralFBzi, q0i, q1i, q2i, halfex)

Arguments

gxi

Gyroscope x axis measurement in radians/s.

gyi

Gyroscope y axis measurement in radians/s.

gzi

Gyroscope z axis measurement in radians/s.

axi

Accelerometer x axis measurement in any calibrated units.

ayi

Accelerometer y axis measurement in any calibrated units.

azi

Accelerometer z axis measurement in any calibrated units.

mxi

Magnetometer x axis measurement in any calibrated units.

myi

Magnetometer y axis measurement in any calibrated units.

mzi

Magnetometer z axis measurement in any calibrated units.

sampleFreqi

Sample frequency.

twoKpi

Kp constant passed as a parameter

twoKii

Ki constant passed as a parameter

integralFBxi

Integral FB[0]

integralFByi

Integral FB[1]

integralFBzi

Integral FB[2]

q0i

Quaternion[0]

q1i

Quaternion[1]

q2i

Quaternion[2]

halfex

Half

Value

output quaternion describing the Earth relative to the sensor.

Author(s)

Jose Gama

References

S. O. H. Madgwick, An efficient orientation filter for inertial and inertial/magnetic sensor arrays, Technical report, University of. Bristol University, UK, 2010.

R. Mahony, T. Hamel, and J.-M. Pflimlin. Nonlinear complementary filters on the special orthogonal group. Automatic Control, IEEE Transactions on, 53(5):1203 –1218, june 2008.