Robotics Fundamentals 1, Chapter 6: Challenge 2 solution (gyro, accelerometer, compass and the complementary filter)

Formulas: rate = raw / sensitivity ;  angle += rate x dt ;  drift = bias x t
          roll = atan2(ay, az) ;  pitch = atan2(ax, sqrt(ay^2 + az^2)) ;  |a| = sqrt(ax^2 + ay^2 + az^2)
          complementary: angle(new) = alpha (angle + gyro dt) + (1 - alpha) accel_angle ;  tau = alpha dt / (1 - alpha) ;  bias error = tau b
          compass: heading = atan2(my, mx) ;  offset = (max + min) / 2

(a) Sensitivity on the +-500 degree/s range:  32768 / 500 = 65.536 LSB per degree/s (the datasheet value is about 65.5).
    Bias:  33 / 65.536 = 0.5035 degrees/s.    Drift:  0.5035 x 60 = 30.2 degrees per minute.
    While turning:  raw 3277 / 65.536 = 50.00 degrees/s before subtracting the bias.
    Corrected:      (3277 - 33) / 65.536 = 3244 / 65.536 = 49.50 degrees/s.
    Angle turned in 3 s at the corrected rate:  49.50 x 3 = 148.5 degrees.  (Using the uncorrected rate would give 150.0, an excess of 1.5 degrees from the bias alone.)

(b) ax = 4096 / 16384 = 0.2500 g.    az = 15854 / 16384 = 0.9677 g.
    Pitch = atan2(0.2500, 0.9677) = 14.49 degrees.
    |a| = sqrt(0.0625 + 0.9364) = sqrt(0.9989) = 0.9994 g, within 0.1 % of 1 g: consistent, so the reading can be trusted.
    Accelerating forward at 0.2 g:  ax = 0.2500 + 0.2 = 0.4500 g (az unchanged).
    Pitch reported = atan2(0.4500, 0.9677) = 24.94 degrees, which is an error of 24.94 - 14.49 = 10.45 degrees.
    |a| = sqrt(0.2025 + 0.9364) = sqrt(1.1389) = 1.067 g.
    A gate of 1.00 g +- 0.05 g (0.95 to 1.05) rejects it, because 1.067 is outside it. A gate of +-0.1 g would NOT (1.067 is inside 0.9 to 1.1), so the width of the gate is a trade: too
    narrow and the tilt is almost never trusted, too wide and a smaller error passes. The filter of part (c) is what makes the remaining errors harmless.

(c) tau = alpha dt / (1 - alpha) = 0.95 x 0.02 / 0.05 = 0.38 s.
    The robot is still at 10 degrees, accel_angle = 10, gyro rate reads 0.5 degrees/s (the bias), dt = 0.02 so gyro x dt = 0.01 degrees per step, and the filter starts at 0.
        Step 1:  0.95 x (0 + 0.01) + 0.05 x 10 = 0.0095 + 0.5 = 0.5095
        Step 2:  0.95 x (0.5095 + 0.01) + 0.5 = 0.95 x 0.5195 + 0.5 = 0.4935 + 0.5 = 0.9935
        Step 3:  0.95 x (0.9935 + 0.01) + 0.5 = 0.95 x 1.0035 + 0.5 = 0.9533 + 0.5 = 1.4533
        Step 4:  0.95 x (1.4533 + 0.01) + 0.5 = 1.3896 + 0.5 = 1.8902
        Step 5:  0.95 x (1.8902 + 0.01) + 0.5 = 1.8052 + 0.5 = 2.3052
    Final value:  accel_angle + tau x b = 10 + 0.38 x 0.5 = 10.19 degrees (a bias error of only 0.19 degrees, instead of 30 degrees a minute for the gyro alone).
    Time to cover 95 % of the way:  about 3 tau = 3 x 0.38 = 1.14 s (about 57 steps; after 57 steps the gap has shrunk to 0.95^57 = 5.4 % of its start value).

(d) The sensor reads (28, 16), so heading = atan2(16, 28) = 29.74 degrees.  The true reading (20, 20) gives atan2(20, 20) = 45.00 degrees, so the error is 29.74 - 45.00 = -15.26 degrees.
    Hard-iron offsets:  mx offset = (36 + (-20)) / 2 = 8.     my offset = (24 + (-32)) / 2 = -4.     (These match the shift between (20, 20) and (28, 16).)
    Corrected reading:  (28 - 8, 16 - (-4)) = (20, 20), so the corrected heading is atan2(20, 20) = 45.0 degrees.
    (A real calibration would also correct the squashing of the circle by iron nearby, the soft-iron effect, which this simple offset does not fix.)