IMU fusion and encoders

Gyro only integrates the body rate exactly, q ← q ⊗ exp(ω·dt/2). A 1 °/s bias is 60° a minute.

Accelerometer only takes roll = atan2(a_y, a_z) and pitch = atan2(−a_x, √(a_y² + a_z²)), and yaw from the tilt-compensated magnetometer (or zero). It is only right when the sensor is not accelerating.

Complementary: integrate the gyro, then turn the estimate a fraction 1 − α of the way to what the accelerometer says is up (about a horizontal axis, so it never touches yaw) and, with a magnetometer, that fraction of the way to north about the vertical. α is per sample, so the time constant is τ = α·dt / (1 − α).

Mahony (Mahony, Hamel & Pflimlin, 2008, as in Mahony’s AHRS code): the cross product between measured and predicted gravity (and field) is an error that feeds back into the gyro rate through a PI controller, ω′ = ω + K_pe + K_i∫e. The integral learns the gyro bias, which the page shows.

Madgwick (Madgwick, 2010): one normalised gradient-descent step per sample on the misfit between the predicted and measured gravity and field, q̇ = ½ q ⊗ ω − β ∇f/‖∇f‖. β is the rate (rad/s) at which it may correct; Madgwick suggests √(3/4) times the gyro’s error rate. The Jacobian here is derived for a field of [b_x, 0, b_z] and tested against finite differences.

Gyro bias: the mean of a still recording; the standard deviation is the white noise, and the bias is known to about σ/√N. MEMS gyro bias changes with temperature — measure it at start-up.

Accelerometer: with each axis up then down, offset = (up + down)/2 and scale = (up − down)/2 for that axis (the method in ST’s AN3192 and similar app notes). Corrected = (raw − offset) / scale.

Magnetometer: a least-squares fit of the general quadric ax² + by² + cz² + 2fyz + 2gxz + 2hxy
2px + 2qy + 2rz = 1 to the samples, after centring and scaling them. The centre is the hard-iron offset; the shape matrix M gives the soft-iron correction W = r·√M (its symmetric square root, scaled to the geometric-mean field strength r), so W(raw − centre) lies on a sphere (as in NXP’s AN4246). Coverage counts how many of 26 directions have a sample within 30°; below about 85 % the ellipsoid is poorly constrained, and a sphere fit is the safer choice.

The quadrature state is (A, B) with A leading B for forward rotation: 00 → 10 → 11 → 01. The 16-entry table is +1 for forward steps, −1 for reverse, 0 for no change and invalid when both lines change. x2 counts only A’s transitions, x1 only the 00 ↔ 10 pair, both with the table’s sign, which makes them bounce-proof too. The Arduino sketch uses exactly this table.

Interrupt costs in the resolution panel are rough figures for a lean ISR (including entry and exit) — measure yours with a scope on a spare pin.

Speed: the M method’s error is one count in N (N edges per window), the T method’s one clock tick in m (ticks per edge), the M/T method’s (Ohmae et al., IEEE Trans. Industrial Electronics, 1982) one tick over the whole window. The dots are simulated readings at a random phase.

AS5600: datasheet register map (RAW ANGLE 0x0C/0x0D, STATUS bits MD/ML/MH, AGC, MAGNITUDE, ZPOS/MPOS). AS5048A: 16-bit SPI frames with even parity in bit 15 and the error flag in bit 14; reading the angle is the command 0xFFFF. AS5048B: I²C angle registers 0xFE/0xFF.

Odometry uses the midpoint heading; the truth integrates exact arcs. The fused estimate blends the bias-corrected gyro with the wheels' turn rate (weight α on the gyro) and re-estimates the bias whenever the robot is stopped. UMBmark is from Borenstein & Feng, 1996.