m5::imu::MadgwickFilter Class Reference

Gradient-descent orientation filter (Madgwick 2010) More...

#include <madgwick.hpp>

Inheritance diagram for m5::imu::MadgwickFilter:

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	writeBeta (const float beta)
	Set the gradient descent gain.
float	readBeta () const
	Current gradient descent gain.
bool	writeQuaternion (const float w, const float x, const float y, const float z)
	Overwrite the current attitude estimate directly.

Detailed Description

Gradient-descent orientation filter (Madgwick 2010)

Implements the gradient descent algorithm from S. Madgwick's "An efficient orientation filter for inertial and inertial/magnetic sensor arrays" (2010), re-derived for this repository's coordinate/quaternion convention (see filter_base.hpp): body Z-up, v_world = q.rotate(v_body). Each update() blends a gyro-integrated quaternion rate with a gradient-descent correction step that drives 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) toward the measured accel/mag directions.

Member Function Documentation

quaternion()

void m5::imu::MadgwickFilter::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::MadgwickFilter::reset ( )

overridevirtual

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

Implements m5::imu::AhrsFilterBase.

update()

void m5::imu::MadgwickFilter::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.

writeBeta()

bool m5::imu::MadgwickFilter::writeBeta

(

const float

beta

)

Set the gradient descent gain.

Parameters

beta	Step size applied to the normalized gradient correction, must be > 0

Returns

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

writeQuaternion()

bool m5::imu::MadgwickFilter::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