filter.hpp

Go to the documentation of this file.

 
#ifndef INCLUDE_IMUNANO33_FILTER_HPP_
#define INCLUDE_IMUNANO33_FILTER_HPP_
 
#ifdef IMUNANO33_EMBED
#include <math.h>
#else
#include <cmath>
#endif
 
#include "imunano33/mathutil.hpp"
#include "imunano33/quaternion.hpp"
#ifdef IMUNANO33_EMBED
#include "imunano33/sv_embed.hpp"
#else
#include "imunano33/simplevectors.hpp"
#endif
#include "imunano33/unit.hpp"
 
namespace imunano33 {
#ifdef IMUNANO33_EMBED
using Vector3D =
    svector::EmbVec3D; 
#else
using svector::Vector3D;
#endif
 
class Filter {
public:
  Filter() : m_gyroFavoring{0.98}, m_qRot{1, Vector3D{}} {}
 
  Filter(const num_t gyroFavoring) : m_qRot{1, Vector3D{}} {
#ifdef IMUNANO33_EMBED
    m_gyroFavoring = MathUtil::clamp(gyroFavoring, 0.0F, 1.0F);
#else
    m_gyroFavoring = MathUtil::clamp(gyroFavoring, 0.0, 1.0);
#endif
  }
 
  Filter(const num_t gyroFavoring, const Quaternion &initialQ)
      : m_qRot{initialQ.unit()} {
#ifdef IMUNANO33_EMBED
    m_gyroFavoring = MathUtil::clamp(gyroFavoring, 0.0F, 1.0F);
#else
    m_gyroFavoring = MathUtil::clamp(gyroFavoring, 0.0, 1.0);
#endif
  }
 
  Filter(const Filter &other) = default;
 
  Filter &operator=(const Filter &other) = default;
 
  ~Filter() = default;
 
  Filter(Filter &&) = default;
 
  Filter &operator=(Filter &&) = default;
 
  void updateGyro(const Vector3D &gyro, const num_t time) {
    if (MathUtil::nearZero(gyro)) {
      // if gyro reading is 0, then don't correct
      return;
    }
 
    // otherwise integrate quaternion reading
    const Quaternion qGyroDelta{normalize(gyro), time * magn(gyro)};
    m_qRot *= qGyroDelta;
  }
 
  void updateAccel(const Vector3D &accel) {
    // don't bother with acceleration correction if acceleration is basically
    // 0
    if (MathUtil::nearZero(accel)) {
      return;
    }
 
    // gravity vector rotation
    const Quaternion qAccelBody{0, accel};
    const Quaternion qAccelWorld =
        m_qRot * qAccelBody *
        m_qRot.conj(); // rotates body acceleration by gyro measurements
 
    // correcting gyro drift with accelerometer
    const Vector3D vecAccelWorldNorm = normalize(qAccelWorld.vec());
    const Vector3D vecAccelGravity{0, 0, -1};
    const Vector3D vecRotAxis =
        cross(vecAccelWorldNorm,
              vecAccelGravity); // rotation axis for correction rotation from
                                // estimated gravity vector (from gyro
                                // readings) to true gravity vector
 
#ifdef IMUNANO33_EMBED
    const num_t rotAngle = acosf(MathUtil::clamp(
        dot(vecAccelGravity, vecAccelWorldNorm) /
            (magn(vecAccelGravity) * magn(vecAccelWorldNorm)),
        -1.0F,
        1.0F)); // angle to rotate to correct acceleration vector
#else
    const num_t rotAngle = std::acos(
        MathUtil::clamp(dot(vecAccelGravity, vecAccelWorldNorm) /
                            (magn(vecAccelGravity) * magn(vecAccelWorldNorm)),
                        -1.0,
                        1.0)); // angle to rotate to correct acceleration vector
#endif
 
    // if the axis to rotate around is 0, then don't bother correcting
    if (MathUtil::nearZero(vecRotAxis)) {
      return;
    }
 
    // complementary filter
    const Quaternion qAccelCur{normalize(vecRotAxis),
                               (1 - m_gyroFavoring) * rotAngle};
    m_qRot = qAccelCur * m_qRot;
  }
 
  void update(const Vector3D &accel, const Vector3D &gyro, const num_t time) {
    // math from:
    // https://stanford.edu/class/ee267/lectures/lecture10.pdf
    // https://stanford.edu/class/ee267/notes/ee267_notes_imu.pdf
 
    updateGyro(gyro, time);
    updateAccel(accel);
  }
 
  void reset() { setRotQ({1, {0, 0, 0}}); }
 
  Quaternion getRotQ() const { return m_qRot; }
 
  num_t getGyroFavoring() const { return m_gyroFavoring; }
 
  void setRotQ(const Quaternion &q) { m_qRot = q.unit(); }
 
  void setGyroFavoring(const num_t favoring) {
#ifdef IMUNANO33_EMBED
    m_gyroFavoring = MathUtil::clamp(favoring, 0.0F, 1.0F);
#else
    m_gyroFavoring = MathUtil::clamp(favoring, 0.0, 1.0);
#endif
  }
 
private:
  num_t m_gyroFavoring;
 
  Quaternion m_qRot;
};
 
} // namespace imunano33
 
#endif