Problem 610764 · easy · Level 06 Heuristics & Optimization

Tilt of a Balancing Robot From Two Cheap Sensors

complementary filter · sensor fusion · gyroscope · accelerometer · drift

A two-wheeled balancing robot needs its tilt angle (degrees from upright, positive leaning forward) a hundred times a second. It has two cheap sensors, each with a flaw:

  • a gyroscope measures the rate of tilt gyro[k] (degrees per second). Adding up rate * dt gives the angle smoothly, but every tiny bias adds up too, and the angle drifts away over a minute.
  • an accelerometer measures gravity along the robot's forward axis ax[k] and its vertical axis az[k] (in units of g). The angle degrees(atan2(ax, az)) never drifts, but every bump and wheel jolt shakes it.

A complementary filter takes the gyro's short-term changes and the accelerometer's long-term level. Start with angle = degrees(atan2(ax[0], az[0])); then for each later sample k = 1, 2, ...:

acc_angle = degrees(atan2(ax[k], az[k]))
angle = alpha * (angle + gyro[k] * dt) + (1 - alpha) * acc_angle

With alpha near 1 (such as 0.98) the gyro dominates from one sample to the next, while the small share of the accelerometer slowly pulls any drift back.

Write tilt(gyro, ax, az, dt, alpha) that returns the list of angles, one per sample.

Examples

Input:  gyro = [0.0, 10.0, 10.0, 0.0], ax = [0.0, 0.02, 0.05, 0.03], az = [1.0, 1.0, 0.99, 1.0], dt = 0.1, alpha = 0.9
Output: [0.0, 1.0145762838175103, 2.1022456150578157, 2.0638568537175797]
Explanation: at k = 1 the accelerometer says 1.146°, the gyro says 0 + 10 * 0.1 = 1°; the blend is
0.9 * 1 + 0.1 * 1.146 = 1.015°.

Input:  gyro = [2.0, 2.0, 2.0], ax = [0.0, 0.0, 0.0], az = [1.0, 1.0, 1.0], dt = 0.01, alpha = 0.98
Output: [0.0, 0.0196, 0.038807999999999995]
Explanation: a gyro with a bias of 2 °/s on a robot standing still. Integrated alone it would drift by
2° every second; the accelerometer holds the filtered angle to a steady error of 0.98°.

Constraints

  • gyro, ax and az have the same length; math.atan2 and math.degrees do the conversion
  • answers are compared with a tolerance of 1e-6

Goals

  • Turn an accelerometer's two axes into a tilt angle with atan2
  • Blend an integrated gyro rate with the accelerometer angle in one line per sample
  • Explain why each sensor covers the other's weakness: gyro drift against accelerometer jitter
Starting Python…