基于IMU陀螺仪与加速度计数据的头部姿态角度计算咨询

Step 1: Preprocess Raw Data #

First, you need synchronized timestamped samples from both sensors. Your current gyro and accelerometer data have non-overlapping timestamps (gyro ~1685442192, accelerometer ~1685442458), so capture data from both at the same time for accurate fusion.

For each valid synchronized sample, extract:

• Gyroscope: ABS_X, ABS_Y, ABS_Z values + timestamp
• Accelerometer: ABS_X, ABS_Y, ABS_Z values + timestamp

Parsed example from your data:

# Gyro Sample
Timestamp: 1685442192.084194
Gyro_X: 1, Gyro_Y:164, Gyro_Z:9

# Accelerometer Sample
Timestamp: 1685442458.944228
Accel_X: -4100, Accel_Y:-923, Accel_Z:16880

Step 2: Convert Raw Values to Physical Units #

Raw IMU values are in LSB (least significant bits)—convert them to meaningful units using your sensor's datasheet sensitivity values:

Gyroscope (Angular Velocity, °/s) #

Use the gyro's sensitivity scale (e.g., 16 LSB/(°/s) for ±500°/s range):
angular_vel = raw_gyro_value / sensitivity_scale

Example (assuming 16 LSB/(°/s)):

• Gyro_X = 1 /16 = 0.0625 °/s
• Gyro_Y =164/16=10.25 °/s
• Gyro_Z=9/16=0.5625 °/s

Accelerometer (G-Force, g) #

Use accelerometer sensitivity (e.g.,16384 LSB/g for ±2g range):
accel_g = raw_accel_value / sensitivity_scale

Example:

• Accel_X = -4100 /16384 ≈-0.2503 g
• Accel_Y =-923/16384≈-0.0563 g
• Accel_Z=16880/16384≈1.0303 g

Step3: Compute Acceleration-Based Angles (accAngle) #

Accelerometer data gives static orientation relative to gravity. Calculate pitch (X-axis rotation) and roll (Y-axis rotation):

Pitch Angle (around X-axis): #

pitch_acc = arctan2(accel_Y, sqrt(accel_X² + accel_Z²)) * (180/π)

Roll Angle (around Y-axis): #

roll_acc = arctan2(-accel_X, accel_Z) * (180/π)

Using the accelerometer example:

• pitch_acc ≈ arctan2(-0.0563, 1.060) * 180/π ≈-3.07°
• roll_acc ≈ arctan2(0.2503,1.0303)*180/π≈13.65°

Note: Yaw (Z-axis) can't be calculated from accelerometer—you need a magnetometer for that.

Step4: Apply the Complementary Filter #

The formula angle = 0.98*(angle + gyro_data*dt) + 0.02*accAngle fuses gyro (dynamic, drift-prone) and accelerometer (static, noise-prone) data. Here's how to use it:

Variable Breakdown: #

• angle: The angle from the last iteration (initialize with accAngle for the first sample).
• gyro_data: Angular velocity of the corresponding axis (pitch uses gyro_X, roll uses gyro_Y) in °/s.
• dt: Time difference between current and previous sample (seconds) → current_timestamp - previous_timestamp.
• 0.98/0.02: Weights—prioritize gyro for fast motion, accelerometer to correct drift. Adjust based on your use case.

Example Calculation (Pitch Angle): #

Take two consecutive gyro samples:

1. First sample: Timestamp=1685442192.084194, gyro_X=0.0625 °/s
2. Second sample: Timestamp=1685442192.094199, gyro_X=0.125 °/s

Calculate dt: 0.010005 ≈0.01s.

Initialize pitch_angle with -3.07° (from accelerometer):

Second iteration:

pitch_angle = 0.98*(-3.07 + 0.0625*0.01) + 0.02*(-3.07)
= 0.98*(-3.069375) - 0.0614
≈ -3.008 -0.0614 ≈ -3.069°

Repeat this for every new sample, updating angle each time.

Critical Notes #

• Synchronized Data: Your current data has disjoint timestamps—fix this by capturing both sensors at the same time or interpolating timestamps.
• Sensitivity Values: Use your sensor's actual datasheet values (e.g., gyro might be 131 LSB/(°/s) for ±250°/s range).
• Yaw: To track yaw, add a magnetometer (compass) since gyro drifts over time and accelerometer can't measure it.
• Weights: Adjust 0.98/0.02—increase gyro weight (e.g.,0.99) for fast head movements, decrease it for static positions to correct drift faster.

内容的提问来源于stack exchange，提问作者ravi