python imu

 

import time
import math
import numpy as np

# A small helper function to limit angles between -pi and pi
def wrap_to_pi(angle):
    while angle > math.pi:
        angle -= 2.0 * math.pi
    while angle < -math.pi:
        angle += 2.0 * math.pi
    return angle

class ComplementaryFilter:
    def __init__(self, alpha=0.98):
        # alpha controls the blending of gyro vs. accel data
        self.alpha = alpha

        # Initialize roll, pitch, yaw to 0 for demonstration
        self.roll = 0.0
        self.pitch = 0.0
        self.yaw = 0.0
        
        self.last_time = time.time()

    def update(self, gx, gy, gz, ax, ay, az):
        """
        gx, gy, gz = gyroscope in rad/s
        ax, ay, az = accelerometer in m/s^2
        """
        current_time = time.time()
        dt = current_time - self.last_time
        self.last_time = current_time

        # 1) Integrate gyro to get incremental angles
        self.roll += gx * dt
        self.pitch += gy * dt
        self.yaw += gz * dt

        # 2) Convert accel to approximate roll/pitch
        #    Assuming sensor is mostly stable along 1g (i.e. z-axis)
        roll_acc = math.atan2(ay, az)
        pitch_acc = math.atan2(-ax, math.sqrt(ay**2 + az**2))

        # 3) Complementary filter blending
        self.roll = self.alpha * self.roll + (1.0 - self.alpha) * roll_acc
        self.pitch = self.alpha * self.pitch + (1.0 - self.alpha) * pitch_acc
        
        # Normalize angles to [-pi, pi]
        self.roll = wrap_to_pi(self.roll)
        self.pitch = wrap_to_pi(self.pitch)
        self.yaw = wrap_to_pi(self.yaw)

        return self.roll, self.pitch, self.yaw

def demo_imu_without_ros():
    # Example usage of ComplementaryFilter
    filter_ = ComplementaryFilter(alpha=0.98)

    # In real code, you would read from actual IMU here
    # For demo, we'll just simulate small random values
    import random
    
    while True:
        # Fake sensor readings
        gx = random.uniform(-0.01, 0.01)  # rad/s
        gy = random.uniform(-0.01, 0.01)
        gz = random.uniform(-0.01, 0.01)
        
        ax = 0.0  # pretend no acceleration except gravity on Z
        ay = 0.0
        az = 9.81
        
        roll, pitch, yaw = filter_.update(gx, gy, gz, ax, ay, az)
        
        # Print updated posture
        print(f"Roll={math.degrees(roll):.2f}°, Pitch={math.degrees(pitch):.2f}°, Yaw={math.degrees(yaw):.2f}°")

        # Sleep for demonstration
        time.sleep(0.01)

if __name__ == "__main__":
    demo_imu_without_ros()

 

 

 

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