Calibrate IMUs for Stable Bipedal Walking in 45 Minutes

# Verify your IMU connection first
import smbus2
import time

bus = smbus2.SMBus(1)
IMU_ADDR = 0x68  # MPU6050 default

try:
    whoami = bus.read_byte_data(IMU_ADDR, 0x75)
    print(f"IMU detected: 0x{whoami:02X}")  # Should be 0x68
except:
    print("ERROR: IMU not responding - check wiring")

import numpy as np

def calibrate_gyro_bias(imu, samples=1000):
    """
    Collect stationary readings to find gyro zero point.
    This removes the constant drift error.
    """
    gyro_readings = []
    
    for i in range(samples):
        # Read gyro X, Y, Z (adjust for your IMU library)
        gx, gy, gz = imu.read_gyro_raw()
        gyro_readings.append([gx, gy, gz])
        time.sleep(0.01)  # 100Hz sampling
    
    # Calculate mean as bias offset
    bias = np.mean(gyro_readings, axis=0)
    
    # Verify standard deviation is low (sensor is stable)
    std = np.std(gyro_readings, axis=0)
    
    print(f"Gyro bias: X={bias[0]:.3f}, Y={bias[1]:.3f}, Z={bias[2]:.3f} °/s")
    print(f"Noise: X={std[0]:.3f}, Y={std[1]:.3f}, Z={std[2]:.3f} °/s")
    
    # Store these values - subtract from all future readings
    return bias

# Usage
gyro_bias = calibrate_gyro_bias(imu)
# Save to config file or EEPROM

def calibrate_accelerometer_6pos(imu, samples_per_position=500):
    """
    6-position tumble test for accelerometer calibration.
    Corrects bias and scale factor simultaneously.
    """
    positions = {
        'X_up': [0, 0, 1],    # X-axis pointing up
        'X_down': [0, 0, -1],
        'Y_up': [0, 1, 0],
        'Y_down': [0, -1, 0],
        'Z_up': [1, 0, 0],    # Z-axis pointing up (robot standing)
        'Z_down': [-1, 0, 0]
    }
    
    measurements = {}
    
    for pos_name, expected_g in positions.items():
        input(f"Position robot with {pos_name}, then press Enter...")
        
        readings = []
        for _ in range(samples_per_position):
            ax, ay, az = imu.read_accel_raw()  # Raw ADC counts
            readings.append([ax, ay, az])
            time.sleep(0.01)
        
        measurements[pos_name] = np.mean(readings, axis=0)
        print(f"{pos_name}: {measurements[pos_name]}")
    
    # Solve for bias and scale using least squares
    # See: "Six-Position Test for MEMS Inertial Sensors" (IEEE)
    bias, scale = solve_calibration_params(measurements, positions)
    
    return bias, scale

def solve_calibration_params(measurements, positions):
    """
    Fit: accel_corrected = (accel_raw - bias) * scale
    to match 1g in each orientation.
    """
    # Build equations: (raw - bias) * scale = expected_g
    # This is simplified - use full matrix solution for production
    
    # Average opposite positions to find bias
    bias_x = (measurements['X_up'][0] + measurements['X_down'][0]) / 2
    bias_y = (measurements['Y_up'][1] + measurements['Y_down'][1]) / 2
    bias_z = (measurements['Z_up'][2] + measurements['Z_down'][2]) / 2
    bias = np.array([bias_x, bias_y, bias_z])
    
    # Calculate scale from amplitude
    gravity = 9.81  # m/s² (adjust for local gravity)
    scale_x = gravity / ((measurements['X_up'][0] - measurements['X_down'][0]) / 2)
    scale_y = gravity / ((measurements['Y_up'][1] - measurements['Y_down'][1]) / 2)
    scale_z = gravity / ((measurements['Z_up'][2] - measurements['Z_down'][2]) / 2)
    scale = np.array([scale_x, scale_y, scale_z])
    
    print(f"Accel bias: {bias}")
    print(f"Accel scale: {scale}")
    
    return bias, scale

def test_calibration(imu, gyro_bias, accel_bias, accel_scale):
    """
    Rotate robot slowly and check for drift reduction.
    """
    print("Rotate robot through 360° slowly (30 sec), then return to start")
    input("Press Enter to begin...")
    
    start_time = time.time()
    integrated_angle = np.array([0.0, 0.0, 0.0])
    
    while time.time() - start_time < 30:
        # Apply calibration
        gx, gy, gz = imu.read_gyro_raw()
        gyro_corrected = np.array([gx, gy, gz]) - gyro_bias
        
        ax, ay, az = imu.read_accel_raw()
        accel_corrected = (np.array([ax, ay, az]) - accel_bias) * accel_scale
        
        # Simple integration (use Madgwick/Mahony filter in production)
        dt = 0.01
        integrated_angle += gyro_corrected * dt
        
        time.sleep(dt)
    
    print(f"Final angle error: {integrated_angle} degrees")
    print(f"Should be near [0, 0, 0] - absolute error <2° is good")
    
    # Check accelerometer magnitude
    accel_mag = np.linalg.norm(accel_corrected)
    print(f"Accel magnitude: {accel_mag:.2f} m/s² (should be ~9.81)")
    
    return np.linalg.norm(integrated_angle) < 2.0  # Pass if <2° drift

# Usage
if test_calibration(imu, gyro_bias, accel_bias, accel_scale):
    print("✓ Calibration verified - deploy to robot")
else:
    print("✗ Calibration failed - repeat Step 2-3")

import json

def save_calibration(filename='/etc/robot/imu_cal.json'):
    """
    Save to filesystem (for SBC) or EEPROM (for microcontroller).
    """
    cal_data = {
        'timestamp': time.time(),
        'sensor_id': 'MPU6050_TORSO',
        'gyro_bias': gyro_bias.tolist(),
        'accel_bias': accel_bias.tolist(),
        'accel_scale': accel_scale.tolist(),
        'temperature': imu.read_temperature()  # Calibration temp
    }
    
    with open(filename, 'w') as f:
        json.dump(cal_data, f, indent=2)
    
    print(f"Saved calibration to {filename}")

def load_calibration(filename='/etc/robot/imu_cal.json'):
    """
    Load at boot and apply to IMU readings.
    """
    try:
        with open(filename, 'r') as f:
            cal_data = json.load(f)
        
        # Check if calibration is stale (>30 days old)
        age_days = (time.time() - cal_data['timestamp']) / 86400
        if age_days > 30:
            print(f"WARNING: Calibration is {age_days:.0f} days old - recalibrate")
        
        return (
            np.array(cal_data['gyro_bias']),
            np.array(cal_data['accel_bias']),
            np.array(cal_data['accel_scale'])
        )
    except FileNotFoundError:
        print("ERROR: No calibration file - run calibration first")
        return None, None, None

# For microcontrollers (Arduino/ESP32):
# Use EEPROM.put(addr, struct) to store floats
# See: https://docs.arduino.cc/learn/programming/eeprom-guide

class IMUFilter:
    """
    Complementary filter with calibrated IMU data.
    For production, use Madgwick or EKF.
    """
    def __init__(self, gyro_bias, accel_bias, accel_scale):
        self.gyro_bias = gyro_bias
        self.accel_bias = accel_bias
        self.accel_scale = accel_scale
        self.angle = np.array([0.0, 0.0, 0.0])  # Roll, pitch, yaw
        self.alpha = 0.98  # Complementary filter weight
    
    def update(self, imu, dt):
        # Read and calibrate
        gx, gy, gz = imu.read_gyro_raw()
        gyro = np.array([gx, gy, gz]) - self.gyro_bias
        
        ax, ay, az = imu.read_accel_raw()
        accel = (np.array([ax, ay, az]) - self.accel_bias) * self.accel_scale
        
        # Gyro integration (high-pass)
        self.angle += gyro * dt
        
        # Accelerometer angle (low-pass)
        accel_roll = np.arctan2(accel[1], accel[2])
        accel_pitch = np.arctan2(-accel[0], np.sqrt(accel[1]**2 + accel[2]**2))
        
        # Complementary fusion
        self.angle[0] = self.alpha * self.angle[0] + (1 - self.alpha) * accel_roll
        self.angle[1] = self.alpha * self.angle[1] + (1 - self.alpha) * accel_pitch
        
        return self.angle  # Return roll, pitch, yaw in radians

# Usage in balance control loop
imu_filter = IMUFilter(gyro_bias, accel_bias, accel_scale)

while robot.is_walking():
    dt = 0.01  # 100Hz control loop
    
    roll, pitch, yaw = imu_filter.update(imu, dt)
    
    # Feed to balance controller
    torso_tilt = pitch
    balance_controller.update(torso_tilt)
    
    time.sleep(dt)

# Command robot to walk 20 steps in place
rosrun my_robot walk_in_place.py --steps 20

# Log IMU data during walk test
# Plot: gyro should stay near zero when standing still
# Plot: accel magnitude should be ~9.81 m/s² at stance phases

def check_calibration_health(imu_filter, threshold_deg=1.0):
    """
    Monitor for calibration degradation during operation.
    Trigger recalibration if drift exceeds threshold.
    """
    if robot.is_standing_still():
        gyro_reading = imu.read_gyro_raw() - gyro_bias
        drift = np.linalg.norm(gyro_reading)
        
        if drift > threshold_deg:
            print(f"WARNING: Gyro drift detected ({drift:.2f}°/s)")
            print("Schedule recalibration within 24 hours")
            return False
    
    return True

# Example: Torso + Left Foot + Right Foot
imu_torso = IMUFilter(gyro_bias_torso, accel_bias_torso, accel_scale_torso)
imu_left_foot = IMUFilter(gyro_bias_left, accel_bias_left, accel_scale_left)
imu_right_foot = IMUFilter(gyro_bias_right, accel_bias_right, accel_scale_right)