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()
#####################
QQ 3087438119