m5::imu::MahonyFilter Class Reference

Explicit complementary filter with proportional-integral feedback (Mahony et al. 2008) More...

#include <mahony.hpp>

Inheritance diagram for m5::imu::MahonyFilter:

Public Member Functions
virtual void	reset () override
	Reset the internal attitude estimate (and any filter state) to identity.
virtual void	update (const float dt, const float accel[3], const float gyro[3], const float *mag) override
	Fuse one IMU sample into the attitude estimate.
virtual void	quaternion (float &w, float &x, float &y, float &z) const override
	Current fused attitude.
bool	writeKp (const float kp)
	Set the proportional feedback gain.
float	readKp () const
	Current proportional feedback gain.
bool	writeKi (const float ki)
	Set the integral feedback gain.
float	readKi () const
	Current integral feedback gain.
bool	writeQuaternion (const float w, const float x, const float y, const float z)
	Overwrite the current attitude estimate directly.

Detailed Description

Explicit complementary filter with proportional-integral feedback (Mahony et al. 2008)

Implements the explicit complementary filter on SO(3) from Mahony, Hamel and Pflimlin's "Nonlinear Complementary Filters on the Special Orthogonal Group" (2008), re-derived for this repository's coordinate/quaternion convention (see filter_base.hpp): body Z-up, v_world = q.rotate(v_body). Each update() integrates the gyro rate after correcting it with a proportional-integral term driven by the body-frame cross-product error between the measured accel/mag directions and the predicted reference direction(s) (gravity, and – when a magnetometer sample is supplied – the horizontal magnetic field recomputed every step from the current attitude estimate).

Member Function Documentation

quaternion()

void m5::imu::MahonyFilter::quaternion ( float & w, float & x, float & y, float & z ) const

overridevirtual

Current fused attitude.

Parameters

[out]	w	Quaternion scalar part
[out]	x	Quaternion vector part X
[out]	y	Quaternion vector part Y
[out]	z	Quaternion vector part Z

Implements m5::imu::AhrsFilterBase.

reset()

void m5::imu::MahonyFilter::reset ( )

overridevirtual

Reset the internal attitude estimate (and any filter state) to identity.

Implements m5::imu::AhrsFilterBase.

update()

void m5::imu::MahonyFilter::update ( const float dt, const float accel[3], const float gyro[3], const float * mag )

overridevirtual

Fuse one IMU sample into the attitude estimate.

Parameters

dt	Time elapsed since the previous update(), in seconds
accel	Accelerometer reading (G), body frame, X/Y/Z
gyro	Gyroscope reading (dps), body frame, X/Y/Z
mag	Magnetometer reading (uT), body frame, X/Y/Z; pass nullptr for 6-axis (accel+gyro only) operation

Implements m5::imu::AhrsFilterBase.

writeKi()

bool m5::imu::MahonyFilter::writeKi

(

const float

ki

)

Set the integral feedback gain.

Parameters

ki	Gain applied to the accumulated cross-product error, must be >= 0 (0 disables integral action; the accumulator itself is left untouched but contributes nothing while ki is 0)

Returns

False (and keeps the previous value) if ki is negative

writeKp()

bool m5::imu::MahonyFilter::writeKp

(

const float

kp

)

Set the proportional feedback gain.

Parameters

kp	Gain applied to the instantaneous cross-product error, must be > 0

Returns

False (and keeps the previous value) if kp is not positive

writeQuaternion()

bool m5::imu::MahonyFilter::writeQuaternion	(	const float	w,
		const float	x,
		const float	y,
		const float	z )

Overwrite the current attitude estimate directly.

Not part of AhrsFilterBase; intended for seeding a known starting attitude (e.g. tests, or resuming from a previously stored orientation).

Parameters

w	W component of the quaternion
x	X component of the quaternion
y	Y component of the quaternion
z	Z component of the quaternion

Returns

False (and keeps the previous value) if the resulting quaternion is (near) zero