2023-10-19 07:37:56 +00:00
///
/// \file Madwick.cc
/// \author K. Isom <kyle@imap.cc>
/// \date 2019-08-06
/// \brief Implementation of a Madgwick filter.
///
/// See https://courses.cs.washington.edu/courses/cse466/14au/labs/l4/madgwick_internal_report.pdf.
///
/// Copyright 2019 K. Isom <kyle@imap.cc>
///
/// Permission to use, copy, modify, and/or distribute this software for
/// any purpose with or without fee is hereby granted, provided that
/// the above copyright notice and this permission notice appear in all /// copies.
///
/// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
/// WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
/// WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE
/// AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL
/// DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA
/// OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
/// TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
/// PERFORMANCE OF THIS SOFTWARE.
///
/// \file madgwick.h
/// \brief Implementation of a Madgwick filter.
///
/// See
# ifndef SCMP_FILTER_MADGWICK_H
# define SCMP_FILTER_MADGWICK_H
# include <scmp/geom/Vector.h>
# include <scmp/geom/Quaternion.h>
2023-10-20 03:32:46 +00:00
/// scmp contains the chimmering clarity math and physics code.
2023-10-19 07:37:56 +00:00
namespace scmp {
2023-10-20 03:32:46 +00:00
2023-10-19 07:37:56 +00:00
/// filter contains filtering algorithms.
namespace filter {
/// @brief Madgwick implements an efficient Orientation filter for IMUs.
///
/// Madgwick is a novel Orientation filter applicable to IMUs
/// consisting of tri-axis gyroscopes and accelerometers, and MARG
/// sensor arrays that also include tri-axis magnetometers. The MARG
/// implementation incorporates magnetic distortionand gyroscope bias
/// drift compensation.
///
/// It is described in the paper [An efficient Orientation filter for inertial and inertial/magnetic sensor arrays](http://x-io.co.uk/res/doc/madgwick_internal_report.pdf).
///
/// \tparam T A floating point type.
template < typename T >
class Madgwick {
public :
2023-10-20 03:32:46 +00:00
/// \brief The Madgwick filter is initialised with an identity quaternion.
2023-10-19 07:37:56 +00:00
Madgwick ( ) : deltaT ( 0.0 ) , previousSensorFrame ( ) , sensorFrame ( ) { } ;
2023-10-20 03:32:46 +00:00
/// \brief The Madgwick filter is initialised with a sensor frame.
2023-10-19 07:37:56 +00:00
///
/// \param sf A sensor frame; if zero, the sensor frame will be
/// initialised as an identity quaternion.
Madgwick ( scmp : : geom : : Vector < T , 3 > sf ) : deltaT ( 0.0 ) , previousSensorFrame ( )
{
if ( ! sf . isZero ( ) ) {
2023-10-20 03:32:46 +00:00
sensorFrame = scmp : : geom : : quaternion < T > ( sf , 0.0 ) ;
2023-10-19 07:37:56 +00:00
}
}
2023-10-20 03:32:46 +00:00
/// \brief Initialise the filter with a sensor frame quaternion.
2023-10-19 07:37:56 +00:00
///
/// \param sf A quaternion representing the current Orientation.
Madgwick ( scmp : : geom : : Quaternion < T > sf ) :
deltaT ( 0.0 ) , previousSensorFrame ( ) , sensorFrame ( sf ) { } ;
2023-10-20 03:32:46 +00:00
/// \brief Return the current Orientation as measured by the filter.
2023-10-19 07:37:56 +00:00
///
/// \return The current sensor frame.
scmp : : geom : : Quaternion < T >
Orientation ( ) const
{
return this - > sensorFrame ;
}
2023-10-20 03:32:46 +00:00
/// \brief Return the filter's rate of angular change from a
/// sensor frame.
///
2023-10-19 07:37:56 +00:00
/// Return the rate of change of the Orientation of the earth frame
/// with respect to the sensor frame.
///
/// \param gyro A three-dimensional vector containing gyro readings
/// as w_x, w_y, w_z.
/// \return A quaternion representing the rate of angular change.
scmp : : geom : : Quaternion < T >
AngularRate ( const scmp : : geom : : Vector < T , 3 > & gyro ) const
{
return ( this - > sensorFrame * 0.5 ) * scmp : : geom : : Quaternion < T > ( gyro , 0.0 ) ;
}
2023-10-20 03:32:46 +00:00
/// \brief Update the sensor frame to a new frame.
2023-10-19 07:37:56 +00:00
///
/// \param sf The new sensor frame replacing the previous one.
/// \param delta The time delta since the last update.
void
UpdateFrame ( const scmp : : geom : : Quaternion < T > & sf , T delta )
{
this - > previousSensorFrame = this - > sensorFrame ;
this - > sensorFrame = sf ;
this - > deltaT = delta ;
}
2023-10-20 03:32:46 +00:00
/// \brief Update the sensor frame with a gyroscope reading.
2023-10-19 07:37:56 +00:00
///
/// \param gyro A three-dimensional vector containing gyro readings
/// as w_x, w_y, w_z.
/// \param delta The time step between readings. It must not be zero.
void
UpdateAngularOrientation ( const scmp : : geom : : Vector < T , 3 > & gyro , T delta )
{
// Ensure the delta isn't zero within a 100 μs tolerance.
2023-10-20 03:32:46 +00:00
assert ( ! scmp : : WithinTolerance < T > ( delta , 0.0 , 0.0001 ) ) ;
2023-10-19 07:37:56 +00:00
scmp : : geom : : Quaternion < T > q = this - > AngularRate ( gyro ) * delta ;
this - > UpdateFrame ( this - > sensorFrame + q , delta ) ;
}
2023-10-20 03:32:46 +00:00
/// \brief Retrieve a vector of the Euler angles in ZYX Orientation.
2023-10-19 07:37:56 +00:00
///
/// \return A vector of Euler angles as <ψ, θ, ϕ>.
scmp : : geom : : Vector < T , 3 >
Euler ( )
{
return this - > sensorFrame . euler ( ) ;
}
private :
T deltaT ;
scmp : : geom : : Quaternion < T > previousSensorFrame ;
scmp : : geom : : Quaternion < T > sensorFrame ;
} ;
2023-10-20 03:32:46 +00:00
/// \brief Madgwickd is a shorthand alias for a Madgwick<double>.
using Madgwickd = Madgwick < double > ;
2023-10-19 07:37:56 +00:00
2023-10-20 03:32:46 +00:00
/// \brief Madgwickf is a shorthand alias for a Madgwick<float>.
using Madgwickf = Madgwick < float > ;
2023-10-19 07:37:56 +00:00
} // namespace filter
} // namespace scmp
# endif // SCMP_FILTER_MADGWICK_H