Mixed-Precision Gyro Filter

Source: tests/examples/dsp_lowpass.cpp (runs in CI on every build).

Flight controllers low-pass the gyro signal to strip motor and propeller vibration before the rate loop sees it. This example builds that filter and asks a hardware question: which number format should it run in?

The signal

A gyro on a multirotor measures two things at once: the body motion the controller cares about, and vibration from the motors and propellers, which it does not. Here the motion is a 2 Hz roll rate (0.5 rad/s) and the vibration a 180 Hz tone (0.3 rad/s), sampled at 1 kHz:

200 ms around t = 1 s. The vibration is nearly as large as the motion and 90 times faster; fed straight into the rate controller, it would make the motors chase noise. Hover (or focus and use the arrow keys) for values.

The filter

A 2nd-order Butterworth low-pass with cutoff $f_c = 30$ Hz at sample rate $f_s = 1$ kHz, designed with the (prewarped) bilinear transform. The terms are explained below:

$$K = \tan\frac{\pi f_c}{f_s}, \qquad H(z) = \frac{b_0 + b_1 z^{-1} + b_2 z^{-2}}{1 + a_1 z^{-1} + a_2 z^{-2}}$$

realized in transposed direct form II as a 2-state state-space system with MTL5 dense matrices:

$$x_{k+1} = A x_k + B u_k, \qquad y_k = C x_k + D u_k$$

In double the design meets its spec: $|H(2\,\text{Hz})| = 0.99999$, and the 180 Hz vibration is cut by 33.1 dB, to about 2.2% of its amplitude:

The settled run (t = 0.5 to 2 s; the first 0.5 s start-up transient is excluded). The thick gray band is what the gyro reports; the filter output sits on the true motion. At this scale the output's delay is invisible; the zoom below shows it.

The price: delay

A filter cannot remove the vibration without also delaying the motion it passes. Zoomed in on the moment the roll rate crosses zero:

60 ms around the rising zero crossing at t = 1 s. The filter output crosses zero 7.5 ms after the true motion. The small wobble on the output is the residual 180 Hz ripple.

From the exact coefficients, evaluating $H$ on the unit circle:

Frequency	Gain $\lvert H \rvert$	Phase	Phase delay	Group delay
2 Hz (body motion)	0.99999 (−0.0 dB)	−5.4°	7.49 ms	7.51 ms
30 Hz (= $f_c$)	0.7071 (−3.0 dB)	−90.0°	8.33 ms	7.55 ms
180 Hz (vibration)	0.0222 (−33.1 dB)	−167.8°	2.59 ms	0.24 ms

• Phase delay is how far a sinusoid at that frequency is shifted in time, $-\angle H / (2\pi f)$. Group delay is how far the envelope of a narrow band around it is shifted, $-d\angle H / d\omega$. Across the passband the two agree, about 7.5 ms from DC up to $f_c$, so the motion comes out as a near-pure time shift, not a distortion.
• The test suite measures the lag (by projecting the filtered motion onto sine and cosine over three settled periods) and requires it to match the analytic phase delay to within 50 µs. Today they agree to the microsecond: 7.491 ms.

Why it matters. The rate controller now sees the vehicle’s motion 7.5 ms late. Delay in a feedback loop costs phase margin: the controller corrects for where the vehicle was. Lowering $f_c$ removes more vibration but adds more delay. That is the central trade-off in gyro filtering, and the reason flight controllers track the motor frequency with narrow dynamic notch filters instead of reaching for an ever-lower low-pass.

Number formats compared

The same filter runs in each format, and its output is compared with the double reference (signal RMS 0.354 rad/s):

Format	Bits	Error RMS (rad/s)	SNR
float	32	5.5e-7	116.1 dB
posit<32,2>	32	2.6e-8	142.8 dB
IEEE half (cfloat<16,5>)	16	5.0e-3	37.0 dB
Q3.12 fixed point (fixpnt<16,12>)	16	3.3e-3	40.6 dB
posit<16,1>	16	1.1e-3	49.9 dB

Identical under GCC 13 and Clang 18.

Reading it. A gyro signal lives near magnitude 1, where posits put most of their precision (tapered accuracy). At 32 bits that makes posit<32,2> about 27 dB more accurate than float. At 16 bits, the width an embedded DSP path or a narrow accelerator datapath would use, posit<16,1> beats both IEEE half (+13 dB) and the classic Q-format fixed-point choice (+9 dB). Fixed point needs the designer to pick the binary point; the posit needs no such tuning.

The terms explained

$f_s$, the sample rate (1 kHz). How often the gyro is read. A sampled signal can only represent frequencies up to the Nyquist frequency $f_s/2$ = 500 Hz; everything in a digital filter’s design lives on the interval $0 \ldots f_s/2$.

$f_c$, the cutoff frequency (30 Hz). Where the filter’s gain has fallen to $1/\sqrt{2}$, i.e. −3 dB (half the power). Below $f_c$ the motion passes almost unchanged. Above it the gain falls at 40 dB per decade, the slope of a 2nd-order filter: −33 dB by 180 Hz. Butterworth is the choice of filter shape: the flattest possible passband, with no ripple below $f_c$.

$H(z)$, the transfer function. The filter’s input-to-output relation in the $z$-domain. Each $z^{-1}$ means “one sample ago”, so

$$H(z) = \frac{b_0 + b_1 z^{-1} + b_2 z^{-2}}{1 + a_1 z^{-1} + a_2 z^{-2}}$$

is a compact way of writing the difference equation the filter executes every sample:

$$y_k = b_0 u_k + b_1 u_{k-1} + b_2 u_{k-2} - a_1 y_{k-1} - a_2 y_{k-2}$$

The $b$ coefficients weight recent inputs; the $a$ coefficients feed back recent outputs, which is what makes this a recursive (IIR) filter, and what makes it cheap: five multiplies per sample. Here $b_0 = b_2 = 0.00782$, $b_1 = 0.01564$, $a_1 = -1.7347$, $a_2 = 0.7660$. Setting $z = e^{j 2\pi f / f_s}$, a point on the unit circle, gives the frequency response: $|H|$ is the gain and $\angle H$ the phase in the table above.

The example runs the same filter in state-space form. With two internal states holding the “memory” of past samples (transposed direct form II):

$$A = \begin{bmatrix} -a_1 & 1 \\ -a_2 & 0 \end{bmatrix},\quad B = \begin{bmatrix} b_1 - a_1 b_0 \\ b_2 - a_2 b_0 \end{bmatrix},\quad C = \begin{bmatrix} 1 & 0 \end{bmatrix},\quad D = b_0 .$$

Same input-output behavior, written as matrix operations MTL5 can execute in any number type.

The bilinear transform and $K$. Butterworth filters are defined in continuous time (the analog $s$-domain). The bilinear transform turns an analog design into a digital one by substituting

$$s = 2 f_s \, \frac{1 - z^{-1}}{1 + z^{-1}} .$$

This mapping is stable and exact in shape, but it squeezes the analog frequency axis, which runs to infinity, into the digital interval $0 \ldots f_s/2$. Frequencies are warped: analog frequency $\Omega$ and digital frequency $f$ are related by

$$\Omega = 2 f_s \tan\frac{\pi f}{f_s} .$$

Near 0 Hz the warping is negligible ($\tan x \approx x$). Toward Nyquist it grows without bound. $K = \tan(\pi f_c / f_s)$ (= 0.0945 here) is this relation evaluated at the cutoff: the cutoff as the analog prototype must see it.

Prewarping. Designing the analog prototype at the warped frequency, i.e. using $K = \tan(\pi f_c/f_s)$ instead of the naive $\pi f_c / f_s$, so that after the bilinear transform the digital filter’s −3 dB point lands exactly on $f_c$. Without prewarping, the cutoff drifts low, and the drift grows as $f_c$ approaches Nyquist. At $f_s$ = 1 kHz:

Designed $f_c$	Actual −3 dB, prewarped	Actual −3 dB, not prewarped
30 Hz (this example)	30.0 Hz	29.9 Hz (negligible)
100 Hz	100.0 Hz	96.9 Hz
250 Hz	250.0 Hz	211.9 Hz

The same warping explains the stopband number. An analog 2nd-order Butterworth gives about −31 dB at 6× $f_c$. Compressing the frequency axis toward Nyquist adds attenuation up there, hence −33.1 dB at 180 Hz.

Regenerating the figures

The figures are generated from the C++ example, so they show exactly what the filter computes:

cmake --preset gcc-debug && cmake --build --preset gcc-debug
mkdir -p build/gyro-trace
REFLEX_DSP_TRACE_DIR=build/gyro-trace ./build/gcc-debug/tests/dsp_lowpass "*phase delay*"
node docs-site/scripts/gen-gyro-figures.mjs build/gyro-trace

The SVGs under docs-site/src/figures/gyro-lowpass/ are committed; the docs build doesn’t run C++.

MTL5 note

The filter’s input enters as a 1-vector (B * u with u a dense_vector) rather than B * scalar. MTL5’s scalar×matrix and scalar×vector operators currently accept only built-in arithmetic types (stillwater-sc/mtl5#538).