Sensor Fusion — Complementary, Mahony and Madgwick Filters, and the Gyro over an Optical Fix

4.1 The Problem Fusion Solves

Volume 3 ended with two kinds of sensor. A gyroscope responds within a millisecond and has almost no noise, but its integral drifts by tens of pixels per second. An absolute reference — gravity for “down”, the camera for where the gun points — does not drift, but is wrong while the gun is accelerating (gravity), slow (a camera at 209 Hz), or simply absent (markers out of view). Fusion is the arithmetic that uses each where it is good: the gyroscope for what changed in the last few milliseconds, the reference for where things really are over seconds.

The smoothing filters that turn a noisy camera position into a steady cursor — averaging, the One-Euro filter in the ESP32 port — are a different job and are covered in Aiming Maths and Calibration. Fusion feeds them; it does not replace them.

4.2 The Complementary Filter

For one axis the whole idea fits in a line:

θ ← α·(θ + ω·Δt) + (1 − α)·θ_ref, with α = τ / (τ + Δt).

The gyroscope’s contribution passes through a high-pass filter with time constant τ; the reference’s through the complementary low-pass. Below the crossover frequency 1/(2πτ) the reference wins, above it the gyroscope. With τ = 1 s and 1 kHz samples, α = 0.999 and the crossover is 0.16 Hz: the gyroscope carries every hand movement, and the reference slowly pulls the estimate back to the truth.

Its one weakness is the reference being wrong. An accelerometer measures gravity plus the gun’s own acceleration. During a swing it is off by the swing’s acceleration; during a solenoid kick it is off by several g. A complementary filter takes those as tilt. The usual fixes are to drop the reference’s weight when its magnitude is far from 1 g, and — for this gun — to freeze it outright while the solenoid fires (the recoil gate of Volume 6).

And it does not learn the bias. A constant gyroscope bias b leaves a steady offset in θ of about b·τ — 0.05 °/s for one second is 0.05°, small — but during a gap in the reference the estimate drifts at the full b. A filter that estimates b removes the drift as well as the offset.

4.3 Mahony: Learning the Bias

Robert Mahony, Tarek Hamel and Jean-Michel Pflimlin’s 2008 paper, “Nonlinear Complementary Filters on the Special Orthogonal Group”, generalises the complementary filter to full three-dimensional orientation and gives it an integral term.¹ In the form most implementations use:

• the orientation estimate q is propagated with the gyroscope rate plus a correction δ;
• each reference vector the sensors measure — gravity from the accelerometer, magnetic north from a magnetometer, or any other known direction — is compared with where the current estimate predicts it should be, and the cross product of the two is an error e;
• the correction is δ = K_p·e + K_i·∫e dt.

The integral term is the point. At steady state the only way for ∫e to stop changing is for it to cancel the gyroscope’s bias, so the integrator converges on the bias and the filter keeps estimating it as long as references keep arriving. With gravity alone, only the pitch and roll components of the bias are observable; the yaw bias needs a yaw reference. A camera that sees the screen is one.

4.4 Madgwick, and Where Kalman Fits

Sebastian Madgwick’s filter (internal report 2010; with Andrew Harrison and Ravi Vaidyanathan at ICORR 2011) does the same job with a different correction: one step of gradient descent on the error between measured and predicted reference directions, scaled by a gain β that Madgwick relates to the gyroscope’s measurement error (β = √(3/4)·ω̃_β, with ω̃_β the gyroscope’s error in each axis).² Its appeal is that it is cheap, needs one tuning parameter and is published with open code; the variant with gyroscope bias compensation adds the same kind of integral as Mahony’s.

A Kalman filter (in practice an extended or error-state Kalman filter for orientation) is the general case: it carries the uncertainty of each state, weights each measurement by its noise, and handles references that arrive at different rates. It is what a product with a camera, three inertial sensors and a magnetometer would use. For a light gun it is more than the problem needs (§4.8).

4.5 A Gyroscope over a Camera Fix

The light-gun version of fusion is simpler than any of the above, because the reference — the camera — measures the aim point directly. The gyroscope’s jobs are:

1. Bridging a gap. When the markers are lost — the gun swung past the screen’s edge, a hand across the camera, a sunlit window swamping the sensor — the gyroscope carries the aim forward from the last good fix.
2. Filling between frames. The camera delivers a position every 4.8 ms; the gyroscope can update the aim every millisecond in between. Whether that helps depends on the rest of the latency chain (Aiming Maths and Calibration works the budget); it does not change where the cursor ends up.
3. Telling the firmware what happened off-screen. Volume 6.

How well it bridges depends on how well the bias is known when the gap begins, and that is where the camera helps twice: while the markers are in view, the difference between the camera’s motion and the integrated gyroscope over each interval is a direct measurement of the yaw and pitch bias. The calculation script puts numbers on a bridged gap at 2 m:

Table 1 — How well it bridges depends on how well the bias is known when the gap begins, and that is where the camera helps twice: while the markers are in view, the difference between the camera's motion and the integrated gyroscope over each interval is a direct measurement of the yaw and pitch bias. The calculation script puts numbers on a bridged gap at 2 m

Error source	0.25 s gap	1 s gap	2 s gap
Residual bias 0.01 °/s	0.1 px	0.6 px	1.1 px
Residual bias 0.05 °/s	0.7 px	2.8 px	5.5 px
Scale error 0.5 % on a 10° swing	2.8 px	(per swing, not per second)
Scale error 0.5 % on a 30° swing	8.3 px
Gun translated 2 cm, no rotation	32 px	(unseen by the gyro)
Gun translated 5 cm, no rotation	79 px

The bias terms are small; the scale error matters for big swings; the translation term dominates. A gyroscope bridges a gap correctly only if the player turns the gun rather than moving it, and a player swinging past the edge of the screen to reload is doing both. That sets the honest use: bridge short gaps, keep the bridge short, and when the camera comes back, blend to it rather than jump.

4.6 How the Products Did It

Wii MotionPlus (2009). Gyroscopes added to a remote that already had an accelerometer and an infrared camera (Volume 3, §3.9). InvenSense’s release described the result, with the remote’s accelerometer and the Sensor Bar, as “more comprehensive tracking of a player’s arm position and orientation”.³ The division of labour is the one above: gyros for fast motion, the accelerometer for down, the Sensor Bar’s camera for yaw when the remote faces the screen.

PlayStation Move (2010). The reverse arrangement: the camera at the television tracks the glowing sphere for position, and the controller’s gyroscope, accelerometer and magnetometer report orientation (Volume 3, §3.8). History of Light Guns, Volume 7 states the consequence for aiming: the aim “is only as good as the orientation estimate and the calibration that ties the two together”. The Sharp Shooter in Volume 1’s hero photograph is that system in a gun shell.

Freespace pointers (2007 on). Hillcrest Laboratories’ Freespace, as sold in Logitech’s MX Air mouse in 2007, is a pointer with no camera at all. Logitech’s release (12 July 2007) describes it as “based on a combination of MEMS (microelectromechanical systems) sensors” and says “sophisticated algorithms distinguish between intentional and unintentional hand movements”.⁴ Hillcrest’s line continued, after CEVA bought the company, in the BNO08x fusion chips (§4.7). A Freespace pointer moves a cursor relative to where it was, like a mouse; it does not know where the screen is.

Gyro as a mouse (the modern consoles’ gyro aiming). The gyro-aiming community’s design rule is the same conclusion reached from the other side: treat the gyroscope as relative input, give the player a button to disengage it (“lifting the mouse”), and do not try to recover absolute orientation — “this isn’t VR, this is a mouse” (Jibb Smart, 2019).⁵ It works in first-person games because the game draws the cursor and the player corrects by eye. It is not a light gun, because nothing ties the cursor to where the barrel points.

The pattern across all four is consistent: no product that wanted absolute pointing relied on inertial sensors alone. Each paired them with a camera, or gave up absolute pointing.

4.7 Fusion on a Chip

CEVA’s BNO085, sold on an Adafruit breakout (product 4754, $29.50 for one, in stock on 2026-10-19), runs Hillcrest’s SH-2 firmware on a Cortex-M0 inside the package and reports orientation as a quaternion, gravity, linear acceleration and “application-optimized rotation vectors for AR/VR”; Adafruit notes that it “uses the same hardware as the BNO055 but very different firmware”.⁶

For a camera gun it is the wrong shape. Its absolute yaw comes from its magnetometer (Volume 3, §3.8 rules that out beside a solenoid), and its magnetometer-free “game” rotation vector drifts in yaw like any gyroscope. The correction against the camera — the only yaw reference the gun has — has to happen in the gun’s own microcontroller either way, so the on-chip filter adds cost and a second layer of filtering without removing the work. It is listed among the candidates in Volume 6 for completeness.

4.8 How Little a Light Gun Needs

A full orientation filter estimates three angles in a world frame. A camera gun needs much less, because the camera already gives the aim point and its roll (the four-point solution recovers roll; see IR Point Tracking, Volume 3). During a short gap the firmware only needs:

1. the gyroscope’s two rates about the camera’s horizontal and vertical axes — the IMU mounted square to the camera so its axes match;
2. those rates rotated by the last known roll, so that “turning left” means left on the screen even when the gun is canted;
3. the integral of each over the gap, times the pixels-per-degree factor the camera’s last fixes imply;
4. a bias estimate per axis, updated while the camera is valid.

That is two multiply-adds per axis per sample plus a slow bias update: trivial on the RP2040’s Cortex-M0+ cores even in software floating point, where a full Madgwick or Kalman update would be the most expensive thing in the loop. The full filters are the right tool when there is no camera, or when the gun must estimate its pose for its own sake — for a VR-style game, say. For bridging gaps in an aim the camera already measures, the integrator with a learned bias is enough, and it is what Volume 6 proposes.

4.9 What Could Not Be Verified

• How the PlayStation Move combines camera position and inertial orientation. Sony’s algorithm is not public.
• Freespace’s tremor-filtering method: only Logitech’s one-line description was found.
• The BNO085’s game-rotation-vector yaw drift in numbers: CEVA’s figures were not read; the drift is stated from first principles.
• The cost of a Madgwick or Mahony update on the RP2040 in cycles: not measured. It belongs on the bench list in Volume 6.

Sources

Footnotes

1. Robert Mahony, Tarek Hamel and Jean-Michel Pflimlin, “Nonlinear Complementary Filters on the Special Orthogonal Group”, IEEE Transactions on Automatic Control 53(5), 2008, pp. 1203–1218, doi:10.1109/TAC.2008.923738. ↩

2. Sebastian O. H. Madgwick, An efficient orientation filter for inertial and inertial/magnetic sensor arrays, internal report, University of Bristol, 2010; and S. O. H. Madgwick, A. J. L. Harrison and R. Vaidyanathan, “Estimation of IMU and MARG orientation using a gradient descent algorithm”, IEEE International Conference on Rehabilitation Robotics (ICORR), 2011, doi:10.1109/ICORR.2011.5975346. ↩

3. Nintendo World Report, “Wii MotionPlus Technology Provider Reveals the Guts Behind the Device”, 17 July 2008, reporting InvenSense’s release of 15 July 2008, https://www.nintendoworldreport.com/news/16410/wii-motionplus-technology-provider-reveals-the-guts-behind-the-device, read 2026-10-19. ↩

4. Logitech, “Logitech Introduces the Future of PC Navigation”, 12 July 2007, as reproduced by DigiTimes, https://www.digitimes.com/news/a20070712PR202.html, read 2026-10-19 (Logitech’s own page returned HTTP 429). ↩

5. Jibb Smart, “Good Gyro Controls Part 1: The Gyro is a Mouse”, GyroWiki, 23 May 2019, http://gyrowiki.jibbsmart.com/blog:good-gyro-controls-part-1:the-gyro-is-a-mouse, read 2026-10-19. ↩

6. Adafruit, “Adafruit 9-DOF Orientation IMU Fusion Breakout - BNO085 (BNO080)”, product 4754, https://www.adafruit.com/product/4754, read 2026-10-19. ↩