imusensorfusion.h

Go to the documentation of this file.

/*
    IMU Sensor Fusion - Madgwick and Mahony AHRS filters
    Copyright (C) 2026 Jasem Mutlaq ([email protected])
 
    Based on:
    - Madgwick, S.O.H. (2010). "An efficient orientation filter for inertial and inertial/magnetic sensor arrays"
    - Mahony, R. et al. (2008). "Nonlinear Complementary Filters on the Special Orthogonal Group"
 
    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.
 
    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.
 
    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
*/
 
#pragma once
 
#include <cmath>
 
namespace INDI
{
 
class IMUSensorFusion
{
    public:
        virtual ~IMUSensorFusion() = default;
 
        virtual void update(double gx, double gy, double gz,
                            double ax, double ay, double az,
                            double mx, double my, double mz,
                            double dt, bool useMag = true) = 0;
 
        virtual void getQuaternion(double &w, double &i, double &j, double &k) const = 0;
 
        virtual void reset() = 0;
};
 
// ─────────────────────────────────────────────────────────────────────────────
class MadgwickFilter : public IMUSensorFusion
{
    public:
        explicit MadgwickFilter(double beta = 0.1);
        virtual ~MadgwickFilter() = default;
 
        void update(double gx, double gy, double gz,
                    double ax, double ay, double az,
                    double mx, double my, double mz,
                    double dt, bool useMag = true) override;
 
        void getQuaternion(double &w, double &i, double &j, double &k) const override;
 
        void reset() override;
 
        void setBeta(double beta)
        {
            m_beta = beta;
        }
        double getBeta() const
        {
            return m_beta;
        }
 
    private:
        double m_beta;          
        double m_q0, m_q1, m_q2, m_q3; 
 
        // 9-DOF update (with magnetometer)
        void update9DOF(double gx, double gy, double gz,
                        double ax, double ay, double az,
                        double mx, double my, double mz,
                        double dt);
 
        // 6-DOF update (without magnetometer)
        void update6DOF(double gx, double gy, double gz,
                        double ax, double ay, double az,
                        double dt);
};
 
// ─────────────────────────────────────────────────────────────────────────────
class MahonyFilter : public IMUSensorFusion
{
    public:
        explicit MahonyFilter(double kp = 2.0, double ki = 0.005);
        virtual ~MahonyFilter() = default;
 
        void update(double gx, double gy, double gz,
                    double ax, double ay, double az,
                    double mx, double my, double mz,
                    double dt, bool useMag = true) override;
 
        void getQuaternion(double &w, double &i, double &j, double &k) const override;
 
        void reset() override;
 
        void setKp(double kp)
        {
            m_kp = kp;
        }
        double getKp() const
        {
            return m_kp;
        }
 
        void setKi(double ki)
        {
            m_ki = ki;
        }
        double getKi() const
        {
            return m_ki;
        }
 
    private:
        double m_kp, m_ki;                    
        double m_q0, m_q1, m_q2, m_q3;       
        double m_integralFBx, m_integralFBy, m_integralFBz; 
};
 
} // namespace INDI