Madgwick.h

Go to the documentation of this file.

 
 
#ifndef SCMP_FILTER_MADGWICK_H
#define SCMP_FILTER_MADGWICK_H
 
 
#include <scmp/geom/Vector.h>
#include <scmp/geom/Quaternion.h>
 
 
namespace scmp {
 
namespace estimation {
 
 
template <typename T>
class Madgwick {
public:
    Madgwick() : deltaT(0.0), previousSensorFrame(), sensorFrame()
    {};
 
    Madgwick(scmp::geom::Vector<T, 3> sf) : deltaT(0.0), previousSensorFrame()
    {
        if (!sf.IsZero()) {
            sensorFrame = scmp::geom::MakeQuaternion<T>(sf, 0.0);
        }
    }
 
    Madgwick(scmp::geom::Quaternion<T> sf) :
        deltaT(0.0), previousSensorFrame(), sensorFrame(sf)
    {};
 
    scmp::geom::Quaternion<T>
    Orientation() const
    {
        return this->sensorFrame;
    }
 
    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);
    }
 
    void
    UpdateFrame(const scmp::geom::Quaternion<T> &sf, T delta)
    {
        this->previousSensorFrame = this->sensorFrame;
        this->sensorFrame         = sf;
        this->deltaT              = delta;
    }
 
    void
    UpdateFrame(const scmp::geom::Quaternion<T> &sf)
    {
        this->UpdateFrame(sf, this->deltaT);
    }
 
    void
    UpdateAngularOrientation(const scmp::geom::Vector<T, 3> &gyro, T delta)
    {
        // Ensure the delta isn't zero within a 100 μs
        // tolerance.
        if (scmp::WithinTolerance<T>(delta, 0.0, 0.00001)) {
            return;
        }
        scmp::geom::Quaternion<T> q = this->AngularRate(gyro) * delta;
 
        this->UpdateFrame(this->sensorFrame + q, delta);
    }
 
    void
    UpdateAngularOrientation(const scmp::geom::Vector<T, 3> &gyro)
    {
        this->UpdateAngularOrientation(gyro, this->deltaT);
    }
 
    scmp::geom::Vector<T, 3>
    Euler()
    {
        return this->sensorFrame.Euler();
    }
 
    void
    DeltaT(T newDeltaT)
    {
        this->deltaT = newDeltaT;
    }
 
    T   DeltaT() { return this->deltaT; }
 
private:
    T               deltaT;
    scmp::geom::Quaternion<T>   previousSensorFrame;
    scmp::geom::Quaternion<T>   sensorFrame;
};
 
 
using Madgwickd = Madgwick<double>;
 
using Madgwickf = Madgwick<float>;
 
 
} // namespace estimation
} // namespace scmp
 
 
#endif // SCMP_FILTER_MADGWICK_H