ekf.hpp

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD
 *
 * SPDX-License-Identifier: MIT
 */
#ifndef M5_UNIT_IMU_IMU_FILTER_EKF_HPP
#define M5_UNIT_IMU_IMU_FILTER_EKF_HPP
 
#include <math/matrix.hpp>
#include <math/quaternion.hpp>
#include <math/vector3.hpp>
 
#include "filter_base.hpp"
 
namespace m5 {
namespace imu {
 
class EKFFilter : public AhrsFilterBase {
public:
    EKFFilter()
    {
        reset_state();
    }
 
    virtual void reset() override;
    virtual void update(const float dt, const float accel[3], const float gyro[3], const float* mag) override;
    virtual void quaternion(float& w, float& x, float& y, float& z) const override;
 
    bool writeNoiseParams(const float gyro_noise_dps, const float accel_noise_g, const float mag_noise_ut,
                          const float bias_random_walk_dps);
    inline float readGyroNoise() const
    {
        return _gyro_noise_dps;
    }
    inline float readAccelNoise() const
    {
        return _accel_noise_g;
    }
    inline float readMagNoise() const
    {
        return _mag_noise_ut;
    }
    inline float readBiasRandomWalk() const
    {
        return _bias_random_walk_dps;
    }
 
    m5::math::Vector3 readGyroBias() const;
 
    bool writeQuaternion(const float w, const float x, const float y, const float z);
 
private:
    // Non-virtual state initializer shared by ctor and reset(); safe to call from ctor
    void reset_state();
    void predict(const float dt, const m5::math::Vector3& gyro_rad_s);
    bool apply_axis3_update(const m5::math::Matrix<3, 7>& h, const m5::math::Vector3& innovation,
                            const float noise_var);
 
    m5::math::Quaternion _q{};        // Current attitude estimate (identity by default)
    m5::math::Vector3 _bias_rad_s{};  // Gyro bias estimate, rad/s, body frame
    m5::math::Matrix<7, 7> _p{};      // State covariance (state = [q(4), bias(3)])
 
    float _gyro_noise_dps{0.5f};         // Gyro measurement noise, 1-sigma, dps
    float _accel_noise_g{0.05f};         // Accelerometer measurement noise, 1-sigma, G
    float _mag_noise_ut{2.0f};           // Magnetometer measurement noise, 1-sigma, uT
    float _bias_random_walk_dps{0.01f};  // Gyro bias random walk, 1-sigma, dps
};
 
}  // namespace imu
}  // namespace m5
#endif