c++ imu

#include <iostream>
#include <cmath>
#include <chrono>
#include <thread>
#include <random>

// Simple helper: wraps angle to [-pi, pi]
double wrapToPi(double angle) {
    while (angle > M_PI) {
        angle -= 2.0 * M_PI;
    }
    while (angle < -M_PI) {
        angle += 2.0 * M_PI;
    }
    return angle;
}

// Complementary Filter Class
class ComplementaryFilter {
public:
    ComplementaryFilter(double alpha = 0.98)
        : alpha_(alpha),
          roll_(0.0), pitch_(0.0), yaw_(0.0)
    {
        lastTime_ = std::chrono::steady_clock::now();
    }

    // Update filter with new IMU data
    void update(double gx, double gy, double gz, double ax, double ay, double az) {
        // Time delta
        auto currentTime = std::chrono::steady_clock::now();
        std::chrono::duration<double> elapsed = currentTime - lastTime_;
        double dt = elapsed.count();
        lastTime_ = currentTime;

        // 1) Integrate gyro to get incremental angles
        roll_  += gx * dt;
        pitch_ += gy * dt;
        yaw_   += gz * dt;

        // 2) Estimate roll & pitch from accelerometer
        // Assuming sensor is mostly aligned so that az ~ +9.81 for “upright”
        double rollAcc = std::atan2(ay, az);
        // Negative sign for pitch if x axis forward and z axis down/up depends on your orientation
        double pitchAcc = std::atan2(-ax, std::sqrt(ay * ay + az * az));
        // We ignore magnetometer → yaw drifts

        // 3) Complementary filter to fuse gyro (fast changes) with accel (long-term reference)
        roll_  = alpha_ * roll_  + (1.0 - alpha_) * rollAcc;
        pitch_ = alpha_ * pitch_ + (1.0 - alpha_) * pitchAcc;

        // Wrap angles to [-pi, pi] for consistency
        roll_  = wrapToPi(roll_);
        pitch_ = wrapToPi(pitch_);
        yaw_   = wrapToPi(yaw_);
    }

    // Get current posture in radians
    void getPosture(double& roll, double& pitch, double& yaw) const {
        roll  = roll_;
        pitch = pitch_;
        yaw   = yaw_;
    }

private:
    double alpha_;  // Complementary filter parameter
    double roll_;
    double pitch_;
    double yaw_;
    std::chrono::steady_clock::time_point lastTime_;
};

int main() {
    // Instantiate our ComplementaryFilter
    // alpha_ near 1.0 → rely heavily on gyro
    // alpha_ near 0.0 → rely heavily on accelerometer
    ComplementaryFilter filter(0.98);

    // For demonstration, we’ll simulate small random gyro noise.
    // In a real application, replace these with your actual sensor reads.
    std::default_random_engine generator;
    std::uniform_real_distribution<double> distributionGyro(-0.01, 0.01); // rad/s
    // We'll keep accel near (0, 0, 9.81) for "upright" scenario
    // but you can try randomizing that, too, for more “motion.”

    while (true) {
        // Simulated IMU values
        double gx = distributionGyro(generator);
        double gy = distributionGyro(generator);
        double gz = distributionGyro(generator);

        // For now, assume only gravity on Z
        double ax = 0.0;
        double ay = 0.0;
        double az = 9.81;

        // Update the filter
        filter.update(gx, gy, gz, ax, ay, az);

        // Get the current posture (roll, pitch, yaw)
        double roll, pitch, yaw;
        filter.getPosture(roll, pitch, yaw);

        // Convert to degrees for printing
        double rollDeg  = roll  * 180.0 / M_PI;
        double pitchDeg = pitch * 180.0 / M_PI;
        double yawDeg   = yaw   * 180.0 / M_PI;

        // Print
        std::cout << "Roll:  " << rollDeg
                  << "°, Pitch: " << pitchDeg
                  << "°, Yaw: " << yawDeg << "°" << std::endl;

        // Sleep ~10ms to simulate ~100 Hz loop
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

    return 0;
}

###################