163 lines
4.9 KiB
C++
163 lines
4.9 KiB
C++
///
|
|
/// \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>
|
|
|
|
|
|
/// scmp contains the chimmering clarity math and physics code.
|
|
namespace scmp {
|
|
|
|
/// 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:
|
|
/// \brief The Madgwick filter is initialised with an identity quaternion.
|
|
Madgwick() : deltaT(0.0), previousSensorFrame(), sensorFrame() {};
|
|
|
|
|
|
/// \brief The Madgwick filter is initialised with a sensor frame.
|
|
///
|
|
/// \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()) {
|
|
sensorFrame = scmp::geom::quaternion<T>(sf, 0.0);
|
|
}
|
|
}
|
|
|
|
|
|
/// \brief Initialise the filter with a sensor frame quaternion.
|
|
///
|
|
/// \param sf A quaternion representing the current Orientation.
|
|
Madgwick(scmp::geom::Quaternion<T> sf) :
|
|
deltaT(0.0), previousSensorFrame(), sensorFrame(sf) {};
|
|
|
|
|
|
/// \brief Return the current Orientation as measured by the filter.
|
|
///
|
|
/// \return The current sensor frame.
|
|
scmp::geom::Quaternion<T>
|
|
Orientation() const
|
|
{
|
|
return this->sensorFrame;
|
|
}
|
|
|
|
|
|
/// \brief Return the filter's rate of angular change from a
|
|
/// sensor frame.
|
|
///
|
|
/// 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);
|
|
}
|
|
|
|
/// \brief Update the sensor frame to a new frame.
|
|
///
|
|
/// \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;
|
|
}
|
|
|
|
|
|
/// \brief Update the sensor frame with a gyroscope reading.
|
|
///
|
|
/// \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.
|
|
assert(!scmp::WithinTolerance<T>(delta, 0.0, 0.0001));
|
|
scmp::geom::Quaternion<T> q = this->AngularRate(gyro) * delta;
|
|
|
|
this->UpdateFrame(this->sensorFrame + q, delta);
|
|
}
|
|
|
|
|
|
/// \brief Retrieve a vector of the Euler angles in ZYX Orientation.
|
|
///
|
|
/// \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;
|
|
};
|
|
|
|
|
|
/// \brief Madgwickd is a shorthand alias for a Madgwick<double>.
|
|
using Madgwickd = Madgwick<double>;
|
|
|
|
/// \brief Madgwickf is a shorthand alias for a Madgwick<float>.
|
|
using Madgwickf = Madgwick<float>;
|
|
|
|
|
|
} // namespace filter
|
|
} // namespace scmp
|
|
|
|
|
|
#endif // SCMP_FILTER_MADGWICK_H
|