2023-10-19 07:37:56 +00:00
///
2023-10-21 02:05:55 +00:00
/// \file include/scmp/filter/Madgwick.h
2023-10-19 07:37:56 +00:00
/// \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.
///
# 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
2023-10-21 03:45:39 +00:00
/// consisting of tri-Axis gyroscopes and accelerometers, and MARG
/// sensor arrays that also include tri-Axis magnetometers. The MARG
2023-10-19 07:37:56 +00:00
/// 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-21 03:45:39 +00:00
/// \brief The Madgwick filter is initialised with an identity MakeQuaternion.
2023-10-20 08:31:06 +00:00
Madgwick ( ) : deltaT ( 0.0 ) , previousSensorFrame ( ) , sensorFrame ( )
{ } ;
2023-10-19 07:37:56 +00:00
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
2023-10-21 03:45:39 +00:00
/// initialised as an identity MakeQuaternion.
2023-10-19 07:37:56 +00:00
Madgwick ( scmp : : geom : : Vector < T , 3 > sf ) : deltaT ( 0.0 ) , previousSensorFrame ( )
{
2023-10-21 03:45:39 +00:00
if ( ! sf . IsZero ( ) ) {
sensorFrame = scmp : : geom : : MakeQuaternion < T > ( sf , 0.0 ) ;
2023-10-19 07:37:56 +00:00
}
}
2023-10-21 03:45:39 +00:00
/// \brief Initialise the filter with a sensor frame MakeQuaternion.
2023-10-19 07:37:56 +00:00
///
2023-10-21 03:45:39 +00:00
/// \param sf A MakeQuaternion representing the current Orientation.
2023-10-19 07:37:56 +00:00
Madgwick ( scmp : : geom : : Quaternion < T > sf ) :
2023-10-20 08:31:06 +00:00
deltaT ( 0.0 ) , previousSensorFrame ( ) , sensorFrame ( sf )
{ } ;
2023-10-19 07:37:56 +00:00
2023-10-20 08:31:06 +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.
2023-10-21 03:45:39 +00:00
/// \return A MakeQuaternion representing the rate of angular change.
2023-10-19 07:37:56 +00:00
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 ;
2023-10-20 08:31:06 +00:00
this - > sensorFrame = sf ;
this - > deltaT = delta ;
2023-10-19 07:37:56 +00:00
}
2023-10-20 08:31:06 +00:00
/// \brief Update the sensor frame to a new frame.
///
/// \warning The filter's default Δt must be set before calling
// this.
///
/// \param sf The new sensor frame replacing the previous one.
void
UpdateFrame ( const scmp : : geom : : Quaternion < T > & sf )
{
this - > UpdateFrame ( sf , this - > deltaT ) ;
}
2023-10-19 07:37:56 +00:00
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
///
2023-10-20 08:31:06 +00:00
/// This method will assert that the Δt value is not zero
/// within a 100μs tolerance. This assert is compiled out with
/// the compile flag NDEBUG, but may be useful to catch
/// possible errors.
///
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 )
{
2023-10-20 08:31:06 +00:00
// Ensure the delta isn't zero within a 100 μs
2023-10-20 23:13:49 +00:00
// tolerance.
2023-10-20 08:31:06 +00:00
if ( scmp : : WithinTolerance < T > ( delta , 0.0 , 0.00001 ) ) {
return ;
}
scmp : : geom : : Quaternion < T > q = this - > AngularRate ( gyro ) * delta ;
2023-10-19 07:37:56 +00:00
this - > UpdateFrame ( this - > sensorFrame + q , delta ) ;
}
2023-10-20 08:31:06 +00:00
/// \brief Update the sensor frame with a gyroscope reading.
///
/// If no Δt is provided, the filter's default is used.
///
/// \warning The default Δt must be explicitly set using DeltaT
/// before calling this.
///
/// \param gyro A three-dimensional vector containing gyro readings
/// as w_x, w_y, w_z.
void
UpdateAngularOrientation ( const scmp : : geom : : Vector < T , 3 > & gyro )
{
this - > UpdateAngularOrientation ( gyro , this - > deltaT ) ;
}
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 ( )
{
2023-10-21 03:45:39 +00:00
return this - > sensorFrame . Euler ( ) ;
2023-10-19 07:37:56 +00:00
}
2023-10-20 08:31:06 +00:00
/// \brief Set the default Δt.
///
/// \note This must be explicitly called before calling any
/// method which uses the filter's internal Δt.
///
/// \param The time delta to use when no time delta is
/// provided.
void
DeltaT ( T newDeltaT )
{
this - > deltaT = newDeltaT ;
}
/// \brief Retrieve the filter's current ΔT.
///
/// \return The current value the filter will default to using
/// if no time delta is provided.
T DeltaT ( ) { return this - > deltaT ; }
2023-10-19 07:37:56 +00:00
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